A fast 2D and 3D game engine — TypeScript SDK, C++/WebAssembly core, visual editor. Ship one project to web, desktop, WeChat MiniGames, playable ads, and native Android / iOS.
TypeScript 2d-game-engine3d3d-game-engineandroidcppecsgame-editorgame-enginegamedeviostypescriptwebassemblywebglwebgpuwechat-minigame
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// SPDX-FileCopyrightText: Copyright (c) 2008-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
// SPDX-License-Identifier: Apache-2.0
#include "DyTGSDynamics.h"
#include "common/PxProfileZone.h"
#include "DyBodyCoreIntegrator.h"
#include "DySolverCore.h"
#include "DyConstraintPrep.h"
#include "DyConstraintPartition.h"
#include "CmFlushPool.h"
#include "PxsSimpleIslandManager.h"
#include "DyContactPrepShared.h"
#include "DySolverContext.h"
#include "DyTGSContactPrep.h"
#include "DySleep.h"
#include "DyTGS.h"
#include "DyIslandManager.h"
#define PX_USE_BLOCK_SOLVER 1
#define PX_USE_BLOCK_1D 1
namespace physx
{
namespace Dy
{
constexpr bool tUpdateDeltaMotion = true;
constexpr bool tSaveVelocity = true;
constexpr bool tIsVelIter = true;
constexpr bool tIsLastPosIter = true;
static inline void waitForBodyProgress(PxTGSSolverBodyVel& body, PxU32 desiredProgress, PxU32 iteration)
{
const PxI32 target = PxI32(desiredProgress + body.nbStaticInteractions * iteration);
volatile PxI32* progress = reinterpret_cast<PxI32*>(&body.partitionMask);
WAIT_FOR_PROGRESS(progress, target);
}
static inline void incrementBodyProgress(PxTGSSolverBodyVel& body)
{
if (body.nbStaticInteractions != 0)
(*reinterpret_cast<volatile PxU32*>(&body.partitionMask))++;
}
static inline void waitForArticulationProgress(Dy::FeatherstoneArticulation& artic, PxU32 desiredProgress, PxU32 iteration)
{
const PxI32 target = PxI32(desiredProgress + artic.maxSolverFrictionProgress * iteration);
volatile PxI32* progress = reinterpret_cast<PxI32*>(&artic.solverProgress);
WAIT_FOR_PROGRESS(progress, target);
}
static inline void incrementArticulationProgress(Dy::FeatherstoneArticulation& artic)
{
(*reinterpret_cast<volatile PxU32*>(&artic.solverProgress))++;
}
static inline void waitForProgresses(const PxSolverConstraintDesc& desc, PxU32 iteration)
{
if (desc.linkIndexA == PxSolverConstraintDesc::RIGID_BODY)
waitForBodyProgress(*desc.tgsBodyA, desc.progressA, iteration);
else
waitForArticulationProgress(*getArticulationA(desc), desc.progressA, iteration);
if (desc.linkIndexB == PxSolverConstraintDesc::RIGID_BODY)
waitForBodyProgress(*desc.tgsBodyB, desc.progressB, iteration);
else
waitForArticulationProgress(*getArticulationB(desc), desc.progressB, iteration);
}
static inline void incrementProgress(const PxSolverConstraintDesc& desc)
{
if (desc.linkIndexA == PxSolverConstraintDesc::RIGID_BODY)
{
incrementBodyProgress(*desc.tgsBodyA);
}
else
incrementArticulationProgress(*getArticulationA(desc));
if (desc.linkIndexB == PxSolverConstraintDesc::RIGID_BODY)
incrementBodyProgress(*desc.tgsBodyB);
else if(desc.articulationA != desc.articulationB)
incrementArticulationProgress(*getArticulationB(desc));
}
Context* createTGSDynamicsContext( PxcNpMemBlockPool* memBlockPool, Cm::FlushPool& taskPool, PxvSimStats& simStats,
Cm::VirtualAllocatorCallback& allocator, PxsMaterialManager* materialManager,
IG::SimpleIslandManager& islandManager, PxU64 contextID, PxReal lengthScale, PxSceneFlags sceneFlags)
{
return PX_NEW(DynamicsTGSContext)(memBlockPool, taskPool, simStats, allocator, materialManager, islandManager, contextID, lengthScale, sceneFlags);
}
void DynamicsTGSContext::destroy()
{
this->~DynamicsTGSContext();
PX_FREE_THIS;
}
void copyToSolverBodyDataStep(const PxVec3& linearVelocity, const PxVec3& angularVelocity, PxReal invMass, const PxVec3& invInertia, const PxTransform& globalPose,
PxReal maxDepenetrationVelocity, PxReal maxContactImpulse, PxU32 nodeIndex, PxReal reportThreshold,
PxReal maxAngVelSq, PxU32 lockFlags, bool isKinematic,
PxTGSSolverBodyVel& solverVel, PxTGSSolverBodyTxInertia& solverBodyTxInertia, PxTGSSolverBodyData& solverBodyData, PxReal dt,
bool gyroscopicForces)
{
const PxMat33Padded rotation(globalPose.q);
const PxVec3 sqrtInvInertia = computeSafeSqrtInertia(invInertia);
const PxVec3 sqrtBodySpaceInertia = Cm::safeRecip<PxVec3>(sqrtInvInertia);
// PT: we reuse rotation below so here we call the initial transformInertiaTensor code.
// We could probably do better, perhaps merging the two transformInertiaTensor calls.
Cm::transformInertiaTensor(sqrtInvInertia, rotation, solverBodyTxInertia.sqrtInvInertia);
solverBodyTxInertia.body2WorldP = globalPose.p;
solverBodyTxInertia.deltaBody2WorldQ = PxQuat(PxIdentity);
PxMat33 sqrtInertia;
Cm::transformInertiaTensor(sqrtBodySpaceInertia, rotation, sqrtInertia);
PxVec3 lv = linearVelocity;
PxVec3 av = angularVelocity;
if (gyroscopicForces)
{
const PxVec3 localInertia = Cm::safeRecip<PxVec3>(invInertia);
PxVec3 localAngVel = globalPose.q.rotateInv(av);
PxVec3 origMom = localInertia.multiply(localAngVel);
PxVec3 torque = -localAngVel.cross(origMom);
PxVec3 newMom = origMom + torque * dt;
const PxReal denom = newMom.magnitude();
PxReal ratio = denom > 0.f ? origMom.magnitude() / denom : 0.f;
newMom *= ratio;
PxVec3 newDeltaAngVel = globalPose.q.rotate(invInertia.multiply(newMom) - localAngVel);
av += newDeltaAngVel;
}
if (lockFlags)
{
if (lockFlags & PxRigidDynamicLockFlag::eLOCK_LINEAR_X)
lv.x = 0.f;
if (lockFlags & PxRigidDynamicLockFlag::eLOCK_LINEAR_Y)
lv.y = 0.f;
if (lockFlags & PxRigidDynamicLockFlag::eLOCK_LINEAR_Z)
lv.z = 0.f;
//KS - technically, we can zero the inertia columns and produce stiffer constraints. However, this can cause numerical issues with the
//joint solver, which is fixed by disabling joint preprocessing and setting minResponseThreshold to some reasonable value > 0. However, until
//this is handled automatically, it's probably better not to zero these inertia rows
if (lockFlags & PxRigidDynamicLockFlag::eLOCK_ANGULAR_X)
{
av.x = 0.f;
//data.sqrtInvInertia.column0 = PxVec3(0.f);
}
if (lockFlags & PxRigidDynamicLockFlag::eLOCK_ANGULAR_Y)
{
av.y = 0.f;
//data.sqrtInvInertia.column1 = PxVec3(0.f);
}
if (lockFlags & PxRigidDynamicLockFlag::eLOCK_ANGULAR_Z)
{
av.z = 0.f;
//data.sqrtInvInertia.column2 = PxVec3(0.f);
}
}
solverVel.linearVelocity = lv;
solverVel.angularVelocity = sqrtInertia * av;
solverVel.deltaLinDt = PxVec3(0.f);
solverVel.deltaAngDt = PxVec3(0.f);
solverVel.lockFlags = PxU16(lockFlags);
solverVel.isKinematic = isKinematic;
solverVel.maxAngVel = PxSqrt(maxAngVelSq);
solverVel.partitionMask = 0;
solverBodyData.nodeIndex = nodeIndex;
solverBodyData.invMass = invMass;
solverBodyData.penBiasClamp = maxDepenetrationVelocity;
solverBodyData.maxContactImpulse = maxContactImpulse;
solverBodyData.reportThreshold = reportThreshold;
solverBodyData.originalLinearVelocity = lv;
solverBodyData.originalAngularVelocity = av;
PX_ASSERT(lv.isFinite());
PX_ASSERT(av.isFinite());
}
static void copyToSolverBodyDataStepKinematic(const PxVec3& linearVelocity, const PxVec3& angularVelocity, const PxTransform& globalPose,
PxReal maxDepenetrationVelocity, PxReal maxContactImpulse, PxU32 nodeIndex, PxReal reportThreshold, PxReal maxAngVelSq,
PxTGSSolverBodyVel& solverVel, PxTGSSolverBodyTxInertia& solverBodyTxInertia, PxTGSSolverBodyData& solverBodyData)
{
const PxMat33Padded rotation(globalPose.q);
solverBodyTxInertia.body2WorldP = globalPose.p;
solverBodyTxInertia.deltaBody2WorldQ = PxQuat(PxIdentity);
solverBodyTxInertia.sqrtInvInertia = PxMat33(PxVec3(0.f), PxVec3(0.f), PxVec3(0.f));
solverVel.linearVelocity = PxVec3(0.f);
solverVel.angularVelocity = PxVec3(0.f);
solverVel.deltaLinDt = PxVec3(0.f);
solverVel.deltaAngDt = PxVec3(0.f);
solverVel.lockFlags = 0;
solverVel.isKinematic = true;
solverVel.maxAngVel = PxSqrt(maxAngVelSq);
solverVel.partitionMask = 0;
solverVel.maxDynamicPartition = 0;
solverVel.nbStaticInteractions = 0;
solverBodyData.nodeIndex = nodeIndex;
solverBodyData.invMass = 0.f;
solverBodyData.penBiasClamp = maxDepenetrationVelocity;
solverBodyData.maxContactImpulse = maxContactImpulse;
solverBodyData.reportThreshold = reportThreshold;
solverBodyData.originalLinearVelocity = linearVelocity;
solverBodyData.originalAngularVelocity = angularVelocity;
}
DynamicsTGSContext::DynamicsTGSContext( PxcNpMemBlockPool* memBlockPool,
Cm::FlushPool& taskPool,
PxvSimStats& simStats,
Cm::VirtualAllocatorCallback& allocator,
PxsMaterialManager* materialManager,
IG::SimpleIslandManager& islandManager,
PxU64 contextID,
PxReal lengthScale,
PxSceneFlags sceneFlags) :
DynamicsContextBase (memBlockPool, taskPool, simStats, allocator, materialManager, islandManager, contextID, PX_MAX_F32, lengthScale, sceneFlags),
mIsExternalForcesEveryTgsIterationEnabled(sceneFlags & PxSceneFlag::eENABLE_EXTERNAL_FORCES_EVERY_ITERATION_TGS)
{
PxMemZero(&mWorldSolverBodyVel, sizeof(mWorldSolverBodyVel));
mWorldSolverBodyVel.lockFlags = 0;
mWorldSolverBodyVel.isKinematic = false;
mWorldSolverBodyTxInertia.sqrtInvInertia = PxMat33(PxZero);
mWorldSolverBodyTxInertia.body2WorldP = PxVec3(PxZero);
mWorldSolverBodyTxInertia.deltaBody2WorldQ = PxQuat(PxIdentity);
mWorldSolverBodyData2.penBiasClamp = -PX_MAX_REAL;
mWorldSolverBodyData2.maxContactImpulse = PX_MAX_REAL;
mWorldSolverBodyData2.nodeIndex = PX_INVALID_NODE;
mWorldSolverBodyData2.invMass = 0;
mWorldSolverBodyData2.reportThreshold = PX_MAX_REAL;
mWorldSolverBodyData2.originalLinearVelocity = PxVec3(0.f);
mWorldSolverBodyData2.originalAngularVelocity = PxVec3(0.f);
}
DynamicsTGSContext::~DynamicsTGSContext() {}
PX_COMPILE_TIME_ASSERT(PxsIndexedInteraction::eBODY == 0);
PX_COMPILE_TIME_ASSERT(PxsIndexedInteraction::eKINEMATIC == 1);
void DynamicsTGSContext::setDescFromIndices_Contacts(PxSolverConstraintDesc& desc, const IG::IslandSim& islandSim,
const PxsIndexedInteraction& constraint, PxU32 solverBodyOffset, PxTGSSolverBodyVel* solverBodies)
{
const PxU32 offsetMap[] = { solverBodyOffset, 0 };
//const PxU32 offsetMap[] = {mKinematicCount, 0};
if (constraint.indexType0 == PxsIndexedInteraction::eARTICULATION)
{
const PxNodeIndex nodeIndex0(constraint.articulation0);
const IG::Node& node0 = islandSim.getNode(nodeIndex0);
desc.articulationA = node0.mObject;
desc.linkIndexA = nodeIndex0.articulationLinkId();
desc.bodyADataIndex = 0;
}
else
{
desc.tgsBodyA = constraint.indexType0 == PxsIndexedInteraction::eWORLD ? &mWorldSolverBodyVel
: &solverBodies[PxU32(constraint.solverBody0) + offsetMap[constraint.indexType0] + 1];
desc.bodyADataIndex = constraint.indexType0 == PxsIndexedInteraction::eWORLD ? 0
: PxU32(constraint.solverBody0) + offsetMap[constraint.indexType0] + 1;
desc.linkIndexA = PxSolverConstraintDesc::RIGID_BODY;
}
if (constraint.indexType1 == PxsIndexedInteraction::eARTICULATION)
{
const PxNodeIndex nodeIndex1(constraint.articulation1);
const IG::Node& node1 = islandSim.getNode(nodeIndex1);
desc.articulationB = node1.mObject;
desc.linkIndexB = nodeIndex1.articulationLinkId();// PxTo8(getLinkIndex(constraint.articulation1));
desc.bodyBDataIndex = 0;
}
else
{
desc.tgsBodyB = constraint.indexType1 == PxsIndexedInteraction::eWORLD ? &mWorldSolverBodyVel
: &solverBodies[PxU32(constraint.solverBody1) + offsetMap[constraint.indexType1] + 1];
desc.bodyBDataIndex = constraint.indexType1 == PxsIndexedInteraction::eWORLD ? 0
: PxU32(constraint.solverBody1) + offsetMap[constraint.indexType1] + 1;
desc.linkIndexB = PxSolverConstraintDesc::RIGID_BODY;
}
}
void DynamicsTGSContext::setDescFromIndices_Constraints(PxSolverConstraintDesc& desc, const IG::IslandSim& islandSim, IG::EdgeIndex edgeIndex,
const PxU32* bodyRemap, PxU32 solverBodyOffset, PxTGSSolverBodyVel* solverBodies)
{
const PxNodeIndex node1 = islandSim.mCpuData.getNodeIndex1(edgeIndex);
if (node1.isStaticBody())
{
desc.tgsBodyA = &mWorldSolverBodyVel;
desc.bodyADataIndex = 0;
desc.linkIndexA = PxSolverConstraintDesc::RIGID_BODY;
}
else
{
const IG::Node& node = islandSim.getNode(node1);
if (node.getNodeType() == IG::Node::eARTICULATION_TYPE)
{
PX_ASSERT(node1.isArticulation());
desc.articulationA = getArticulationFromIG(islandSim, node1);
desc.linkIndexA = node1.articulationLinkId();
desc.bodyADataIndex = 0;
}
else
{
PX_ASSERT(!node1.isArticulation());
const PxU32 activeIndex = islandSim.getActiveNodeIndex(node1);
const PxU32 index = node.isKinematic() ? activeIndex : bodyRemap[activeIndex] + solverBodyOffset;
desc.tgsBodyA = &solverBodies[index + 1];
desc.bodyADataIndex = index + 1;
desc.linkIndexA = PxSolverConstraintDesc::RIGID_BODY;
}
}
const PxNodeIndex node2 = islandSim.mCpuData.getNodeIndex2(edgeIndex);
if (node2.isStaticBody())
{
desc.tgsBodyB = &mWorldSolverBodyVel;
desc.bodyBDataIndex = 0;
desc.linkIndexB = PxSolverConstraintDesc::RIGID_BODY;
}
else
{
const IG::Node& node = islandSim.getNode(node2);
if (node.getNodeType() == IG::Node::eARTICULATION_TYPE)
{
PX_ASSERT(node2.isArticulation());
desc.articulationB = getArticulationFromIG(islandSim, node2);
desc.linkIndexB = node2.articulationLinkId();
desc.bodyBDataIndex = 0;
}
else
{
PX_ASSERT(!node2.isArticulation());
const PxU32 activeIndex = islandSim.getActiveNodeIndex(node2);
const PxU32 index = node.isKinematic() ? activeIndex : bodyRemap[activeIndex] + solverBodyOffset;
desc.tgsBodyB = &solverBodies[index + 1];
desc.bodyBDataIndex = index + 1;
desc.linkIndexB = PxSolverConstraintDesc::RIGID_BODY;
}
}
}
namespace
{
class DynamicsMergeTask : public Cm::Task
{
PxBaseTask* mSecondContinuation;
public:
DynamicsMergeTask(PxU64 contextId) : Cm::Task(contextId), mSecondContinuation(NULL)
{
}
void setSecondContinuation(PxBaseTask* task) { task->addReference(); mSecondContinuation = task; }
virtual const char* getName() const PX_OVERRIDE { return "MergeTask"; }
virtual void runInternal() PX_OVERRIDE
{
}
virtual void release() PX_OVERRIDE
{
mSecondContinuation->removeReference();
Cm::Task::release();
}
};
class KinematicCopyTGSTask : public Cm::Task
{
const PxNodeIndex* const mKinematicIndices;
const PxU32 mNbKinematics;
const IG::IslandSim& mIslandSim;
PxTGSSolverBodyVel* mVels;
PxTGSSolverBodyTxInertia* mInertia;
PxTGSSolverBodyData* mBodyData;
PX_NOCOPY(KinematicCopyTGSTask)
public:
static const PxU32 NbKinematicsPerTask = 1024;
KinematicCopyTGSTask(const PxNodeIndex* const kinematicIndices,
PxU32 nbKinematics, const IG::IslandSim& islandSim, PxTGSSolverBodyVel* vels,
PxTGSSolverBodyTxInertia* inertias, PxTGSSolverBodyData* datas, PxU64 contextID) : Cm::Task(contextID),
mKinematicIndices(kinematicIndices), mNbKinematics(nbKinematics),
mIslandSim(islandSim), mVels(vels), mInertia(inertias), mBodyData(datas)
{
}
virtual const char* getName() const PX_OVERRIDE { return "KinematicCopyTask"; }
virtual void runInternal() PX_OVERRIDE
{
for (PxU32 i = 0; i<mNbKinematics; i++)
{
PxsRigidBody* rigidBody = getRigidBodyFromIG(mIslandSim, mKinematicIndices[i]);
const PxsBodyCore& core = rigidBody->getCore();
copyToSolverBodyDataStepKinematic(core.linearVelocity, core.angularVelocity, core.body2World, core.maxPenBias,
core.maxContactImpulse, mKinematicIndices[i].index(), core.contactReportThreshold, core.maxAngularVelocitySq,
mVels[i], mInertia[i], mBodyData[i]);
rigidBody->saveLastCCDTransform();
}
}
};
class UpdateContinuationTGSTask : public Cm::Task
{
DynamicsTGSContext& mContext;
PxBaseTask* mLostTouchTask;
PxU32 mMaxArticulationLinks;
PX_NOCOPY(UpdateContinuationTGSTask)
public:
UpdateContinuationTGSTask(DynamicsTGSContext& context,
PxBaseTask* lostTouchTask, PxU64 contextID, PxU32 maxLinks) :
Cm::Task(contextID), mContext(context),
mLostTouchTask(lostTouchTask), mMaxArticulationLinks(maxLinks)
{
}
virtual const char* getName() const PX_OVERRIDE { return "UpdateContinuationTask";}
virtual void runInternal() PX_OVERRIDE
{
mContext.updatePostKinematic(mCont, mLostTouchTask, mMaxArticulationLinks);
//Allow lost touch task to run once all tasks have be scheduled
mLostTouchTask->removeReference();
}
};
}
void DynamicsTGSContext::update(Cm::FlushPool& /*flushPool*/, PxBaseTask* continuation, PxBaseTask* /*postPartitioningTask*/, PxBaseTask* lostTouchTask,
PxvNphaseImplementationContext* nphase, PxU32 /*maxPatchesPerCM*/, PxU32 maxArticulationLinks,
PxReal dt, const PxVec3& gravity, Cm::PinnableBitMap& /*changedHandleMap*/)
{
if(!updateShared(nphase, dt, gravity))
return;
PX_PROFILE_ZONE("Dynamics.solverQueueTasks", mContextID);
//Block to make sure it doesn't run before stage2 of update!
lostTouchTask->addReference();
UpdateContinuationTGSTask* task = PX_PLACEMENT_NEW(mTaskPool.allocate(sizeof(UpdateContinuationTGSTask)), UpdateContinuationTGSTask)
(*this, lostTouchTask, mContextID, maxArticulationLinks);
task->setContinuation(continuation);
//KS - test that world solver body's velocities are finite and 0, then set it to 0.
//Technically, the velocity should always be 0 but can be stomped if a NAN creeps into the simulation.
PX_ASSERT(mWorldSolverBodyVel.linearVelocity == PxVec3(0.f));
PX_ASSERT(mWorldSolverBodyVel.angularVelocity == PxVec3(0.f));
PX_ASSERT(mWorldSolverBodyVel.linearVelocity.isFinite());
PX_ASSERT(mWorldSolverBodyVel.angularVelocity.isFinite());
mWorldSolverBodyVel.linearVelocity = mWorldSolverBodyVel.angularVelocity = PxVec3(0.f);
const IG::IslandSim& islandSim = mIslandManager.getAccurateIslandSim();
const PxU32 kinematicCount = islandSim.getNbActiveKinematics();
const PxNodeIndex* const kinematicIndices = islandSim.getActiveKinematics();
mKinematicCount = kinematicCount;
{
const PxU32 bodyCount = islandSim.getNbActiveNodes(IG::Node::eRIGID_BODY_TYPE);
if (kinematicCount + bodyCount > mSolverBodyVelPool.capacity())
{
PX_PROFILE_ZONE("reserve", mContextID);
mSolverBodyRemapTable.reserve((kinematicCount + bodyCount + 31 + 1) & ~31);
mSolverBodyVelPool.reserve((kinematicCount + bodyCount +31 + 1) & ~31);
mSolverBodyTxInertiaPool.reserve((kinematicCount + bodyCount +31 + 1) & ~31);
mSolverBodyDataPool2.reserve((kinematicCount + bodyCount +31 + 1) & ~31);
}
{
PX_PROFILE_ZONE("resize", mContextID);
mSolverBodyVelPool.resize(kinematicCount + bodyCount +1);
mSolverBodyTxInertiaPool.resize(kinematicCount + bodyCount +1);
mSolverBodyDataPool2.resize(kinematicCount + bodyCount +1);
mSolverBodyRemapTable.resize(kinematicCount + bodyCount + 1);
}
// integrate and copy all the kinematics - overkill, since not all kinematics
// need solver bodies
mSolverBodyVelPool[0] = mWorldSolverBodyVel;
mSolverBodyTxInertiaPool[0] = mWorldSolverBodyTxInertia;
mSolverBodyDataPool2[0] = mWorldSolverBodyData2;
if(kinematicCount)
{
PX_PROFILE_ZONE("Dynamics.updateKinematics", mContextID);
// PT: TODO: why no PxMemZero here compared to PGS?
for (PxU32 i = 0; i < kinematicCount; i+= KinematicCopyTGSTask::NbKinematicsPerTask)
{
const PxU32 nbToProcess = PxMin(kinematicCount - i, KinematicCopyTGSTask::NbKinematicsPerTask);
KinematicCopyTGSTask* kinematicTask = PX_PLACEMENT_NEW(mTaskPool.allocate(sizeof(KinematicCopyTGSTask)), KinematicCopyTGSTask)
(kinematicIndices + i, nbToProcess, islandSim, &mSolverBodyVelPool[i+1],
&mSolverBodyTxInertiaPool[i + 1], &mSolverBodyDataPool2[i + 1], mContextID);
kinematicTask->setContinuation(task);
kinematicTask->removeReference();
}
}
}
{
PX_PROFILE_ZONE("reserve2", mContextID);
const PxU32 totalConstraintCount = reserveSharedSolverConstraintsArrays(islandSim, maxArticulationLinks);
mSolverConstraintDescPool.forceSize_Unsafe(0);
mSolverConstraintDescPool.reserve((totalConstraintCount + 63) & (~63));
mSolverConstraintDescPool.forceSize_Unsafe(totalConstraintCount);
mOrderedSolverConstraintDescPool.forceSize_Unsafe(0);
mOrderedSolverConstraintDescPool.reserve((totalConstraintCount + 63) & (~63));
mOrderedSolverConstraintDescPool.forceSize_Unsafe(totalConstraintCount);
mTempSolverConstraintDescPool.forceSize_Unsafe(0);
mTempSolverConstraintDescPool.reserve((totalConstraintCount + 63) & (~63));
mTempSolverConstraintDescPool.forceSize_Unsafe(totalConstraintCount);
}
task->removeReference();
}
void DynamicsTGSContext::updatePostKinematic(PxBaseTask* continuation, PxBaseTask* lostTouchTask, PxU32 maxLinks)
{
const IG::IslandSim& islandSim = mIslandManager.getAccurateIslandSim();
const IG::IslandId*const islandIds = islandSim.getActiveIslands();
DynamicsMergeTask* mergeTask = PX_PLACEMENT_NEW(mTaskPool.allocate(sizeof(DynamicsMergeTask)), DynamicsMergeTask)(mContextID);
mergeTask->setContinuation(continuation);
mergeTask->setSecondContinuation(lostTouchTask);
PxU32 currentIsland = 0;
PxU32 currentBodyIndex = 0;
PxU32 currentArticulation = 0;
PxU32 currentContact = 0;
PxU32 constraintIndex = 0;
const PxU32 minIslandSize = mSolverBatchSize;
const PxU32 islandCount = islandSim.getNbActiveIslands();
const PxU32 articulationBatchSize = mSolverArticBatchSize;
while (currentIsland < islandCount)
{
SolverIslandObjectsStep objectStarts;
objectStarts.articulations = mArticulationArray.begin() + currentArticulation;
objectStarts.bodies = mRigidBodyArray.begin() + currentBodyIndex;
objectStarts.externalAccelerations = &mRigidExternalAccelerations;
objectStarts.contactManagers = mContactList.begin() + currentContact;
objectStarts.constraintDescs = mSolverConstraintDescPool.begin() + constraintIndex;
objectStarts.orderedConstraintDescs = mOrderedSolverConstraintDescPool.begin() + constraintIndex;
objectStarts.constraintBatchHeaders = mContactConstraintBatchHeaders.begin() + constraintIndex;
objectStarts.tempConstraintDescs = mTempSolverConstraintDescPool.begin() + constraintIndex;
objectStarts.motionVelocities = mMotionVelocityArray.begin() + currentBodyIndex;
objectStarts.bodyCoreArray = mBodyCoreArray.begin() + currentBodyIndex;
objectStarts.islandIds = islandIds + currentIsland;
objectStarts.bodyRemapTable = mSolverBodyRemapTable.begin();
objectStarts.nodeIndexArray = mNodeIndexArray.begin() + currentBodyIndex;
PxU32 startIsland = currentIsland;
PxU32 constraintCount = 0;
PxU32 nbArticulations = 0;
PxU32 nbBodies = 0;
PxU32 nbConstraints = 0;
PxU32 nbContactManagers = 0;
// PT: TODO: why is this sequence of code different from PGS?
while (nbBodies < minIslandSize && currentIsland < islandCount && nbArticulations < articulationBatchSize)
{
const IG::Island& island = islandSim.getIsland(islandIds[currentIsland]);
nbBodies += island.mNodeCount[IG::Node::eRIGID_BODY_TYPE];
nbArticulations += island.mNodeCount[IG::Node::eARTICULATION_TYPE];
nbConstraints += island.mEdges.getCount(IG::Edge::eCONSTRAINT);
nbContactManagers += island.mEdges.getCount(IG::Edge::eCONTACT_MANAGER);
constraintCount = nbConstraints + nbContactManagers;
currentIsland++;
if(mUseEnhancedDeterminism)
break;
}
objectStarts.numIslands = currentIsland - startIsland;
constraintIndex += nbArticulations * maxLinks;
PxsIslandIndices counts;
counts.bodies = nbBodies;
counts.articulations = nbArticulations;
counts.contactManagers = nbContactManagers;
counts.constraints = nbConstraints;
solveIsland(objectStarts, counts,
mKinematicCount + currentBodyIndex, mSolverBodyRemapTable.begin(), mMaterialManager, mOutputIterator, mergeTask);
currentBodyIndex += nbBodies;
currentArticulation += nbArticulations;
currentContact += nbContactManagers;
constraintIndex += constraintCount;
}
mergeTask->removeReference();
}
void DynamicsTGSContext::prepareBodiesAndConstraints(const SolverIslandObjectsStep& objects, IslandContextStep& islandContext)
{
Dy::ThreadContext& threadContext = *islandContext.mThreadContext;
threadContext.mMaxSolverPositionIterations = 0;
threadContext.mMaxSolverVelocityIterations = 0;
threadContext.mAxisConstraintCount = 0;
threadContext.mContactDescPtr = threadContext.contactConstraintDescArray;
threadContext.mNumDifferentBodyConstraints = 0;
threadContext.mNumStaticConstraints = 0;
threadContext.numContactConstraintBatches = 0;
threadContext.contactDescArraySize = 0;
threadContext.motionVelocityArray = objects.motionVelocities;
threadContext.mBodyCoreArray = objects.bodyCoreArray;
threadContext.mRigidBodyArray = objects.bodies;
threadContext.mArticulationArray = objects.articulations;
threadContext.bodyRemapTable = objects.bodyRemapTable;
threadContext.mNodeIndexArray = objects.nodeIndexArray;
threadContext.resizeArrays(islandContext.mCounts.articulations);
const PxU32 nbIslands = objects.numIslands;
const IG::IslandId* const islandIds = objects.islandIds;
PxU32* PX_RESTRICT bodyRemapTable = threadContext.bodyRemapTable;
iterateIslandsNodes(islandContext.mCounts, nbIslands, islandIds, threadContext.mBodyCoreArray, threadContext.mRigidBodyArray, threadContext.mArticulationArray, bodyRemapTable, threadContext.mNodeIndexArray);
islandContext.mCounts.contactManagers = iterateIslandsContactEdges(islandContext.mCounts, nbIslands, islandIds, objects.contactManagers, bodyRemapTable);
}
void DynamicsTGSContext::setupDescs(IslandContextStep& mIslandContext, const SolverIslandObjectsStep& mObjects,
PxU32* mBodyRemapTable, PxU32 mSolverBodyOffset, PxsContactManagerOutputIterator& /*outputs*/)
{
PX_PROFILE_ZONE("setupDescs", mContextID);
ThreadContext& threadContext = *mIslandContext.mThreadContext;
PxSolverConstraintDesc* contactDescPtr = mObjects.constraintDescs;
//PxU32 constraintCount = mCounts.constraints + mCounts.contactManagers;
PxU32 nbIslands = mObjects.numIslands;
const IG::IslandId* const islandIds = mObjects.islandIds;
const IG::IslandSim& islandSim = mIslandManager.getAccurateIslandSim();
{
// PT: TODO: refactor with PGS
PX_PROFILE_ZONE("IterateIslandsConstraintEdges", mContextID);
for (PxU32 i = 0; i < nbIslands; ++i)
{
const IG::Island& island = islandSim.getIsland(islandIds[i]);
START_ENUMERATING_ISLAND_EDGES(IG::Edge::eCONSTRAINT)
{
GET_CURRENT_ISLAND_EDGE
PxSolverConstraintDesc& desc = *contactDescPtr;
Dy::Constraint* constraint = mIslandManager.getConstraint(edgeId);
setDescFromIndices_Constraints(desc, islandSim, edgeId, mBodyRemapTable, mSolverBodyOffset, mSolverBodyVelPool.begin());
desc.constraint = reinterpret_cast<PxU8*>(constraint);
desc.constraintType = DY_SC_TYPE_RB_1D;
contactDescPtr++;
GET_NEXT_ISLAND_EDGE
}
}
}
{
PX_PROFILE_ZONE("PxSort", mContextID);
PxSort(mObjects.constraintDescs, PxU32(contactDescPtr - mObjects.constraintDescs), ConstraintLess());
}
const PxU32 nbCms = mIslandContext.mCounts.contactManagers;
if(nbCms)
{
// PT: TODO: refactor with PGS
PX_PROFILE_ZONE("setDescFromIndices_contacts", mContextID);
for (PxU32 a = 0; a < nbCms; ++a)
{
//PxsContactManagerOutput& output = outputs.getContactManager(mObjects.contactManagers[a].contactManager->getWorkUnit().mNpIndex);
//if (output.nbContacts > 0)
{
PxSolverConstraintDesc& desc = *contactDescPtr;
setDescFromIndices_Contacts(desc, islandSim, mObjects.contactManagers[a], mSolverBodyOffset, mSolverBodyVelPool.begin());
desc.constraint = reinterpret_cast<PxU8*>(mObjects.contactManagers[a].contactManager);
desc.constraintType = DY_SC_TYPE_RB_CONTACT;
contactDescPtr++;
}
//else
//{
// //Clear friction state!
// mObjects.contactManagers[a].contactManager->getWorkUnit().frictionDataPtr = NULL;
// mObjects.contactManagers[a].contactManager->getWorkUnit().frictionPatchCount = 0;
//}
}
}
threadContext.contactDescArraySize = PxU32(contactDescPtr - mObjects.constraintDescs);
}
void DynamicsTGSContext::preIntegrateBodies(PxsBodyCore** bodyArray, PxsRigidBody** originalBodyArray,
PxTGSSolverBodyVel* solverBodyVelPool, PxTGSSolverBodyTxInertia* solverBodyTxInertia, PxTGSSolverBodyData* solverBodyDataPool2, PxU32* nodeIndexArray, PxU32 bodyCount, const PxVec3& gravity, PxReal dt, PxU32& posIters, PxU32& velIters, PxU32 /*iteration*/)
{
PX_PROFILE_ZONE("PreIntegrate", mContextID);
const bool skipGravity = mIsExternalForcesEveryTgsIterationEnabled;
PxU32 localMaxPosIter = 0;
PxU32 localMaxVelIter = 0;
for (PxU32 i = 0; i < bodyCount; ++i)
{
PxsBodyCore& core = *bodyArray[i];
const PxsRigidBody& rBody = *originalBodyArray[i];
const PxU16 iterWord = core.solverIterationCounts;
localMaxPosIter = PxMax<PxU32>(PxU32(iterWord & 0xff), localMaxPosIter);
localMaxVelIter = PxMax<PxU32>(PxU32(iterWord >> 8), localMaxVelIter);
bodyCoreComputeUnconstrainedVelocity(gravity, dt, core.linearDamping, core.angularDamping, rBody.mAccelScale, core.maxLinearVelocitySq, core.maxAngularVelocitySq,
core.linearVelocity, core.angularVelocity, core.disableGravity!=0 || skipGravity);
copyToSolverBodyDataStep(core.linearVelocity, core.angularVelocity, core.inverseMass, core.inverseInertia, core.body2World, core.maxPenBias, core.maxContactImpulse, nodeIndexArray[i],
core.contactReportThreshold, core.maxAngularVelocitySq, core.lockFlags, false,
solverBodyVelPool[i+1], solverBodyTxInertia[i+1], solverBodyDataPool2[i+1], dt, core.mFlags & PxRigidBodyFlag::eENABLE_GYROSCOPIC_FORCES);
}
posIters = localMaxPosIter;
velIters = localMaxVelIter;
}
void DynamicsTGSContext::createSolverConstraints(PxSolverConstraintDesc* contactDescPtr, PxConstraintBatchHeader* headers, PxU32 nbHeaders,
PxsContactManagerOutputIterator& outputs, Dy::ThreadContext& islandThreadContext, Dy::ThreadContext& threadContext, PxReal stepDt, PxReal totalDt, PxReal invStepDt,
const PxReal rigidContactBiasCoefficient, const PxReal jointBiasCoefficient)
{
PX_UNUSED(totalDt);
//PX_PROFILE_ZONE("CreateConstraints", 0);
PxTransform idt(PxIdentity);
BlockAllocator blockAllocator(islandThreadContext.mConstraintBlockManager, threadContext.mConstraintBlockStream, threadContext.mFrictionPatchStreamPair, threadContext.mConstraintSize);
PxTGSSolverBodyTxInertia* txInertias = mSolverBodyTxInertiaPool.begin();
PxTGSSolverBodyData* solverBodyDatas = mSolverBodyDataPool2.begin();
const PxReal invTotalDt = 1.0f / totalDt;
for (PxU32 h = 0; h < nbHeaders; ++h)
{
PxConstraintBatchHeader& hdr = headers[h];
PxU32 startIdx = hdr.startIndex;
PxU32 endIdx = startIdx + hdr.stride;
if (contactDescPtr[startIdx].constraintType == DY_SC_TYPE_RB_CONTACT)
{
PxTGSSolverContactDesc blockDescs[4];
PxsContactManagerOutput* cmOutputs[4];
PxsContactManager* cms[4];
for (PxU32 a = startIdx, i = 0; a < endIdx; ++a, i++)
{
PxSolverConstraintDesc& desc = contactDescPtr[a];
PxsContactManager* cm = reinterpret_cast<PxsContactManager*>(desc.constraint);
PxTGSSolverContactDesc& blockDesc = blockDescs[i];
cms[i] = cm;
PxcNpWorkUnit& unit = cm->getWorkUnit();
PxsContactManagerOutput* cmOutput = &outputs.getContactManagerOutput(unit.mNpIndex);
cmOutputs[i] = cmOutput;
PxTGSSolverBodyVel& b0 = *desc.tgsBodyA;
PxTGSSolverBodyVel& b1 = *desc.tgsBodyB;
PxTGSSolverBodyTxInertia& txI0 = txInertias[desc.bodyADataIndex];
PxTGSSolverBodyTxInertia& txI1 = txInertias[desc.bodyBDataIndex];
PxTGSSolverBodyData& data0 = solverBodyDatas[desc.bodyADataIndex];
PxTGSSolverBodyData& data1 = solverBodyDatas[desc.bodyBDataIndex];
blockDesc.body0 = &b0;
blockDesc.body1 = &b1;
blockDesc.bodyFrame0 = unit.mRigidCore0->body2World;
blockDesc.bodyFrame1 = unit.mRigidCore1->body2World;
blockDesc.shapeInteraction = cm->getShapeInteraction();
blockDesc.contactForces = cmOutput->contactForces;
blockDesc.desc = &desc;
blockDesc.body0TxI = &txI0;
blockDesc.body1TxI = &txI1;
blockDesc.bodyData0 = &data0;
blockDesc.bodyData1 = &data1;
blockDesc.hasForceThresholds = !!(unit.mFlags & PxcNpWorkUnitFlag::eFORCE_THRESHOLD);
blockDesc.disableStrongFriction = !!(unit.mFlags & PxcNpWorkUnitFlag::eDISABLE_STRONG_FRICTION);
blockDesc.bodyState0 = (unit.mFlags & PxcNpWorkUnitFlag::eARTICULATION_BODY0) ? PxSolverContactDesc::eARTICULATION : PxSolverContactDesc::eDYNAMIC_BODY;
if (unit.mFlags & PxcNpWorkUnitFlag::eARTICULATION_BODY1)
{
//kinematic link
if (desc.linkIndexB == 0xff)
{
blockDesc.bodyState1 = PxSolverContactDesc::eSTATIC_BODY;
}
else
{
blockDesc.bodyState1 = PxSolverContactDesc::eARTICULATION;
}
}
else
{
blockDesc.bodyState1 = (unit.mFlags & PxcNpWorkUnitFlag::eHAS_KINEMATIC_ACTOR) ? PxSolverContactDesc::eKINEMATIC_BODY :
((unit.mFlags & PxcNpWorkUnitFlag::eDYNAMIC_BODY1) ? PxSolverContactDesc::eDYNAMIC_BODY : PxSolverContactDesc::eSTATIC_BODY);
}
//blockDesc.flags = unit.flags;
const PxReal maxImpulse0 = (unit.mFlags & PxcNpWorkUnitFlag::eARTICULATION_BODY0) ? static_cast<const PxsBodyCore*>(unit.mRigidCore0)->maxContactImpulse : data0.maxContactImpulse;
const PxReal maxImpulse1 = (unit.mFlags & PxcNpWorkUnitFlag::eARTICULATION_BODY1) ? static_cast<const PxsBodyCore*>(unit.mRigidCore1)->maxContactImpulse : data1.maxContactImpulse;
unit.setInvMassScaleFromDominance(blockDesc.invMassScales);
blockDesc.restDistance = unit.mRestDistance;
blockDesc.frictionPtr = unit.mFrictionDataPtr;
blockDesc.frictionCount = unit.mFrictionPatchCount;
blockDesc.maxCCDSeparation = PX_MAX_F32;
blockDesc.maxImpulse = PxMin(maxImpulse0, maxImpulse1);
blockDesc.torsionalPatchRadius = unit.mTorsionalPatchRadius;
blockDesc.minTorsionalPatchRadius = unit.mMinTorsionalPatchRadius;
blockDesc.offsetSlop = unit.mOffsetSlop;
}
SolverConstraintPrepState::Enum buildState = SolverConstraintPrepState::eUNBATCHABLE;
#if PX_USE_BLOCK_SOLVER
if (hdr.stride == 4)
{
buildState = createFinalizeSolverContacts4Step(
cmOutputs,
threadContext,
blockDescs,
invStepDt,
totalDt,
invTotalDt,
stepDt,
mBounceThreshold,
mFrictionOffsetThreshold,
mCorrelationDistance,
rigidContactBiasCoefficient,
blockAllocator);
}
#endif
if (buildState != SolverConstraintPrepState::eSUCCESS)
{
for (PxU32 a = startIdx, i = 0; a < endIdx; ++a, i++)
{
PxSolverConstraintDesc& desc = contactDescPtr[a];
PxsContactManager* cm = reinterpret_cast<PxsContactManager*>(desc.constraint);
//PxcNpWorkUnit& n = cm->getWorkUnit();
PxsContactManagerOutput& output = *cmOutputs[i];
//PX_ASSERT(output.nbContacts != 0);
createFinalizeSolverContactsStep(blockDescs[i], output, threadContext,
invStepDt, invTotalDt, totalDt, stepDt, mBounceThreshold, mFrictionOffsetThreshold, mCorrelationDistance,
rigidContactBiasCoefficient, blockAllocator);
getContactManagerConstraintDesc(output, *cm, desc);
//PX_ASSERT(desc.constraint != NULL);
}
}
for (PxU32 a = startIdx, i = 0; a < endIdx; ++a, i++)
{
updateFrictionAnchorCountAndPosition(contactDescPtr[a], *cmOutputs[i], blockDescs[i]);
}
for (PxU32 i = 0; i < hdr.stride; ++i)
{
PxsContactManager* cm = cms[i];
PxcNpWorkUnit& unit = cm->getWorkUnit();
unit.mFrictionDataPtr = blockDescs[i].frictionPtr;
unit.mFrictionPatchCount = blockDescs[i].frictionCount;
}
}
else if (contactDescPtr[startIdx].constraintType == DY_SC_TYPE_RB_1D)
{
SolverConstraintShaderPrepDesc shaderPrepDescs[4];
PxTGSSolverConstraintPrepDesc prepDescs[4];
for (PxU32 a = startIdx, i = 0; a < endIdx; ++a, i++)
{
SolverConstraintShaderPrepDesc& shaderPrepDesc = shaderPrepDescs[i];
PxTGSSolverConstraintPrepDesc& prepDesc = prepDescs[i];
const PxTransform id(PxIdentity);
{
PxSolverConstraintDesc& desc = contactDescPtr[a];
const Constraint* constraint = reinterpret_cast<const Constraint*>(desc.constraint);
const PxConstraintSolverPrep solverPrep = constraint->solverPrep;
const void* constantBlock = constraint->constantBlock;
const PxU32 constantBlockByteSize = constraint->constantBlockSize;
const PxTransform& pose0 = (constraint->body0 ? constraint->body0->getPose() : id);
const PxTransform& pose1 = (constraint->body1 ? constraint->body1->getPose() : id);
const PxTGSSolverBodyVel* sbody0 = desc.tgsBodyA;
const PxTGSSolverBodyVel* sbody1 = desc.tgsBodyB;
PxTGSSolverBodyTxInertia& txI0 = txInertias[desc.bodyADataIndex];
PxTGSSolverBodyTxInertia& txI1 = txInertias[desc.bodyBDataIndex];
PxTGSSolverBodyData& data0 = solverBodyDatas[desc.bodyADataIndex];
PxTGSSolverBodyData& data1 = solverBodyDatas[desc.bodyBDataIndex];
shaderPrepDesc.constantBlock = constantBlock;
shaderPrepDesc.constantBlockByteSize = constantBlockByteSize;
shaderPrepDesc.constraint = constraint;
shaderPrepDesc.solverPrep = solverPrep;
prepDesc.desc = &desc;
prepDesc.bodyFrame0 = pose0;
prepDesc.bodyFrame1 = pose1;
prepDesc.body0 = sbody0;
prepDesc.body1 = sbody1;
prepDesc.body0TxI = &txI0;
prepDesc.body1TxI = &txI1;
prepDesc.bodyData0 = &data0;
prepDesc.bodyData1 = &data1;
prepDesc.linBreakForce = constraint->linBreakForce;
prepDesc.angBreakForce = constraint->angBreakForce;
prepDesc.writeback = &getConstraintWriteBackPool()[constraint->index];
setupConstraintFlags(prepDesc, constraint->flags);
prepDesc.minResponseThreshold = constraint->minResponseThreshold;
prepDesc.bodyState0 = desc.linkIndexA == PxSolverConstraintDesc::RIGID_BODY ? PxSolverContactDesc::eDYNAMIC_BODY : PxSolverContactDesc::eARTICULATION;
prepDesc.bodyState1 = desc.linkIndexB == PxSolverConstraintDesc::RIGID_BODY ? PxSolverContactDesc::eDYNAMIC_BODY : PxSolverContactDesc::eARTICULATION;
}
}
SolverConstraintPrepState::Enum buildState = SolverConstraintPrepState::eUNBATCHABLE;
#if PX_USE_BLOCK_SOLVER
#if PX_USE_BLOCK_1D
if (hdr.stride == 4)
{
PxU32 totalRows;
buildState = setupSolverConstraintStep4
(shaderPrepDescs, prepDescs, stepDt, totalDt, invStepDt, invTotalDt, totalRows, blockAllocator, mLengthScale, jointBiasCoefficient);
}
#endif
#endif
if (buildState != SolverConstraintPrepState::eSUCCESS)
{
for (PxU32 a = startIdx, i = 0; a < endIdx; ++a, i++)
{
PxReal clampedInvDt = invStepDt;
SetupSolverConstraintStep(shaderPrepDescs[i], prepDescs[i], blockAllocator, stepDt, totalDt, clampedInvDt, invTotalDt, mLengthScale,
jointBiasCoefficient);
}
}
}
}
}
void solveContactBlock (DY_TGS_SOLVE_METHOD_PARAMS);
void solve1DBlock (DY_TGS_SOLVE_METHOD_PARAMS);
void solveExtContactBlock (DY_TGS_SOLVE_METHOD_PARAMS);
void solveExt1DBlock (DY_TGS_SOLVE_METHOD_PARAMS);
void solveContact4 (DY_TGS_SOLVE_METHOD_PARAMS);
void solve1D4 (DY_TGS_SOLVE_METHOD_PARAMS);
TGSSolveBlockMethod g_SolveTGSMethods[] =
{
0,
solveContactBlock, // DY_SC_TYPE_RB_CONTACT
solve1DBlock, // DY_SC_TYPE_RB_1D
solveExtContactBlock, // DY_SC_TYPE_EXT_CONTACT
solveExt1DBlock, // DY_SC_TYPE_EXT_1D
solveContactBlock, // DY_SC_TYPE_STATIC_CONTACT
solveContact4, // DY_SC_TYPE_BLOCK_RB_CONTACT
solveContact4, // DY_SC_TYPE_BLOCK_STATIC_RB_CONTACT
solve1D4, // DY_SC_TYPE_BLOCK_1D,
};
void writeBackContact (DY_TGS_WRITEBACK_METHOD_PARAMS);
void writeBack1D (DY_TGS_WRITEBACK_METHOD_PARAMS);
void writeBackContact4 (DY_TGS_WRITEBACK_METHOD_PARAMS);
void writeBack1D4 (DY_TGS_WRITEBACK_METHOD_PARAMS);
TGSWriteBackMethod g_WritebackTGSMethods[] =
{
0,
writeBackContact, // DY_SC_TYPE_RB_CONTACT
writeBack1D, // DY_SC_TYPE_RB_1D
writeBackContact, // DY_SC_TYPE_EXT_CONTACT
writeBack1D, // DY_SC_TYPE_EXT_1D
writeBackContact, // DY_SC_TYPE_STATIC_CONTACT
writeBackContact4, // DY_SC_TYPE_BLOCK_RB_CONTACT
writeBackContact4, // DY_SC_TYPE_BLOCK_STATIC_RB_CONTACT
writeBack1D4, // DY_SC_TYPE_BLOCK_1D,
};
void solveConclude1DBlock (DY_TGS_CONCLUDE_METHOD_PARAMS);
void solveConcludeContactBlock (DY_TGS_CONCLUDE_METHOD_PARAMS);
void solveConcludeContact4 (DY_TGS_CONCLUDE_METHOD_PARAMS);
void solveConclude1D4 (DY_TGS_CONCLUDE_METHOD_PARAMS);
void solveConcludeContactExtBlock (DY_TGS_CONCLUDE_METHOD_PARAMS);
void solveConclude1DBlockExt (DY_TGS_CONCLUDE_METHOD_PARAMS);
TGSSolveConcludeMethod g_SolveConcludeTGSMethods[] =
{
0,
solveConcludeContactBlock, // DY_SC_TYPE_RB_CONTACT
solveConclude1DBlock, // DY_SC_TYPE_RB_1D
solveConcludeContactExtBlock, // DY_SC_TYPE_EXT_CONTACT
solveConclude1DBlockExt, // DY_SC_TYPE_EXT_1D
solveConcludeContactBlock, // DY_SC_TYPE_STATIC_CONTACT
solveConcludeContact4, // DY_SC_TYPE_BLOCK_RB_CONTACT
solveConcludeContact4, // DY_SC_TYPE_BLOCK_STATIC_RB_CONTACT
solveConclude1D4, // DY_SC_TYPE_BLOCK_1D,
};
static void solveConstraintsIteration(const PxSolverConstraintDesc* const contactDescPtr, const PxConstraintBatchHeader* const batchHeaders, PxU32 nbHeaders,
PxReal invStepDt, const PxTGSSolverBodyTxInertia* const solverTxInertia, PxReal elapsedTime, PxReal minPenetration, SolverContext& cache)
{
PX_UNUSED(invStepDt);
for (PxU32 h = 0; h < nbHeaders; ++h)
{
const PxConstraintBatchHeader& hdr = batchHeaders[h];
g_SolveTGSMethods[hdr.constraintType](hdr, contactDescPtr, solverTxInertia, minPenetration, elapsedTime, cache);
}
}
template <bool TSync>
static void parallelSolveConstraints(const PxSolverConstraintDesc* const contactDescPtr, const PxConstraintBatchHeader* const batchHeaders, PxU32 nbHeaders,
PxTGSSolverBodyTxInertia* solverTxInertia, PxReal elapsedTime, PxReal minPenetration,
SolverContext& cache, PxU32 iterCount)
{
for (PxU32 h = 0; h < nbHeaders; ++h)
{
const PxConstraintBatchHeader& hdr = batchHeaders[h];
const PxSolverConstraintDesc& desc = contactDescPtr[hdr.startIndex];
if (TSync)
{
PX_ASSERT(hdr.stride == 1);
waitForProgresses(desc, iterCount);
}
g_SolveTGSMethods[hdr.constraintType](hdr, contactDescPtr, solverTxInertia, minPenetration, elapsedTime, cache);
if (TSync)
{
PxMemoryBarrier();
incrementProgress(desc);
}
}
}
void DynamicsTGSContext::writebackConstraintsIteration(const PxConstraintBatchHeader* const hdrs, const PxSolverConstraintDesc* const contactDescPtr, PxU32 nbHeaders, SolverContext& cache)
{
PX_PROFILE_ZONE("Writeback", mContextID);
for (PxU32 h = 0; h < nbHeaders; ++h)
{
const PxConstraintBatchHeader& hdr = hdrs[h];
g_WritebackTGSMethods[hdr.constraintType](hdr, contactDescPtr, &cache);
}
}
void DynamicsTGSContext::parallelWritebackConstraintsIteration(const PxSolverConstraintDesc* const contactDescPtr, const PxConstraintBatchHeader* const batchHeaders, PxU32 nbHeaders, SolverContext& cache)
{
for (PxU32 h = 0; h < nbHeaders; ++h)
{
const PxConstraintBatchHeader& hdr = batchHeaders[h];
g_WritebackTGSMethods[hdr.constraintType](hdr, contactDescPtr, &cache);
}
}
template <bool TSync>
static void solveConcludeConstraintsIteration(const PxSolverConstraintDesc* const contactDescPtr,
const PxConstraintBatchHeader* const batchHeaders, PxU32 nbHeaders, PxTGSSolverBodyTxInertia* solverTxInertia,
PxReal elapsedTime, SolverContext& cache, PxU32 iterCount)
{
for (PxU32 h = 0; h < nbHeaders; ++h)
{
const PxConstraintBatchHeader& hdr = batchHeaders[h];
const PxSolverConstraintDesc& desc = contactDescPtr[hdr.startIndex];
if (TSync)
waitForProgresses(desc, iterCount);
g_SolveConcludeTGSMethods[hdr.constraintType](hdr, contactDescPtr, solverTxInertia, elapsedTime, cache);
if (TSync)
{
PxMemoryBarrier();
incrementProgress(desc);
}
}
}
void integrateCoreStep(PxTGSSolverBodyVel& vel, PxTGSSolverBodyTxInertia& txInertia, PxF32 dt)
{
PxU32 lockFlags = vel.lockFlags;
if (lockFlags)
{
if (lockFlags & PxRigidDynamicLockFlag::eLOCK_LINEAR_X)
vel.linearVelocity.x = 0.f;
if (lockFlags & PxRigidDynamicLockFlag::eLOCK_LINEAR_Y)
vel.linearVelocity.y = 0.f;
if (lockFlags & PxRigidDynamicLockFlag::eLOCK_LINEAR_Z)
vel.linearVelocity.z = 0.f;
//The angular velocity should be 0 because it is now impossible to make it rotate around that axis!
if (lockFlags & PxRigidDynamicLockFlag::eLOCK_ANGULAR_X)
vel.angularVelocity.x = 0.f;
if (lockFlags & PxRigidDynamicLockFlag::eLOCK_ANGULAR_Y)
vel.angularVelocity.y = 0.f;
if (lockFlags & PxRigidDynamicLockFlag::eLOCK_ANGULAR_Z)
vel.angularVelocity.z = 0.f;
}
PxVec3 linearMotionVel = vel.linearVelocity;
const PxVec3 delta = linearMotionVel * dt;
//The solver accumulates deltaAngularMomocity.
//We need to translate from deltaAngularMomocity to deltaAngularVelocity.
//deltaAngularVelocity = I^(-1/2)*deltaAngularMomocity
PxVec3 unmolestedAngVel = vel.angularVelocity;
PxVec3 angularMotionVel = txInertia.sqrtInvInertia * vel.angularVelocity;
PxReal w2 = angularMotionVel.magnitudeSquared();
txInertia.body2WorldP += delta;
PX_ASSERT(txInertia.body2WorldP.isFinite());
// Integrate the rotation using closed form quaternion integrator
if (w2 != 0.0f)
{
PxReal w = PxSqrt(w2);
//KS - we allow a little bit more angular velocity than the default
//maxAngVel member otherwise the simulation feels a little flimsy
/*const PxReal maxW = PxMax(50.f, vel.maxAngVel);
if (w > maxW)
{
PxReal ratio = maxW / w;
vel.angularVelocity *= ratio;
}*/
const PxReal v = dt * w * 0.5f;
PxReal s, q;
PxSinCos(v, s, q);
s /= w;
const PxVec3 pqr = angularMotionVel * s;
const PxQuat quatVel(pqr.x, pqr.y, pqr.z, 0);
PxQuat result = quatVel * txInertia.deltaBody2WorldQ;
result += txInertia.deltaBody2WorldQ * q;
txInertia.deltaBody2WorldQ = result.getNormalized();
PX_ASSERT(txInertia.deltaBody2WorldQ.isSane());
PX_ASSERT(txInertia.deltaBody2WorldQ.isFinite());
//Accumulate the angular rotations in a space we can project the angular constraints to
}
vel.deltaAngDt += unmolestedAngVel * dt;
vel.deltaLinDt += delta;
/*solverBodyData.body2World = txInertia.body2World;
solverBodyData.deltaLinDt = vel.deltaLinDt;
solverBodyData.deltaAngDt = vel.deltaAngDt;*/
}
void DynamicsTGSContext::applySubstepGravity(PxsRigidBody** bodies, PxsExternalAccelerationProvider& externalAccelerations,
PxU32 count, PxTGSSolverBodyVel* vels, PxReal dt, PxTGSSolverBodyTxInertia* PX_RESTRICT txInertias, PxU32* nodeIndexArray)
{
const PxVec3 gravityTimesDt = dt * mGravity;
const bool hasExternalAcceleration = externalAccelerations.hasAccelerations();
for (PxU32 k = 0; k < count; k++)
{
const PxsRigidBody& rBody = *bodies[k];
const PxsBodyCore& core = rBody.getCore();
const PxsRigidBodyExternalAcceleration* acc = hasExternalAcceleration ? &externalAccelerations.get(nodeIndexArray[k]) : NULL;
const PxU16 lockFlags = core.lockFlags;
if (acc || !core.disableGravity)
{
const PxVec3 gravityContribution = core.disableGravity ? PxVec3(0.0f) : (gravityTimesDt * rBody.mAccelScale);
const PxVec3 linearAcceleration = acc ? (acc->linearAcceleration * dt) : PxVec3(0.0f);
PxVec3 v = vels[k + 1].linearVelocity + gravityContribution + linearAcceleration;
v.x = lockFlags & PxRigidDynamicLockFlag::eLOCK_LINEAR_X ? 0.f : v.x;
v.y = lockFlags & PxRigidDynamicLockFlag::eLOCK_LINEAR_Y ? 0.f : v.y;
v.z = lockFlags & PxRigidDynamicLockFlag::eLOCK_LINEAR_Z ? 0.f : v.z;
vels[k + 1].linearVelocity = v;
}
if (acc && acc->angularAcceleration != PxVec3(0.0f))
{
PxVec3 a = vels[k + 1].angularVelocity + txInertias[k + 1].sqrtInvInertia.getInverse() * acc->angularAcceleration * dt;
a.x = lockFlags & PxRigidDynamicLockFlag::eLOCK_ANGULAR_X ? 0.f : a.x;
a.y = lockFlags & PxRigidDynamicLockFlag::eLOCK_ANGULAR_Y ? 0.f : a.y;
a.z = lockFlags & PxRigidDynamicLockFlag::eLOCK_ANGULAR_Z ? 0.f : a.z;
vels[k + 1].angularVelocity = a;
}
}
}
void DynamicsTGSContext::integrateBodies(PxU32 count, PxTGSSolverBodyVel* PX_RESTRICT vels,
PxTGSSolverBodyTxInertia* PX_RESTRICT txInertias, PxReal dt)
{
for (PxU32 k = 0; k < count; k++)
{
integrateCoreStep(vels[k + 1], txInertias[k + 1], dt);
}
}
void DynamicsTGSContext::integrateBodiesAndApplyGravity(const SolverIslandObjectsStep& objects,
PxU32 count, PxTGSSolverBodyVel* PX_RESTRICT vels, PxTGSSolverBodyTxInertia* PX_RESTRICT txInertias,
PxReal dt, PxU32 posIters)
{
for (PxU32 i = 0; i < posIters; i++)
{
applySubstepGravity(objects.bodies, *objects.externalAccelerations, count, vels, dt, txInertias, objects.nodeIndexArray);
integrateBodies(count, vels, txInertias, dt);
}
}
static void parallelIntegrateBodies(PxTGSSolverBodyVel* vels, PxTGSSolverBodyTxInertia* txInertias,
PxU32 count, PxReal dt, PxU32 iteration)
{
for (PxU32 k = 0; k < count; k++)
{
PX_ASSERT(vels[k + 1].partitionMask == (iteration * vels[k + 1].nbStaticInteractions));
PX_UNUSED(iteration);
integrateCoreStep(vels[k + 1], txInertias[k + 1], dt);
}
}
void DynamicsTGSContext::copyBackBodies(const SolverIslandObjectsStep& objects,
PxTGSSolverBodyVel* vels, PxTGSSolverBodyTxInertia* txInertias,
PxTGSSolverBodyData* solverBodyDatas, PxReal invDt, IG::IslandSim& islandSim,
PxU32 startIdx, PxU32 endIdx)
{
const bool sleepingDisabled = isSleepingDisabled();
for (PxU32 k = startIdx; k < endIdx; k++)
{
//PxStepSolverBody& solverBodyData = solverBodyData2[k + 1];
PxTGSSolverBodyVel& solverBodyVel = vels[k + 1];
PxTGSSolverBodyTxInertia& solverBodyTxI = txInertias[k + 1];
PxTGSSolverBodyData& solverBodyData = solverBodyDatas[k + 1];
const Cm::SpatialVector motionVel(solverBodyVel.deltaLinDt*invDt, solverBodyTxI.sqrtInvInertia*(solverBodyVel.deltaAngDt*invDt));
PxsRigidBody& rBody = *objects.bodies[k];
PxsBodyCore& core = rBody.getCore();
rBody.mLastTransform = core.body2World;
core.body2World.q = (solverBodyTxI.deltaBody2WorldQ * core.body2World.q).getNormalized();
core.body2World.p = solverBodyTxI.body2WorldP;
/*core.linearVelocity = (solverBodyVel.linearVelocity);
core.angularVelocity = solverBodyTxI.sqrtInvInertia*(solverBodyVel.angularVelocity);*/
const PxVec3& linearVelocity = solverBodyVel.linearVelocity;
PxVec3 angularVelocity = solverBodyTxI.sqrtInvInertia*(solverBodyVel.angularVelocity);
core.linearVelocity = linearVelocity;
core.angularVelocity = angularVelocity;
if(!sleepingDisabled)
{
const PxU32 hasStaticTouch = islandSim.getIslandStaticTouchCount(PxNodeIndex(solverBodyData.nodeIndex));
sleepCheck(&rBody, mDt, mEnableStabilization, motionVel, hasStaticTouch);
}
}
}
void DynamicsTGSContext::stepArticulations(Dy::ThreadContext& threadContext, const PxsIslandIndices& counts, PxReal dt)
{
for (PxU32 a = 0; a < counts.articulations; ++a)
{
//if(d.articulation->numTotalConstraints > 0)
//d.articulation->solveInternalConstraints(dt, 1.f / dt, threadContext.mZVector.begin(), threadContext.mDeltaV.begin(), false);
FeatherstoneArticulation::recordDeltaMotionTGS(threadContext.mArticulationArray[a], dt, threadContext.mDeltaV.begin());
}
}
void DynamicsTGSContext::updateArticulations(Dy::ThreadContext& threadContext, PxU32 startIdx, PxU32 endIdx, PxReal dt)
{
for (PxU32 a = startIdx; a < endIdx; ++a)
FeatherstoneArticulation::updateBodiesTGS(threadContext.mArticulationArray[a], threadContext.mDeltaV.begin(), dt);
}
void DynamicsTGSContext::applyArticulationTgsSubstepForces(Dy::ThreadContext& threadContext, PxU32 numArticulations, PxReal stepDt)
{
for(PxU32 i = 0; i < numArticulations; ++i)
{
FeatherstoneArticulation* articulation = threadContext.mArticulationArray[i];
// zVector just used as temp buffer
threadContext.mZVector.resizeUninitialized(articulation->getBodyCount());
FeatherstoneArticulation::applyTgsSubstepForces(articulation, stepDt, threadContext.mZVector.begin());
}
}
namespace
{
class ArticulationTask : public Cm::Task
{
Dy::DynamicsTGSContext& mContext;
FeatherstoneArticulation** const mDescs;
const PxU32 mNbDescs;
const PxVec3 mGravity;
const PxReal mDt;
const bool mExternalForcesEveryTgsIterationEnabled;
PX_NOCOPY(ArticulationTask)
public:
static const PxU32 MaxNbPerTask = 32;
ArticulationTask(Dy::DynamicsTGSContext& context, FeatherstoneArticulation** descs, PxU32 nbDescs,
const PxVec3& gravity, PxReal dt, PxU64 contextId, bool externalForcesEveryTgsIterationEnabled)
: Cm::Task(contextId)
, mContext(context)
, mDescs(descs)
, mNbDescs(nbDescs)
, mGravity(gravity)
, mDt(dt)
, mExternalForcesEveryTgsIterationEnabled(externalForcesEveryTgsIterationEnabled)
{
}
virtual const char* getName() const PX_OVERRIDE { return "ArticulationTask"; }
virtual void runInternal() PX_OVERRIDE
{
PxU32 maxLinks = 0;
for (PxU32 i = 0; i < mNbDescs; i++)
maxLinks = PxMax(maxLinks, mDescs[i]->getBodyCount());
Dy::ThreadContext& threadContext = *mContext.getThreadContext();
threadContext.mDeltaV.forceSize_Unsafe(0);
threadContext.mDeltaV.reserve(maxLinks);
threadContext.mDeltaV.forceSize_Unsafe(maxLinks);
const PxReal invLengthScale = 1.f / mContext.getLengthScale();
for (PxU32 a = 0; a < mNbDescs; ++a)
{
FeatherstoneArticulation::computeUnconstrainedVelocities(mDescs[a], mDt,
mGravity, invLengthScale, mExternalForcesEveryTgsIterationEnabled, true);
}
mContext.putThreadContext(&threadContext);
}
};
}
void DynamicsTGSContext::setupArticulations(IslandContextStep& islandContext, const PxVec3& gravity, PxReal dt, PxU32& posIters,
PxU32& velIters, PxBaseTask* continuation)
{
Dy::ThreadContext* threadContext = islandContext.mThreadContext;
Dy::FeatherstoneArticulation** articulations = threadContext->mArticulationArray;
PxU32 nbArticulations = islandContext.mCounts.articulations;
PxU32 maxVelIters = 0;
PxU32 maxPosIters = 0;
for (PxU32 a = 0; a < nbArticulations; a+= ArticulationTask::MaxNbPerTask)
{
// PT: TODO: why not do that in the task, like we do for PGS?
const PxU32 endIdx = PxMin(nbArticulations, a + ArticulationTask::MaxNbPerTask);
for (PxU32 b = a; b < endIdx; ++b)
{
const PxU16 iterWord = articulations[b]->getIterationCounts();
maxVelIters = PxMax<PxU32>(PxU32(iterWord >> 8), maxVelIters);
maxPosIters = PxMax<PxU32>(PxU32(iterWord & 0xff), maxPosIters);
}
ArticulationTask* task = PX_PLACEMENT_NEW(mTaskPool.allocate(sizeof(ArticulationTask)), ArticulationTask)(*this, articulations + a,
endIdx - a, gravity, dt, getContextId(), mIsExternalForcesEveryTgsIterationEnabled);
task->setContinuation(continuation);
task->removeReference();
}
velIters = PxMax(maxVelIters, velIters);
posIters = PxMax(maxPosIters, posIters);
}
void DynamicsTGSContext::setupArticulationInternalConstraints(IslandContextStep& islandContext, PxReal dt, PxReal invStepDt)
{
Dy::FeatherstoneArticulation** articulations = islandContext.mThreadContext->mArticulationArray;
PxU32 nbArticulations = islandContext.mCounts.articulations;
ThreadContext* threadContext = getThreadContext();
threadContext->mConstraintBlockStream.reset();
for (PxU32 a = 0; a < nbArticulations; ++a)
FeatherstoneArticulation::setupSolverConstraintsTGS(articulations[a], islandContext.mStepDt, invStepDt, dt);
putThreadContext(threadContext);
}
class SetupDescsTask : public Cm::Task
{
Dy::IslandContextStep& mIslandContext;
const SolverIslandObjectsStep& mObjects;
PxU32* mBodyRemapTable;
PxU32 mSolverBodyOffset;
PxsContactManagerOutputIterator& mOutputs;
DynamicsTGSContext& mContext;
PX_NOCOPY(SetupDescsTask)
public:
SetupDescsTask(IslandContextStep& islandContext, const SolverIslandObjectsStep& objects,
PxU32* bodyRemapTable, PxU32 solverBodyOffset, PxsContactManagerOutputIterator& outputs, DynamicsTGSContext& context) : Cm::Task(context.getContextId()),
mIslandContext(islandContext), mObjects(objects), mBodyRemapTable(bodyRemapTable), mSolverBodyOffset(solverBodyOffset),
mOutputs(outputs), mContext(context)
{
}
virtual const char* getName() const PX_OVERRIDE { return "SetupDescsTask"; }
virtual void runInternal() PX_OVERRIDE
{
mContext.setupDescs(mIslandContext, mObjects, mBodyRemapTable, mSolverBodyOffset, mOutputs);
mIslandContext.mArticulationOffset = mIslandContext.mThreadContext->contactDescArraySize;
}
};
class PreIntegrateParallelTask : public Cm::Task
{
PxsBodyCore** mBodyArray;
PxsRigidBody** mOriginalBodyArray;
PxTGSSolverBodyVel* mSolverBodyVelPool;
PxTGSSolverBodyTxInertia* mSolverBodyTxInertia;
PxTGSSolverBodyData* mSolverBodyDataPool2;
PxU32* mNodeIndexArray;
const PxU32 mBodyCount;
const PxVec3& mGravity;
const PxReal mDt;
PxU32& mPosIters;
PxU32& mVelIters;
DynamicsTGSContext& mContext;
PX_NOCOPY(PreIntegrateParallelTask)
public:
PreIntegrateParallelTask(PxsBodyCore** bodyArray, PxsRigidBody** originalBodyArray,
PxTGSSolverBodyVel* solverBodyVelPool, PxTGSSolverBodyTxInertia* solverBodyTxInertia, PxTGSSolverBodyData* solverBodyDataPool2,
PxU32* nodeIndexArray, PxU32 bodyCount, const PxVec3& gravity, PxReal dt, PxU32& posIters, PxU32& velIters,
DynamicsTGSContext& context) : Cm::Task(context.getContextId()),
mBodyArray(bodyArray), mOriginalBodyArray(originalBodyArray), mSolverBodyVelPool(solverBodyVelPool),
mSolverBodyTxInertia(solverBodyTxInertia), mSolverBodyDataPool2(solverBodyDataPool2), mNodeIndexArray(nodeIndexArray),
mBodyCount(bodyCount), mGravity(gravity), mDt(dt), mPosIters(posIters), mVelIters(velIters), mContext(context)
{
}
virtual const char* getName() const PX_OVERRIDE { return "PreIntegrateParallelTask"; }
virtual void runInternal() PX_OVERRIDE
{
PxU32 posIters = 0;
PxU32 velIters = 0;
mContext.preIntegrateBodies(mBodyArray, mOriginalBodyArray, mSolverBodyVelPool, mSolverBodyTxInertia, mSolverBodyDataPool2, mNodeIndexArray, mBodyCount, mGravity, mDt, posIters, velIters, 0);
PxAtomicMax(reinterpret_cast<PxI32*>(&mPosIters), PxI32(posIters));
PxAtomicMax(reinterpret_cast<PxI32*>(&mVelIters), PxI32(velIters));
}
};
class PreIntegrateTask : public Cm::Task
{
PxsBodyCore** mBodyArray;
PxsRigidBody** mOriginalBodyArray;
PxTGSSolverBodyVel* mSolverBodyVelPool;
PxTGSSolverBodyTxInertia* mSolverBodyTxInertia;
PxTGSSolverBodyData* mSolverBodyDataPool2;
PxU32* mNodeIndexArray;
const PxU32 mBodyCount;
const PxVec3& mGravity;
const PxReal mDt;
PxU32& mPosIters;
PxU32& mVelIters;
DynamicsTGSContext& mContext;
PX_NOCOPY(PreIntegrateTask)
public:
PreIntegrateTask(PxsBodyCore** bodyArray, PxsRigidBody** originalBodyArray,
PxTGSSolverBodyVel* solverBodyVelPool, PxTGSSolverBodyTxInertia* solverBodyTxInertia, PxTGSSolverBodyData* solverBodyDataPool2,
PxU32* nodeIndexArray, PxU32 bodyCount, const PxVec3& gravity, PxReal dt, PxU32& posIters, PxU32& velIters,
DynamicsTGSContext& context) : Cm::Task(context.getContextId()),
mBodyArray(bodyArray), mOriginalBodyArray(originalBodyArray), mSolverBodyVelPool(solverBodyVelPool),
mSolverBodyTxInertia(solverBodyTxInertia), mSolverBodyDataPool2(solverBodyDataPool2), mNodeIndexArray(nodeIndexArray),
mBodyCount(bodyCount), mGravity(gravity), mDt(dt), mPosIters(posIters), mVelIters(velIters), mContext(context)
{
}
virtual const char* getName() const PX_OVERRIDE { return "PreIntegrateTask"; }
virtual void runInternal() PX_OVERRIDE
{
const PxU32 BodiesPerTask = 512;
if (mBodyCount <= BodiesPerTask)
{
PxU32 posIters = 0;
PxU32 velIters = 0;
mContext.preIntegrateBodies(mBodyArray, mOriginalBodyArray, mSolverBodyVelPool, mSolverBodyTxInertia, mSolverBodyDataPool2, mNodeIndexArray, mBodyCount, mGravity, mDt, posIters, velIters, 0);
PxAtomicMax(reinterpret_cast<PxI32*>(&mPosIters), PxI32(posIters));
PxAtomicMax(reinterpret_cast<PxI32*>(&mVelIters), PxI32(velIters));
}
else
{
for (PxU32 i = 0; i < mBodyCount; i += BodiesPerTask)
{
const PxU32 nbToProcess = PxMin(mBodyCount - i, BodiesPerTask);
PreIntegrateParallelTask* task = PX_PLACEMENT_NEW(mContext.getTaskPool().allocate(sizeof(PreIntegrateParallelTask)), PreIntegrateParallelTask)
(mBodyArray+i, mOriginalBodyArray+i, mSolverBodyVelPool+i, mSolverBodyTxInertia+i, mSolverBodyDataPool2+i,mNodeIndexArray+i,
nbToProcess, mGravity, mDt, mPosIters, mVelIters, mContext);
task->setContinuation(mCont);
task->removeReference();
}
}
}
};
class SetStepperTask : public Cm::Task
{
Dy::IslandContextStep& mIslandContext;
Dy::DynamicsTGSContext& mContext;
PxBaseTask* mAdditionalContinuation;
PX_NOCOPY(SetStepperTask)
public:
SetStepperTask(Dy::IslandContextStep& islandContext,
DynamicsTGSContext& context) : Cm::Task(context.getContextId()),
mIslandContext(islandContext), mContext(context),
mAdditionalContinuation(NULL)
{
}
virtual const char* getName() const PX_OVERRIDE { return "SetStepperTask"; }
void setAdditionalContinuation(PxBaseTask* cont)
{
mAdditionalContinuation = cont;
cont->addReference();
}
virtual void runInternal() PX_OVERRIDE
{
PxReal dt = mContext.getDt();
mIslandContext.mStepDt = dt / PxReal(mIslandContext.mPosIters);
mIslandContext.mInvStepDt = 1.f/mIslandContext.mStepDt;
mIslandContext.mBiasCoefficients.set(false, true, mIslandContext.mPosIters);
}
virtual void release() PX_OVERRIDE
{
Cm::Task::release();
mAdditionalContinuation->removeReference();
}
};
class SetupArticulationTask : public Cm::Task
{
IslandContextStep& mIslandContext;
const PxVec3& mGravity;
const PxReal mDt;
PxU32& mPosIters;
PxU32& mVelIters;
DynamicsTGSContext& mContext;
PX_NOCOPY(SetupArticulationTask)
public:
SetupArticulationTask(IslandContextStep& islandContext, const PxVec3& gravity, PxReal dt, PxU32& posIters,
PxU32& velIters, DynamicsTGSContext& context) : Cm::Task(context.getContextId()),
mIslandContext(islandContext), mGravity(gravity), mDt(dt), mPosIters(posIters), mVelIters(velIters), mContext(context)
{
}
virtual const char* getName() const PX_OVERRIDE { return "SetupArticulationTask"; }
virtual void runInternal() PX_OVERRIDE
{
PxU32 posIters = 0, velIters = 0;
mContext.setupArticulations(mIslandContext, mGravity, mDt, posIters, velIters, mCont);
PxAtomicMax(reinterpret_cast<PxI32*>(&mPosIters), PxI32(posIters));
PxAtomicMax(reinterpret_cast<PxI32*>(&mVelIters), PxI32(velIters));
}
};
class SetupArticulationInternalConstraintsTask : public Cm::Task
{
IslandContextStep& mIslandContext;
const PxReal mDt;
const PxReal mInvDt;
DynamicsTGSContext& mContext;
PX_NOCOPY(SetupArticulationInternalConstraintsTask)
public:
SetupArticulationInternalConstraintsTask(IslandContextStep& islandContext, PxReal dt, PxReal invDt, DynamicsTGSContext& context) : Cm::Task(context.getContextId()),
mIslandContext(islandContext), mDt(dt), mInvDt(invDt), mContext(context)
{
}
virtual const char* getName() const PX_OVERRIDE { return "SetupArticulationInternalConstraintsTask"; }
virtual void runInternal() PX_OVERRIDE
{
mContext.setupArticulationInternalConstraints(mIslandContext, mDt, mIslandContext.mInvStepDt);
}
};
class SetupSolverConstraintsSubTask : public Cm::Task
{
PxSolverConstraintDesc* mContactDescPtr;
PxConstraintBatchHeader* mHeaders;
const PxU32 mNbHeaders;
PxsContactManagerOutputIterator& mOutputs;
PxReal mStepDt;
PxReal mTotalDt;
PxReal mInvStepDt;
PxReal mInvDtTotal;
PxReal mRigidContactBiasCoefficient;
PxReal mJointBiasCoefficient;
DynamicsTGSContext& mContext;
ThreadContext& mIslandThreadContext;
PX_NOCOPY(SetupSolverConstraintsSubTask)
public:
static const PxU32 MaxPerTask = 64;
SetupSolverConstraintsSubTask(PxSolverConstraintDesc* contactDescPtr, PxConstraintBatchHeader* headers, PxU32 nbHeaders,
PxsContactManagerOutputIterator& outputs, PxReal stepDt, PxReal totalDt, PxReal invStepDt, PxReal invDtTotal,
PxReal rigidContactBiasCoefficient, PxReal jointBiasCoefficient, ThreadContext& islandThreadContext, DynamicsTGSContext& context) : Cm::Task(context.getContextId()),
mContactDescPtr(contactDescPtr), mHeaders(headers), mNbHeaders(nbHeaders), mOutputs(outputs), mStepDt(stepDt), mTotalDt(totalDt), mInvStepDt(invStepDt),
mInvDtTotal(invDtTotal), mRigidContactBiasCoefficient(rigidContactBiasCoefficient), mJointBiasCoefficient(jointBiasCoefficient),
mContext(context), mIslandThreadContext(islandThreadContext)
{
}
virtual const char* getName() const PX_OVERRIDE { return "SetupSolverConstraintsSubTask"; }
virtual void runInternal() PX_OVERRIDE
{
ThreadContext* tempContext = mContext.getThreadContext();
tempContext->mConstraintBlockStream.reset();
mContext.createSolverConstraints(mContactDescPtr, mHeaders, mNbHeaders, mOutputs, mIslandThreadContext, *tempContext, mStepDt, mTotalDt, mInvStepDt,
mRigidContactBiasCoefficient, mJointBiasCoefficient);
mContext.putThreadContext(tempContext);
}
};
class PxsCreateArticConstraintsSubTask : public Cm::Task
{
PxsCreateArticConstraintsSubTask& operator=(const PxsCreateArticConstraintsSubTask&);
public:
static const PxU32 NbArticsPerTask = 64;
PxsCreateArticConstraintsSubTask(Dy::FeatherstoneArticulation** articulations, const PxU32 nbArticulations,
PxTGSSolverBodyData* solverBodyData, PxTGSSolverBodyTxInertia* solverBodyTxInertia,
ThreadContext& threadContext, DynamicsTGSContext& context, PxsContactManagerOutputIterator& outputs,
Dy::IslandContextStep& islandContext) :
Cm::Task(context.getContextId()),
mArticulations(articulations),
mNbArticulations(nbArticulations),
mSolverBodyData(solverBodyData),
mSolverBodyTxInertia(solverBodyTxInertia),
mThreadContext(threadContext),
mDynamicsContext(context),
mOutputs(outputs),
mIslandContext(islandContext)
{}
virtual void runInternal() PX_OVERRIDE
{
const PxReal correlationDist = mDynamicsContext.getCorrelationDistance();
const PxReal bounceThreshold = mDynamicsContext.getBounceThreshold();
const PxReal frictionOffsetThreshold = mDynamicsContext.getFrictionOffsetThreshold();
const PxReal dt = mDynamicsContext.getDt();
const PxReal invStepDt = PxMin(mDynamicsContext.getMaxBiasCoefficient(), mIslandContext.mInvStepDt);
const PxReal invDt = 1.0f / dt;
ThreadContext* threadContext = mDynamicsContext.getThreadContext();
threadContext->mConstraintBlockStream.reset(); //ensure there's no left-over memory that belonged to another island
/*threadContext->mZVector.forceSize_Unsafe(0);
threadContext->mZVector.reserve(mThreadContext.mMaxArticulationLinks);
threadContext->mZVector.forceSize_Unsafe(mThreadContext.mMaxArticulationLinks);*/
for (PxU32 i = 0; i < mNbArticulations; ++i)
{
mArticulations[i]->prepareStaticConstraintsTGS(mIslandContext.mStepDt, dt, invStepDt, invDt, mOutputs, *threadContext, correlationDist, bounceThreshold, frictionOffsetThreshold,
mSolverBodyData, mSolverBodyTxInertia, mThreadContext.mConstraintBlockManager, mDynamicsContext.getConstraintWriteBackPool().begin(),
mIslandContext.mBiasCoefficients.rigidContact, mIslandContext.mBiasCoefficients.joint, mDynamicsContext.getLengthScale());
}
mDynamicsContext.putThreadContext(threadContext);
}
virtual const char* getName() const PX_OVERRIDE
{
return "PxsDynamics.PxsCreateArticConstraintsSubTask";
}
public:
Dy::FeatherstoneArticulation** mArticulations;
PxU32 mNbArticulations;
PxTGSSolverBodyData* mSolverBodyData;
PxTGSSolverBodyTxInertia* mSolverBodyTxInertia;
ThreadContext& mThreadContext;
DynamicsTGSContext& mDynamicsContext;
PxsContactManagerOutputIterator& mOutputs;
IslandContextStep& mIslandContext;
};
class SetupSolverConstraintsTask : public Cm::Task
{
IslandContextStep& mIslandContext;
PxSolverConstraintDesc* mContactDescPtr;
PxsContactManagerOutputIterator& mOutputs;
PxReal mTotalDt;
DynamicsTGSContext& mContext;
PX_NOCOPY(SetupSolverConstraintsTask)
public:
SetupSolverConstraintsTask(IslandContextStep& islandContext, PxSolverConstraintDesc* contactDescPtr,
PxsContactManagerOutputIterator& outputs, PxReal totalDt, DynamicsTGSContext& context) : Cm::Task(context.getContextId()),
mIslandContext(islandContext), mContactDescPtr(contactDescPtr), mOutputs(outputs),
mTotalDt(totalDt), mContext(context)
{
}
virtual const char* getName() const PX_OVERRIDE { return "SetupSolverConstraintsTask"; }
virtual void runInternal() PX_OVERRIDE
{
Dy::ThreadContext& threadContext = *mIslandContext.mThreadContext;
const PxU32 nbBatches = threadContext.numContactConstraintBatches;
PxConstraintBatchHeader* hdr = mIslandContext.mObjects.constraintBatchHeaders;
//for (PxU32 a = 0; a < mIslandContext.mArticulationOffset; a += SetupSolverConstraintsSubTask::MaxPerTask)
for (PxU32 a = 0; a < nbBatches; a += SetupSolverConstraintsSubTask::MaxPerTask)
{
const PxU32 nbConstraints = PxMin(nbBatches - a, SetupSolverConstraintsSubTask::MaxPerTask);
SetupSolverConstraintsSubTask* task = PX_PLACEMENT_NEW(mContext.mTaskPool.allocate(sizeof(SetupSolverConstraintsSubTask)), SetupSolverConstraintsSubTask)
(mContactDescPtr, hdr + a, nbConstraints, mOutputs, mIslandContext.mStepDt, mTotalDt, mIslandContext.mInvStepDt, mContext.mInvDt,
mIslandContext.mBiasCoefficients.rigidContact, mIslandContext.mBiasCoefficients.joint, threadContext, mContext);
task->setContinuation(mCont);
task->removeReference();
}
const PxU32 articCount = mIslandContext.mCounts.articulations;
for (PxU32 i = 0; i < articCount; i += PxsCreateArticConstraintsSubTask::NbArticsPerTask)
{
const PxU32 nbToProcess = PxMin(articCount - i, PxsCreateArticConstraintsSubTask::NbArticsPerTask);
PxsCreateArticConstraintsSubTask* task = PX_PLACEMENT_NEW(mContext.getTaskPool().allocate(sizeof(PxsCreateArticConstraintsSubTask)), PxsCreateArticConstraintsSubTask)
(threadContext.mArticulationArray + i, nbToProcess, mContext.mSolverBodyDataPool2.begin(), mContext.mSolverBodyTxInertiaPool.begin(), threadContext, mContext, mOutputs,
mIslandContext);
task->setContinuation(mCont);
task->removeReference();
}
}
};
class PartitionTask : public Cm::Task
{
IslandContextStep& mIslandContext;
const PxSolverConstraintDesc* mContactDescPtr;
PxTGSSolverBodyVel* mSolverBodyData;
DynamicsTGSContext& mContext;
PX_NOCOPY(PartitionTask)
public:
PartitionTask(IslandContextStep& islandContext, PxSolverConstraintDesc* contactDescPtr, PxTGSSolverBodyVel* solverBodyData,
DynamicsTGSContext& context) : Cm::Task(context.getContextId()),
mIslandContext(islandContext), mContactDescPtr(contactDescPtr), mSolverBodyData(solverBodyData),
mContext(context)
{
}
virtual const char* getName() const PX_OVERRIDE { return "PartitionTask"; }
virtual void runInternal() PX_OVERRIDE
{
Dy::ThreadContext& threadContext = *mIslandContext.mThreadContext;
const PxU32 totalDescCount = threadContext.contactDescArraySize;
threadContext.mConstraintsPerPartition.forceSize_Unsafe(0);
threadContext.mConstraintsPerPartition.resize(1);
threadContext.mConstraintsPerPartition[0] = 0;
const PxU32 numArticulations = threadContext.mNbArticulations;
Dy::FeatherstoneArticulation** eaArticulations = threadContext.mArticulationArray;
// PT: maxPartitions = PX_MAX_U32 in PGS. Was there a reason to limit it for TGS?
const ConstraintPartitionIn in( reinterpret_cast<PxU8*>(mSolverBodyData), mIslandContext.mCounts.bodies, sizeof(PxTGSSolverBodyVel),
eaArticulations, numArticulations, mContactDescPtr, totalDescCount, 64, false);
//args.mBitField = &threadContext.mPartitionNormalizationBitmap; // PT: removed, unused
ConstraintPartitionOut out(mIslandContext.mObjects.orderedConstraintDescs, mIslandContext.mObjects.tempConstraintDescs, &threadContext.mConstraintsPerPartition);
threadContext.mMaxPartitions = partitionContactConstraints(out, in);
threadContext.mNumDifferentBodyConstraints = out.mNumDifferentBodyConstraints;
threadContext.mNumStaticConstraints = out.mNumStaticConstraints;
threadContext.mHasOverflowPartitions = out.mNumOverflowConstraints != 0;
{
const PxU32 descCount = threadContext.mNumDifferentBodyConstraints;
const PxU32 selfConstraintDescCount = threadContext.contactDescArraySize - (threadContext.mNumDifferentBodyConstraints + threadContext.mNumStaticConstraints);
PxArray<PxU32>& accumulatedConstraintsPerPartition = threadContext.mConstraintsPerPartition;
PxU32 numHeaders = 0;
PxU32 currentPartition = 0;
PxU32 maxJ = descCount == 0 ? 0 : accumulatedConstraintsPerPartition[0];
const PxU32 maxBatchPartition = 0xFFFFFFFF;
const PxU32 maxBatchSize2 = mContext.isEnhancedDeterminismEnabled() ? 1u : 4u;
PxConstraintBatchHeader* batchHeaders = mIslandContext.mObjects.constraintBatchHeaders;
PxSolverConstraintDesc* orderedConstraints = mIslandContext.mObjects.orderedConstraintDescs;
PxU32 headersPerPartition = 0;
//KS - if we have overflowed the partition limit, overflow constraints are pushed
//into 0th partition. If that's the case, we need to disallow batching of constraints
//in 0th partition.
PxU32 maxBatchSize = out.mNumOverflowConstraints == 0 ? maxBatchSize2 : 1;
for (PxU32 a = 0; a < descCount;)
{
PxU32 loopMax = PxMin(maxJ - a, maxBatchSize);
PxU16 j = 0;
if (loopMax > 0)
{
PxConstraintBatchHeader& header = batchHeaders[numHeaders++];
j = 1;
PxSolverConstraintDesc& desc = orderedConstraints[a];
if (!isArticulationConstraint(desc) && (desc.constraintType == DY_SC_TYPE_RB_CONTACT ||
desc.constraintType == DY_SC_TYPE_RB_1D) && currentPartition < maxBatchPartition)
{
for (; j < loopMax && desc.constraintType == orderedConstraints[a + j].constraintType &&
!isArticulationConstraint(orderedConstraints[a + j]); ++j);
}
header.startIndex = a;
header.stride = j;
header.constraintType = desc.constraintType;
headersPerPartition++;
}
if (maxJ == (a + j) && maxJ != descCount)
{
//Go to next partition!
accumulatedConstraintsPerPartition[currentPartition] = headersPerPartition;
headersPerPartition = 0;
currentPartition++;
maxJ = accumulatedConstraintsPerPartition[currentPartition];
maxBatchSize = maxBatchSize2;
}
a += j;
}
if (descCount)
accumulatedConstraintsPerPartition[currentPartition] = headersPerPartition;
accumulatedConstraintsPerPartition.forceSize_Unsafe(threadContext.mMaxPartitions);
for (PxU32 a = 0; a < selfConstraintDescCount; ++a)
{
PxConstraintBatchHeader& header = batchHeaders[numHeaders++];
header.startIndex = a + descCount;
header.stride = 1;
header.constraintType = DY_SC_TYPE_EXT_1D;
}
threadContext.numContactConstraintBatches = numHeaders;
}
}
};
class ParallelSolveTask : public Cm::Task
{
IslandContextStep& mIslandContext;
const SolverIslandObjectsStep& mObjects;
const PxsIslandIndices& mCounts;
PxReal mTotalDt;
DynamicsTGSContext& mContext;
PX_NOCOPY(ParallelSolveTask)
public:
ParallelSolveTask(IslandContextStep& islandContext, const SolverIslandObjectsStep& objects, const PxsIslandIndices& counts, PxReal totalDt,
DynamicsTGSContext& context) : Cm::Task(context.getContextId()),
mIslandContext(islandContext), mObjects(objects), mCounts(counts), mTotalDt(totalDt), mContext(context)
{
}
virtual const char* getName() const PX_OVERRIDE { return "ParallelSolveTask"; }
virtual void runInternal() PX_OVERRIDE
{
mContext.iterativeSolveIslandParallel(mObjects, mCounts, *mIslandContext.mThreadContext, mIslandContext.mStepDt, mTotalDt, mIslandContext.mPosIters, mIslandContext.mVelIters,
mIslandContext.mBiasCoefficients.articulation,
&mIslandContext.mSharedSolverIndex, &mIslandContext.mSharedRigidBodyIndex, &mIslandContext.mSharedArticulationIndex, &mIslandContext.mSharedGravityIndex,
&mIslandContext.mSolvedCount, &mIslandContext.mRigidBodyIntegratedCount, &mIslandContext.mArticulationIntegratedCount, &mIslandContext.mGravityIntegratedCount,
4, 128);
}
};
class SolveIslandTask : public Cm::Task
{
IslandContextStep& mIslandContext;
const SolverIslandObjectsStep& mObjects;
const PxsIslandIndices& mCounts;
PxReal mTotalDt;
DynamicsTGSContext& mContext;
PX_NOCOPY(SolveIslandTask)
public:
SolveIslandTask(IslandContextStep& islandContext, const SolverIslandObjectsStep& objects, const PxsIslandIndices& counts, PxReal totalDt,
DynamicsTGSContext& context) : Cm::Task(context.getContextId()),
mIslandContext(islandContext), mObjects(objects), mCounts(counts), mTotalDt(totalDt), mContext(context)
{
}
virtual const char* getName() const PX_OVERRIDE { return "SolveIslandTask"; }
virtual void runInternal() PX_OVERRIDE
{
PxU32 totalCount = 0;
PxU32 totalPartitions = 0;
PxU32 j = 0, i = 0;
PxU32 numBatches = 0;
PxU32 currIndex = 0;
// PT: TODO: share with PGS
Dy::ThreadContext& threadContext = *mIslandContext.mThreadContext;
PxSolverConstraintDesc* PX_RESTRICT contactDescBegin = mObjects.orderedConstraintDescs;
PxConstraintBatchHeader* PX_RESTRICT headers = mObjects.constraintBatchHeaders;
for(PxU32 a = 0; a < threadContext.mConstraintsPerPartition.size(); ++a)
{
const PxU32 endIndex = currIndex + threadContext.mConstraintsPerPartition[a];
PxU32 numBatchesInPartition = 0;
for(PxU32 b = currIndex; b < endIndex; ++b)
{
PxConstraintBatchHeader& _header = headers[b];
PxU16 stride = _header.stride, newStride = _header.stride;
PxU32 startIndex = j;
for(PxU16 c = 0; c < stride; ++c)
{
if(getConstraintLength(contactDescBegin[i]) == 0)
{
newStride--;
i++;
}
else
{
if(i != j)
contactDescBegin[j] = contactDescBegin[i];
i++;
j++;
}
}
if(newStride != 0)
{
headers[numBatches].startIndex = startIndex;
headers[numBatches].stride = newStride;
PxU8 type = *contactDescBegin[startIndex].constraint;
if(type == DY_SC_TYPE_STATIC_CONTACT)
{
//Check if any block of constraints is classified as type static (single) contact constraint.
//If they are, iterate over all constraints grouped with it and switch to "dynamic" contact constraint
//type if there's a dynamic contact constraint in the group.
for(PxU32 c = 1; c < newStride; ++c)
{
if(*contactDescBegin[startIndex + c].constraint == DY_SC_TYPE_RB_CONTACT)
{
type = DY_SC_TYPE_RB_CONTACT;
}
}
}
headers[numBatches].constraintType = type;
numBatches++;
numBatchesInPartition++;
}
}
currIndex += threadContext.mConstraintsPerPartition[a];
threadContext.mConstraintsPerPartition[totalPartitions] = numBatchesInPartition;
if (numBatchesInPartition)
totalPartitions++;
else if (a == 0)
threadContext.mHasOverflowPartitions = false; //If our first partition is now empty, we have no overflows so clear the overflow flag
totalCount += numBatchesInPartition;
}
currIndex = totalCount;
processOverflowConstraints(reinterpret_cast<PxU8*>(mContext.mSolverBodyVelPool.begin() + mObjects.solverBodyOffset+1), sizeof(PxTGSSolverBodyVel),
mCounts.bodies, threadContext.mArticulationArray, threadContext.mNbArticulations, contactDescBegin,
threadContext.mHasOverflowPartitions ? threadContext.mConstraintsPerPartition[0] : 0);
//Decision whether to spawn multi-threaded solver or single-threaded solver...
threadContext.mConstraintsPerPartition.forceSize_Unsafe(totalPartitions);
threadContext.numContactConstraintBatches = totalCount;
PxU32 maxLinks = 0;
for (PxU32 a = 0; a < mCounts.articulations; ++a)
maxLinks = PxMax(maxLinks, threadContext.mArticulationArray[a]->getBodyCount());
threadContext.mDeltaV.forceSize_Unsafe(0);
threadContext.mDeltaV.reserve(maxLinks);
threadContext.mDeltaV.forceSize_Unsafe(maxLinks);
threadContext.mZVector.forceSize_Unsafe(0);
threadContext.mZVector.reserve(maxLinks);
threadContext.mZVector.forceSize_Unsafe(maxLinks);
SolverContext cache;
cache.deltaV = threadContext.mDeltaV.begin();
if (threadContext.mConstraintsPerPartition.size())
{
const PxU32 threadCount = getTaskManager()->getCpuDispatcher()->getWorkerCount();
PxU32 nbHeadersPerPartition;
if (threadContext.mHasOverflowPartitions)
{
// jcarius: mitigating potential divide-by-zero problem. It's unclear whether size originally includes
// the overflow partition or not.
const PxU32 size = threadContext.mConstraintsPerPartition.size();
const PxU32 nbPartitionsMinusOverflow = (size > 1) ? (size - 1) : 1;
PxU32 nbConstraintsMinusOverflow = currIndex - threadContext.mConstraintsPerPartition[0];
nbHeadersPerPartition = ((nbConstraintsMinusOverflow + nbPartitionsMinusOverflow - 1) / nbPartitionsMinusOverflow);
}
else
nbHeadersPerPartition = ((currIndex + threadContext.mConstraintsPerPartition.size() - 1) / threadContext.mConstraintsPerPartition.size());
const PxU32 NbBatchesPerThread = 8;
//const PxU32 NbBatchesPerThread = 4;
const PxU32 nbIdealThreads = (nbHeadersPerPartition + NbBatchesPerThread-1) / NbBatchesPerThread;
if (threadCount < 2 || nbIdealThreads < 2) // not great if we have many articulations but no contact constraints => PX-4708
mContext.iterativeSolveIsland(mObjects, mCounts, threadContext, mIslandContext.mStepDt, mIslandContext.mInvStepDt, mTotalDt,
mIslandContext.mPosIters, mIslandContext.mVelIters, mIslandContext.mBiasCoefficients.articulation, cache);
else
{
mIslandContext.mSharedSolverIndex = 0;
mIslandContext.mSolvedCount = 0;
mIslandContext.mSharedRigidBodyIndex = 0;
mIslandContext.mRigidBodyIntegratedCount = 0;
mIslandContext.mSharedArticulationIndex = 0;
mIslandContext.mArticulationIntegratedCount = 0;
mIslandContext.mSharedGravityIndex = 0;
mIslandContext.mGravityIntegratedCount = 0;
PxU32 nbThreads = PxMin(threadCount, nbIdealThreads);
ParallelSolveTask* tasks = reinterpret_cast<ParallelSolveTask*>(mContext.getTaskPool().allocate(sizeof(ParallelSolveTask)*nbThreads));
for (PxU32 a = 0; a < nbThreads; ++a)
{
PX_PLACEMENT_NEW(&tasks[a], ParallelSolveTask)(mIslandContext, mObjects, mCounts, mTotalDt, mContext);
tasks[a].setContinuation(mCont);
tasks[a].removeReference();
}
}
}
else
{
mContext.iterativeSolveIsland(mObjects, mCounts, threadContext, mIslandContext.mStepDt,
mIslandContext.mInvStepDt, mTotalDt, mIslandContext.mPosIters, mIslandContext.mVelIters,
mIslandContext.mBiasCoefficients.articulation, cache);
}
}
};
class EndIslandTask : public Cm::Task
{
ThreadContext& mThreadContext;
DynamicsTGSContext& mContext;
PX_NOCOPY(EndIslandTask)
public:
EndIslandTask(ThreadContext& threadContext, DynamicsTGSContext& context) : Cm::Task(context.getContextId()),
mThreadContext(threadContext), mContext(context)
{
}
virtual const char* getName() const PX_OVERRIDE { return "EndIslandTask"; }
virtual void runInternal() PX_OVERRIDE
{
mContext.endIsland(mThreadContext);
}
};
class FinishSolveIslandTask : public Cm::Task
{
ThreadContext& mThreadContext;
const SolverIslandObjectsStep& mObjects;
const PxsIslandIndices& mCounts;
IG::SimpleIslandManager& mIslandManager;
DynamicsTGSContext& mContext;
PX_NOCOPY(FinishSolveIslandTask)
public:
FinishSolveIslandTask(ThreadContext& threadContext, const SolverIslandObjectsStep& objects,
const PxsIslandIndices& counts, IG::SimpleIslandManager& islandManager, DynamicsTGSContext& context) : Cm::Task(context.getContextId()),
mThreadContext(threadContext), mObjects(objects), mCounts(counts), mIslandManager(islandManager), mContext(context)
{
}
virtual const char* getName() const PX_OVERRIDE { return "FinishSolveIslandTask"; }
virtual void runInternal() PX_OVERRIDE
{
mContext.finishSolveIsland(mThreadContext, mObjects, mCounts, mIslandManager, mCont);
}
};
template<bool isVelIter, bool isLastPosIter>
static void processIterationIsland
(const PxReal totalDt, const PxReal stepDt, const PxReal recipStepDt, const PxReal elapsedTime,
const bool isExternalForcesEveryTgsIterationEnabled,
//solve artics:
const bool solveArticulationContactLast,
FeatherstoneArticulation** articulations, const PxU32 nbArticulations, const PxReal articulationBiasCoefficient,
//solve rbodies:
const PxSolverConstraintDesc* const contactDescPtr,
const PxConstraintBatchHeader* const batchHeaders, const PxU32 nbHeaders, PxTGSSolverBodyTxInertia* solverTxInertia,
const PxReal minPenetration, SolverContext& cache)
{
if(solveArticulationContactLast)
{
const ArticulationConstraintProcessingConfigCPU firstPassArticulationConstraintProcessingConfig = ArticulationConstraintProcessingConfigCPU::getFirstPassConfig();
const ArticulationConstraintProcessingConfigCPU secondPassArticulationConstraintProcessingConfig = ArticulationConstraintProcessingConfigCPU::getSecondPassConfig();
for (PxU32 i = 0; i < nbArticulations; ++i)
{
FeatherstoneArticulation* articulation = articulations[i];
articulation->solveInternalConstraints(
totalDt, stepDt, recipStepDt,
isVelIter, true,
firstPassArticulationConstraintProcessingConfig,
elapsedTime,
articulationBiasCoefficient,
isExternalForcesEveryTgsIterationEnabled);
}
if(isLastPosIter)
{
solveConcludeConstraintsIteration<false>(contactDescPtr, batchHeaders, nbHeaders,
solverTxInertia, elapsedTime, cache, 0);
}
else
{
solveConstraintsIteration(contactDescPtr, batchHeaders, nbHeaders, recipStepDt,
solverTxInertia, elapsedTime, minPenetration, cache);
}
for (PxU32 i = 0; i < nbArticulations; ++i)
{
FeatherstoneArticulation* articulation = articulations[i];
articulation->solveInternalConstraints(
totalDt, stepDt, recipStepDt,
isVelIter, true,
secondPassArticulationConstraintProcessingConfig,
elapsedTime,
articulationBiasCoefficient,
isExternalForcesEveryTgsIterationEnabled);
if(isLastPosIter)
articulation->concludeInternalConstraints(true);
}
}
else
{
const ArticulationConstraintProcessingConfigCPU singlePassArticulationConstraintProcessingConfig = ArticulationConstraintProcessingConfigCPU::getSinglePassConfig(solveArticulationContactLast);
if(isLastPosIter)
solveConcludeConstraintsIteration<false>(contactDescPtr, batchHeaders, nbHeaders,
solverTxInertia, elapsedTime, cache, 0);
else
solveConstraintsIteration(contactDescPtr, batchHeaders, nbHeaders, recipStepDt,
solverTxInertia, elapsedTime, minPenetration, cache);
for (PxU32 i = 0; i < nbArticulations; ++i)
{
FeatherstoneArticulation* articulation = articulations[i];
articulation->solveInternalConstraints(
totalDt, stepDt, recipStepDt,
isVelIter, true,
singlePassArticulationConstraintProcessingConfig,
elapsedTime,
articulationBiasCoefficient,
isExternalForcesEveryTgsIterationEnabled);
if(isLastPosIter)
articulation->concludeInternalConstraints(true);
}
}
}
void DynamicsTGSContext::iterativeSolveIsland(const SolverIslandObjectsStep& objects, const PxsIslandIndices& counts, ThreadContext& threadContext,
PxReal stepDt, PxReal invStepDt, PxReal totalDt, PxU32 posIters, PxU32 velIters, PxReal articulationBiasCoefficient, SolverContext& cache)
{
PX_UNUSED(invStepDt);
PX_PROFILE_ZONE("Dynamics:solveIsland", mContextID);
PxReal elapsedTime = 0.0f;
const PxReal recipStepDt = 1.0f/stepDt;
const PxU32 bodyOffset = objects.solverBodyOffset;
const bool externalForcesEveryTgsIterationEnabled = mIsExternalForcesEveryTgsIterationEnabled;
const bool solveArticulationContactLast = mSolveArticulationContactLast;
if (threadContext.numContactConstraintBatches == 0)
{
const ArticulationConstraintProcessingConfigCPU singlePassArticulationConstraintProcessingConfig = ArticulationConstraintProcessingConfigCPU::getSinglePassConfig(solveArticulationContactLast);
for (PxU32 i = 0; i < counts.articulations; ++i)
{
elapsedTime = 0.0f;
FeatherstoneArticulation* articulation = threadContext.mArticulationArray[i];
for (PxU32 a = 0; a < posIters; a++)
{
if (externalForcesEveryTgsIterationEnabled)
{
PX_ASSERT(threadContext.mZVector.size() >= articulation->getBodyCount());
FeatherstoneArticulation::applyTgsSubstepForces(articulation, stepDt, threadContext.mZVector.begin());
}
articulation->solveInternalConstraints(
totalDt, stepDt, recipStepDt,
false, true,
singlePassArticulationConstraintProcessingConfig,
elapsedTime,
articulationBiasCoefficient,
mIsExternalForcesEveryTgsIterationEnabled);
FeatherstoneArticulation::recordDeltaMotionTGS(articulation, stepDt, threadContext.mDeltaV.begin());
elapsedTime += stepDt;
}
FeatherstoneArticulation::saveVelocityTGS(articulation, mInvDt);
articulation->concludeInternalConstraints(true);
for (PxU32 a = 0; a < velIters; ++a)
{
articulation->solveInternalConstraints(
totalDt, stepDt, recipStepDt,
true, true,
singlePassArticulationConstraintProcessingConfig,
elapsedTime, articulationBiasCoefficient,
mIsExternalForcesEveryTgsIterationEnabled);
}
articulation->writebackInternalConstraints(true);
}
if (externalForcesEveryTgsIterationEnabled)
integrateBodiesAndApplyGravity(objects, counts.bodies, mSolverBodyVelPool.begin() + bodyOffset, mSolverBodyTxInertiaPool.begin() + bodyOffset, stepDt, posIters);
else
integrateBodies(counts.bodies, mSolverBodyVelPool.begin() + bodyOffset, mSolverBodyTxInertiaPool.begin() + bodyOffset, mDt);
return;
}
for (PxU32 a = 1; a < posIters; a++)
{
if(externalForcesEveryTgsIterationEnabled)
{
applySubstepGravity(objects.bodies, *objects.externalAccelerations, counts.bodies, mSolverBodyVelPool.begin() + bodyOffset, stepDt, mSolverBodyTxInertiaPool.begin() + bodyOffset, objects.nodeIndexArray);
applyArticulationTgsSubstepForces(threadContext, counts.articulations, stepDt);
}
processIterationIsland<!tIsVelIter, !tIsLastPosIter>(
totalDt, stepDt, recipStepDt, elapsedTime,
mIsExternalForcesEveryTgsIterationEnabled,
//solve artics:
solveArticulationContactLast,
threadContext.mArticulationArray, counts.articulations, articulationBiasCoefficient,
//solve rbodies:
objects.orderedConstraintDescs, objects.constraintBatchHeaders,
threadContext.numContactConstraintBatches, mSolverBodyTxInertiaPool.begin(),
-PX_MAX_F32, cache);
integrateBodies(counts.bodies, mSolverBodyVelPool.begin() + bodyOffset, mSolverBodyTxInertiaPool.begin() + bodyOffset, stepDt);
stepArticulations(threadContext, counts, stepDt);
elapsedTime += stepDt;
}
//last pos iter
{
if(externalForcesEveryTgsIterationEnabled)
{
applySubstepGravity(objects.bodies, *objects.externalAccelerations, counts.bodies, mSolverBodyVelPool.begin() + bodyOffset, stepDt, mSolverBodyTxInertiaPool.begin() + bodyOffset, objects.nodeIndexArray);
applyArticulationTgsSubstepForces(threadContext, counts.articulations, stepDt);
}
processIterationIsland<!tIsVelIter, tIsLastPosIter>(
totalDt, stepDt, recipStepDt, elapsedTime,
mIsExternalForcesEveryTgsIterationEnabled,
//solve artics:
solveArticulationContactLast,
threadContext.mArticulationArray, counts.articulations, articulationBiasCoefficient,
//solve rbodies:
objects.orderedConstraintDescs, objects.constraintBatchHeaders,
threadContext.numContactConstraintBatches, mSolverBodyTxInertiaPool.begin(),
-PX_MAX_F32, cache);
elapsedTime += stepDt;
integrateBodies(counts.bodies, mSolverBodyVelPool.begin() + bodyOffset, mSolverBodyTxInertiaPool.begin() + bodyOffset,
stepDt);
stepArticulations(threadContext, counts, stepDt);
}
const PxReal invDt = mInvDt;
for(PxU32 a = 0; a < counts.articulations; ++a)
{
//ArticulationPImpl::updateDeltaMotion(desc, stepDt, threadContext.mDeltaV.begin());
FeatherstoneArticulation::saveVelocityTGS(threadContext.mArticulationArray[a], invDt);
}
for (PxU32 a = 0; a < velIters; ++a)
{
processIterationIsland<tIsVelIter, !tIsLastPosIter>(
totalDt, stepDt, recipStepDt, elapsedTime,
mIsExternalForcesEveryTgsIterationEnabled,
//solve artics:
solveArticulationContactLast,
threadContext.mArticulationArray, counts.articulations, articulationBiasCoefficient,
//solve rbodies:
objects.orderedConstraintDescs, objects.constraintBatchHeaders,
threadContext.numContactConstraintBatches, mSolverBodyTxInertiaPool.begin(),
0.0f, cache);
}
writebackConstraintsIteration(objects.constraintBatchHeaders, objects.orderedConstraintDescs, threadContext.numContactConstraintBatches, cache);
for (PxU32 i = 0; i < counts.articulations; ++i)
threadContext.mArticulationArray[i]->writebackInternalConstraints(true);
}
void DynamicsTGSContext::applySubstepGravityParallel(const SolverIslandObjectsStep& objects, PxTGSSolverBodyVel* solverVels, const PxU32 bodyOffset, PxReal stepDt,
const PxU32 nbBodies, PxU32& startGravityIdx, PxU32& nbGravityRemaining, PxU32& targetGravityProgressCount,
PxI32* gravityProgressCount, PxI32* gravityCounts, PxU32 unrollSize)
{
PxU32 gravityStartIdx = startGravityIdx - targetGravityProgressCount;
PxU32 nbGravity = 0;
while (gravityStartIdx < nbBodies)
{
const PxU32 nbToGravity = PxMin(nbBodies - gravityStartIdx, nbGravityRemaining);
applySubstepGravity(objects.bodies + gravityStartIdx, *objects.externalAccelerations, nbToGravity,
solverVels + bodyOffset + gravityStartIdx, stepDt, mSolverBodyTxInertiaPool.begin() + bodyOffset + gravityStartIdx, &objects.nodeIndexArray[gravityStartIdx]);
nbGravityRemaining -= nbToGravity;
startGravityIdx += nbToGravity;
gravityStartIdx += nbToGravity;
nbGravity += nbToGravity;
if (nbGravityRemaining == 0)
{
startGravityIdx = PxU32(PxAtomicAdd(gravityCounts, PxI32(unrollSize))) - unrollSize;
nbGravityRemaining = unrollSize;
gravityStartIdx = startGravityIdx - targetGravityProgressCount;
}
}
if (nbGravity)
PxAtomicAdd(gravityProgressCount, PxI32(nbGravity));
targetGravityProgressCount += nbBodies;
}
void DynamicsTGSContext::applyArticulationSubstepGravityParallel(
PxU32& startArticulationIdx, PxU32& targetArticulationProgressCount, PxI32* articulationProgressCount,
PxI32* articulationIntegrationCounts, FeatherstoneArticulation** articulationDescs, PxU32 nbArticulations,
PxReal stepDt, ThreadContext& threadContext)
{
PxU32 artIcStartIdx = startArticulationIdx - targetArticulationProgressCount;
PxU32 nbArticsProcessed = 0;
while(artIcStartIdx < nbArticulations)
{
FeatherstoneArticulation::applyTgsSubstepForces(articulationDescs[artIcStartIdx], stepDt, threadContext.mZVector.begin());
nbArticsProcessed++;
startArticulationIdx = PxU32(PxAtomicAdd(articulationIntegrationCounts, PxI32(1))) - 1;
artIcStartIdx = startArticulationIdx - targetArticulationProgressCount;
}
if(nbArticsProcessed)
PxAtomicAdd(articulationProgressCount, PxI32(nbArticsProcessed));
targetArticulationProgressCount += nbArticulations;
}
static void solveParallelPartition
(const PxU32 b, const PxU32 solverUnrollSize, const PxReal elapsedTime, const bool overflow, const PxU32 iterCount, const PxReal minPenetration,
const bool isLastPositionIteration,
const PxU32* constraintsPerPartitions,
PxSolverConstraintDesc* contactDescs, PxConstraintBatchHeader* batchHeaders, PxTGSSolverBodyTxInertia* solverTxInertias,
SolverContext& cache,
PxU32& targetSolverProgressCount, PxU32& startSolveIdx, PxU32& nbSolveRemaining, PxU32& offset,
PxI32* solverProgressCount, PxI32* solverCounts)
{
//Find the startIdx in the partition to process
PxU32 startIdx = startSolveIdx - targetSolverProgressCount;
const PxU32 nbBatches = constraintsPerPartitions[b];
PxU32 nbSolved = 0;
while (startIdx < nbBatches)
{
const PxU32 nbToSolve = PxMin(nbBatches - startIdx, nbSolveRemaining);
if(!isLastPositionIteration)
{
if (b == 0 && overflow)
parallelSolveConstraints<true>(contactDescs, batchHeaders + startIdx + offset, nbToSolve,
solverTxInertias, elapsedTime, minPenetration, cache, iterCount);
else
parallelSolveConstraints<false>(contactDescs, batchHeaders + startIdx + offset, nbToSolve,
solverTxInertias, elapsedTime, minPenetration, cache, iterCount);
}
else
{
if (b == 0 && overflow)
solveConcludeConstraintsIteration<true>(contactDescs, batchHeaders + startIdx + offset, nbToSolve,
solverTxInertias, elapsedTime, cache, iterCount);
else
solveConcludeConstraintsIteration<false>(contactDescs, batchHeaders + startIdx + offset, nbToSolve,
solverTxInertias, elapsedTime, cache, iterCount);
}
nbSolveRemaining -= nbToSolve;
startSolveIdx += nbToSolve;
startIdx += nbToSolve;
nbSolved += nbToSolve;
if (nbSolveRemaining == 0)
{
startSolveIdx = PxU32(PxAtomicAdd(solverCounts, PxI32(solverUnrollSize))) - solverUnrollSize;
nbSolveRemaining = solverUnrollSize;
startIdx = startSolveIdx - targetSolverProgressCount;
}
}
if (nbSolved)
PxAtomicAdd(solverProgressCount, PxI32(nbSolved));
targetSolverProgressCount += nbBatches;
offset += nbBatches;
}
template<bool isLastPositionIteration>
static void solveParallelPartitionsAndWaitForCompletion
(const PxU32 nbPartitions, const PxU32 solverUnrollSize, const PxReal elapsedTime, const bool overflow, const PxU32 iterCount, const PxReal minPenetration,
const PxU32* constraintsPerPartitions,
PxSolverConstraintDesc* contactDescs, PxConstraintBatchHeader* batchHeaders, PxTGSSolverBodyTxInertia* solverTxInertias,
SolverContext& cache,
PxU32& targetSolverProgressCount, PxU32& startSolveIdx, PxU32& nbSolveRemaining,
PxI32* solverProgressCount, PxI32* solverCounts)
{
PxU32 offset = 0;
for (PxU32 b = 0; b < nbPartitions; ++b)
{
solveParallelPartition
(b, solverUnrollSize, elapsedTime, overflow, iterCount, minPenetration, isLastPositionIteration,
constraintsPerPartitions,
contactDescs, batchHeaders, solverTxInertias,
cache,
targetSolverProgressCount, startSolveIdx, nbSolveRemaining, offset,
solverProgressCount, solverCounts);
WAIT_FOR_PROGRESS(solverProgressCount, PxI32(targetSolverProgressCount));
}
}
static void parallelIntegrateBodiesAndNoWait
(const PxU32 integrationUnrollSize, const PxU32 iterCount, const PxU32 nbBodies, const PxU32 bodyOffset,
const PxReal stepDt,
PxTGSSolverBodyTxInertia* solverTxInertias, PxTGSSolverBodyVel* solverVels,
PxU32& startIntegrateIdx, PxU32& targetIntegrationProgressCount, PxU32& nbIntegrateRemaining,
PxI32* integrationCounts, PxI32* integrationProgressCount)
{
PxU32 integStartIdx = startIntegrateIdx - targetIntegrationProgressCount;
PxU32 nbIntegrated = 0;
while (integStartIdx < nbBodies)
{
const PxU32 nbToIntegrate = PxMin(nbBodies - integStartIdx, nbIntegrateRemaining);
parallelIntegrateBodies(solverVels + integStartIdx + bodyOffset, solverTxInertias + integStartIdx + bodyOffset,
nbToIntegrate, stepDt, iterCount);
nbIntegrateRemaining -= nbToIntegrate;
startIntegrateIdx += nbToIntegrate;
integStartIdx += nbToIntegrate;
nbIntegrated += nbToIntegrate;
if (nbIntegrateRemaining == 0)
{
startIntegrateIdx = PxU32(PxAtomicAdd(integrationCounts, PxI32(integrationUnrollSize))) - integrationUnrollSize;
nbIntegrateRemaining = integrationUnrollSize;
integStartIdx = startIntegrateIdx - targetIntegrationProgressCount;
}
}
if (nbIntegrated)
PxAtomicAdd(integrationProgressCount, PxI32(nbIntegrated));
targetIntegrationProgressCount += nbBodies;
}
struct SolverDt
{
PxReal totalDt;
PxReal invTotalDt;
PxReal stepDt;
PxReal invStepDt;
};
template<bool isVelocityIteration, bool updateDeltaMotion, bool saveVelocity>
static void parallelSolveInternalConstraintsAndWaitForCompletion
(SolverContext& cache, ThreadContext& threadContext,
const PxU32 nbArticulations,
const bool isExternalForcesEveryTgsIterationEnabled,
const SolverDt& solverDt, const PxReal elapsedTime,
const ArticulationConstraintProcessingConfigCPU& articulationConstraintProcessingConfig,
const PxReal biasCoefficient,
PxU32& startArticulationIdx, PxU32& targetArticulationProgressCount,
PxI32* articulationProgressCount, PxI32* articulationIntegrationCounts)
{
constexpr bool isTGS = true;
PxU32 artIcStartIdx = startArticulationIdx - targetArticulationProgressCount;
PxU32 nbArticsProcessed = 0;
while (artIcStartIdx < nbArticulations)
{
FeatherstoneArticulation* articulation = threadContext.mArticulationArray[artIcStartIdx];
articulation->solveInternalConstraints(
solverDt.totalDt, solverDt.stepDt, solverDt.invStepDt,
isVelocityIteration, isTGS,
articulationConstraintProcessingConfig,
elapsedTime, biasCoefficient,
isExternalForcesEveryTgsIterationEnabled);
if(updateDeltaMotion)
FeatherstoneArticulation::recordDeltaMotionTGS(articulation, solverDt.stepDt, cache.deltaV);
if(saveVelocity)
FeatherstoneArticulation::saveVelocityTGS(articulation, solverDt.invTotalDt);
nbArticsProcessed++;
startArticulationIdx = PxU32(PxAtomicAdd(articulationIntegrationCounts, PxI32(1))) - 1;
artIcStartIdx = startArticulationIdx - targetArticulationProgressCount;
}
if (nbArticsProcessed)
PxAtomicAdd(articulationProgressCount, PxI32(nbArticsProcessed));
targetArticulationProgressCount += nbArticulations;
WAIT_FOR_PROGRESS(articulationProgressCount, PxI32(targetArticulationProgressCount));
}
template<bool updateDeltaMotion, bool saveVelocity, bool isVelocityIteration, bool isLastPositionIteration>
static void processSolverIterationParallel
(const bool solveArticulationContactLast, const bool isExternalForcesEveryTgsIterationEnabled,
PxU32& iterCount, const SolverDt& solverDt, const PxReal elapsedTime,
SolverContext& cache, ThreadContext& threadContext,
//artic solve:
const PxU32 nbArticulations,
const PxReal articulationBiasCoefficient,
PxU32& startArticulationIdx, PxU32& targetArticulationProgressCount,
PxI32* articulationProgressCount, PxI32* articulationIntegrationCounts,
//rbody solve:
const PxU32 nbPartitions, const PxU32 solverUnrollSize, const bool overflow, const PxReal minPenetration,
const PxU32* constraintsPerPartitions,
PxSolverConstraintDesc* contactDescs, PxConstraintBatchHeader* batchHeaders, PxTGSSolverBodyTxInertia* solverTxInertias,
PxU32& targetSolverProgressCount, PxU32& startSolveIdx, PxU32& nbSolveRemaining,
PxI32* solverProgressCount, PxI32* solverCounts,
//rbody integration:
const PxU32 integrationUnrollSize, const PxU32 nbBodies, const PxU32 bodyOffset,
PxTGSSolverBodyVel* solverVels,
PxU32& startIntegrateIdx, PxU32& targetIntegrationProgressCount, PxU32& nbIntegrateRemaining,
PxI32* integrationCounts, PxI32* integrationProgressCount)
{
if(solveArticulationContactLast)
{
const ArticulationConstraintProcessingConfigCPU firstPassArticulationConstraintProcessingConfig = ArticulationConstraintProcessingConfigCPU::getFirstPassConfig();
const ArticulationConstraintProcessingConfigCPU secondPassArticulationConstraintProcessingConfig = ArticulationConstraintProcessingConfigCPU::getSecondPassConfig();
parallelSolveInternalConstraintsAndWaitForCompletion<isVelocityIteration, false, false>
(cache, threadContext,
nbArticulations,
isExternalForcesEveryTgsIterationEnabled,
solverDt, elapsedTime,
firstPassArticulationConstraintProcessingConfig,
articulationBiasCoefficient,
startArticulationIdx, targetArticulationProgressCount,
articulationProgressCount, articulationIntegrationCounts);
solveParallelPartitionsAndWaitForCompletion<isLastPositionIteration>
(nbPartitions, solverUnrollSize, elapsedTime, overflow, iterCount, minPenetration,
constraintsPerPartitions,
contactDescs, batchHeaders, solverTxInertias,
cache,
targetSolverProgressCount, startSolveIdx, nbSolveRemaining,
solverProgressCount, solverCounts);
iterCount++;
if(updateDeltaMotion)
{
parallelIntegrateBodiesAndNoWait
(integrationUnrollSize, iterCount, nbBodies, bodyOffset,
solverDt.stepDt,
solverTxInertias, solverVels,
startIntegrateIdx, targetIntegrationProgressCount, nbIntegrateRemaining,
integrationCounts, integrationProgressCount);
}
// Note: Articulation internal constraints do not need to wait for rigid bodies integration because they don't need the
// rigid velocities anymore.
parallelSolveInternalConstraintsAndWaitForCompletion<isVelocityIteration, updateDeltaMotion, saveVelocity>
(cache, threadContext,
nbArticulations,
isExternalForcesEveryTgsIterationEnabled,
solverDt, elapsedTime,
secondPassArticulationConstraintProcessingConfig,
articulationBiasCoefficient,
startArticulationIdx, targetArticulationProgressCount,
articulationProgressCount, articulationIntegrationCounts);
}
else
{
const ArticulationConstraintProcessingConfigCPU singlePassArticulationConstraintProcessingConfig = ArticulationConstraintProcessingConfigCPU::getSinglePassConfig(solveArticulationContactLast);
solveParallelPartitionsAndWaitForCompletion<isLastPositionIteration>
(nbPartitions, solverUnrollSize, elapsedTime, overflow, iterCount, minPenetration,
constraintsPerPartitions,
contactDescs, batchHeaders, solverTxInertias,
cache,
targetSolverProgressCount, startSolveIdx, nbSolveRemaining,
solverProgressCount, solverCounts);
iterCount++;
if(updateDeltaMotion)
{
parallelIntegrateBodiesAndNoWait
(integrationUnrollSize, iterCount, nbBodies, bodyOffset,
solverDt.stepDt,
solverTxInertias, solverVels,
startIntegrateIdx, targetIntegrationProgressCount, nbIntegrateRemaining,
integrationCounts, integrationProgressCount);
}
// Note: Articulation internal constraints do not need to wait for rigid bodies integration because they don't need the
// rigid velocities anymore.
parallelSolveInternalConstraintsAndWaitForCompletion<isVelocityIteration, updateDeltaMotion, saveVelocity>
(cache, threadContext,
nbArticulations,
isExternalForcesEveryTgsIterationEnabled,
solverDt, elapsedTime,
singlePassArticulationConstraintProcessingConfig,
articulationBiasCoefficient,
startArticulationIdx, targetArticulationProgressCount,
articulationProgressCount, articulationIntegrationCounts);
}
}
void DynamicsTGSContext::iterativeSolveIslandParallel(const SolverIslandObjectsStep& objects, const PxsIslandIndices& counts, ThreadContext& threadContext,
PxReal stepDt, PxReal totalDt, PxU32 posIters, PxU32 velIters, PxReal articulationBiasCoefficient,
PxI32* solverCounts, PxI32* integrationCounts, PxI32* articulationIntegrationCounts, PxI32* gravityCounts,
PxI32* solverProgressCount, PxI32* integrationProgressCount, PxI32* articulationProgressCount, PxI32* gravityProgressCount, PxU32 solverUnrollSize, PxU32 integrationUnrollSize)
{
PX_PROFILE_ZONE("Dynamics:solveIslandParallel", mContextID);
Dy::ThreadContext& threadContextLocal = *getThreadContext();
PxU32 startSolveIdx = PxU32(PxAtomicAdd(solverCounts, PxI32(solverUnrollSize))) - solverUnrollSize;
PxU32 nbSolveRemaining = solverUnrollSize;
PxU32 startIntegrateIdx = PxU32(PxAtomicAdd(integrationCounts, PxI32(integrationUnrollSize))) - integrationUnrollSize;
PxU32 nbIntegrateRemaining = integrationUnrollSize;
PxU32 startGravityIdx = PxU32(PxAtomicAdd(gravityCounts, PxI32(integrationUnrollSize))) - integrationUnrollSize;
PxU32 nbGravityRemaining = integrationUnrollSize;
//For now, just do articulations 1 at a time. Might need to tweak this later depending on performance
PxU32 startArticulationIdx = PxU32(PxAtomicAdd(articulationIntegrationCounts, PxI32(1))) - 1;
PxU32 targetSolverProgressCount = 0, targetIntegrationProgressCount = 0, targetArticulationProgressCount = 0, targetGravityProgressCount = 0;
const PxU32 nbSolverBatches = threadContext.numContactConstraintBatches;
const PxU32 nbBodies = counts.bodies;// + mKinematicCount;
const PxU32 nbArticulations = counts.articulations;
PxSolverConstraintDesc* contactDescs = objects.orderedConstraintDescs;
PxConstraintBatchHeader* batchHeaders = objects.constraintBatchHeaders;
PxTGSSolverBodyVel* solverVels = mSolverBodyVelPool.begin();
PxTGSSolverBodyTxInertia* solverTxInertias = mSolverBodyTxInertiaPool.begin();
PxU32* constraintsPerPartitions = threadContext.mConstraintsPerPartition.begin();
const PxU32 nbPartitions = threadContext.mConstraintsPerPartition.size();
const PxU32 bodyOffset = objects.solverBodyOffset;
threadContextLocal.mDeltaV.reserve(threadContext.mDeltaV.size());
threadContextLocal.mZVector.resizeUninitialized(threadContext.mZVector.size());
SolverContext cache;
cache.deltaV = threadContextLocal.mDeltaV.begin();
PxReal elapsedTime = 0.0f;
PxReal invStepDt = 1.0f/ stepDt;
const bool overflow = threadContext.mHasOverflowPartitions;
const SolverDt solverDt = {totalDt, mInvDt, stepDt, invStepDt};
PxU32 iterCount = 0;
const bool externalForcesEveryTgsIterationEnabled = mIsExternalForcesEveryTgsIterationEnabled;
const bool solveArticulationContactLast = mSolveArticulationContactLast;
//Run (posIters-1) position iterations.
//We'll run the final pos iter later because we need custom code to conclude constraints when we do that.
for (PxU32 a = 1; a < posIters; ++a)
{
if(externalForcesEveryTgsIterationEnabled)
{
applySubstepGravityParallel(objects, solverVels, bodyOffset, stepDt, nbBodies, startGravityIdx,
nbGravityRemaining, targetGravityProgressCount, gravityProgressCount,
gravityCounts, integrationUnrollSize);
applyArticulationSubstepGravityParallel(startArticulationIdx, targetArticulationProgressCount,
articulationProgressCount, articulationIntegrationCounts,
threadContext.mArticulationArray, nbArticulations, stepDt,
threadContextLocal);
// Make all threads wait until gravity is applied to all articulations and all bodies.
// The target progress counters have been incremented inside the substep gravity functions.
WAIT_FOR_PROGRESS(articulationProgressCount, PxI32(targetArticulationProgressCount));
WAIT_FOR_PROGRESS(gravityProgressCount, PxI32(targetGravityProgressCount));
}
processSolverIterationParallel<tUpdateDeltaMotion, !tSaveVelocity, !tIsVelIter, !tIsLastPosIter>
(solveArticulationContactLast, mIsExternalForcesEveryTgsIterationEnabled,
iterCount, solverDt, elapsedTime,
cache, threadContext,
//artic solve:
nbArticulations,
articulationBiasCoefficient,
startArticulationIdx, targetArticulationProgressCount,
articulationProgressCount, articulationIntegrationCounts,
//rbody solve:
nbPartitions, solverUnrollSize, overflow, -PX_MAX_F32, //minPen=-PX_MAX_F32
constraintsPerPartitions,
contactDescs, batchHeaders, solverTxInertias,
targetSolverProgressCount, startSolveIdx, nbSolveRemaining,
solverProgressCount, solverCounts,
//rbody integration:
integrationUnrollSize, nbBodies, bodyOffset,
solverVels,
startIntegrateIdx, targetIntegrationProgressCount, nbIntegrateRemaining,
integrationCounts, integrationProgressCount);
//Now is the time to wait for the body integration to complete.
WAIT_FOR_PROGRESS(integrationProgressCount, PxI32(targetIntegrationProgressCount));
elapsedTime += stepDt;
}
// Perform the last position iteration with conclude constraint.
{
if(externalForcesEveryTgsIterationEnabled)
{
applySubstepGravityParallel(objects, solverVels, bodyOffset, stepDt, nbBodies, startGravityIdx,
nbGravityRemaining, targetGravityProgressCount, gravityProgressCount,
gravityCounts, integrationUnrollSize);
applyArticulationSubstepGravityParallel(startArticulationIdx, targetArticulationProgressCount,
articulationProgressCount, articulationIntegrationCounts,
threadContext.mArticulationArray, nbArticulations, stepDt,
threadContextLocal);
// Make all threads wait until gravity is applied to all articulations and all bodies.
// The target progress counters have been incremented inside the substep gravity functions.
WAIT_FOR_PROGRESS(articulationProgressCount, PxI32(targetArticulationProgressCount));
WAIT_FOR_PROGRESS(gravityProgressCount, PxI32(targetGravityProgressCount));
}
processSolverIterationParallel<tUpdateDeltaMotion, tSaveVelocity, !tIsVelIter, tIsLastPosIter>
(solveArticulationContactLast, mIsExternalForcesEveryTgsIterationEnabled,
iterCount, solverDt, elapsedTime,
cache, threadContext,
//artic solve:
nbArticulations,
articulationBiasCoefficient,
startArticulationIdx, targetArticulationProgressCount,
articulationProgressCount, articulationIntegrationCounts,
//rbody solve:
nbPartitions, solverUnrollSize, overflow, -PX_MAX_F32, //minPen=-PX_MAX_F32
constraintsPerPartitions,
contactDescs, batchHeaders, solverTxInertias,
targetSolverProgressCount, startSolveIdx, nbSolveRemaining,
solverProgressCount, solverCounts,
//rbody integration:
integrationUnrollSize, nbBodies, bodyOffset,
solverVels,
startIntegrateIdx, targetIntegrationProgressCount, nbIntegrateRemaining,
integrationCounts, integrationProgressCount);
putThreadContext(&threadContextLocal);
//Wait for parallelIntegrateBodiesAndNoWait to complete.
WAIT_FOR_PROGRESS(integrationProgressCount, PxI32(targetIntegrationProgressCount));
elapsedTime += stepDt;
}
// Velocity Iterations. It's legal to skip them (velIters == 0).
const PxU32 nbVelItersMinusOne = (velIters > 1) ? velIters-1 : 0;
for (PxU32 a = 0; a < nbVelItersMinusOne; ++a)
{
processSolverIterationParallel<!tUpdateDeltaMotion, !tSaveVelocity, tIsVelIter, !tIsLastPosIter>
(solveArticulationContactLast, mIsExternalForcesEveryTgsIterationEnabled,
iterCount, solverDt, elapsedTime,
cache, threadContext,
//artic solve:
nbArticulations,
articulationBiasCoefficient,
startArticulationIdx, targetArticulationProgressCount,
articulationProgressCount, articulationIntegrationCounts,
//rbody solve:
nbPartitions, solverUnrollSize, overflow, 0.0f, //minPen=0
constraintsPerPartitions,
contactDescs, batchHeaders, solverTxInertias,
targetSolverProgressCount, startSolveIdx, nbSolveRemaining,
solverProgressCount, solverCounts,
//rbody integration:
integrationUnrollSize, nbBodies, bodyOffset,
solverVels,
startIntegrateIdx, targetIntegrationProgressCount, nbIntegrateRemaining,
integrationCounts, integrationProgressCount);
}
//Last vel iter
for (PxU32 a = nbVelItersMinusOne; a < velIters; ++a)
{
processSolverIterationParallel<!tUpdateDeltaMotion, !tSaveVelocity, tIsVelIter, !tIsLastPosIter>
(solveArticulationContactLast, mIsExternalForcesEveryTgsIterationEnabled,
iterCount, solverDt, elapsedTime,
cache, threadContext,
//artic solve:
nbArticulations,
articulationBiasCoefficient,
startArticulationIdx, targetArticulationProgressCount,
articulationProgressCount, articulationIntegrationCounts,
//rbody solve:
nbPartitions, solverUnrollSize, overflow, 0.0f, //minPen=0
constraintsPerPartitions,
contactDescs, batchHeaders, solverTxInertias,
targetSolverProgressCount, startSolveIdx, nbSolveRemaining,
solverProgressCount, solverCounts,
//rbody integration:
integrationUnrollSize, nbBodies, bodyOffset,
solverVels,
startIntegrateIdx, targetIntegrationProgressCount, nbIntegrateRemaining,
integrationCounts, integrationProgressCount);
}
//Write back constraints...
{
// Rigids
{
//Find the startIdx in the partition to process
PxU32 startIdx = startSolveIdx - targetSolverProgressCount;
const PxU32 nbBatches = nbSolverBatches;
while (startIdx < nbBatches)
{
PxU32 nbToSolve = PxMin(nbBatches - startIdx, nbSolveRemaining);
parallelWritebackConstraintsIteration(contactDescs, batchHeaders + startIdx, nbToSolve, cache);
nbSolveRemaining -= nbToSolve;
startSolveIdx += nbToSolve;
startIdx += nbToSolve;
if (nbSolveRemaining == 0)
{
startSolveIdx = PxU32(PxAtomicAdd(solverCounts, PxI32(solverUnrollSize))) - solverUnrollSize;
nbSolveRemaining = solverUnrollSize;
startIdx = startSolveIdx - targetSolverProgressCount;
}
}
// No need here to atomically add to solverProgressCount because this is the last thing we do during solve
}
// Articulations
{
PxU32 artIcStartIdx = startArticulationIdx - targetArticulationProgressCount;
while(artIcStartIdx < nbArticulations)
{
threadContext.mArticulationArray[artIcStartIdx]->writebackInternalConstraints(true);
startArticulationIdx = PxU32(PxAtomicAdd(articulationIntegrationCounts, PxI32(1))) - 1;
artIcStartIdx = startArticulationIdx - targetArticulationProgressCount;
}
// No need here to atomically add to articulationProgressCount because this is the last thing we do during solve
}
// There is no need to WAIT_FOR_PROGRESS here anymore. Once a thread has reached this point, it has processed
// all work it was assigned and we can let it return to pick up another task. One thread returning here does not
// mean, however, that all work is done because other threads may still work on their rigids or articulations
// they've been assigned via the startSolveIdx and startArticulationIdx atomic calls.
}
}
class CopyBackTask : public Cm::Task
{
const SolverIslandObjectsStep& mObjects;
PxTGSSolverBodyVel* mVels;
PxTGSSolverBodyTxInertia* mTxInertias;
PxTGSSolverBodyData* mSolverBodyDatas;
const PxReal mInvDt;
IG::IslandSim& mIslandSim;
const PxU32 mStartIdx;
const PxU32 mEndIdx;
DynamicsTGSContext& mContext;
PX_NOCOPY(CopyBackTask)
public:
CopyBackTask(const SolverIslandObjectsStep& objects,
PxTGSSolverBodyVel* vels, PxTGSSolverBodyTxInertia* txInertias,
PxTGSSolverBodyData* solverBodyDatas, PxReal invDt, IG::IslandSim& islandSim,
PxU32 startIdx, PxU32 endIdx, DynamicsTGSContext& context) : Cm::Task(context.getContextId()),
mObjects(objects), mVels(vels), mTxInertias(txInertias), mSolverBodyDatas(solverBodyDatas), mInvDt(invDt),
mIslandSim(islandSim), mStartIdx(startIdx), mEndIdx(endIdx), mContext(context)
{
}
virtual const char* getName() const PX_OVERRIDE { return "CopyBackTask"; }
virtual void runInternal() PX_OVERRIDE
{
mContext.copyBackBodies(mObjects, mVels, mTxInertias, mSolverBodyDatas, mInvDt, mIslandSim, mStartIdx, mEndIdx);
}
};
class UpdateArticTask : public Cm::Task
{
Dy::ThreadContext& mThreadContext;
const PxU32 mStartIdx;
const PxU32 mEndIdx;
const PxReal mDt;
DynamicsTGSContext& mContext;
PX_NOCOPY(UpdateArticTask)
public:
UpdateArticTask(Dy::ThreadContext& threadContext, PxU32 startIdx, PxU32 endIdx, PxReal dt, DynamicsTGSContext& context) :
Cm::Task(context.getContextId()),
mThreadContext(threadContext), mStartIdx(startIdx), mEndIdx(endIdx), mDt(dt), mContext(context)
{
}
virtual const char* getName() const PX_OVERRIDE { return "UpdateArticTask"; }
virtual void runInternal() PX_OVERRIDE
{
mContext.updateArticulations(mThreadContext, mStartIdx, mEndIdx, mDt);
}
};
void DynamicsTGSContext::finishSolveIsland(ThreadContext& threadContext, const SolverIslandObjectsStep& objects,
const PxsIslandIndices& counts, IG::SimpleIslandManager& islandManager, PxBaseTask* continuation)
{
threadContext.mConstraintBlockManager.reset();
threadContext.mConstraintBlockStream.reset();
const PxU32 NbBodiesPerTask = 512;
for (PxU32 a = 0; a < counts.bodies; a += NbBodiesPerTask)
{
CopyBackTask* task = PX_PLACEMENT_NEW(mTaskPool.allocate(sizeof(CopyBackTask)), CopyBackTask)
(objects, mSolverBodyVelPool.begin() + objects.solverBodyOffset,
mSolverBodyTxInertiaPool.begin() + objects.solverBodyOffset, mSolverBodyDataPool2.begin() + objects.solverBodyOffset,
mInvDt, islandManager.getAccurateIslandSim(), a, PxMin(a + NbBodiesPerTask, counts.bodies),*this);
task->setContinuation(continuation);
task->removeReference();
}
const PxU32 NbArticsPerTask = 64;
for (PxU32 a = 0; a < counts.articulations; a += NbArticsPerTask)
{
UpdateArticTask* task = PX_PLACEMENT_NEW(mTaskPool.allocate(sizeof(UpdateArticTask)), UpdateArticTask)
(threadContext, a, PxMin(counts.articulations, a+NbArticsPerTask), mDt, *this);
task->setContinuation(continuation);
task->removeReference();
}
}
void DynamicsTGSContext::endIsland(ThreadContext& threadContext)
{
putThreadContext(&threadContext);
}
void DynamicsTGSContext::solveIsland(const SolverIslandObjectsStep& objects,
const PxsIslandIndices& counts,
PxU32 solverBodyOffset,
PxU32* bodyRemapTable, PxsMaterialManager* /*materialManager*/,
PxsContactManagerOutputIterator& iterator,
PxBaseTask* continuation)
{
ThreadContext& threadContext = *getThreadContext();
IslandContextStep& islandContext = *reinterpret_cast<IslandContextStep*>(mTaskPool.allocate(sizeof(IslandContextStep)));
islandContext.mThreadContext = &threadContext;
islandContext.mCounts = counts;
islandContext.mObjects = objects;
islandContext.mPosIters = 0;
islandContext.mVelIters = 0;
islandContext.mObjects.solverBodyOffset = solverBodyOffset;
prepareBodiesAndConstraints(islandContext.mObjects, islandContext);
////Create task chain...
SetupDescsTask* descTask = PX_PLACEMENT_NEW(mTaskPool.allocate(sizeof(SetupDescsTask)), SetupDescsTask)(islandContext, islandContext.mObjects,
bodyRemapTable, solverBodyOffset, iterator, *this);
PreIntegrateTask* intTask = PX_PLACEMENT_NEW(mTaskPool.allocate(sizeof(PreIntegrateTask)), PreIntegrateTask)(islandContext.mObjects.bodyCoreArray, islandContext.mObjects.bodies,
mSolverBodyVelPool.begin() + solverBodyOffset,
mSolverBodyTxInertiaPool.begin() + solverBodyOffset, mSolverBodyDataPool2.begin() + solverBodyOffset,
threadContext.mNodeIndexArray, islandContext.mCounts.bodies, mGravity, mDt, islandContext.mPosIters,
islandContext.mVelIters, *this);
SetupArticulationTask* articTask = PX_PLACEMENT_NEW(mTaskPool.allocate(sizeof(SetupArticulationTask)), SetupArticulationTask)(islandContext, mGravity,
mDt, islandContext.mPosIters, islandContext.mVelIters, *this);
SetStepperTask* stepperTask = PX_PLACEMENT_NEW(mTaskPool.allocate(sizeof(SetStepperTask)), SetStepperTask)(islandContext, *this);
SetupArticulationInternalConstraintsTask* articConTask = PX_PLACEMENT_NEW(mTaskPool.allocate(sizeof(SetupArticulationInternalConstraintsTask)), SetupArticulationInternalConstraintsTask)
(islandContext, mDt, mInvDt, *this);
PartitionTask* partitionTask = PX_PLACEMENT_NEW(mTaskPool.allocate(sizeof(PartitionTask)), PartitionTask)
(islandContext, islandContext.mObjects.constraintDescs, mSolverBodyVelPool.begin()+solverBodyOffset+1, *this);
SetupSolverConstraintsTask* constraintTask = PX_PLACEMENT_NEW(mTaskPool.allocate(sizeof(SetupSolverConstraintsTask)), SetupSolverConstraintsTask)
(islandContext, islandContext.mObjects.orderedConstraintDescs, iterator, mDt, *this);
SolveIslandTask* solveTask = PX_PLACEMENT_NEW(mTaskPool.allocate(sizeof(SolveIslandTask)), SolveIslandTask)(islandContext, islandContext.mObjects, islandContext.mCounts, mDt, *this);
FinishSolveIslandTask* finishTask = PX_PLACEMENT_NEW(mTaskPool.allocate(sizeof(FinishSolveIslandTask)), FinishSolveIslandTask)(threadContext, islandContext.mObjects, islandContext.mCounts, mIslandManager, *this);
EndIslandTask* endTask = PX_PLACEMENT_NEW(mTaskPool.allocate(sizeof(EndIslandTask)), EndIslandTask)(threadContext, *this);
// PT:
// "X -> Y" means X has Y as continuation
// "X => Y" means X spawns Y task(s)
//
// Chain is:
// articTask -> stepperTask -> articConTask -> partitionTask -> constraintTask -> solveTask -> finishTask -> endTask -> continuation
// intTask / \--------------------------------->
// descTask /
endTask->setContinuation(continuation);
finishTask->setContinuation(endTask);
solveTask->setContinuation(finishTask);
constraintTask->setContinuation(solveTask);
partitionTask->setContinuation(constraintTask);
articConTask->setContinuation(partitionTask);
//Stepper triggers both articCon and constraintTask
stepperTask->setContinuation(articConTask);
stepperTask->setAdditionalContinuation(constraintTask);
articTask->setContinuation(stepperTask);
intTask->setContinuation(stepperTask);
descTask->setContinuation(stepperTask);
endTask->removeReference();
finishTask->removeReference();
solveTask->removeReference();
constraintTask->removeReference();
partitionTask->removeReference();
articConTask->removeReference();
stepperTask->removeReference();
articTask->removeReference();
intTask->removeReference();
descTask->removeReference();
}
void DynamicsTGSContext::mergeResults()
{
}
}
}