Revision control
++ // 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 "ScBodySim.h"
++ #include "ScStaticSim.h"
++ #include "ScConstraintCore.h"
++ #include "ScConstraintSim.h"
++ #include "ScConstraintInteraction.h"
++ #include "ScElementSimInteraction.h"
++
++ using namespace physx;
++ using namespace Sc;
++ using namespace Cm;
++
++ static ConstraintInteraction* createInteraction(ConstraintSim* sim, RigidCore* r0, RigidCore* r1, Scene& scene)
++ {
++ return scene.getConstraintInteractionPool().construct( sim,
++ r0 ? *r0->getSim() : scene.getStaticAnchor(),
++ r1 ? *r1->getSim() : scene.getStaticAnchor());
++ }
++
++ static void releaseInteraction(ConstraintInteraction* interaction, const ConstraintSim* sim, Scene& scene)
++ {
++ if(!sim->isBroken())
++ interaction->destroy();
++
++ scene.getConstraintInteractionPool().destroy(interaction);
++ }
++
++ Sc::ConstraintSim::ConstraintSim(ConstraintCore& core, RigidCore* r0, RigidCore* r1, Scene& scene) :
++ mScene (scene),
++ mCore (core),
++ mInteraction(NULL),
++ mFlags (0)
++ {
++ mBodies[0] = (r0 && (r0->getActorCoreType() != PxActorType::eRIGID_STATIC)) ? static_cast<BodySim*>(r0->getSim()) : 0;
++ mBodies[1] = (r1 && (r1->getActorCoreType() != PxActorType::eRIGID_STATIC)) ? static_cast<BodySim*>(r1->getSim()) : 0;
++
++ const PxU32 id = scene.getConstraintIDTracker().createID();
++
++ mLowLevelConstraint.index = id;
++ PinnableArray<Dy::ConstraintWriteback>& writeBackPool = scene.getDynamicsContext()->getConstraintWriteBackPool();
++ if(id >= writeBackPool.capacity())
++ writeBackPool.reserve(writeBackPool.capacity() * 2);
++
++ writeBackPool.resize(PxMax(writeBackPool.size(), id + 1));
++ writeBackPool[id].initialize();
++
++ if(!createLLConstraint())
++ return;
++
++ PxReal linBreakForce, angBreakForce;
++ core.getBreakForce(linBreakForce, angBreakForce);
++ if ((linBreakForce < PX_MAX_F32) || (angBreakForce < PX_MAX_F32))
++ setFlag(eBREAKABLE);
++
++ core.setSim(this);
++
++ mInteraction = createInteraction(this, r0, r1, scene);
++
++ PX_ASSERT(!mInteraction->isRegistered()); // constraint interactions must not register in the scene, there is a list of Sc::ConstraintSim instead
++ }
++
++ Sc::ConstraintSim::~ConstraintSim()
++ {
++ PX_ASSERT(mInteraction); // This is fine now, a body which gets removed from the scene removes all constraints automatically
++ PX_ASSERT(!mInteraction->isRegistered()); // constraint interactions must not register in the scene, there is a list of Sc::ConstraintSim instead
++
++ releaseInteraction(mInteraction, this, mScene);
++
++ mScene.getConstraintIDTracker().releaseID(mLowLevelConstraint.index);
++ destroyLLConstraint();
++
++ mCore.setSim(NULL);
++ }
++
++ static PX_FORCE_INLINE void setLLBodies(Dy::Constraint& c, BodySim* b0, BodySim* b1)
++ {
++ PxsRigidBody* body0 = b0 ? &b0->getLowLevelBody() : NULL;
++ PxsRigidBody* body1 = b1 ? &b1->getLowLevelBody() : NULL;
++
++ c.body0 = body0;
++ c.body1 = body1;
++
++ c.bodyCore0 = body0 ? &body0->getCore() : NULL;
++ c.bodyCore1 = body1 ? &body1->getCore() : NULL;
++ }
++
++ bool Sc::ConstraintSim::createLLConstraint()
++ {
++ ConstraintCore& core = getCore();
++ const PxU32 constantBlockSize = core.getConstantBlockSize();
++
++ void* constantBlock = mScene.allocateConstraintBlock(constantBlockSize);
++ if(!constantBlock)
++ return PxGetFoundation().error(PxErrorCode::eINTERNAL_ERROR, PX_FL, "Constraint: could not allocate low-level resources.");
++
++ //Ensure the constant block isn't just random data because some functions may attempt to use it before it is
++ //setup. Specifically pvd visualization of joints
++ //-CN
++
++ PxMemZero(constantBlock, constantBlockSize);
++
++ Dy::Constraint& llc = mLowLevelConstraint;
++ core.getBreakForce(llc.linBreakForce, llc.angBreakForce);
++ llc.flags = core.getFlags();
++ llc.constantBlockSize = PxU16(constantBlockSize);
++
++ llc.solverPrep = core.getSolverPrep();
++ llc.constantBlock = constantBlock;
++ llc.minResponseThreshold = core.getMinResponseThreshold();
++
++ //llc.index = mLowLevelConstraint.index;
++ setLLBodies(llc, mBodies[0], mBodies[1]);
++
++ return true;
++ }
++
++ void Sc::ConstraintSim::destroyLLConstraint()
++ {
++ if(mLowLevelConstraint.constantBlock)
++ mScene.deallocateConstraintBlock(mLowLevelConstraint.constantBlock, mLowLevelConstraint.constantBlockSize);
++ }
++
++ void Sc::ConstraintSim::setBodies(RigidCore* r0, RigidCore* r1)
++ {
++ PX_ASSERT(mInteraction);
++
++ mScene.removeConstraintFromMap(*mInteraction);
++
++ releaseInteraction(mInteraction, this, mScene);
++
++ BodySim* b0 = (r0 && (r0->getActorCoreType() != PxActorType::eRIGID_STATIC)) ? static_cast<BodySim*>(r0->getSim()) : 0;
++ BodySim* b1 = (r1 && (r1->getActorCoreType() != PxActorType::eRIGID_STATIC)) ? static_cast<BodySim*>(r1->getSim()) : 0;
++
++ setLLBodies(mLowLevelConstraint, b0, b1);
++
++ mBodies[0] = b0;
++ mBodies[1] = b1;
++
++ mInteraction = createInteraction(this, r0, r1, mScene);
++
++ mScene.addConstraintToMap(mCore, r0, r1);
++ }
++
++ void Sc::ConstraintSim::getForce(PxVec3& lin, PxVec3& ang)
++ {
++ const PxReal recipDt = mScene.getOneOverDt();
++ Dy::ConstraintWriteback& solverOutput = mScene.getDynamicsContext()->getConstraintWriteBackPool()[mLowLevelConstraint.index];
++ lin = solverOutput.linearImpulse * recipDt;
++ ang = solverOutput.angularImpulse * recipDt;
++ }
++
++ void Sc::ConstraintSim::setBreakForceLL(PxReal linear, PxReal angular)
++ {
++ PxU8 wasBreakable = readFlag(eBREAKABLE);
++ PxU8 isBreakable;
++ if ((linear < PX_MAX_F32) || (angular < PX_MAX_F32))
++ isBreakable = eBREAKABLE;
++ else
++ isBreakable = 0;
++
++ if (isBreakable != wasBreakable)
++ {
++ if (isBreakable)
++ {
++ PX_ASSERT(!readFlag(eCHECK_MAX_FORCE_EXCEEDED));
++ setFlag(eBREAKABLE);
++ if (mInteraction->readInteractionFlag(InteractionFlag::eIS_ACTIVE))
++ mScene.addActiveBreakableConstraint(this, mInteraction);
++ }
++ else
++ {
++ if (readFlag(eCHECK_MAX_FORCE_EXCEEDED))
++ mScene.removeActiveBreakableConstraint(this);
++ clearFlag(eBREAKABLE);
++ }
++ }
++
++ mLowLevelConstraint.linBreakForce = linear;
++ mLowLevelConstraint.angBreakForce = angular;
++ }
++
++ void Sc::ConstraintSim::postFlagChange(PxConstraintFlags /*oldFlags*/, PxConstraintFlags newFlags)
++ {
++ mLowLevelConstraint.flags = newFlags;
++ }
++
++ PxConstraintGPUIndex Sc::ConstraintSim::getGPUIndex() const
++ {
++ //
++ // The constraint ID is used as GPU index
++ //
++
++ if (mLowLevelConstraint.flags & PxConstraintFlag::eGPU_COMPATIBLE)
++ {
++ PX_COMPILE_TIME_ASSERT(sizeof(Dy::Constraint::index) <= sizeof(PxConstraintGPUIndex));
++ return PxConstraintGPUIndex(mLowLevelConstraint.index);
++ }
++ else
++ return PX_INVALID_CONSTRAINT_GPU_INDEX;
++ }