This commit is contained in:
2025-11-28 23:13:44 +05:30
commit a3a8e79709
7360 changed files with 1156074 additions and 0 deletions

View File

@ -0,0 +1,517 @@
//
// 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-2021 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_TGS_DYNAMICS_H
#define DY_TGS_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"
#include "PxsIslandSim.h"
namespace physx
{
namespace Cm
{
class FlushPool;
}
namespace IG
{
class SimpleIslandManager;
struct Edge;
}
class PxsRigidBody;
class PxsStreamedThresholdTable;
struct PxsBodyCore;
struct PxsIslandObjects;
class PxsIslandIndices;
struct PxsIndexedInteraction;
class PxsIslandManager;
struct PxsIndexedConstraint;
struct PxsIndexedContactManager;
class PxsHeapMemoryAllocator;
class PxsMemoryManager;
class PxsDefaultMemoryManager;
namespace Cm
{
class Bitmap;
class SpatialVector;
}
namespace Dy
{
class SolverCore;
struct SolverIslandParams;
struct ArticulationSolverDesc;
class ArticulationV;
class DynamicsContext;
struct SolverContext;
struct SolverIslandObjectsStep
{
PxsRigidBody** bodies;
ArticulationV** articulations;
ArticulationV** articulationOwners;
PxsIndexedContactManager* contactManagers;
const IG::IslandId* islandIds;
PxU32 numIslands;
PxU32* bodyRemapTable;
PxU32* nodeIndexArray;
PxSolverConstraintDesc* constraintDescs;
PxSolverConstraintDesc* orderedConstraintDescs;
PxSolverConstraintDesc* tempConstraintDescs;
PxConstraintBatchHeader* constraintBatchHeaders;
Cm::SpatialVector* motionVelocities;
PxsBodyCore** bodyCoreArray;
PxU32 solverBodyOffset;
SolverIslandObjectsStep() : bodies(NULL), articulations(NULL), articulationOwners(NULL),
contactManagers(NULL), islandIds(NULL), numIslands(0), nodeIndexArray(NULL), constraintDescs(NULL), motionVelocities(NULL), bodyCoreArray(NULL),
solverBodyOffset(0)
{
}
};
struct IslandContextStep
{
//The thread context for this island (set in in the island start task, released in the island end task)
ThreadContext* mThreadContext;
PxsIslandIndices mCounts;
SolverIslandObjectsStep mObjects;
PxU32 mPosIters;
PxU32 mVelIters;
PxU32 mArticulationOffset;
PxReal mStepDt;
PxReal mInvStepDt;
PxI32 mSharedSolverIndex;
PxI32 mSolvedCount;
PxI32 mSharedRigidBodyIndex;
PxI32 mRigidBodyIntegratedCount;
PxI32 mSharedArticulationIndex;
PxI32 mArticulationIntegratedCount;
};
struct SolverIslandObjectsStep;
class SolverBodyVelDataPool : public Ps::Array<PxTGSSolverBodyVel, Ps::AlignedAllocator<128, Ps::ReflectionAllocator<PxTGSSolverBodyVel> > >
{
PX_NOCOPY(SolverBodyVelDataPool)
public:
SolverBodyVelDataPool() {}
};
class SolverBodyTxInertiaPool : public Ps::Array<PxTGSSolverBodyTxInertia, Ps::AlignedAllocator<128, Ps::ReflectionAllocator<PxTGSSolverBodyTxInertia> > >
{
PX_NOCOPY(SolverBodyTxInertiaPool)
public:
SolverBodyTxInertiaPool() {}
};
class SolverBodyDataStepPool : public Ps::Array<PxTGSSolverBodyData, Ps::AlignedAllocator<128, Ps::ReflectionAllocator<PxTGSSolverBodyData> > >
{
PX_NOCOPY(SolverBodyDataStepPool)
public:
SolverBodyDataStepPool() {}
};
class SolverStepConstraintDescPool : public Ps::Array<PxSolverConstraintDesc, Ps::AlignedAllocator<128, Ps::ReflectionAllocator<PxSolverConstraintDesc> > >
{
PX_NOCOPY(SolverStepConstraintDescPool)
public:
SolverStepConstraintDescPool() { }
};
#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 DynamicsTGSContext : public Context
{
PX_NOCOPY(DynamicsTGSContext)
public:
/**PxBaseTask* continuation
\brief Creates a DynamicsContext associated with a PxsContext
\return A pointer to the newly-created DynamicsContext.
*/
static DynamicsTGSContext* create(PxcNpMemBlockPool* memBlockPool,
PxcScratchAllocator& scratchAllocator,
Cm::FlushPool& taskPool,
PxvSimStats& simStats,
PxTaskManager* taskManager,
Ps::VirtualAllocatorCallback* allocator,
PxsMaterialManager* materialManager,
IG::IslandSim* accurateIslandSim,
PxU64 contextID,
const bool enableStabilization,
const bool useEnhancedDeterminism,
const bool useAdaptiveForce,
const PxReal lengthScale
);
/**
\brief Destroys this DynamicsContext
*/
void destroy();
/**
\brief Returns the static world solver body
\return The static world solver body.
*/
//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);
#endif
/**
\brief The entry point for the constraint solver.
\param[in] dt The simulation time-step
\param[in] continuation The continuation task for the solver
This method is called after the island generation has completed. Its main responsibilities are:
(1) Reserving the solver body pools
(2) Initializing the static and kinematic solver bodies, which are shared resources between islands.
(3) Construct the solver task chains for each island
Each island is solved as an independent solver task chain in parallel.
*/
virtual void update(IG::SimpleIslandManager& simpleIslandManager, PxBaseTask* continuation, PxBaseTask* lostTouchTask,
PxsContactManager** foundPatchManagers, PxU32 nbFoundPatchManagers, PxsContactManager** lostPatchManagers, PxU32 nbLostPatchManagers,
PxU32 maxPatchesPerCM, PxsContactManagerOutputIterator& iter, PxsContactManagerOutput* gpuOutputs, const PxReal dt, const PxVec3& gravity, const PxU32 bitMapWordCounts);
void updatePostKinematic(IG::SimpleIslandManager& simpleIslandManager, PxBaseTask* continuation,
PxBaseTask* lostTouchTask);
virtual void processLostPatches(IG::SimpleIslandManager& /*simpleIslandManager*/, PxsContactManager** /*lostPatchManagers*/, PxU32 /*nbLostPatchManagers*/, PxsContactManagerOutputIterator& /*iterator*/){}
virtual void updateBodyCore(PxBaseTask* continuation);
virtual void setSimulationController(PxsSimulationController* simulationController){ mSimulationController = simulationController; }
/**
\brief This method combines the results of several islands, e.g. constructing scene-level simulation statistics and merging together threshold streams for contact notification.
*/
virtual void mergeResults();
virtual void getDataStreamBase(void*& /*contactStreamBase*/, void*& /*patchStreamBase*/, void*& /*forceAndIndicesStreamBase*/){}
/**
\brief Allocates and returns a thread context object.
\return A thread context.
*/
PX_FORCE_INLINE ThreadContext* getThreadContext()
{
return mThreadContextPool.get();
}
/**
\brief Returns a thread context to the thread context pool.
\param[in] context The thread context to return to the thread context pool.
*/
void putThreadContext(ThreadContext* context)
{
mThreadContextPool.put(context);
}
PX_FORCE_INLINE PxU32 getKinematicCount() const { return mKinematicCount; }
PX_FORCE_INLINE PxU64 getContextId() const { return mContextID; }
PX_FORCE_INLINE PxReal getLengthScale() const { return mLengthScale; }
protected:
/**
\brief Constructor for DynamicsContext
*/
DynamicsTGSContext(PxcNpMemBlockPool* memBlockPool,
PxcScratchAllocator& scratchAllocator,
Cm::FlushPool& taskPool,
PxvSimStats& simStats,
PxTaskManager* taskManager,
Ps::VirtualAllocatorCallback* allocator,
PxsMaterialManager* materialManager,
IG::IslandSim* accurateIslandSim,
PxU64 contextID,
const bool enableStabilization,
const bool useEnhancedDeterminism,
const bool useAdaptiveForce,
const PxReal lengthScale
);
/**
\brief Destructor for DynamicsContext
*/
virtual ~DynamicsTGSContext();
// Solver helper-methods
/**
\brief Computes the unconstrained velocity for a given PxsRigidBody
\param[in] atom The PxsRigidBody
*/
void computeUnconstrainedVelocity(PxsRigidBody* atom) const;
/**
\brief fills in a PxSolverConstraintDesc from an indexed interaction
\param[in,out] desc The PxSolverConstraintDesc
\param[in] constraint The PxsIndexedInteraction
*/
void setDescFromIndices(PxSolverConstraintDesc& desc,
const PxsIndexedInteraction& constraint, const PxU32 solverBodyOffset, PxTGSSolverBodyVel* solverBodies);
void setDescFromIndices(PxSolverConstraintDesc& desc, IG::EdgeIndex edgeIndex,
const IG::SimpleIslandManager& islandManager, PxU32* bodyRemapTable, const PxU32 solverBodyOffset, PxTGSSolverBodyVel* solverBodies);
void solveIsland(const SolverIslandObjectsStep& objects,
const PxsIslandIndices& counts,
const PxU32 solverBodyOffset,
IG::SimpleIslandManager& islandManager,
PxU32* bodyRemapTable, PxsMaterialManager* materialManager,
PxsContactManagerOutputIterator& iterator,
PxBaseTask* continuation);
void prepareBodiesAndConstraints(const SolverIslandObjectsStep& objects,
IG::SimpleIslandManager& islandManager,
IslandContextStep& islandContext);
void setupDescs(IslandContextStep& islandContext, const SolverIslandObjectsStep& objects, IG::SimpleIslandManager& mIslandManager, PxU32* mBodyRemapTable, PxU32 mSolverBodyOffset,
PxsContactManagerOutputIterator& outputs);
void preIntegrateBodies(PxsBodyCore** bodyArray, PxsRigidBody** originalBodyArray,
PxTGSSolverBodyVel* solverBodyVelPool, PxTGSSolverBodyTxInertia* solverBodyTxInertia, PxTGSSolverBodyData* solverBodyDataPool2,
PxU32* nodeIndexArray, const PxU32 bodyCount, const PxVec3& gravity, const PxReal dt, PxU32& posIters, PxU32& velIters, PxU32 iteration);
void setupArticulations(IslandContextStep& islandContext, const PxVec3& gravity, const PxReal dt, PxU32& posIters, PxU32& velIters, PxBaseTask* continuation);
PxU32 setupArticulationInternalConstraints(IslandContextStep& islandContext, PxReal dt, PxReal invStepDt, PxSolverConstraintDesc* constraintDescs);
void createSolverConstraints(PxSolverConstraintDesc* contactDescPtr, PxConstraintBatchHeader* headers, const PxU32 nbHeaders,
PxsContactManagerOutputIterator& outputs, Dy::ThreadContext& islandThreadContext, Dy::ThreadContext& threadContext, PxReal stepDt, PxReal totalDt,
PxReal invStepDt, PxU32 nbSubsteps);
void solveConstraintsIteration(const PxSolverConstraintDesc* const contactDescPtr, const PxConstraintBatchHeader* const batchHeaders, const PxU32 nbHeaders, PxReal invStepDt,
const PxTGSSolverBodyTxInertia* const solverTxInertia, const PxReal elapsedTime, const PxReal minPenetration, SolverContext& cache);
void solveConcludeConstraintsIteration(const PxSolverConstraintDesc* const contactDescPtr, const PxConstraintBatchHeader* const batchHeaders, const PxU32 nbHeaders,
PxTGSSolverBodyTxInertia* solverTxInertia, const PxReal elapsedTime, SolverContext& cache);
void parallelSolveConstraints(const PxSolverConstraintDesc* const contactDescPtr, const PxConstraintBatchHeader* const batchHeaders, const PxU32 nbHeaders, PxTGSSolverBodyTxInertia* solverTxInertia,
const PxReal elapsedTime, const PxReal minPenetration, SolverContext& cache);
void writebackConstraintsIteration(const PxConstraintBatchHeader* const hdrs, const PxSolverConstraintDesc* const contactDescPtr, const PxU32 nbHeaders);
void parallelWritebackConstraintsIteration(const PxSolverConstraintDesc* const contactDescPtr, const PxConstraintBatchHeader* const batchHeaders, const PxU32 nbHeaders);
void integrateBodies(const SolverIslandObjectsStep& objects,
const PxU32 count, PxTGSSolverBodyVel* vels,
PxTGSSolverBodyTxInertia* txInertias, const PxTGSSolverBodyData*const bodyDatas, PxReal dt);
void parallelIntegrateBodies(PxTGSSolverBodyVel* vels, PxTGSSolverBodyTxInertia* txInertias,
const PxTGSSolverBodyData* const bodyDatas, const PxU32 count, PxReal dt);
void copyBackBodies(const SolverIslandObjectsStep& objects,
PxTGSSolverBodyVel* vels, PxTGSSolverBodyTxInertia* txInertias,
PxTGSSolverBodyData* solverBodyData, PxReal invDt, IG::IslandSim& islandSim,
PxU32 startIdx, PxU32 endIdx);
void updateArticulations(Dy::ThreadContext& threadContext, const PxU32 startIdx, const PxU32 endIdx, PxReal dt);
void stepArticulations(Dy::ThreadContext& threadContext, const PxsIslandIndices& counts, PxReal dt, PxReal stepInvDt);
void iterativeSolveIsland(const SolverIslandObjectsStep& objects, const PxsIslandIndices& counts, ThreadContext& mThreadContext,
const PxReal stepDt, const PxReal invStepDt, const PxU32 posIters, const PxU32 velIters, SolverContext& cache);
void iterativeSolveIslandParallel(const SolverIslandObjectsStep& objects, const PxsIslandIndices& counts, ThreadContext& mThreadContext,
const PxReal stepDt, const PxU32 posIters, const PxU32 velIters, PxI32* solverCounts, PxI32* integrationCounts, PxI32* articulationIntegrationCounts,
PxI32* solverProgressCount, PxI32* integrationProgressCount, PxI32* articulationProgressCount, PxU32 solverUnrollSize, PxU32 integrationUnrollSize);
void endIsland(ThreadContext& mThreadContext);
void finishSolveIsland(ThreadContext& mThreadContext, const SolverIslandObjectsStep& objects,
const PxsIslandIndices& counts, IG::SimpleIslandManager& islandManager, PxBaseTask* continuation);
/**
\brief Resets the thread contexts
*/
void resetThreadContexts();
/**
\brief Returns the scratch memory allocator.
\return The scratch memory allocator.
*/
PX_FORCE_INLINE PxcScratchAllocator& getScratchAllocator() { return mScratchAllocator; }
//Data
PxTGSSolverBodyVel mWorldSolverBodyVel;
PxTGSSolverBodyTxInertia mWorldSolverBodyTxInertia;
PxTGSSolverBodyData mWorldSolverBodyData2;
/**
\brief A thread context pool
*/
PxcThreadCoherentCache<ThreadContext, PxcNpMemBlockPool> mThreadContextPool;
/**
\brief Solver constraint desc array
*/
SolverStepConstraintDescPool mSolverConstraintDescPool;
SolverStepConstraintDescPool mOrderedSolverConstraintDescPool;
SolverStepConstraintDescPool mTempSolverConstraintDescPool;
Ps::Array<PxConstraintBatchHeader> mContactConstraintBatchHeaders;
/**
\brief Array of motion velocities for all bodies in the scene.
*/
Ps::Array<Cm::SpatialVector> mMotionVelocityArray;
/**
\brief Array of body core pointers for all bodies in the scene.
*/
Ps::Array<PxsBodyCore*> mBodyCoreArray;
/**
\brief Array of rigid body pointers for all bodies in the scene.
*/
Ps::Array<PxsRigidBody*> mRigidBodyArray;
/**
\brief Array of articulationpointers for all articulations in the scene.
*/
Ps::Array<ArticulationV*> mArticulationArray;
SolverBodyVelDataPool mSolverBodyVelPool;
SolverBodyTxInertiaPool mSolverBodyTxInertiaPool;
SolverBodyDataStepPool mSolverBodyDataPool2;
ThresholdStream* mExceededForceThresholdStream[2]; //this store previous and current exceeded force thresholdStream
Ps::Array<PxU32> mExceededForceThresholdStreamMask;
Ps::Array<PxU32> mSolverBodyRemapTable; //Remaps from the "active island" index to the index within a solver island
Ps::Array<PxU32> mNodeIndexArray; //island node index
Ps::Array<PxsIndexedContactManager> mContactList;
/**
\brief The total number of kinematic bodies in the scene
*/
PxU32 mKinematicCount;
/**
\brief Atomic counter for the number of threshold stream elements.
*/
PxI32 mThresholdStreamOut;
PxsMaterialManager* mMaterialManager;
PxsContactManagerOutputIterator mOutputIterator;
PxReal mLengthScale;
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;
friend class SetupDescsTask;
friend class PreIntegrateTask;
friend class SetupArticulationTask;
friend class SetupArticulationInternalConstraintsTask;
friend class SetupSolverConstraintsTask;
friend class SolveIslandTask;
friend class EndIslandTask;
friend class SetupSolverConstraintsSubTask;
friend class ParallelSolveTask;
friend class PreIntegrateParallelTask;
friend class CopyBackTask;
friend class UpdateArticTask;
friend class FinishSolveIslandTask;
};
#if PX_VC
#pragma warning(pop)
#endif
}
}
#endif