PxsCCD.h

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of NVIDIA CORPORATION nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// Copyright (c) 2008-2022 NVIDIA Corporation. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
 
#include "geometry/PxGeometry.h"
#include "foundation/PxHashMap.h"
#include "foundation/PxUserAllocated.h"
#include "GuCCDSweepConvexMesh.h"
#include "PxsIslandSim.h"
 
#ifndef PXS_CCD_H
#define PXS_CCD_H
 
#define CCD_DEBUG_PRINTS            0
#define CCD_POST_DEPENETRATE_DIST   0.001f
#define CCD_ROTATION_LOCKING        0
#define CCD_MIN_TIME_LEFT           0.01f
 
#define DEBUG_RENDER_CCD            0
 
#if CCD_DEBUG_PRINTS
namespace physx {
    extern void printCCDDebug(const char* msg, const PxsRigidBody* atom0, PxGeometryType::Enum g0, bool printPtr = true);
    extern void printShape(PxsRigidBody* atom0, PxGeometryType::Enum g0, const char* annotation, PxReal dt, PxU32 pass, bool printPtr = true);
}
#define PRINTCCDSHAPE(x) printShape x
#define PRINTCCDDEBUG(x) printCCDDebug x
#else
#define PRINTCCDSHAPE(x)
#define PRINTCCDDEBUG(x)
#endif
 
namespace physx
{
    float   computeCCDThreshold(const PxGeometry& geometry);
 
// ------------------------------------------------------------------------------------------------------------
// a fraction of objects will be CCD active so this is dynamic, not a member of PsxRigidBody
// CCD code builds a temporary array of PxsCCDPair objects (allocated in blocks)
// this is done to gather scattered data from memory and also to reduce PxsRidigBody permanent memory footprint
// we have to do it every pass since new CMs can become fast moving after each pass (and sometimes cease to be)
//
struct PxsCCDBody;
class PxsRigidBody;
struct PxsShapeCore;
struct PxsRigidCore;
class PxsContactManager;
class PxsContext;
class PxCCDContactModifyCallback;
class PxcNpThreadContext;
 
class PxvNphaseImplementationContext;
 
namespace Dy
{
    class ThresholdStream;
}
 
struct PxsCCDOverlap
{
    //The body the interaction relates to
    PxsCCDBody* mBody;
    //The next interaction in the list
    PxsCCDOverlap* mNext;
};
 
struct PxsCCDShape : public Gu::CCDShape
{
public:
    const PxsShapeCore*     mShapeCore;     //Shape core (can be shared)
    const PxsRigidCore*     mRigidCore;     //Rigid body core
    PxNodeIndex             mNodeIndex;
 
    PxTransform             getAbsPose(const PxsRigidBody* atom)            const;
    PxTransform             getLastCCDAbsPose(const PxsRigidBody* atom)     const;                      
};
 
struct PxsCCDBody
{
    Cm::SpatialVector       mPreSolverVelocity;
    PxU16                   mIndex;                     //The CCD body's index
    bool                    mPassDone;                  //Whether it has been processed in the current CCD pass
    bool                    mHasAnyPassDone;            //Whether this body was influenced by any passes
    PxReal                  mTimeLeft;                  //CCD time left to elapse (normalized in range 0-1)
    PxsRigidBody*           mBody;                      //The rigid body 
    PxsCCDOverlap*          mOverlappingObjects;        //A list of overlapping bodies for island update
    PxU32                   mUpdateCount;               //How many times this body has eben updated in the CCD. This is correlated with CCD shapes' update counts.
    PxU32                   mNbInteractionsThisPass;    //How many interactions this pass
 
    PX_FORCE_INLINE PxU32 getIndex() const { return mIndex; }
 
    bool overlaps(PxsCCDBody* body) const
    {
        PxsCCDOverlap* overlaps = mOverlappingObjects;
 
        while(overlaps)
        {
            if(overlaps->mBody == body)
                return true;
            overlaps = overlaps->mNext;
        }
        return false;
    }
 
    void addOverlap(PxsCCDOverlap* overlap)
    {
        overlap->mNext = mOverlappingObjects;
        mOverlappingObjects = overlap;
    }
 
};
 
template<typename T, int BLOCK_SIZE>
struct PxsCCDBlockArray
{
    struct Block : PxUserAllocated { T items[BLOCK_SIZE]; };
    struct BlockInfo
    {
        Block* block;
        PxU32 count; // number of elements in this block
        BlockInfo(Block* aBlock, PxU32 aCount) : block(aBlock), count(aCount) {}
    };
    /*
    \brief An array of block headers
    */
    PxArray<BlockInfo> blocks;
    PxU32 currentBlock;
 
    PxsCCDBlockArray() : currentBlock(0)
    {
        blocks.pushBack(BlockInfo(PX_NEW(Block), 0));
    }
 
    ~PxsCCDBlockArray()
    {
        for (PxU32 i = 0; i < blocks.size(); i++)
        {
            PX_DELETE(blocks[i].block);
        }
        currentBlock = 0;
    }
 
    void clear()
    {
        for (PxU32 i = 0; i < blocks.size(); i++)
        {
            PX_DELETE(blocks[i].block);
        }
        blocks.clear();
        blocks.pushBack(BlockInfo(PX_NEW(Block), 0)); // at least one block is expected to always be present in the array
        currentBlock = 0;
    }
 
    void clear_NoDelete()
    {
        currentBlock = 0;
        blocks[0].count = 0;
    }
 
    T& pushBack()
    {
        PxU32 numBlocks = blocks.size();
        if (blocks[currentBlock].count == BLOCK_SIZE)
        {
            if((currentBlock + 1) == numBlocks)
            {
                blocks.pushBack(BlockInfo(PX_NEW(Block), 0));
                numBlocks ++;
            }
            currentBlock++;
            blocks[currentBlock].count = 0;
        }
        const PxU32 count = blocks[currentBlock].count ++;
 
        return blocks[currentBlock].block->items[count];
    }
 
    T& pushBack(T& data)
    {
        PxU32 numBlocks = blocks.size();
        if (blocks[currentBlock].count == BLOCK_SIZE)
        {
            if((currentBlock + 1) == numBlocks)
            {
                blocks.pushBack(BlockInfo(PX_NEW(Block), 0));
                numBlocks ++;
            }
            currentBlock++;
            blocks[currentBlock].count = 0;
        }
        const PxU32 count = blocks[currentBlock].count ++;
        blocks[currentBlock].block->items[count] = data;
        return blocks[currentBlock].block->items[count];
    }
 
    void popBack()
    {
        PX_ASSERT(blocks[currentBlock].count > 0);
        if (blocks[currentBlock].count > 1)
            blocks[currentBlock].count --;
        else
        {
            PX_DELETE(blocks[currentBlock].block);
            blocks.popBack();
            currentBlock--;
        }
    }
 
    PxU32 size() const
    {
        return (currentBlock)*BLOCK_SIZE + blocks[currentBlock].count;
    }
 
    T& operator[] (PxU32 index) const
    {
        PX_ASSERT(index/BLOCK_SIZE < blocks.size());
        PX_ASSERT(index%BLOCK_SIZE < blocks[index/BLOCK_SIZE].count);
        return blocks[index/BLOCK_SIZE].block->items[index%BLOCK_SIZE];
    }
};
 
struct PxsCCDPair
{
    enum E_TOIType
    {
        eEstimate,
        ePrecise
    };
    PxsRigidBody*           mBa0;                   // Body A. Can be NULL for statics
    PxsRigidBody*           mBa1;                   // Body B. Can be NULL for statics
    PxsCCDShape*            mCCDShape0;             // Shape A
    PxsCCDShape*            mCCDShape1;             // Shape B
    PxVec3                  mMinToiNormal;          // The contact normal. Only valid for precise results. On the surface of body/shape A
    PxReal                  mMinToi;                // Min TOI. Valid for both precise and estimated results but estimates may be too early (i.e. conservative).
    PxReal                  mPenetrationPostStep;   // Valid only for precise sweeps. Only used for initial intersections (i.e. at TOI = 0).
    PxVec3                  mMinToiPoint;           // The contact point. Only valid for precise sweep results.
    PxReal                  mPenetration;           // The penetration. Only valid for precise sweep results.
    PxsContactManager*      mCm;                    // The contact manager.
    PxU32                   mIslandId;              // The index of the island this pair is in
    PxGeometryType::Enum    mG0, mG1;               // The geometry types for shapes 0 and 1
    bool                    mIsEarliestToiHit;      // Indicates this was the earliest hit for one of the bodies in the pair
    bool                    mIsModifiable;          // Indicates whether this contact is modifiable
    PxU32                   mFaceIndex;             // The face index. Only valid for precise sweeps involving meshes or heightfields.
    PxU16                   mMaterialIndex0;        // The material index for shape 0
    PxU16                   mMaterialIndex1;        // The material index for shape 1
    PxReal                  mDynamicFriction;       // The dynamic friction coefficient
    PxReal                  mStaticFriction;        // The static friction coefficient
    PxReal                  mRestitution;           // The restitution coefficient
    PxU32                   mEstimatePass;          // The current estimation pass. Used after a sweep hit was found to determine if the pair needs re-estimating.
    PxReal                  mAppliedForce;          // The applied force for this pair. Only valid if the pair has been responded to.
    PxReal                  mMaxImpulse;            // The maximum impulse to be applied 
 
    E_TOIType               mToiType;               // The TOI type (estimate, precise).
    bool                    mHasFriction;           // Whether we want to simulate CCD friction for this pair
 
    PxReal  sweepFindToi(PxcNpThreadContext& threadContext, PxReal dt, PxU32 pass, PxReal ccdThreshold);
    PxReal  sweepEstimateToi(PxReal ccdThreshold);
    bool    sweepAdvanceToToi(PxReal dt, bool clipTrajectoryToToi);
    void    updateShapes();
};
 
typedef PxsCCDBlockArray<PxsCCDBody, 128> PxsCCDBodyArray;
typedef PxsCCDBlockArray<PxsCCDPair, 128> PxsCCDPairArray;
typedef PxsCCDBlockArray<PxsCCDOverlap, 128> PxsCCDOverlapArray;
typedef PxsCCDBlockArray<PxsCCDShape, 128> PxsCCDShapeArray;
 
typedef PxPair<const PxsRigidCore*, const PxsShapeCore*> PxsRigidShapePair;
 
class PxsCCDContext : public PxUserAllocated
{
public:
    PxsCCDContext(PxsContext* context, Dy::ThresholdStream& thresholdStream, PxvNphaseImplementationContext& nPhaseContext, PxReal ccdThreshold);
    ~PxsCCDContext();
 
    PX_FORCE_INLINE     PxCCDContactModifyCallback* getCCDContactModifyCallback()                   const       { return mCCDContactModifyCallback; }
    PX_FORCE_INLINE     void                        setCCDContactModifyCallback(PxCCDContactModifyCallback* c)  { mCCDContactModifyCallback = c;    }
    PX_FORCE_INLINE     PxU32                   getCCDMaxPasses()               const                       { return mCCDMaxPasses; }
    PX_FORCE_INLINE     void                    setCCDMaxPasses(PxU32 ccdMaxPasses)                         { mCCDMaxPasses = ccdMaxPasses;     }
    PX_FORCE_INLINE     PxU32                   getCurrentCCDPass()             const                       { return miCCDPass;                 }
    PX_FORCE_INLINE     PxI32                   getNumSweepHits()               const                       { return mSweepTotalHits;           }
    PX_FORCE_INLINE     PxU32                   getNumUpdatedBodies()           const                       { return mUpdatedCCDBodies.size(); }
    PX_FORCE_INLINE     PxsRigidBody*const*         getUpdatedBodies()              const                       { return mUpdatedCCDBodies.begin(); }
 
    PX_FORCE_INLINE     void                        clearUpdatedBodies()                                        { mUpdatedCCDBodies.forceSize_Unsafe(0); }
 
    PX_FORCE_INLINE     PxReal                      getCCDThreshold() const                                     { return mCCDThreshold; }
    PX_FORCE_INLINE     void                        setCCDThreshold(PxReal t)                                   { mCCDThreshold = t;    }
 
                        void                    runCCDModifiableContact(PxModifiableContact* PX_RESTRICT contacts, PxU32 contactCount, const PxsShapeCore* PX_RESTRICT shapeCore0, 
                                                    const PxsShapeCore* PX_RESTRICT shapeCore1, const PxsRigidCore* PX_RESTRICT rigidCore0, const PxsRigidCore* PX_RESTRICT rigidCore1,
                                                    const PxsRigidBody* PX_RESTRICT rigid0, const PxsRigidBody* PX_RESTRICT rigid1);
    
                        void                    updateCCD(PxReal dt, PxBaseTask* continuation, IG::IslandSim& islandSim, bool disableResweep, PxI32 numFastMovingShapes);
 
                        void                    updateCCDBegin();
 
                        void                    resetContactManagers();
 
private:
    
                        void                    verifyCCDBegin();
 
                        void                    updateCCDEnd();
 
                        void                    postCCDSweep(PxBaseTask* continuation);
                        void                    postCCDAdvance(PxBaseTask* continuation);
                        void                    postCCDDepenetrate(PxBaseTask* continuation);
 
        typedef Cm::DelegateTask<PxsCCDContext, &PxsCCDContext::postCCDSweep> PostCCDSweepTask;
        typedef Cm::DelegateTask<PxsCCDContext, &PxsCCDContext::postCCDAdvance> PostCCDAdvanceTask;
        typedef Cm::DelegateTask<PxsCCDContext, &PxsCCDContext::postCCDDepenetrate> PostCCDDepenetrateTask;
 
        PostCCDSweepTask mPostCCDSweepTask;
        PostCCDAdvanceTask mPostCCDAdvanceTask;
        PostCCDDepenetrateTask mPostCCDDepenetrateTask;
 
        PxCCDContactModifyCallback*                 mCCDContactModifyCallback;
        
        // CCD global data
        bool                    mDisableCCDResweep;
        PxU32                   miCCDPass;
        PxI32                   mSweepTotalHits;
 
        // a fraction of objects will be CCD active so PxsCCDBody is dynamic, not a member of PxsRigidBody
        PxsCCDBodyArray mCCDBodies;
        PxsCCDOverlapArray mCCDOverlaps;
        PxsCCDShapeArray mCCDShapes;
        PxArray<PxsCCDBody*> mIslandBodies;
        PxArray<PxU16> mIslandSizes;
        PxArray<PxsRigidBody*> mUpdatedCCDBodies;
        PxHashMap<PxsRigidShapePair, PxsCCDShape*> mMap;
 
        // temporary array updated during CCD update
        //Array<PxsCCDPair> mCCDPairs;
        PxsCCDPairArray mCCDPairs;
        PxArray<PxsCCDPair*> mCCDPtrPairs;
        // number of pairs per island
        PxArray<PxU32> mCCDIslandHistogram; 
        // thread context valid during CCD update
        PxcNpThreadContext* mCCDThreadContext;
        // number of pairs to process per thread
        PxU32 mCCDPairsPerBatch;
        PxU32 mCCDMaxPasses;
 
        PxsContext* mContext;
        Dy::ThresholdStream& mThresholdStream;
 
        PxvNphaseImplementationContext& mNphaseContext;
 
        PxMutex mMutex;
 
        PxReal mCCDThreshold;
 
private:
 
    PX_NOCOPY(PxsCCDContext)
};
 
 
}
 
 
 
#endif