DyDynamics.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.  
 
#ifndef DY_DYNAMICS_H
#define DY_DYNAMICS_H
 
#include "PxvConfig.h"
#include "CmSpatialVector.h"
#include "CmTask.h"
#include "CmPool.h"
#include "PxcThreadCoherentCache.h"
#include "DyThreadContext.h"
#include "PxcConstraintBlockStream.h"
#include "DySolverBody.h"
#include "DyContext.h"
#include "PxsIslandManagerTypes.h"
#include "PxvNphaseImplementationContext.h"
#include "solver/PxSolverDefs.h"
 
namespace physx
{
 
namespace Cm
{
    class FlushPool;
}
 
namespace IG
{
    class SimpleIslandManager;
    struct Edge;
}
 
class PxsRigidBody;
 
struct PxsBodyCore;
class PxsIslandIndices;
struct PxsIndexedInteraction;
struct PxsIndexedContactManager;
struct PxSolverConstraintDesc;
 
namespace Cm
{
    class SpatialVector;
}
 
namespace Dy
{
    class SolverCore;
    struct SolverIslandParams;
    struct ArticulationSolverDesc;
    class Articulation;
    class DynamicsContext;
 
 
 
 
#define SOLVER_PARALLEL_METHOD_ARGS                                 \
    DynamicsContext&    context,                                    \
    SolverIslandParams& params,                                     \
    IG::IslandSim& islandSim
 
//typedef   void (*PxsSolveParallelMethod)(SOLVER_PARALLEL_METHOD_ARGS);
//extern PxsSolveParallelMethod solveParallel[3];
 
void solveParallel(SOLVER_PARALLEL_METHOD_ARGS);
void solveParallelCouloumFriction(SOLVER_PARALLEL_METHOD_ARGS);
 
 
struct SolverIslandObjects;
 
class SolverBodyPool : public PxArray<PxSolverBody, PxAlignedAllocator<128, PxReflectionAllocator<PxSolverBody> > > 
{ 
    PX_NOCOPY(SolverBodyPool)
public:
    SolverBodyPool() {}
};
 
class SolverBodyDataPool : public PxArray<PxSolverBodyData, PxAlignedAllocator<128, PxReflectionAllocator<PxSolverBodyData> > >
{
    PX_NOCOPY(SolverBodyDataPool)
public:
    SolverBodyDataPool() {}
};
 
class SolverConstraintDescPool : public PxArray<PxSolverConstraintDesc, PxAlignedAllocator<128, PxReflectionAllocator<PxSolverConstraintDesc> > >
{
    PX_NOCOPY(SolverConstraintDescPool)
public:
    SolverConstraintDescPool() { }
};
 
struct IslandContext
{
    //The thread context for this island (set in in the island start task, released in the island end task)
    ThreadContext*      mThreadContext;
    PxsIslandIndices    mCounts;
};
 
 
#if PX_VC 
    #pragma warning(push)
    #pragma warning( disable : 4324 ) // Padding was added at the end of a structure because of a __declspec(align) value.
#endif
 
 
class DynamicsContext : public Context
{
    PX_NOCOPY(DynamicsContext)
public:
    
    static DynamicsContext* create( PxcNpMemBlockPool* memBlockPool,
                                    PxcScratchAllocator& scratchAllocator,
                                    Cm::FlushPool& taskPool,
                                    PxvSimStats& simStats,
                                    PxTaskManager* taskManager,
                                    PxVirtualAllocatorCallback* allocator,
                                    PxsMaterialManager* materialManager,
                                    IG::SimpleIslandManager* islandManager,
                                    PxU64 contextID,
                                    const bool enableStabilization,
                                    const bool useEnhancedDeterminism,
                                    const PxReal maxBiasCoefficient,
                                    const bool frictionEveryIteration,
                                    const PxReal lengthScale
                                    );
    
    void                        destroy();
 
    PX_FORCE_INLINE PxSolverBody&       getWorldSolverBody()                    { return mWorldSolverBody;  }
 
    PX_FORCE_INLINE Cm::FlushPool&          getTaskPool()                       { return mTaskPool;         }
 
    PX_FORCE_INLINE ThresholdStream&        getThresholdStream()                    { return *mThresholdStream; }
 
    PX_FORCE_INLINE PxvSimStats&            getSimStats()                           { return mSimStats;         }
 
#if PX_ENABLE_SIM_STATS
    void                                    addThreadStats(const ThreadContext::ThreadSimStats& stats);
#else
    PX_CATCH_UNDEFINED_ENABLE_SIM_STATS
#endif
 
    virtual void                        update(IG::SimpleIslandManager& simpleIslandManager, PxBaseTask* continuation, PxBaseTask* lostTouchTask,
        PxvNphaseImplementationContext* nPhase, const PxU32 maxPatchesPerCM, const PxU32 maxArticulationLinks, const PxReal dt, const PxVec3& gravity, PxBitMapPinned& changedHandleMap);
 
 
    void updatePostKinematic(IG::SimpleIslandManager& simpleIslandManager, PxBaseTask* continuation, PxBaseTask* lostTouchTask, const PxU32 maxLinks);
 
    virtual void                        processLostPatches(IG::SimpleIslandManager& /*simpleIslandManager*/, PxsContactManager** /*lostPatchManagers*/, PxU32 /*nbLostPatchManagers*/, PxsContactManagerOutputCounts* /*outCounts*/){}
    virtual void                        processFoundPatches(IG::SimpleIslandManager& /*simpleIslandManager*/, PxsContactManager** /*foundPatchManagers*/, PxU32 /*nbFoundPatchManagers*/, PxsContactManagerOutputCounts* /*outCounts*/) {}
 
    virtual void                        updateBodyCore(PxBaseTask* continuation);
 
    virtual void                        setSimulationController(PxsSimulationController* simulationController ){ mSimulationController = simulationController; }
    virtual void                            mergeResults();
 
    virtual void                            getDataStreamBase(void*& /*contactStreamBase*/, void*& /*patchStreamBase*/, void*& /*forceAndIndicesStreamBase*/){}
 
    virtual PxSolverType::Enum              getSolverType() const   { return PxSolverType::ePGS;    }
 
    PX_FORCE_INLINE ThreadContext*                  getThreadContext()
    {
        return mThreadContextPool.get();
    }
 
    void                                putThreadContext(ThreadContext* context)
    {
        mThreadContextPool.put(context);
    }
 
 
    PX_FORCE_INLINE PxU32                   getKinematicCount()     const   { return mKinematicCount;   }
    PX_FORCE_INLINE PxU64                   getContextId()          const   { return mContextID;        }
 
protected:
 
                                        DynamicsContext(PxcNpMemBlockPool* memBlockPool,
                                                        PxcScratchAllocator& scratchAllocator,
                                                        Cm::FlushPool& taskPool,
                                                        PxvSimStats& simStats,
                                                        PxTaskManager* taskManager,
                                                        PxVirtualAllocatorCallback* allocator,
                                                        PxsMaterialManager* materialManager,
                                                        IG::SimpleIslandManager* islandManager,
                                                        PxU64 contextID,
                                                        const bool enableStabilization,
                                                        const bool useEnhancedDeterminism,
                                                        const PxReal maxBiasCoefficient,
                                                        const bool frictionEveryIteration,
                                                        const PxReal lengthScale
                                                        );
    virtual                             ~DynamicsContext();
 
 
    // Solver helper-methods
    void                                computeUnconstrainedVelocity(PxsRigidBody* atom)    const;
 
    void                                setDescFromIndices(PxSolverConstraintDesc& desc, const IG::IslandSim& islandSim,
                                        const PxsIndexedInteraction& constraint, const PxU32 solverBodyOffset);
 
 
    void                                setDescFromIndices(PxSolverConstraintDesc& desc, IG::EdgeIndex edgeIndex,
                                            const IG::SimpleIslandManager& islandManager, PxU32* bodyRemapTable, const PxU32 solverBodyOffset);
 
    void                                preIntegrationParallel(
                                               const PxF32 dt,
                                               PxsBodyCore*const* bodyArray,                    // INOUT: core body attributes
                                               PxsRigidBody*const* originalBodyArray,           // IN: original body atom names (LEGACY - DON'T deref the ptrs!!)
                                               PxU32 const* nodeIndexArray,                     // IN: island node index
                                               PxU32 bodyCount,                                 // IN: body count
                                               PxSolverBody* solverBodyPool,                    // IN: solver atom pool (space preallocated)
                                               PxSolverBodyData* solverBodyDataPool,
                                               Cm::SpatialVector* motionVelocityArray,          // OUT: motion velocities
                                               PxU32& maxSolverPositionIterations,
                                               PxU32& maxSolverVelocityIterations,
                                               PxBaseTask& integrateTask
                                               );
 
    void                                solveParallel(SolverIslandParams& params, IG::IslandSim& islandSim, Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV);
 
    
 
    void                                integrateCoreParallel(SolverIslandParams& params, Cm::SpatialVectorF* deltaV, IG::IslandSim& islandSim);
 
 
 
 
    void                                    resetThreadContexts();
 
    PX_FORCE_INLINE PxcScratchAllocator&    getScratchAllocator() { return mScratchAllocator; }
 
    //Data
 
    PX_ALIGN(16, PxSolverBody               mWorldSolverBody);
    PX_ALIGN(16, PxSolverBodyData           mWorldSolverBodyData);
 
    PxcThreadCoherentCache<ThreadContext, PxcNpMemBlockPool> mThreadContextPool;
 
    SolverConstraintDescPool    mSolverConstraintDescPool;
 
    SolverConstraintDescPool    mOrderedSolverConstraintDescPool;
 
    SolverConstraintDescPool    mTempSolverConstraintDescPool;
 
    PxArray<PxConstraintBatchHeader> mContactConstraintBatchHeaders;
 
    PxArray<Cm::SpatialVector> mMotionVelocityArray;
 
    PxArray<PxsBodyCore*>   mBodyCoreArray;
 
    PxArray<PxsRigidBody*> mRigidBodyArray;
 
    PxArray<FeatherstoneArticulation*> mArticulationArray;
 
    SolverBodyPool          mSolverBodyPool;
    SolverBodyDataPool      mSolverBodyDataPool;
 
 
    ThresholdStream*        mExceededForceThresholdStream[2]; //this store previous and current exceeded force thresholdStream  
 
    PxArray<PxU32>      mExceededForceThresholdStreamMask;
 
    SolverCore*             mSolverCore[PxFrictionType::eFRICTION_COUNT];
 
    PxArray<PxU32>      mSolverBodyRemapTable;              //Remaps from the "active island" index to the index within a solver island
 
    PxArray<PxU32>      mNodeIndexArray;                    //island node index
 
    PxArray<PxsIndexedContactManager> mContactList;
    
    PxU32                       mKinematicCount;
 
    PxI32                       mThresholdStreamOut;
 
    
 
    PxsMaterialManager*         mMaterialManager;
 
    PxsContactManagerOutputIterator mOutputIterator;
    
private:
    //private:
    PxcScratchAllocator&                        mScratchAllocator;
    Cm::FlushPool&                              mTaskPool;
    PxTaskManager*                              mTaskManager;
    PxU32                                       mCurrentIndex; // this is the index point to the current exceeded force threshold stream
 
    PxU64                                       mContextID;
 
    protected:
 
    friend class PxsSolverStartTask;
    friend class PxsSolverAticulationsTask;
    friend class PxsSolverSetupConstraintsTask;
    friend class PxsSolverCreateFinalizeConstraintsTask;    
    friend class PxsSolverConstraintPartitionTask;
    friend class PxsSolverSetupSolveTask;
    friend class PxsSolverIntegrateTask;
    friend class PxsSolverEndTask;
    friend class PxsSolverConstraintPostProcessTask;
    friend class PxsForceThresholdTask;
    friend class SolverArticulationUpdateTask;
 
    friend void solveParallel(SOLVER_PARALLEL_METHOD_ARGS);
};
 
#if PX_VC 
    #pragma warning(pop)
#endif
 
}
}
 
#endif