Init

physx/source/simulationcontroller/include/ScActorCore.h

@@ -0,0 +1,131 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef PX_COLLISION_ACTOR_CORE
+#define PX_COLLISION_ACTOR_CORE
+
+#include "common/PxMetaData.h"
+#include "PxActor.h"
+#include "PsUserAllocated.h"
+#include "CmPhysXCommon.h"
+
+namespace physx
+{
+
+class PxActor;
+
+namespace Sc
+{
+
+	class Scene;
+	class ActorSim;
+
+	class ActorCore : public Ps::UserAllocated
+	{
+	//= ATTENTION! =====================================================================================
+	// Changing the data layout of this class breaks the binary serialization format.  See comments for 
+	// PX_BINARY_SERIAL_VERSION.  If a modification is required, please adjust the getBinaryMetaData 
+	// function.  If the modification is made on a custom branch, please change PX_BINARY_SERIAL_VERSION
+	// accordingly.
+	//==================================================================================================
+	public:
+// PX_SERIALIZATION
+											ActorCore(const PxEMPTY) :	mSim(NULL), mActorFlags(PxEmpty)
+											{
+											}
+		static			void				getBinaryMetaData(PxOutputStream& stream);
+//~PX_SERIALIZATION
+											ActorCore(PxActorType::Enum actorType, PxU8 actorFlags, 
+													  PxClientID owner, PxDominanceGroup dominanceGroup);
+		/*virtual*/							~ActorCore();
+
+		PX_FORCE_INLINE	ActorSim*			getSim()						const	{ return mSim;							}
+		PX_FORCE_INLINE	void				setSim(ActorSim* sim)
+											{
+												PX_ASSERT((sim==NULL) ^ (mSim==NULL));
+												mSim = sim;
+											}
+
+		PX_FORCE_INLINE	PxActorFlags		getActorFlags()					const	{ return mActorFlags;					}
+						void				setActorFlags(PxActorFlags af);
+
+		PX_FORCE_INLINE	PxDominanceGroup	getDominanceGroup()				const
+											{
+												return PxDominanceGroup(mDominanceGroup);	
+											}
+						void				setDominanceGroup(PxDominanceGroup g);
+
+		PX_FORCE_INLINE	void				setOwnerClient(PxClientID inId)
+											{
+												const PxU32 aggid = mAggregateIDOwnerClient & 0x00ffffff;
+												mAggregateIDOwnerClient = (PxU32(inId)<<24) | aggid;
+											}
+		PX_FORCE_INLINE	PxClientID			getOwnerClient()				const
+											{
+												return mAggregateIDOwnerClient>>24;
+											}
+
+		PX_FORCE_INLINE	PxActorType::Enum	getActorCoreType()				const 	{ return PxActorType::Enum(mActorType);	}
+
+						void				reinsertShapes();
+
+		PX_FORCE_INLINE	void				setAggregateID(PxU32 id)
+											{
+												PX_ASSERT(id==0xffffffff || id<(1<<24));
+												const PxU32 ownerClient = mAggregateIDOwnerClient & 0xff000000;
+												mAggregateIDOwnerClient = (id & 0x00ffffff) | ownerClient;
+											}
+		PX_FORCE_INLINE	PxU32				getAggregateID()				const
+											{
+												const PxU32 id = mAggregateIDOwnerClient & 0x00ffffff;
+												return id == 0x00ffffff ? PX_INVALID_U32 : id;
+											}
+	private:
+						ActorSim*			mSim;						// 
+						PxU32				mAggregateIDOwnerClient;	// PxClientID (8bit) | aggregate ID (24bit)
+		// PT: TODO: the remaining members could be packed into just a 16bit mask
+						PxActorFlags		mActorFlags;				// PxActor's flags (PxU8) => only 4 bits used
+						PxU8				mActorType;					// Actor type (8 bits, but 3 would be enough)
+						PxU8				mDominanceGroup;			// Dominance group (8 bits, but 5 would be enough because "must be < 32")
+	};
+
+#if PX_P64_FAMILY
+	PX_COMPILE_TIME_ASSERT(sizeof(Sc::ActorCore)==16);
+#else
+	PX_COMPILE_TIME_ASSERT(sizeof(Sc::ActorCore)==12);
+#endif
+
+} // namespace Sc
+
+}
+
+//////////////////////////////////////////////////////////////////////////
+
+#endif

physx/source/simulationcontroller/include/ScArticulationCore.h

@@ -0,0 +1,227 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef PX_PHYSICS_SCP_ARTICULATION_CORE
+#define PX_PHYSICS_SCP_ARTICULATION_CORE
+
+#include "ScActorCore.h"
+#include "DyArticulation.h"
+#include "DyFeatherstoneArticulation.h"
+
+namespace physx
+{
+
+class PxvArticulation;
+
+namespace IG
+{
+	class NodeIndex;
+}
+
+namespace Sc
+{
+	typedef Dy::FsData ArticulationDriveCache;
+
+	class ArticulationSim;
+	class BodyCore;
+	class BodySim;
+	class ArticulationJointCore;
+
+	class ArticulationCore
+	{
+	//= ATTENTION! =====================================================================================
+	// Changing the data layout of this class breaks the binary serialization format.  See comments for 
+	// PX_BINARY_SERIAL_VERSION.  If a modification is required, please adjust the getBinaryMetaData 
+	// function.  If the modification is made on a custom branch, please change PX_BINARY_SERIAL_VERSION
+	// accordingly.
+	//==================================================================================================
+
+		//---------------------------------------------------------------------------------
+		// Construction, destruction & initialization
+		//---------------------------------------------------------------------------------
+
+// PX_SERIALIZATION
+		public:
+													ArticulationCore(const PxEMPTY) : mSim(NULL)	{}
+		static		void							getBinaryMetaData(PxOutputStream& stream);
+//~PX_SERIALIZATION
+													ArticulationCore(bool reducedCoordinate);
+													~ArticulationCore();
+
+		//---------------------------------------------------------------------------------
+		// External API
+		//---------------------------------------------------------------------------------
+		PX_FORCE_INLINE	PxU32						getInternalDriveIterations()		const	{ return mCore.internalDriveIterations;	}
+		PX_FORCE_INLINE	void						setInternalDriveIterations(const PxU32 v)	{ mCore.internalDriveIterations = v;	}
+
+		PX_FORCE_INLINE	PxU32						getExternalDriveIterations()		const	{ return mCore.externalDriveIterations;	}
+		PX_FORCE_INLINE	void						setExternalDriveIterations(const PxU32 v)	{ mCore.externalDriveIterations = v;	}
+
+		PX_FORCE_INLINE	PxU32						getMaxProjectionIterations()		const	{ return mCore.maxProjectionIterations;	}
+		PX_FORCE_INLINE	void						setMaxProjectionIterations(const PxU32 v)	{ mCore.maxProjectionIterations = v;	}
+
+		PX_FORCE_INLINE	PxReal						getSeparationTolerance()			const	{ return mCore.separationTolerance;		}
+		PX_FORCE_INLINE	void						setSeparationTolerance(const PxReal v)		{ mCore.separationTolerance = v;		}
+
+		PX_FORCE_INLINE	PxReal						getSleepThreshold()					const	{ return mCore.sleepThreshold;			}
+		PX_FORCE_INLINE	void						setSleepThreshold(const PxReal v)			{ mCore.sleepThreshold = v;				}
+
+		PX_FORCE_INLINE	PxReal						getFreezeThreshold()				const	{ return mCore.freezeThreshold;			}
+		PX_FORCE_INLINE	void						setFreezeThreshold(const PxReal v)			{ mCore.freezeThreshold = v;			}
+
+		PX_FORCE_INLINE	PxU16						getSolverIterationCounts()			const	{ return mCore.solverIterationCounts;	}
+		PX_FORCE_INLINE	void						setSolverIterationCounts(PxU16 c)			{ mCore.solverIterationCounts = c;		}
+
+		PX_FORCE_INLINE	PxReal						getWakeCounter()					const	{ return mCore.wakeCounter;				}
+		PX_FORCE_INLINE	void						setWakeCounterInternal(const PxReal v)		{ mCore.wakeCounter = v;				}
+						void						setWakeCounter(const PxReal v);
+
+						bool						isSleeping() const;
+						void						wakeUp(PxReal wakeCounter);
+						void						putToSleep();
+
+						PxArticulationBase*			getPxArticulationBase();
+						const PxArticulationBase*	getPxArticulationBase() const;
+
+		//---------------------------------------------------------------------------------
+		// Drive Cache API
+		//---------------------------------------------------------------------------------
+						ArticulationDriveCache*		createDriveCache(PxReal compliance,
+																	 PxU32 driveIterations) const;
+
+						void						updateDriveCache(ArticulationDriveCache& cache,
+																	 PxReal compliance,
+																	 PxU32 driveIterations) const;
+
+						void						releaseDriveCache(ArticulationDriveCache& cache) const;
+
+						PxU32						getCacheLinkCount(const ArticulationDriveCache& cache) const;
+
+						void						applyImpulse(BodyCore& link,
+																 const ArticulationDriveCache& driveCache,
+																 const PxVec3& force,
+																 const PxVec3& torque);
+
+						void						computeImpulseResponse(BodyCore& link,
+																		   PxVec3& linearResponse, 
+																		   PxVec3& angularResponse,
+																		   const ArticulationDriveCache& driveCache,
+																		   const PxVec3& force,
+																		   const PxVec3& torque) const;
+
+		//---------------------------------------------------------------------------------
+		// external reduced coordinate API
+		//---------------------------------------------------------------------------------
+						void						setArticulationFlags(PxArticulationFlags flags);
+						PxArticulationFlags			getArticulationFlags() const { return mCore.flags; }
+
+						PxU32						getDofs() const;
+
+						PxArticulationCache*		createCache() const;
+
+						PxU32						getCacheDataSize() const;
+
+						void						zeroCache(PxArticulationCache& cache) const;
+
+						void						applyCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag)const;
+		
+						void						copyInternalStateToCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag) const;
+
+						void						releaseCache(PxArticulationCache& cache) const;
+
+						void						packJointData(const PxReal* maximum, PxReal* reduced) const;
+
+						void						unpackJointData(const PxReal* reduced, PxReal* maximum) const;
+
+						void						commonInit() const;
+
+						void						computeGeneralizedGravityForce(PxArticulationCache& cache) const;
+
+						void						computeCoriolisAndCentrifugalForce(PxArticulationCache& cache) const;
+
+						void						computeGeneralizedExternalForce(PxArticulationCache& cache) const;
+
+						void						computeJointAcceleration(PxArticulationCache& cache) const;
+
+						void						computeJointForce(PxArticulationCache& cache) const;
+
+
+						void						computeDenseJacobian(PxArticulationCache& cache, PxU32& nRows, PxU32& nCols) const;
+
+						void						computeCoefficientMatrix(PxArticulationCache& cache) const;
+
+						bool						computeLambda(PxArticulationCache& cache, PxArticulationCache& rollBackCache, const PxReal* const jointTorque, const PxVec3 gravity, const PxU32 maxIter) const;
+
+						void						computeGeneralizedMassMatrix(PxArticulationCache& cache) const;
+
+						PxU32						getCoefficientMatrixSize() const;
+
+						PxSpatialVelocity			getLinkVelocity(const PxU32 linkId) const;
+
+						PxSpatialVelocity			getLinkAcceleration(const PxU32 linkId) const;
+
+		//---------------------------------------------------------------------------------
+		// Internal API
+		//---------------------------------------------------------------------------------
+	public:
+		PX_FORCE_INLINE	void						setSim(ArticulationSim* sim)
+													{
+														PX_ASSERT((sim==0) ^ (mSim == 0));
+														mSim = sim;
+													}
+		PX_FORCE_INLINE	ArticulationSim*			getSim()			const	{ return mSim;			}
+
+		PX_FORCE_INLINE	const Dy::ArticulationCore&	getCore()					{ return mCore;			}
+
+		static PX_FORCE_INLINE ArticulationCore&	getArticulationCore(ArticulationCore& core)
+													{
+														const size_t offset = PX_OFFSET_OF(ArticulationCore, mCore);
+														return *reinterpret_cast<ArticulationCore*>(reinterpret_cast<PxU8*>(&core) - offset);
+													}
+
+		PX_INLINE		bool						isReducedCoordinate() const	{ return mIsReducedCoordinate;	}
+
+						IG::NodeIndex				getIslandNodeIndex() const;
+
+						void						setGlobalPose();
+
+						void						setDirty(const bool dirty);
+
+	private:
+						ArticulationSim*			mSim;
+						Dy::ArticulationCore		mCore;
+						bool						mIsReducedCoordinate;
+	};
+
+} // namespace Sc
+
+}
+
+#endif

physx/source/simulationcontroller/include/ScArticulationJointCore.h

@@ -0,0 +1,204 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef PX_PHYSICS_SCP_ARTICULATION_JOINT_CORE
+#define PX_PHYSICS_SCP_ARTICULATION_JOINT_CORE
+
+#include "foundation/PxTransform.h"
+#include "common/PxMetaData.h"
+#include "CmPhysXCommon.h"
+#include "PsUserAllocated.h"
+#include "DyArticulation.h"
+
+namespace physx
+{
+namespace Sc
+{
+	class BodyCore;
+	class ArticulationJointSim;
+	class ArticulationCore;
+
+	class ArticulationJointDesc
+	{
+	public:
+		BodyCore*	parent;
+		BodyCore*	child;
+		PxTransform	parentPose;
+		PxTransform	childPose;
+	};
+
+	class ArticulationJointCore : public Ps::UserAllocated
+	{
+	//= ATTENTION! =====================================================================================
+	// Changing the data layout of this class breaks the binary serialization format.  See comments for 
+	// PX_BINARY_SERIAL_VERSION.  If a modification is required, please adjust the getBinaryMetaData 
+	// function.  If the modification is made on a custom branch, please change PX_BINARY_SERIAL_VERSION
+	// accordingly.
+	//==================================================================================================
+
+		//---------------------------------------------------------------------------------
+		// Construction, destruction & initialization
+		//---------------------------------------------------------------------------------
+	public:
+// PX_SERIALIZATION
+															ArticulationJointCore(const PxEMPTY) : mSim(NULL), mCore(PxEmpty)	{}
+						void								preExportDataReset() { mCore.dirtyFlag = Dy::ArticulationJointCoreDirtyFlag::eALL; }
+		static			void								getBinaryMetaData(PxOutputStream& stream);
+//~PX_SERIALIZATION
+															ArticulationJointCore( const PxTransform& parentFrame, const PxTransform& childFrame, bool reducedCoordinate);
+															~ArticulationJointCore();
+
+		//---------------------------------------------------------------------------------
+		// External API
+		//---------------------------------------------------------------------------------
+
+		PX_FORCE_INLINE	const PxTransform&					getParentPose() const { return mCore.parentPose; }
+						void								setParentPose(const PxTransform&);
+
+		PX_FORCE_INLINE	const PxTransform&					getChildPose() const { return mCore.childPose; }
+						void								setChildPose(const PxTransform&);
+
+		PX_FORCE_INLINE	const PxQuat&						getTargetOrientation() const { return mCore.targetPosition; }
+						void								setTargetOrientation(const PxQuat&);
+
+		PX_FORCE_INLINE	const PxVec3&						getTargetVelocity() const { return mCore.targetVelocity; }
+						void								setTargetVelocity(const PxVec3&);
+
+		PX_FORCE_INLINE	PxReal								getStiffness() const { return mCore.spring; }
+						void								setStiffness(PxReal);
+
+		PX_FORCE_INLINE	PxReal								getDamping() const { return mCore.damping; }
+						void								setDamping(PxReal);
+
+		PX_FORCE_INLINE	PxReal								getInternalCompliance() const { return mCore.internalCompliance; }
+						void								setInternalCompliance(PxReal);
+
+		PX_FORCE_INLINE	PxReal								getExternalCompliance() const { return mCore.externalCompliance; }
+						void								setExternalCompliance(PxReal);
+
+		PX_FORCE_INLINE	void								getSwingLimit(PxReal& yLimit, PxReal& zLimit) const { yLimit = mCore.limits[PxArticulationAxis::eSWING1].low; zLimit = mCore.limits[PxArticulationAxis::eSWING2].low; }
+						void								setSwingLimit(PxReal yLimit, PxReal zLimit);
+
+		PX_FORCE_INLINE	PxReal								getTangentialStiffness() const { return mCore.tangentialStiffness; }
+						void								setTangentialStiffness(PxReal);
+
+		PX_FORCE_INLINE	PxReal								getTangentialDamping() const { return mCore.tangentialDamping; }
+						void								setTangentialDamping(PxReal);
+
+		PX_FORCE_INLINE	bool								getSwingLimitEnabled() const { return mCore.swingLimited; }
+						void								setSwingLimitEnabled(bool);
+
+		PX_FORCE_INLINE	PxReal								getSwingLimitContactDistance() const { return mCore.swingLimitContactDistance; }
+						void								setSwingLimitContactDistance(PxReal);
+
+		PX_FORCE_INLINE	void								getTwistLimit(PxReal& lower, PxReal& upper) const { lower = mCore.limits[PxArticulationAxis::eTWIST].low; upper = mCore.limits[PxArticulationAxis::eTWIST].high; }
+						void								setTwistLimit(PxReal lower, PxReal upper);
+
+						void								getLimit(PxArticulationAxis::Enum axis, PxReal& lower, PxReal& upper) const
+															{
+																lower = mCore.limits[axis].low;
+																upper = mCore.limits[axis].high;
+															}
+
+						void								setLimit(PxArticulationAxis::Enum axis, PxReal lower, PxReal upper);
+
+						void								getDrive(PxArticulationAxis::Enum axis, PxReal& stiffness, PxReal& damping, PxReal& maxForce, PxArticulationDriveType::Enum& driveType) const
+															{
+																stiffness = mCore.drives[axis].stiffness;
+																damping = mCore.drives[axis].damping;
+																maxForce = mCore.drives[axis].maxForce;
+																driveType = mCore.drives[axis].driveType;
+															}
+
+						void								setDrive(PxArticulationAxis::Enum axis, PxReal stiffness, PxReal damping, PxReal maxForce, PxArticulationDriveType::Enum driveType);
+
+						void								setTargetP(PxArticulationAxis::Enum axis, PxReal targetP);
+		PX_FORCE_INLINE	PxReal								getTargetP(PxArticulationAxis::Enum axis)	const	{ return mCore.targetP[axis];	}
+
+						void								setTargetV(PxArticulationAxis::Enum axis, PxReal targetV);
+		PX_FORCE_INLINE	PxReal								getTargetV(PxArticulationAxis::Enum axis)	const	{ return mCore.targetV[axis];	}
+
+		PX_FORCE_INLINE	bool								getTwistLimitEnabled()						const	{ return mCore.twistLimited; }
+						void								setTwistLimitEnabled(bool);
+
+		PX_FORCE_INLINE	PxReal								getTwistLimitContactDistance()				const	{ return mCore.twistLimitContactDistance; }
+						void								setTwistLimitContactDistance(PxReal);
+
+						void								setDriveType(PxArticulationJointDriveType::Enum type);
+						PxArticulationJointDriveType::Enum	getDriveType()								const	{ return PxArticulationJointDriveType::Enum(mCore.driveType); }
+
+						void								setJointType(PxArticulationJointType::Enum type);
+						PxArticulationJointType::Enum		getJointType()								const;
+
+						void								setMotion(PxArticulationAxis::Enum axis, PxArticulationMotion::Enum motion);
+						PxArticulationMotion::Enum			getMotion(PxArticulationAxis::Enum axis)	const;
+
+						void								setFrictionCoefficient(const PxReal coefficient);
+						PxReal								getFrictionCoefficient()					const;
+
+						void								setMaxJointVelocity(const PxReal maxJointV);
+						PxReal								getMaxJointVelocity()						const;
+
+						PxArticulationJointBase*			getPxArticulationJointBase();
+						const PxArticulationJointBase*		getPxArticulationJointBase()				const;
+
+		//---------------------------------------------------------------------------------
+		// Low Level data access - some wouldn't be needed if the interface wasn't virtual
+		//---------------------------------------------------------------------------------
+
+		PX_FORCE_INLINE	ArticulationJointSim*				getSim()									const	{ return mSim;	}
+		PX_FORCE_INLINE	void								setSim(ArticulationJointSim* sim)
+															{
+																PX_ASSERT((sim==0) ^ (mSim == 0));
+																mSim = sim;
+															}
+
+		PX_FORCE_INLINE	Dy::ArticulationJointCore&			getCore()											{ return mCore; }
+
+		PX_FORCE_INLINE void								setArticulation(ArticulationCore* articulation)		{ mArticulation = articulation;	}
+		PX_FORCE_INLINE	const ArticulationCore*				getArticulation()							const	{ return mArticulation; }
+
+		PX_FORCE_INLINE void								setRoot(PxArticulationJointBase* base)				{ mRootType = base; }
+		PX_FORCE_INLINE PxArticulationJointBase*			getRoot()									const	{ return mRootType; }
+
+	private:
+
+		void setDirty(Dy::ArticulationJointCoreDirtyFlag::Enum dirtyFlag);
+						ArticulationJointSim*				mSim;
+						Dy::ArticulationJointCore			mCore;
+						ArticulationCore*					mArticulation;
+						PxArticulationJointBase*			mRootType;
+	};
+
+} // namespace Sc
+
+}
+
+#endif

physx/source/simulationcontroller/include/ScBodyCore.h

@@ -0,0 +1,214 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef PX_PHYSICS_SCP_BODYCORE
+#define PX_PHYSICS_SCP_BODYCORE
+
+#include "foundation/PxTransform.h"
+#include "ScRigidCore.h"
+#include "PxRigidDynamic.h"
+#include "PxvDynamics.h"
+#include "PxvConfig.h"
+#include "PsPool.h"
+
+namespace physx
+{
+class PxRigidBodyDesc;
+
+namespace Sc
+{
+	class BodySim;
+	struct SimStateData;
+
+	struct KinematicTransform
+	{
+		PxTransform		targetPose;		// The body will move to this pose over the superstep following this getting set.
+		PxU8			targetValid;	// User set a kinematic target.
+		PxU8			pad[2];
+		PxU8			type;		
+	};
+
+	class BodyCore : public RigidCore
+	{
+	//= ATTENTION! =====================================================================================
+	// Changing the data layout of this class breaks the binary serialization format.  See comments for 
+	// PX_BINARY_SERIAL_VERSION.  If a modification is required, please adjust the getBinaryMetaData 
+	// function.  If the modification is made on a custom branch, please change PX_BINARY_SERIAL_VERSION
+	// accordingly.
+	//==================================================================================================
+
+		//---------------------------------------------------------------------------------
+		// Construction, destruction & initialization
+		//---------------------------------------------------------------------------------
+	public:
+// PX_SERIALIZATION
+											BodyCore(const PxEMPTY) : RigidCore(PxEmpty), mCore(PxEmpty), mSimStateData(NULL)	{}
+		static			void				getBinaryMetaData(PxOutputStream& stream);
+						void				disableInternalCaching(bool disable);
+						void				restoreDynamicData();
+
+//~PX_SERIALIZATION
+											BodyCore(PxActorType::Enum type, const PxTransform& bodyPose);
+		/*virtual*/							~BodyCore();
+
+		//---------------------------------------------------------------------------------
+		// External API
+		//---------------------------------------------------------------------------------
+		PX_FORCE_INLINE	const PxTransform&	getBody2World()				const	{ return mCore.body2World;			}
+						void				setBody2World(const PxTransform& p);
+
+		PX_FORCE_INLINE	const PxVec3&		getLinearVelocity()			const	{ return mCore.linearVelocity;		}
+						void				setLinearVelocity(const PxVec3& v); 
+	
+		PX_FORCE_INLINE	const PxVec3&		getAngularVelocity()		const	{ return mCore.angularVelocity;		}
+						void				setAngularVelocity(const PxVec3& v);
+	
+		PX_FORCE_INLINE	void				updateVelocities(const PxVec3& linearVelModPerStep, const PxVec3& angularVelModPerStep)
+											{
+												mCore.linearVelocity += linearVelModPerStep;
+												mCore.angularVelocity += angularVelModPerStep;
+											}
+
+		PX_FORCE_INLINE	const PxTransform&	getBody2Actor()				const	{ return mCore.getBody2Actor();			}
+						void				setBody2Actor(const PxTransform& p);
+
+						void				addSpatialAcceleration(Ps::Pool<SimStateData>* simStateDataPool, const PxVec3* linAcc, const PxVec3* angAcc);
+						void				setSpatialAcceleration(Ps::Pool<SimStateData>* simStateDataPool, const PxVec3* linAcc, const PxVec3* angAcc);
+						void				clearSpatialAcceleration(bool force, bool torque);
+						void				addSpatialVelocity(Ps::Pool<SimStateData>* simStateDataPool, const PxVec3* linVelDelta, const PxVec3* angVelDelta);
+						void				clearSpatialVelocity(bool force, bool torque);
+
+		PX_FORCE_INLINE PxReal				getMaxPenetrationBias() const		{ return mCore.maxPenBias; }
+		PX_FORCE_INLINE void				setMaxPenetrationBias(PxReal p)		{ mCore.maxPenBias = p; }
+
+						PxReal				getInverseMass()			const;
+						void				setInverseMass(PxReal m);
+						const PxVec3&		getInverseInertia()			const;
+						void				setInverseInertia(const PxVec3& i);
+
+						PxReal				getLinearDamping()			const;
+						void				setLinearDamping(PxReal d);
+
+						PxReal				getAngularDamping()			const;
+						void				setAngularDamping(PxReal d);
+
+		PX_FORCE_INLINE	PxRigidBodyFlags	getFlags()					const	{ return mCore.mFlags;		}
+						void				setFlags(Ps::Pool<SimStateData>* simStateDataPool, PxRigidBodyFlags f);
+
+		PX_FORCE_INLINE	PxRigidDynamicLockFlags	getRigidDynamicLockFlags()					const	{ return mCore.lockFlags; }
+
+		PX_FORCE_INLINE void				setRigidDynamicLockFlags(PxRigidDynamicLockFlags flags) { mCore.lockFlags = flags; }
+
+		PX_FORCE_INLINE	PxReal				getSleepThreshold()	const	{ return mCore.sleepThreshold;	}
+						void				setSleepThreshold(PxReal t);		
+
+		PX_FORCE_INLINE	PxReal				getFreezeThreshold()	const	{ return mCore.freezeThreshold;	}
+						void				setFreezeThreshold(PxReal t);
+
+		PX_FORCE_INLINE PxReal				getMaxContactImpulse() const	{ return mCore.maxContactImpulse;  }
+						void				setMaxContactImpulse(PxReal m);
+
+						PxU32				getInternalIslandNodeIndex() const;
+
+		PX_FORCE_INLINE PxReal				getWakeCounter() const { return mCore.wakeCounter; }
+						void				setWakeCounter(PxReal wakeCounter, bool forceWakeUp=false);
+
+						bool				isSleeping() const;
+		PX_FORCE_INLINE	void				wakeUp(PxReal wakeCounter)			{ setWakeCounter(wakeCounter, true);	}
+						void				putToSleep();
+
+						PxReal				getMaxAngVelSq() const;
+						void				setMaxAngVelSq(PxReal v);
+
+						PxReal				getMaxLinVelSq() const;
+						void				setMaxLinVelSq(PxReal v);
+
+		PX_FORCE_INLINE	PxU16				getSolverIterationCounts()	const	{ return mCore.solverIterationCounts;	}
+						void				setSolverIterationCounts(PxU16 c);
+
+						bool				getKinematicTarget(PxTransform& p) const;
+						bool				getHasValidKinematicTarget() const;
+						void				setKinematicTarget(Ps::Pool<SimStateData>* simStateDataPool, const PxTransform& p, PxReal wakeCounter);
+						void				invalidateKinematicTarget();
+						
+
+		PX_FORCE_INLINE	PxReal				getContactReportThreshold()	const	{ return mCore.contactReportThreshold;	}
+						void				setContactReportThreshold(PxReal t)	{ mCore.contactReportThreshold = t;		}
+
+						void				onOriginShift(const PxVec3& shift);
+
+		//---------------------------------------------------------------------------------
+		// Internal API
+		//---------------------------------------------------------------------------------
+
+		PX_FORCE_INLINE void				setLinearVelocityInternal(const PxVec3& v)	{ mCore.linearVelocity = v; }
+		PX_FORCE_INLINE void				setAngularVelocityInternal(const PxVec3& v)	{ mCore.angularVelocity = v; }
+		PX_FORCE_INLINE	void				setWakeCounterFromSim(PxReal c)		{ mCore.wakeCounter = c;					}
+
+						BodySim*			getSim() const;
+
+		PX_FORCE_INLINE	PxsBodyCore&		getCore()							{ return mCore;						}
+		PX_FORCE_INLINE	const PxsBodyCore&	getCore()			const			{ return mCore;						}
+
+		PX_FORCE_INLINE	PxReal				getCCDAdvanceCoefficient() const	{ return mCore.ccdAdvanceCoefficient;	}
+		PX_FORCE_INLINE	void				setCCDAdvanceCoefficient(PxReal c)	{ mCore.ccdAdvanceCoefficient = c;		}
+
+						bool				setupSimStateData(Ps::Pool<SimStateData>* simStateDataPool, const bool isKinematic, const bool targetValid = false);
+						void				tearDownSimStateData(Ps::Pool<SimStateData>* simStateDataPool, const bool isKinematic);
+
+						bool				checkSimStateKinematicStatus(bool) const;
+
+						Ps::IntBool			isFrozen()							const;
+
+		PX_FORCE_INLINE const SimStateData*	getSimStateData(bool isKinematic)	const	{ return (mSimStateData && (checkSimStateKinematicStatus(isKinematic)) ? mSimStateData : NULL); }
+		PX_FORCE_INLINE SimStateData*		getSimStateData(bool isKinematic)			{ return (mSimStateData && (checkSimStateKinematicStatus(isKinematic)) ? mSimStateData : NULL); }
+
+		PX_FORCE_INLINE SimStateData*		getSimStateData_Unchecked()			const	{ return mSimStateData; }
+
+		static PX_FORCE_INLINE size_t		getCoreOffset()								{ return PX_OFFSET_OF_RT(BodyCore, mCore); }
+
+		static PX_FORCE_INLINE BodyCore&	getCore(PxsBodyCore& core)					{ return *reinterpret_cast<BodyCore*>(reinterpret_cast<PxU8*>(&core) - getCoreOffset()); }
+
+		void				setKinematicLink(const bool value);
+
+	private:
+						void				backup(SimStateData&);
+						void				restore();
+
+						PX_ALIGN_PREFIX(16) PxsBodyCore mCore PX_ALIGN_SUFFIX(16);
+						SimStateData*		mSimStateData;
+	};
+
+	PxActor* getPxActorFromBodyCore(Sc::BodyCore* bodyCore, PxActorType::Enum& type);
+
+} // namespace Sc
+
+}
+
+#endif

physx/source/simulationcontroller/include/ScConstraintCore.h

@@ -0,0 +1,134 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef PX_PHYSICS_CONSTRAINTCORE
+#define PX_PHYSICS_CONSTRAINTCORE
+
+#include "CmPhysXCommon.h"
+#include "PxConstraintDesc.h"
+#include "PsAllocator.h"
+#include "PxConstraint.h"
+
+namespace physx
+{
+
+class PxConstraint;
+
+namespace Sc
+{
+	class ConstraintCore;
+	class ConstraintSim;
+	class RigidCore;
+
+
+	class ConstraintCore : public Ps::UserAllocated	
+	{
+	//= ATTENTION! =====================================================================================
+	// Changing the data layout of this class breaks the binary serialization format.  See comments for 
+	// PX_BINARY_SERIAL_VERSION.  If a modification is required, please adjust the getBinaryMetaData 
+	// function.  If the modification is made on a custom branch, please change PX_BINARY_SERIAL_VERSION
+	// accordingly.
+	//==================================================================================================
+	public:
+// PX_SERIALIZATION
+											ConstraintCore(const PxEMPTY) : mFlags(PxEmpty), mConnector(NULL), mSim(NULL)	{}
+	PX_FORCE_INLINE	void					setConstraintFunctions(PxConstraintConnector& n,
+																   const PxConstraintShaderTable& shaders)
+											{ 
+												mConnector = &n;	
+												mSolverPrep = shaders.solverPrep;
+												mProject = shaders.project;
+												mVisualize = shaders.visualize;
+											}
+		static		void					getBinaryMetaData(PxOutputStream& stream);
+//~PX_SERIALIZATION
+											ConstraintCore(PxConstraintConnector& connector, const PxConstraintShaderTable& shaders, PxU32 dataSize);
+											~ConstraintCore();
+
+					// The two-step protocol here allows us to unlink the constraint prior to deleting
+					// the actors when synchronizing the scene, then set the bodies after new actors have been inserted
+
+					void					prepareForSetBodies();
+					void					setBodies(RigidCore* r0v, RigidCore* r1v);
+
+					PxConstraint*			getPxConstraint();
+					const PxConstraint*		getPxConstraint()									const;
+	PX_FORCE_INLINE	PxConstraintConnector*	getPxConnector()									const	{ return mConnector;	}
+
+	PX_FORCE_INLINE	PxConstraintFlags		getFlags()											const	{ return mFlags;	}
+					void					setFlags(PxConstraintFlags flags);
+
+					void					getForce(PxVec3& force, PxVec3& torque)				const;
+
+					bool					updateConstants(void* addr); 
+
+					void					setBreakForce(PxReal linear, PxReal angular);
+					void					getBreakForce(PxReal& linear, PxReal& angular)	const;
+
+					void					setMinResponseThreshold(PxReal threshold);
+					PxReal					getMinResponseThreshold()							const	{ return mMinResponseThreshold; }
+
+					void					breakApart();
+
+	PX_FORCE_INLINE	PxConstraintVisualize	getVisualize()										const	{ return mVisualize;				}
+	PX_FORCE_INLINE	PxConstraintProject		getProject()										const	{ return mProject;					}
+	PX_FORCE_INLINE	PxConstraintSolverPrep	getSolverPrep()										const	{ return mSolverPrep;				}
+	PX_FORCE_INLINE	PxU32					getConstantBlockSize()								const	{ return mDataSize;					}
+
+	PX_FORCE_INLINE	void					setSim(ConstraintSim* sim)
+											{
+												PX_ASSERT((sim==0) ^ (mSim == 0));
+												mSim = sim;
+											}
+	PX_FORCE_INLINE	ConstraintSim*			getSim()											const	{ return mSim;	}
+	private:
+					PxConstraintFlags		mFlags;
+					PxU16					mPaddingFromFlags;	// PT: because flags are PxU16
+
+					PxVec3					mAppliedForce;
+					PxVec3					mAppliedTorque;
+
+					PxConstraintConnector*	mConnector;
+					PxConstraintProject		mProject;
+					PxConstraintSolverPrep	mSolverPrep;
+					PxConstraintVisualize	mVisualize;
+					PxU32					mDataSize;
+					PxReal					mLinearBreakForce;
+					PxReal					mAngularBreakForce;
+					PxReal					mMinResponseThreshold;
+
+					ConstraintSim*			mSim;
+	};
+
+} // namespace Sc
+
+}
+
+#endif

physx/source/simulationcontroller/include/ScIterators.h

@@ -0,0 +1,104 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+      
+
+#ifndef PX_PHYSICS_SCP_ITERATOR
+#define PX_PHYSICS_SCP_ITERATOR
+
+#include "foundation/PxVec3.h"
+#include "PxContact.h"
+
+namespace physx
+{
+class PxShape;
+class PxsContactManagerOutputIterator;
+
+namespace Sc
+{
+	class ShapeSim;
+	class Interaction;
+	
+	struct Contact
+	{
+		Contact() 
+			: normal(0.0f)
+			, point(0.0f)
+			, separation(0.0f)
+			, normalForce(0.0f)
+		{}
+
+		PxVec3 normal;
+		PxVec3 point;
+		PxShape* shape0;
+		PxShape* shape1;
+		PxReal separation;
+		PxReal normalForce;
+		PxU32 faceIndex0;  // these are the external indices
+		PxU32 faceIndex1;
+		bool normalForceAvailable;
+	};
+
+	class ContactIterator
+	{
+		public:		
+
+			class Pair
+			{
+			public:
+				Pair() : mIter(NULL, NULL, NULL, 0, 0) {}
+				Pair(const void*& contactPatches, const void*& contactPoints, const PxU32 /*contactDataSize*/, const PxReal*& forces, PxU32 numContacts, PxU32 numPatches, ShapeSim& shape0, ShapeSim& shape1);
+				Contact* getNextContact();
+
+			private:
+				PxU32						mIndex;
+				PxU32						mNumContacts;
+				PxContactStreamIterator		mIter;
+				const PxReal*				mForces;
+				Contact						mCurrentContact;
+			};
+
+			ContactIterator() {}
+			explicit ContactIterator(Interaction** first, Interaction** last, PxsContactManagerOutputIterator& outputs): mCurrent(first), mLast(last), mOffset(0), mOutputs(&outputs) {}
+			Pair* getNextPair();
+
+		private:
+			Interaction**					mCurrent;
+			Interaction**					mLast;
+			Pair							mCurrentPair;
+			PxU32							mOffset;
+			PxsContactManagerOutputIterator* mOutputs;
+
+	private:
+	};
+
+}  // namespace Sc
+
+}
+
+#endif

physx/source/simulationcontroller/include/ScMaterialCore.h

@@ -0,0 +1,70 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef PX_PHYSICS_SCP_MATERIAL_CORE
+#define PX_PHYSICS_SCP_MATERIAL_CORE
+
+#include "foundation/PxVec3.h"
+#include "PsUserAllocated.h"
+#include "CmPhysXCommon.h"
+#include "PxMaterial.h"
+#include "PxsMaterialCore.h"
+
+namespace physx
+{
+class PxMaterial;
+
+namespace Sc
+{
+typedef	PxsMaterialData	MaterialData;
+
+class MaterialCore : public PxsMaterialCore
+{
+//= ATTENTION! =====================================================================================
+// Changing the data layout of this class breaks the binary serialization format.  See comments for 
+// PX_BINARY_SERIAL_VERSION.  If a modification is required, please adjust the getBinaryMetaData 
+// function.  If the modification is made on a custom branch, please change PX_BINARY_SERIAL_VERSION
+// accordingly.
+//==================================================================================================
+public:
+							MaterialCore(const MaterialData& desc) : PxsMaterialCore(desc)	{}
+							MaterialCore(const PxEMPTY) : PxsMaterialCore(PxEmpty)			{}
+							MaterialCore()													{}
+							~MaterialCore()													{}
+	static			void	getBinaryMetaData(PxOutputStream& stream);
+
+	PX_FORCE_INLINE	void	save(MaterialData& data)		const	{ data = *this;			}
+	PX_FORCE_INLINE	void	load(const MaterialData& data)			{ static_cast<MaterialData&>(*this) = data; }	// To make synchronization between master material and scene material table less painful
+};
+
+} // namespace Sc
+
+}
+
+#endif

physx/source/simulationcontroller/include/ScPhysics.h

@@ -0,0 +1,139 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef PX_PHYSICS_SC_PHYSICS
+#define PX_PHYSICS_SC_PHYSICS
+
+#include "PxPhysics.h"
+#include "PxScene.h"
+#include "PsUserAllocated.h"
+#include "CmPhysXCommon.h"
+#include "PsBasicTemplates.h"
+#include "PxActor.h"
+
+namespace physx
+{
+
+class PxMaterial;
+class PxTolerancesScale;
+struct PxvOffsetTable;
+
+#if PX_SUPPORT_GPU_PHYSX
+class PxPhysXGpu;
+#endif
+
+namespace Sc
+{
+	class Scene;
+	class StaticCore;
+	class RigidCore;
+	class BodyCore;
+	class ArticulationCore;
+	class ArticulationJointCore;
+	class ConstraintCore;
+	class ShapeCore;
+
+	struct OffsetTable
+	{
+		PX_FORCE_INLINE OffsetTable() {}
+
+		PX_FORCE_INLINE PxActor*					convertScRigidStatic2PxActor(StaticCore* sc)			const	{ return Ps::pointerOffset<PxActor*>(sc, scRigidStatic2PxActor);				}
+		PX_FORCE_INLINE PxActor*					convertScRigidDynamic2PxActor(BodyCore* sc)				const	{ return Ps::pointerOffset<PxActor*>(sc, scRigidDynamic2PxActor);				}
+		PX_FORCE_INLINE PxActor*					convertScArticulationLink2PxActor(BodyCore* sc)			const	{ return Ps::pointerOffset<PxActor*>(sc, scArticulationLink2PxActor);			}
+
+		PX_FORCE_INLINE PxShape*					convertScShape2Px(ShapeCore* sc)						const	{ return Ps::pointerOffset<PxShape*>(sc, scShape2Px);							}
+		PX_FORCE_INLINE const PxShape*				convertScShape2Px(const ShapeCore* sc)					const	{ return Ps::pointerOffset<const PxShape*>(sc, scShape2Px);						}
+
+		PX_FORCE_INLINE PxConstraint*				convertScConstraint2Px(ConstraintCore* sc)				const	{ return Ps::pointerOffset<PxConstraint*>(sc, scConstraint2Px);					}
+		PX_FORCE_INLINE const PxConstraint*			convertScConstraint2Px(const ConstraintCore* sc)		const	{ return Ps::pointerOffset<const PxConstraint*>(sc, scConstraint2Px);			}
+
+		PX_FORCE_INLINE PxArticulationBase*			convertScArticulation2Px(ArticulationCore* sc, bool isRC) const	
+		{
+			ptrdiff_t sc2Px = isRC ? scArticulationRC2Px : scArticulationMC2Px;
+			return Ps::pointerOffset<PxArticulationBase*>(sc, sc2Px);
+		}
+		PX_FORCE_INLINE const PxArticulationBase*	convertScArticulation2Px(const ArticulationCore* sc, bool isRC)	const
+		{
+			ptrdiff_t sc2Px = isRC ? scArticulationRC2Px : scArticulationMC2Px;
+			return Ps::pointerOffset<const PxArticulationBase*>(sc, sc2Px);
+		}
+
+		PX_FORCE_INLINE PxArticulationJointBase*	convertScArticulationJoint2Px(ArticulationJointCore* sc, bool isRC) const	
+		{
+			ptrdiff_t sc2Px = isRC ? scArticulationJointRC2Px : scArticulationJointMC2Px;
+			return Ps::pointerOffset<PxArticulationJointBase*>(sc, sc2Px);
+		}
+		PX_FORCE_INLINE const PxArticulationJointBase* convertScArticulationJoint2Px(const ArticulationJointCore* sc, bool isRC)	const
+		{
+			ptrdiff_t sc2Px = isRC ? scArticulationJointRC2Px : scArticulationJointMC2Px;
+			return Ps::pointerOffset<const PxArticulationJointBase*>(sc, sc2Px);
+		}
+
+
+		ptrdiff_t	scRigidStatic2PxActor;
+		ptrdiff_t 	scRigidDynamic2PxActor;
+		ptrdiff_t 	scArticulationLink2PxActor;
+		ptrdiff_t 	scShape2Px;
+		ptrdiff_t 	scArticulationMC2Px;
+		ptrdiff_t 	scArticulationRC2Px;
+		ptrdiff_t 	scArticulationJointMC2Px;
+		ptrdiff_t 	scArticulationJointRC2Px;
+		ptrdiff_t 	scSoftBody2Px;
+		ptrdiff_t 	scConstraint2Px;
+
+		ptrdiff_t	scCore2PxActor[PxActorType::eACTOR_COUNT];
+	};
+	extern OffsetTable gOffsetTable;
+
+	class Physics : public Ps::UserAllocated
+	{
+	public:
+		PX_FORCE_INLINE static Physics&				getInstance()						{ return *mInstance; }
+
+													Physics(const PxTolerancesScale&, const PxvOffsetTable& pxvOffsetTable);
+													~Physics(); // use release() instead
+	public:
+						void						release();
+
+		PX_FORCE_INLINE	const PxTolerancesScale&	getTolerancesScale()		const	{ return mScale;	}
+
+	private:
+						PxTolerancesScale			mScale;
+		static			Physics*					mInstance;
+
+	public:
+		static			const PxReal				sWakeCounterOnCreation;
+	};
+
+} // namespace Sc
+
+}
+
+#endif

physx/source/simulationcontroller/include/ScRigidCore.h

@@ -0,0 +1,94 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef PX_PHYSICS_SCP_RB_CORE
+#define PX_PHYSICS_SCP_RB_CORE
+
+#include "ScActorCore.h"
+#include "PxvDynamics.h"
+#include "PxShape.h"
+
+namespace physx
+{
+
+namespace Sc
+{
+
+	class RigidSim;
+
+	struct ShapeChangeNotifyFlag
+	{
+		enum Enum
+		{
+			eGEOMETRY			= 1<<0,
+			eMATERIAL			= 1<<1,
+			eSHAPE2BODY			= 1<<2,
+			eFILTERDATA			= 1<<3,
+			eCONTACTOFFSET		= 1<<4,
+			eRESTOFFSET			= 1<<5,
+			eFLAGS				= 1<<6,
+			eRESET_FILTERING	= 1<<7
+
+		};
+	};
+	typedef PxFlags<ShapeChangeNotifyFlag::Enum, PxU32> ShapeChangeNotifyFlags;
+	PX_FLAGS_OPERATORS(ShapeChangeNotifyFlag::Enum,PxU32)
+
+
+	class ShapeCore;
+
+	class RigidCore : public ActorCore
+	{
+	//= ATTENTION! =====================================================================================
+	// Changing the data layout of this class breaks the binary serialization format.  See comments for 
+	// PX_BINARY_SERIAL_VERSION.  If a modification is required, please adjust the getBinaryMetaData 
+	// function.  If the modification is made on a custom branch, please change PX_BINARY_SERIAL_VERSION
+	// accordingly.
+	//==================================================================================================
+	public:
+				PxActor*	getPxActor() const;
+				void		addShapeToScene(ShapeCore& shape);
+				void		removeShapeFromScene(ShapeCore& shape, bool wakeOnLostTouch);
+				void		onShapeChange(ShapeCore& shape, ShapeChangeNotifyFlags notifyFlags, PxShapeFlags newShapeFlags = PxShapeFlags(), bool forceBoundsUpdate = false);
+
+				RigidSim*	getSim() const;
+				PxU32		getRigidID() const;
+		static	void		getBinaryMetaData(PxOutputStream& stream);
+	protected:
+							RigidCore(const PxEMPTY) :	ActorCore(PxEmpty)	{}
+							RigidCore(PxActorType::Enum type);
+							~RigidCore();
+	};
+
+} // namespace Sc
+
+}
+
+#endif

physx/source/simulationcontroller/include/ScScene.h

@@ -0,0 +1,998 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef PX_PHYSICS_SCP_SCENE
+#define PX_PHYSICS_SCP_SCENE
+
+#include "CmPhysXCommon.h"
+#include "PxPhysXConfig.h"
+#include "PxScene.h"
+#include "PxSceneDesc.h"
+#include "PxSimulationEventCallback.h"
+#include "PsPool.h"
+#include "PsHashSet.h"
+#include "CmRenderOutput.h"
+#include "CmTask.h"
+#include "CmFlushPool.h"
+#include "CmPreallocatingPool.h"
+#include "CmBitMap.h"
+#include "ScIterators.h"
+#include "PxsMaterialManager.h"
+#include "PxvManager.h"
+#include "ScBodyCore.h"
+#include "PxArticulationBase.h"
+
+#define PX_MAX_DOMINANCE_GROUP 32
+
+class OverlapFilterTask;
+
+namespace physx
+{
+
+// PT: TODO: move INVALID_FILTER_PAIR_INDEX out of public API
+struct PxFilterInfo
+{
+	PX_FORCE_INLINE	PxFilterInfo()								:	filterFlags(0), pairFlags(0), filterPairIndex(INVALID_FILTER_PAIR_INDEX)			{}
+	PX_FORCE_INLINE	PxFilterInfo(PxFilterFlags filterFlags_)	:	filterFlags(filterFlags_), pairFlags(0), filterPairIndex(INVALID_FILTER_PAIR_INDEX)	{}
+
+	PxFilterFlags	filterFlags;
+	PxPairFlags		pairFlags;
+	PxU32			filterPairIndex;
+};
+
+struct PxTriggerPair;
+
+class PxsContext;
+class PxsIslandManager;
+class PxsSimulationController;
+class PxsSimulationControllerCallback;
+class PxsMemoryManager;
+
+#if PX_SUPPORT_GPU_PHYSX
+class PxsKernelWranglerManager;
+class PxsHeapMemoryAllocatorManager;
+#endif
+
+namespace IG
+{
+	class SimpleIslandManager;
+}
+
+class PxsCCDContext;
+
+namespace Cm
+{
+	class IDPool;
+}
+
+namespace Bp
+{
+	class AABBManager;
+	class BroadPhase;
+	class BoundsArray;
+}
+
+namespace Sq
+{
+	typedef PxU32 PrunerHandle;	// PT: we should get this from SqPruner.h but it cannot be included from here
+}
+
+namespace Dy
+{
+	class ArticulationV;
+	class Context;
+}
+
+namespace Sc
+{
+	class ActorSim;
+	class ElementSim;
+	class Interaction;
+
+	class ShapeCore;
+	class RigidCore;
+	class StaticCore;
+	class ConstraintCore;
+	class MaterialCore;
+	class ArticulationCore;
+	class ArticulationJointCore;
+	class LLArticulationPool;
+	class LLArticulationRCPool;
+	class BodyCore;
+
+	class NPhaseCore;
+	class Client;
+	class ConstraintInteraction;
+
+	class BodySim;
+	class ShapeSim;
+	class RigidSim;
+	class StaticSim;
+	class ConstraintSim;
+	struct ConstraintGroupNode;
+	class ConstraintProjectionManager;
+	struct TriggerPairExtraData;
+	class ObjectIDTracker;
+	class ActorPairReport;
+	class ContactStreamManager;
+	class SqBoundsManager;
+	class ShapeInteraction;
+	class ElementInteractionMarker;
+	class ArticulationSim;
+
+	class SimStats;
+
+	struct SimStateData;
+
+	struct BatchInsertionState
+	{
+		BodySim*  bodySim;
+		StaticSim*staticSim; 
+		ShapeSim* shapeSim;
+		ptrdiff_t staticActorOffset;
+		ptrdiff_t staticShapeTableOffset;
+		ptrdiff_t dynamicActorOffset;
+		ptrdiff_t dynamicShapeTableOffset;
+		ptrdiff_t shapeOffset;
+	};
+
+	struct BatchRemoveState
+	{
+		Ps::InlineArray<Sc::ShapeSim*, 64>				bufferedShapes;
+		Ps::InlineArray<const Sc::ShapeCore*, 64>		removedShapes;
+	};
+
+	struct InteractionType
+	{
+		enum Enum
+		{
+			eOVERLAP		= 0,		// corresponds to ShapeInteraction
+			eTRIGGER,					// corresponds to TriggerInteraction
+			eMARKER,					// corresponds to ElementInteractionMarker
+			eTRACKED_IN_SCENE_COUNT,	// not a real type, interactions above this limit are tracked in the scene
+			eCONSTRAINTSHADER,			// corresponds to ConstraintInteraction
+			eARTICULATION,				// corresponds to ArticulationJointSim
+
+			eINVALID
+		};
+	};
+
+	struct SceneInternalFlag
+	{
+		enum Enum
+		{
+			eSCENE_SIP_STATES_DIRTY_DOMINANCE		= (1<<1),
+			eSCENE_SIP_STATES_DIRTY_VISUALIZATION	= (1<<2),
+			eSCENE_DEFAULT							= 0
+		};
+	};
+
+	struct SimulationStage
+	{
+		enum Enum
+		{
+			eCOMPLETE,
+			eCOLLIDE,
+			eFETCHCOLLIDE,
+			eADVANCE,
+			eFETCHRESULT
+		};
+	};
+
+	// PT: TODO: revisit the need for a virtual interface
+	struct SqBoundsSync
+	{
+		virtual void sync(const Sq::PrunerHandle* handles, const PxU32* indices, const PxBounds3* bounds, PxU32 count, const Cm::BitMap& dirtyShapeSimMap) = 0;
+
+		virtual ~SqBoundsSync() {}
+	};
+
+	// PT: TODO: revisit the need for a virtual interface
+	struct SqRefFinder
+	{
+		virtual Sq::PrunerHandle find(const PxRigidBody* body, const PxShape* shape) = 0;
+
+		virtual ~SqRefFinder() {}
+	};
+
+	class Scene : public Ps::UserAllocated
+	{
+		struct SimpleBodyPair
+		{
+			BodySim* body1;
+			BodySim* body2;
+			PxU32 body1ID;
+			PxU32 body2ID;
+		};
+
+		PX_NOCOPY(Scene)
+
+		//---------------------------------------------------------------------------------
+		// External interface
+		//---------------------------------------------------------------------------------
+	public:    
+   
+					void						release();
+
+	PX_FORCE_INLINE	void						setGravity(const PxVec3& g)			{ mGravity = g;	mBodyGravityDirty = true;			}
+	PX_FORCE_INLINE	PxVec3						getGravity()				const	{ return mGravity;									}
+	PX_FORCE_INLINE void						setElapsedTime(const PxReal t)		{ mDt = t; mOneOverDt = t > 0.0f ? 1.0f/t : 0.0f;	}
+
+					void						setBounceThresholdVelocity(const PxReal t);
+					PxReal						getBounceThresholdVelocity() const;
+
+	PX_FORCE_INLINE	void						setPublicFlags(PxSceneFlags flags)	{ mPublicFlags = flags;						}
+	PX_FORCE_INLINE	PxSceneFlags				getPublicFlags()			const	{ return mPublicFlags;						}
+
+					void						setFrictionType(PxFrictionType::Enum model);
+					PxFrictionType::Enum 		getFrictionType() const;
+					void						setPCM(bool enabled);
+					void						setContactCache(bool enabled);
+
+					void						addStatic(StaticCore&, void*const *shapes, PxU32 nbShapes, size_t shapePtrOffset, PxBounds3* uninflatedBounds);
+					void						removeStatic(StaticCore&, Ps::InlineArray<const Sc::ShapeCore*,64>& removedShapes, bool wakeOnLostTouch);
+
+					void						addBody(BodyCore&, void*const *shapes, PxU32 nbShapes, size_t shapePtrOffset, PxBounds3* uninflatedBounds, bool compound);
+					void						removeBody(BodyCore&, Ps::InlineArray<const Sc::ShapeCore*,64>& removedShapes, bool wakeOnLostTouch);
+
+					// Batch insertion API.
+					// the bounds generated here are the uninflated bounds for the shapes, *if* they are trigger or sim shapes. 
+					// It's up to the caller to ensure the bounds array is big enough.
+					// Some care is required in handling these since sim and SQ tweak the bounds in slightly different ways.
+
+					void						startBatchInsertion(BatchInsertionState&);
+					void						addStatic(PxActor* actor, BatchInsertionState&, PxBounds3* outBounds);
+					void						addBody(PxActor* actor, BatchInsertionState&, PxBounds3* outBounds, bool compound);
+					void						finishBatchInsertion(BatchInsertionState&);
+
+					// Batch remove helpers
+	PX_FORCE_INLINE	void						setBatchRemove(BatchRemoveState* bs)	{ mBatchRemoveState = bs;	}
+	PX_FORCE_INLINE	BatchRemoveState*			getBatchRemove()		const			{ return mBatchRemoveState;	}
+					void						prefetchForRemove(const BodyCore& ) const;
+					void						prefetchForRemove(const StaticCore& ) const;
+
+					void						addConstraint(ConstraintCore&, RigidCore*, RigidCore*);
+					void						removeConstraint(ConstraintCore&);
+
+					void						addArticulation(ArticulationCore&, BodyCore& root);
+					void						removeArticulation(ArticulationCore&);
+
+					void						addArticulationJoint(ArticulationJointCore&, BodyCore& parent, BodyCore& child);
+					void						removeArticulationJoint(ArticulationJointCore&);
+
+					void						addArticulationSimControl(Sc::ArticulationCore& core);
+					void						removeArticulationSimControl(Sc::ArticulationCore& core);
+
+					PxU32						getNbArticulations() const;
+					Sc::ArticulationCore* const* getArticulations();
+	
+					PxU32						getNbConstraints() const;
+					Sc::ConstraintCore*const *	getConstraints();
+
+					void						initContactsIterator(ContactIterator&, PxsContactManagerOutputIterator&);
+
+		// Simulation events
+					void						setSimulationEventCallback(PxSimulationEventCallback* callback);
+					PxSimulationEventCallback*	getSimulationEventCallback() const;
+
+		// Contact modification
+					void						setContactModifyCallback(PxContactModifyCallback* callback);
+					PxContactModifyCallback*	getContactModifyCallback() const;
+					void						setCCDContactModifyCallback(PxCCDContactModifyCallback* callback);
+					PxCCDContactModifyCallback*	getCCDContactModifyCallback() const;
+
+					void						setCCDMaxPasses(PxU32 ccdMaxPasses);
+					PxU32						getCCDMaxPasses() const;
+
+		// Broad-phase callback
+	PX_FORCE_INLINE	void						setBroadPhaseCallback(PxBroadPhaseCallback* callback)	{ mBroadPhaseCallback = callback;	}
+	PX_FORCE_INLINE	PxBroadPhaseCallback*		getBroadPhaseCallback()	const							{ return mBroadPhaseCallback;		}
+
+		// Broad-phase management
+					void						finishBroadPhase(PxBaseTask* continuation);
+					void						finishBroadPhaseStage2(const PxU32 ccdPass);
+					void						preallocateContactManagers(PxBaseTask* continuation);
+
+					void						islandInsertion(PxBaseTask* continuation);
+					void						registerContactManagers(PxBaseTask* continuation);
+					void						registerInteractions(PxBaseTask* continuation);
+					void						registerSceneInteractions(PxBaseTask* continuation);
+
+					void						secondPassNarrowPhase(PxBaseTask* continuation);
+
+		// Groups
+					void						setDominanceGroupPair(PxDominanceGroup group1, PxDominanceGroup group2, const PxDominanceGroupPair& dominance);
+					PxDominanceGroupPair		getDominanceGroupPair(PxDominanceGroup group1, PxDominanceGroup group2) const;
+
+					void						setSolverBatchSize(PxU32 solverBatchSize);
+					PxU32						getSolverBatchSize() const;
+
+					void						setSolverArticBatchSize(PxU32 solverBatchSize);
+					PxU32						getSolverArticBatchSize() const;
+
+					void						setVisualizationParameter(PxVisualizationParameter::Enum param, PxReal value);
+					PxReal						getVisualizationParameter(PxVisualizationParameter::Enum param) const;
+
+					void						setVisualizationCullingBox(const PxBounds3& box);
+					const PxBounds3&			getVisualizationCullingBox() const;
+
+		// Run
+					void						simulate(PxReal timeStep, PxBaseTask* continuation);
+					void						advance(PxReal timeStep, PxBaseTask* continuation);
+					void						collide(PxReal timeStep, PxBaseTask* continuation);
+					void						endSimulation();
+					void						flush(bool sendPendingReports);
+					void						fireBrokenConstraintCallbacks();
+					void						fireTriggerCallbacks();
+					void						fireQueuedContactCallbacks(bool asPartOfFlush);
+					void						fireOnAdvanceCallback();
+
+					const Ps::Array<PxContactPairHeader>&
+												getQueuedContactPairHeaders();
+
+					bool						fireOutOfBoundsCallbacks();
+					void						prepareOutOfBoundsCallbacks();
+					void						postCallbacksPreSync();
+					void						postReportsCleanup();
+					void						fireCallbacksPostSync();
+					void						syncSceneQueryBounds(SqBoundsSync& sync, SqRefFinder& finder);					
+
+					PxU32						getDefaultContactReportStreamBufferSize() const;
+
+					PxReal						getFrictionOffsetThreshold() const;
+
+	PX_FORCE_INLINE	void						setLimits(const PxSceneLimits& limits)	{ mLimits = limits;	}
+	PX_FORCE_INLINE	const PxSceneLimits&		getLimits()						const	{ return mLimits;	}
+
+					void						visualizeStartStep();
+					void						visualizeEndStep();
+					Cm::RenderBuffer&			getRenderBuffer();
+
+					void						setNbContactDataBlocks(PxU32 blockCount);
+					PxU32						getNbContactDataBlocksUsed() const;
+					PxU32						getMaxNbContactDataBlocksUsed() const;
+					PxU32						getMaxNbConstraintDataBlocksUsed() const;
+
+					void						setScratchBlock(void* addr, PxU32 size);
+
+// PX_ENABLE_SIM_STATS
+					void						getStats(PxSimulationStatistics& stats) const;
+	PX_FORCE_INLINE	SimStats&					getStatsInternal() { return *mStats; }
+// PX_ENABLE_SIM_STATS
+
+					void						buildActiveActors();
+					void						buildActiveAndFrozenActors();
+					PxActor**					getActiveActors(PxU32& nbActorsOut);
+					void						setActiveActors(PxActor** actors, PxU32 nbActors);
+
+					PxActor**					getFrozenActors(PxU32& nbActorsOut);
+
+					void						finalizeContactStreamAndCreateHeader(PxContactPairHeader& header, 
+						const ActorPairReport& aPair, 
+						ContactStreamManager& cs, PxU32 removedShapeTestMask);
+
+					PxClientID					createClient();
+	PX_FORCE_INLINE	PxU32						getNbClients()	const	{ return mClients.size();	}
+
+					PxTaskManager*				getTaskManagerPtr() const { return mTaskManager; }
+					PxCudaContextManager*		getCudaContextManager() const { return mCudaContextManager; }
+
+					void						shiftOrigin(const PxVec3& shift);
+
+	PX_FORCE_INLINE	Ps::Pool<SimStateData>*		getSimStateDataPool() { return mSimStateDataPool; }
+
+		//---------------------------------------------------------------------------------
+		// Miscellaneous
+		//---------------------------------------------------------------------------------							
+		//internal public methods:
+	public:
+												Scene(const PxSceneDesc& desc, PxU64 contextID);
+												~Scene() {}	//use release() plz.
+
+					void						preAllocate(PxU32 nbStatics, PxU32 nbBodies, PxU32 nbStaticShapes, PxU32 nbDynamicShapes);
+
+	PX_FORCE_INLINE	PxsContext*					getLowLevelContext() { return mLLContext; }
+	PX_FORCE_INLINE const PxsContext*			getLowLevelContext() const { return mLLContext; }
+
+	PX_FORCE_INLINE	Dy::Context*				getDynamicsContext() { return mDynamicsContext; }
+	PX_FORCE_INLINE const Dy::Context*			getDynamicsContext() const { return mDynamicsContext; }
+
+	PX_FORCE_INLINE	PxsSimulationController*	getSimulationController() { return mSimulationController; }
+	PX_FORCE_INLINE	const PxsSimulationController*	getSimulationController() const { return mSimulationController; }
+
+	PX_FORCE_INLINE	Bp::AABBManager*			getAABBManager()			{ return mAABBManager;	}
+	PX_FORCE_INLINE const Bp::AABBManager*		getAABBManager()	const	{ return mAABBManager;	}
+	PX_FORCE_INLINE Ps::Array<BodySim*>&		getCcdBodies()				{ return mCcdBodies;	}
+
+	/*PX_FORCE_INLINE	PxsIslandManager*			getIslandManager()	{ return mIslandManager; }
+	PX_FORCE_INLINE	const PxsIslandManager*		getIslandManager() const { return mIslandManager; }*/
+
+	PX_FORCE_INLINE	IG::SimpleIslandManager*	getSimpleIslandManager()	{ return mSimpleIslandManager; }
+	PX_FORCE_INLINE	const IG::SimpleIslandManager*		getSimpleIslandManager() const { return mSimpleIslandManager; }
+
+	PX_FORCE_INLINE Sc::SimulationStage::Enum	getSimulationStage() const { return mSimulationStage; }
+	PX_FORCE_INLINE void						setSimulationStage(Sc::SimulationStage::Enum stage) { mSimulationStage = stage; }
+
+					void						addShape_(RigidSim&, ShapeCore&);
+					void						removeShape_(ShapeSim&, bool wakeOnLostTouch);
+
+					void						registerShapeInNphase(const ShapeCore& shapeCore);
+					void						unregisterShapeFromNphase(const ShapeCore& shapeCore);
+
+					void						notifyNphaseOnUpdateShapeMaterial(const ShapeCore& shapeCore);
+
+	// Get an array of the active actors.
+	PX_FORCE_INLINE	BodyCore*const*				getActiveBodiesArray() const { return mActiveBodies.begin(); }
+	PX_FORCE_INLINE	PxU32						getNumActiveBodies() const { return mActiveBodies.size(); }
+
+	PX_FORCE_INLINE	BodyCore*const*				getActiveCompoundBodiesArray() const { return mActiveCompoundBodies.begin(); }
+	PX_FORCE_INLINE	PxU32						getNumActiveCompoundBodies() const { return mActiveCompoundBodies.size(); }
+
+	PX_FORCE_INLINE	PxU32						getNbInteractions(InteractionType::Enum type)		const	{ return mInteractions[type].size();	}
+	PX_FORCE_INLINE	PxU32						getNbActiveInteractions(InteractionType::Enum type)	const	{ return mActiveInteractionCount[type];	}
+	// Get all interactions of a certain type
+	PX_FORCE_INLINE	Interaction**				getInteractions(InteractionType::Enum type)					{ return mInteractions[type].begin();	}
+	PX_FORCE_INLINE	Interaction**				getActiveInteractions(InteractionType::Enum type)			{ return mInteractions[type].begin();	}
+
+					void						registerInteraction(Interaction* interaction, bool active);
+					void						unregisterInteraction(Interaction* interaction);
+
+					void						notifyInteractionActivated(Interaction* interaction);
+					void						notifyInteractionDeactivated(Interaction* interaction);
+
+	// for pool management of interaction arrays, a major cause of dynamic allocation
+					void**						allocatePointerBlock(PxU32 size);
+					void						deallocatePointerBlock(void**, PxU32 size);
+	private:
+	// Get the number of active one-way dominator actors
+	PX_FORCE_INLINE	PxU32						getActiveKinematicBodiesCount() const { return mActiveKinematicBodyCount; }
+
+	// Get an iterator to the active one-way dominator actors
+	PX_FORCE_INLINE	BodyCore*const*				getActiveKinematicBodies() const { return mActiveBodies.begin(); }
+
+	// Get the number of active non-kinematic actors
+	PX_FORCE_INLINE	PxU32						getActiveDynamicBodiesCount() const { return mActiveBodies.size() - mActiveKinematicBodyCount; }
+
+	// Get the active non-kinematic actors
+	PX_FORCE_INLINE	BodyCore*const*				getActiveDynamicBodies() const { return mActiveBodies.begin() + mActiveKinematicBodyCount; }
+
+					void						swapInteractionArrayIndices(PxU32 id1, PxU32 id2, InteractionType::Enum type);
+	public:
+
+		PX_FORCE_INLINE Cm::BitMap&				getDirtyShapeSimMap() { return mDirtyShapeSimMap; }
+
+					void						addToActiveBodyList(BodySim& actor);
+					void						addToActiveCompoundBodyList(BodySim& actor);
+					void						removeFromActiveBodyList(BodySim& actor);
+					void						removeFromActiveCompoundBodyList(BodySim& actor);
+					void						swapInActiveBodyList(BodySim& body); // call when an active body gets switched from dynamic to kinematic or vice versa
+
+					void						addToFrozenBodyList(BodySim& actor);
+					void						removeFromFrozenBodyList(BodySim& actor);
+					void						addBrokenConstraint(Sc::ConstraintCore*);
+					void						addActiveBreakableConstraint(Sc::ConstraintSim*, ConstraintInteraction*);
+					void						removeActiveBreakableConstraint(Sc::ConstraintSim*);
+		//the Actor should register its top level shapes with these.
+					void						removeBody(BodySim&);
+
+					void						raiseSceneFlag(SceneInternalFlag::Enum flag) { mInternalFlags |= flag; }
+
+					//lists of actors woken up or put to sleep last simulate
+					void                        onBodyWakeUp(BodySim* body);
+					void                        onBodySleep(BodySim* body);
+
+	PX_FORCE_INLINE	bool						isValid() const	{ return (mLLContext != NULL);	}
+
+					void						addToLostTouchList(BodySim* body1, BodySim* body2);
+
+					void						initDominanceMatrix();
+					
+					void						setCreateContactReports(bool s);
+
+					PxU32						createAggregate(void* userData, bool selfCollisions);
+					void						deleteAggregate(PxU32 id);
+
+					Dy::ArticulationV*			createLLArticulation(Sc::ArticulationSim* sim);
+					void						destroyLLArticulation(Dy::ArticulationV&);
+
+
+					Ps::Pool<ConstraintInteraction>*
+													getConstraintInteractionPool()			const	{ return mConstraintInteractionPool;	}
+	public:
+						PxBroadPhaseType::Enum		getBroadPhaseType()																				const;
+						bool						getBroadPhaseCaps(PxBroadPhaseCaps& caps)														const;
+						PxU32						getNbBroadPhaseRegions()																		const;
+						PxU32						getBroadPhaseRegions(PxBroadPhaseRegionInfo* userBuffer, PxU32 bufferSize, PxU32 startIndex)	const;
+						PxU32						addBroadPhaseRegion(const PxBroadPhaseRegion& region, bool populateRegion);
+						bool						removeBroadPhaseRegion(PxU32 handle);
+						void**						getOutOfBoundsAggregates();
+						PxU32						getNbOutOfBoundsAggregates();
+						void						clearOutOfBoundsAggregates();
+
+		PX_FORCE_INLINE	const PxsMaterialManager&	getMaterialManager()					const	{ return mMaterialManager;				}
+		PX_FORCE_INLINE	PxsMaterialManager&			getMaterialManager()							{ return mMaterialManager;				}
+
+		//material management functions to be called from Scb::Scene
+						void						registerMaterialInNP(const PxsMaterialCore& materialCore);
+						void						updateMaterialInNP(const PxsMaterialCore& materialCore);
+						void						unregisterMaterialInNP(const PxsMaterialCore& materialCore);
+
+		// Collision filtering
+						void						setFilterShaderData(const void* data, PxU32 dataSize);
+		PX_FORCE_INLINE	const void*					getFilterShaderDataFast()				const	{ return mFilterShaderData;				}
+		PX_FORCE_INLINE	PxU32						getFilterShaderDataSizeFast()			const	{ return mFilterShaderDataSize;			}
+		PX_FORCE_INLINE	PxSimulationFilterShader	getFilterShaderFast()					const	{ return mFilterShader;					}
+		PX_FORCE_INLINE	PxSimulationFilterCallback*	getFilterCallbackFast()					const	{ return mFilterCallback;				}
+		PX_FORCE_INLINE	PxPairFilteringMode::Enum	getKineKineFilteringMode()				const	{ return mKineKineFilteringMode;		}
+		PX_FORCE_INLINE	PxPairFilteringMode::Enum	getStaticKineFilteringMode()			const	{ return mStaticKineFilteringMode;		}
+
+		PX_FORCE_INLINE	PxU32						getTimeStamp()							const	{ return mTimeStamp;					}
+		PX_FORCE_INLINE	PxU32						getReportShapePairTimeStamp()			const	{ return mReportShapePairTimeStamp;		}
+
+		PX_FORCE_INLINE	PxReal						getOneOverDt()							const	{ return mOneOverDt;					}
+		PX_FORCE_INLINE	PxReal						getDt()									const	{ return mDt;							}
+
+		PX_FORCE_INLINE	const PxVec3&				getGravityFast()						const	{ return mGravity;						}
+		PX_FORCE_INLINE	bool						readFlag(SceneInternalFlag::Enum flag)	const	{ return (mInternalFlags & flag) != 0;	}
+		PX_FORCE_INLINE	bool						readPublicFlag(PxSceneFlag::Enum flag)	const	{ return (mPublicFlags & flag);			}
+
+		PX_FORCE_INLINE	NPhaseCore*					getNPhaseCore()							const	{ return mNPhaseCore;					}
+
+						void						checkConstraintBreakage();
+
+		PX_FORCE_INLINE	Ps::Array<TriggerPairExtraData>&		
+													getTriggerBufferExtraData()						{ return *mTriggerBufferExtraData;		}
+		PX_FORCE_INLINE	Ps::Array<PxTriggerPair>&	getTriggerBufferAPI()							{ return mTriggerBufferAPI;				}
+						void						reserveTriggerReportBufferSpace(const PxU32 pairCount, PxTriggerPair*& triggerPairBuffer, TriggerPairExtraData*& triggerPairExtraBuffer);
+
+		PX_FORCE_INLINE	ObjectIDTracker&			getRigidIDTracker()								{ return *mRigidIDTracker;				}
+		PX_FORCE_INLINE	ObjectIDTracker&			getShapeIDTracker()								{ return *mShapeIDTracker;				}
+
+		PX_FORCE_INLINE	void						markReleasedBodyIDForLostTouch(PxU32 id)		{ mLostTouchPairsDeletedBodyIDs.growAndSet(id); }
+						void						resizeReleasedBodyIDMaps(PxU32 maxActors, PxU32 numActors);
+
+		PX_FORCE_INLINE	StaticSim&					getStaticAnchor()								{ return *mStaticAnchor;				}
+
+		PX_FORCE_INLINE	ConstraintProjectionManager& getProjectionManager()							{ return *mProjectionManager;			}
+
+		PX_FORCE_INLINE Bp::BoundsArray&			getBoundsArray()						const	{ return *mBoundsArray; }
+		PX_FORCE_INLINE void						updateContactDistance(PxU32 idx, PxReal distance)	{ (*mContactDistance)[idx] = distance; mHasContactDistanceChanged = true; }
+		PX_FORCE_INLINE SqBoundsManager&			getSqBoundsManager()					const	{ return *mSqBoundsManager; }
+
+		PX_FORCE_INLINE	PxReal						getVisualizationScale()					const	{ return mVisualizationScale;			}  // This is actually redundant but makes checks whether debug visualization is enabled faster
+
+		PX_FORCE_INLINE BodyCore* const*			getSleepBodiesArray(PxU32& count)				{ count = mSleepBodies.size(); return mSleepBodies.getEntries(); }
+
+		PX_FORCE_INLINE PxTaskManager&				getTaskManager()						const	{ PX_ASSERT(mTaskManager); return *mTaskManager; }
+
+		Cm::FlushPool*								getFlushPool();
+	
+		PX_FORCE_INLINE bool						getStabilizationEnabled()				const	{ return mEnableStabilization; }
+
+		PX_FORCE_INLINE void						setPostSolverVelocityNeeded()					{ mContactReportsNeedPostSolverVelocity = true; }
+
+									ObjectIDTracker&	getConstraintIDTracker() { return *mConstraintIDTracker; }
+
+
+									void*			allocateConstraintBlock(PxU32 size);
+									void			deallocateConstraintBlock(void* addr, PxU32 size);
+
+		PX_INLINE					 ObjectIDTracker&	getElementIDPool() { return *mElementIDPool; }
+
+		PX_FORCE_INLINE Cm::BitMap&					getVelocityModifyMap() { return mVelocityModifyMap; }
+
+					void							stepSetupCollide(PxBaseTask* continuation);//This is very important to guarantee thread safty in the collide
+		PX_FORCE_INLINE void						addToPosePreviewList(BodySim& b)				{ PX_ASSERT(!mPosePreviewBodies.contains(&b)); mPosePreviewBodies.insert(&b); }
+		PX_FORCE_INLINE void						removeFromPosePreviewList(BodySim& b)			{ PX_ASSERT(mPosePreviewBodies.contains(&b)); mPosePreviewBodies.erase(&b); }
+#if PX_DEBUG
+		PX_FORCE_INLINE bool						isInPosePreviewList(BodySim& b)			const	{ return mPosePreviewBodies.contains(&b); }
+#endif
+
+		PX_FORCE_INLINE	void						setSpeculativeCCDRigidBody(const PxU32 index) { mSpeculativeCCDRigidBodyBitMap.growAndSet(index); }
+		PX_FORCE_INLINE void						resetSpeculativeCCDRigidBody(const PxU32 index) { if(index < mSpeculativeCCDRigidBodyBitMap.size()) mSpeculativeCCDRigidBodyBitMap.reset(index); }
+
+		PX_FORCE_INLINE	void						setSpeculativeCCDArticulationLink(const PxU32 index) { mSpeculativeCDDArticulationBitMap.growAndSet(index); }
+		PX_FORCE_INLINE void						resetSpeculativeCCDArticulationLink(const PxU32 index) { if(index < mSpeculativeCDDArticulationBitMap.size()) mSpeculativeCDDArticulationBitMap.reset(index); }
+
+		PX_FORCE_INLINE	PxU64						getContextId() const { return mContextId; }
+		PX_FORCE_INLINE bool						isUsingGpuRigidBodies() const { return mUseGpuRigidBodies; }
+
+		// statistics counters increase/decrease
+		PX_FORCE_INLINE	void						increaseNumKinematicsCounter() { mNbRigidKinematic++; }
+		PX_FORCE_INLINE	void						decreaseNumKinematicsCounter() { mNbRigidKinematic--; }
+		PX_FORCE_INLINE	void						increaseNumDynamicsCounter() { mNbRigidDynamics++; }
+		PX_FORCE_INLINE	void						decreaseNumDynamicsCounter() { mNbRigidDynamics--; }
+
+		//internal private methods:
+	private:
+					void						releaseConstraints(bool endOfScene);
+		PX_INLINE	void						clearBrokenConstraintBuffer();
+
+		/////////////////////////////////////////////////////////////
+
+					void						prepareCollide();
+
+					void						collideStep(PxBaseTask* continuation);
+					void						advanceStep(PxBaseTask* continuation);
+
+		// subroutines of collideStep/solveStep:
+					void						kinematicsSetup(PxBaseTask* continuation);
+					void						stepSetupSolve(PxBaseTask* continuation);
+					//void						stepSetupSimulate();
+
+					void						fetchPatchEvents(PxBaseTask*);
+					void						processNarrowPhaseTouchEvents();
+					void						processNarrowPhaseTouchEventsStage2(PxBaseTask*);
+					void						setEdgesConnected(PxBaseTask*);
+					void						processNarrowPhaseLostTouchEvents(PxBaseTask*);
+					void						processNarrowPhaseLostTouchEventsIslands(PxBaseTask*);
+					void						processLostTouchPairs();
+					void						integrateKinematicPose();
+					void						updateKinematicCached(PxBaseTask* task);
+
+					void						beforeSolver(PxBaseTask* continuation);
+					void						checkForceThresholdContactEvents(const PxU32 ccdPass);
+					void						processCallbacks(bool pendingReportsOnly = false);
+					void						endStep();
+	private:
+		PX_FORCE_INLINE void					putObjectsToSleep(PxU32 infoFlag);
+		PX_FORCE_INLINE void					putInteractionsToSleep();
+		PX_FORCE_INLINE void					wakeObjectsUp(PxU32 infoFlag);
+
+					void						collectPostSolverVelocitiesBeforeCCD();
+
+					void						clearSleepWakeBodies(void);
+		PX_INLINE	void						cleanUpSleepBodies();
+		PX_INLINE	void						cleanUpWokenBodies();
+		PX_INLINE	void						cleanUpSleepOrWokenBodies(Ps::CoalescedHashSet<BodyCore*>& bodyList, PxU32 removeFlag, bool& validMarker);
+
+		//internal variables:
+	private:
+		// Material manager
+					PX_ALIGN(16, PxsMaterialManager			mMaterialManager);
+
+					PxU64						mContextId;
+
+					Ps::Array<BodyCore*>		mActiveBodies;  // Sorted: kinematic before dynamic
+					PxU32						mActiveKinematicBodyCount;  // Number of active kinematics. This is also the index in mActiveBodies where the active dynamic bodies start.
+					Ps::Array<BodyCore*>		mActiveCompoundBodies;  
+
+					Ps::Array<Interaction*>		mInteractions[InteractionType::eTRACKED_IN_SCENE_COUNT];
+					PxU32						mActiveInteractionCount[InteractionType::eTRACKED_IN_SCENE_COUNT]; // Interactions with id < activeInteractionCount are active
+
+					template <typename T, PxU32 size>
+					struct Block
+					{
+						PxU8 mem[sizeof(T)*size];
+						Block() {}	// get around VS warning C4345, otherwise useless
+					};
+
+					typedef Block<void*, 8>	PointerBlock8;
+					typedef Block<void*, 16> PointerBlock16;
+					typedef Block<void*, 32> PointerBlock32;
+
+					Ps::Pool<PointerBlock8>		mPointerBlock8Pool;
+					Ps::Pool<PointerBlock16>	mPointerBlock16Pool;
+					Ps::Pool<PointerBlock32>	mPointerBlock32Pool;
+
+					PxsContext*					mLLContext;
+
+					Bp::AABBManager*			mAABBManager;
+					Bp::BroadPhase*				mBP;
+					PxsCCDContext*				mCCDContext;
+					PxI32						mNumFastMovingShapes;
+					PxU32						mCCDPass;
+
+					//PxsIslandManager*			mIslandManager;
+
+					IG::SimpleIslandManager*	mSimpleIslandManager;
+
+					Dy::Context*				mDynamicsContext;
+
+
+					PxsMemoryManager*			mMemoryManager;
+
+#if PX_SUPPORT_GPU_PHYSX
+					PxsKernelWranglerManager*				mGpuWranglerManagers;
+					PxsHeapMemoryAllocatorManager*			mHeapMemoryAllocationManager;
+#endif
+
+					PxsSimulationController*	mSimulationController;
+
+					PxsSimulationControllerCallback*	mSimulationControllerCallback;
+
+					PxSceneLimits				mLimits;
+
+					PxVec3						mGravity;			//!< Gravity vector
+					PxU32						mBodyGravityDirty; // Set to true before body->updateForces() when the mGravity has changed					
+
+					Ps::Array<PxContactPairHeader>
+												mQueuedContactPairHeaders;
+		//time:
+		//constants set with setTiming():
+					PxReal						mDt;						//delta time for current step.
+					PxReal						mOneOverDt;					//inverse of dt.
+		//stepping / counters:
+					PxU32						mTimeStamp;					//Counts number of steps.
+					PxU32						mReportShapePairTimeStamp;	//Timestamp for refreshing the shape pair report structure. Updated before delayed shape/actor deletion and before CCD passes.
+		//containers:
+		// Those ones contain shape ptrs from Actor, i.e. compound level, not subparts
+
+					Ps::CoalescedHashSet<Sc::ConstraintCore*> 
+												mConstraints;
+												
+					Sc::ConstraintProjectionManager*				mProjectionManager;
+					Bp::BoundsArray*								mBoundsArray;
+					Ps::Array<PxReal, Ps::VirtualAllocator>*		mContactDistance;
+					bool											mHasContactDistanceChanged;
+					SqBoundsManager*								mSqBoundsManager;
+
+					Ps::Array<BodySim*>			mCcdBodies;
+					Ps::Array<BodySim*>			mProjectedBodies;
+					Ps::Array<PxTriggerPair>	mTriggerBufferAPI;
+					Ps::Array<TriggerPairExtraData>*		
+												mTriggerBufferExtraData;
+
+					PxU32						mRemovedShapeCountAtSimStart;  // counter used to detect whether there have been buffered shape removals
+
+					Ps::CoalescedHashSet<ArticulationCore*> mArticulations;
+
+					Ps::Array<Sc::ConstraintCore*>	mBrokenConstraints;
+					Ps::CoalescedHashSet<Sc::ConstraintSim*> mActiveBreakableConstraints;
+
+					// pools for joint buffers
+					// Fixed joint is 92 bytes, D6 is 364 bytes right now. So these three pools cover all the internal cases
+					typedef Block<PxU8, 128> MemBlock128;
+					typedef Block<PxU8, 256> MemBlock256;
+					typedef Block<PxU8, 384> MemBlock384;
+
+					Ps::Pool2<MemBlock128, 8192>	mMemBlock128Pool;
+					Ps::Pool2<MemBlock256, 8192>	mMemBlock256Pool;
+					Ps::Pool2<MemBlock384, 8192>	mMemBlock384Pool;
+
+
+					// broad phase data:
+					NPhaseCore*					mNPhaseCore;
+
+					// Collision filtering
+					void*						mFilterShaderData;
+					PxU32						mFilterShaderDataSize;
+					PxU32						mFilterShaderDataCapacity;
+					PxSimulationFilterShader	mFilterShader;
+					PxSimulationFilterCallback*	mFilterCallback;
+
+			const	PxPairFilteringMode::Enum	mKineKineFilteringMode;
+			const	PxPairFilteringMode::Enum	mStaticKineFilteringMode;
+
+					Ps::CoalescedHashSet<BodyCore*> mSleepBodies;
+					Ps::CoalescedHashSet<BodyCore*> mWokeBodies;
+					bool						mWokeBodyListValid;
+					bool						mSleepBodyListValid;
+			const	bool						mEnableStabilization;
+					Ps::Array<Client*>			mClients;	//an array of transform arrays, one for each client.
+
+						Ps::Array<PxActor*>				mActiveActors;
+						Ps::Array<PxActor*>				mFrozenActors;
+
+						Ps::Array<const PxRigidBody*>	mClientPosePreviewBodies;	// buffer for bodies that requested early report of the integrated pose (eENABLE_POSE_INTEGRATION_PREVIEW).
+																			// This buffer gets exposed to users. Is officially accessible from PxSimulationEventCallback::onAdvance()
+																			// until the next simulate()/advance().
+						Ps::Array<PxTransform>			mClientPosePreviewBuffer;	// buffer of newly integrated poses for the bodies that requested a preview. This buffer gets exposed
+																			// to users.
+
+						PxSimulationEventCallback*	mSimulationEventCallback;
+						PxBroadPhaseCallback*		mBroadPhaseCallback;
+
+					SimStats*					mStats;
+					PxU32						mInternalFlags;	//!< Combination of ::SceneFlag
+					PxSceneFlags				mPublicFlags;	//copy of PxSceneDesc::flags, of type PxSceneFlag
+
+					ObjectIDTracker*			mConstraintIDTracker;
+					ObjectIDTracker*			mShapeIDTracker;
+					ObjectIDTracker*			mRigidIDTracker;
+					ObjectIDTracker*			mElementIDPool;
+
+					StaticSim*					mStaticAnchor;
+
+					Cm::PreallocatingPool<ShapeSim>*	mShapeSimPool;
+					Cm::PreallocatingPool<StaticSim>*	mStaticSimPool;
+					Cm::PreallocatingPool<BodySim>*		mBodySimPool;
+					Ps::Pool<ConstraintSim>*			mConstraintSimPool;
+					LLArticulationPool*					mLLArticulationPool;
+					LLArticulationRCPool*				mLLArticulationRCPool;
+														
+					Ps::Pool<ConstraintInteraction>*
+												mConstraintInteractionPool;
+
+					Ps::Pool<SimStateData>*		mSimStateDataPool;
+
+					BatchRemoveState*			mBatchRemoveState;
+
+					Ps::Array<SimpleBodyPair>	mLostTouchPairs;
+					Cm::BitMap					mLostTouchPairsDeletedBodyIDs;	// Need to know which bodies have been deleted when processing the lost touch pair list.
+																				// Can't use the existing rigid object ID tracker class since this map needs to be cleared at
+																				// another point in time.
+
+					Cm::BitMap					mVelocityModifyMap;
+
+					Ps::Array<PxvContactManagerTouchEvent> mTouchFoundEvents;
+					Ps::Array<PxvContactManagerTouchEvent> mTouchLostEvents;
+
+					Ps::Array<PxsContactManager*> mFoundPatchManagers;
+					Ps::Array<PxsContactManager*> mLostPatchManagers;
+
+					Ps::Array<PxU32>			mOutOfBoundsIDs;
+
+					Cm::BitMap					mDirtyShapeSimMap;
+
+					PxU32						mDominanceBitMatrix[PX_MAX_DOMINANCE_GROUP];
+
+					PxReal						mVisualizationScale;  // Redundant but makes checks whether debug visualization is enabled faster
+
+					bool						mVisualizationParameterChanged;
+
+					// statics:
+					PxU32						mNbRigidStatics;
+					PxU32						mNbRigidDynamics;
+					PxU32						mNbRigidKinematic;
+					PxU32						mNbGeometries[PxGeometryType::eGEOMETRY_COUNT];
+
+					PxU32						mNumDeactivatingNodes[2];
+
+					// task decomposition
+					void						preBroadPhase(PxBaseTask* continuation);
+					void						broadPhase(PxBaseTask* continuation);
+					void						postBroadPhase(PxBaseTask* continuation);
+					void						postBroadPhaseContinuation(PxBaseTask* continuation);
+					void						preRigidBodyNarrowPhase(PxBaseTask* continuation);
+					void						postBroadPhaseStage2(PxBaseTask* continuation);
+					void						postBroadPhaseStage3(PxBaseTask* continuation);
+					void						rigidBodyNarrowPhase(PxBaseTask* continuation);
+					void						unblockNarrowPhase(PxBaseTask* continuation);
+					void						islandGen(PxBaseTask* continuation);
+					void						processLostSolverPatches(PxBaseTask* continuation);
+					void						postIslandGen(PxBaseTask* continuation);
+					void						processTriggerInteractions(PxBaseTask* continuation);
+					void						solver(PxBaseTask* continuation);
+					void						updateBodiesAndShapes(PxBaseTask* continuation);
+					void						updateSimulationController(PxBaseTask* continuation);
+					void						updateDynamics(PxBaseTask* continuation);
+					void						processLostContacts(PxBaseTask*);
+					void						processLostContacts2(PxBaseTask*);
+					void						processLostContacts3(PxBaseTask*);
+					void						destroyManagers(PxBaseTask*);
+					void						lostTouchReports(PxBaseTask*);
+					void						unregisterInteractions(PxBaseTask*);
+					void						postThirdPassIslandGen(PxBaseTask*);
+					void						postSolver(PxBaseTask* continuation);
+					void						constraintProjection(PxBaseTask* continuation);
+					void						afterIntegration(PxBaseTask* continuation);  // performs sleep check, for instance
+					void						postCCDPass(PxBaseTask* continuation);
+					void						ccdBroadPhaseAABB(PxBaseTask* continuation);
+					void						ccdBroadPhase(PxBaseTask* continuation);
+					void						updateCCDMultiPass(PxBaseTask* continuation);
+					void						updateCCDSinglePass(PxBaseTask* continuation);
+					void						updateCCDSinglePassStage2(PxBaseTask* continuation);
+					void						updateCCDSinglePassStage3(PxBaseTask* continuation);
+					void						finalizationPhase(PxBaseTask* continuation);
+
+					void						postNarrowPhase(PxBaseTask* continuation);
+
+					void						addShapes(void *const* shapes, PxU32 nbShapes, size_t ptrOffset, RigidSim& sim, PxBounds3* outBounds);
+					void						removeShapes(RigidSim& , Ps::InlineArray<Sc::ShapeSim*, 64>& , Ps::InlineArray<const Sc::ShapeCore*, 64>&, bool wakeOnLostTouch);
+
+
+	private:
+
+					void						addShapes(void *const* shapes, PxU32 nbShapes, size_t ptrOffset, RigidSim& sim, ShapeSim*& prefetchedShapeSim, PxBounds3* outBounds);
+
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::secondPassNarrowPhase>			mSecondPassNarrowPhase;
+					Cm::DelegateFanoutTask<Sc::Scene, &Sc::Scene::postNarrowPhase>			mPostNarrowPhase;
+					Cm::DelegateFanoutTask<Sc::Scene, &Sc::Scene::finalizationPhase>	mFinalizationPhase;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::updateCCDMultiPass>			mUpdateCCDMultiPass;
+
+					//multi-pass ccd stuff
+					Ps::Array<Cm::DelegateTask<Sc::Scene, &Sc::Scene::updateCCDSinglePass> >	mUpdateCCDSinglePass;
+					Ps::Array<Cm::DelegateTask<Sc::Scene, &Sc::Scene::updateCCDSinglePassStage2> >	mUpdateCCDSinglePass2;
+					Ps::Array<Cm::DelegateTask<Sc::Scene, &Sc::Scene::updateCCDSinglePassStage3> >	mUpdateCCDSinglePass3;
+					Ps::Array<Cm::DelegateTask<Sc::Scene, &Sc::Scene::ccdBroadPhaseAABB> >			mCCDBroadPhaseAABB;
+					Ps::Array<Cm::DelegateTask<Sc::Scene, &Sc::Scene::ccdBroadPhase> >			mCCDBroadPhase;
+					Ps::Array<Cm::DelegateTask<Sc::Scene, &Sc::Scene::postCCDPass> >			mPostCCDPass;
+
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::afterIntegration>				mAfterIntegration;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::constraintProjection>			mConstraintProjection;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::postSolver>						mPostSolver;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::solver>							mSolver;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::updateBodiesAndShapes>			mUpdateBodiesAndShapes;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::updateSimulationController>		mUpdateSimulationController;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::updateDynamics>					mUpdateDynamics;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::processLostContacts>			mProcessLostContactsTask;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::processLostContacts2>			mProcessLostContactsTask2;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::processLostContacts3>			mProcessLostContactsTask3;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::destroyManagers>				mDestroyManagersTask;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::lostTouchReports>				mLostTouchReportsTask;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::unregisterInteractions>			mUnregisterInteractionsTask;
+					Cm::DelegateTask<Sc::Scene,
+						&Sc::Scene::processNarrowPhaseLostTouchEventsIslands>				mProcessNarrowPhaseLostTouchTasks;
+					Cm::DelegateTask<Sc::Scene,
+						&Sc::Scene::processNarrowPhaseLostTouchEvents>						mProcessNPLostTouchEvents;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::postThirdPassIslandGen>			mPostThirdPassIslandGenTask;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::postIslandGen>					mPostIslandGen;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::islandGen>						mIslandGen;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::preRigidBodyNarrowPhase>		mPreRigidBodyNarrowPhase;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::setEdgesConnected>				mSetEdgesConnectedTask;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::fetchPatchEvents>				mFetchPatchEventsTask;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::processLostSolverPatches>		mProcessLostPatchesTask;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::rigidBodyNarrowPhase>			mRigidBodyNarrowPhase;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::unblockNarrowPhase>				mRigidBodyNPhaseUnlock;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::postBroadPhase>					mPostBroadPhase;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::postBroadPhaseContinuation>		mPostBroadPhaseCont;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::postBroadPhaseStage2>			mPostBroadPhase2;
+					Cm::DelegateFanoutTask<Sc::Scene, &Sc::Scene::postBroadPhaseStage3>		mPostBroadPhase3;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::preallocateContactManagers>		mPreallocateContactManagers;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::islandInsertion>				mIslandInsertion;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::registerContactManagers>		mRegisterContactManagers;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::registerInteractions>			mRegisterInteractions;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::registerSceneInteractions>		mRegisterSceneInteractions;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::broadPhase>						mBroadPhase;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::advanceStep>					mAdvanceStep;
+					Cm::DelegateTask<Sc::Scene, &Sc::Scene::collideStep>					mCollideStep;
+
+					Cm::FlushPool															mTaskPool;
+					PxTaskManager*															mTaskManager;
+					PxCudaContextManager*													mCudaContextManager;
+
+					bool																	mContactReportsNeedPostSolverVelocity;
+					bool																	mUseGpuRigidBodies;
+
+					SimulationStage::Enum													mSimulationStage;
+
+					ConstraintGroupNode**													mTmpConstraintGroupRootBuffer;  // temporary list of constraint group roots, used for constraint projection
+
+					Ps::CoalescedHashSet<const BodySim*>									mPosePreviewBodies;  // list of bodies that requested early report of the integrated pose (eENABLE_POSE_INTEGRATION_PREVIEW).
+
+					Ps::Array<PxsContactManager*>											mPreallocatedContactManagers;
+					Ps::Array<ShapeInteraction*>											mPreallocatedShapeInteractions;
+					Ps::Array<ElementInteractionMarker*>									mPreallocatedInteractionMarkers;
+
+					OverlapFilterTask*														mOverlapFilterTaskHead;
+					Ps::Array<PxFilterInfo>													mFilterInfo;
+					Cm::BitMap																mSpeculativeCCDRigidBodyBitMap;
+					Cm::BitMap																mSpeculativeCDDArticulationBitMap;
+	};
+
+	bool	activateInteraction(Sc::Interaction* interaction, void* data);
+	bool	deactivateInteraction(Sc::Interaction* interaction);
+
+} // namespace Sc
+
+}
+
+#endif

physx/source/simulationcontroller/include/ScShapeCore.h

@@ -0,0 +1,138 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef PX_PHYSICS_SCP_SHAPECORE
+#define PX_PHYSICS_SCP_SHAPECORE
+
+#include "PsUserAllocated.h"
+#include "GuGeometryUnion.h"
+#include "PxvGeometry.h"
+#include "PsUtilities.h"
+#include "PxFiltering.h"
+#include "PxShape.h"
+
+namespace physx
+{
+class PxShape;
+
+namespace Sc
+{
+	class Scene;
+	class RigidCore;
+	class BodyCore;
+	class ShapeSim;
+	class MaterialCore;
+
+	class ShapeCore : public Ps::UserAllocated
+	{
+	//= ATTENTION! =====================================================================================
+	// Changing the data layout of this class breaks the binary serialization format.  See comments for 
+	// PX_BINARY_SERIAL_VERSION.  If a modification is required, please adjust the getBinaryMetaData 
+	// function.  If the modification is made on a custom branch, please change PX_BINARY_SERIAL_VERSION
+	// accordingly.
+	//==================================================================================================
+	public:
+// PX_SERIALIZATION
+													ShapeCore(const PxEMPTY);
+						void						exportExtraData(PxSerializationContext& stream);
+						void						importExtraData(PxDeserializationContext& context);
+						void						resolveReferences(PxDeserializationContext& context);
+		static			void						getBinaryMetaData(PxOutputStream& stream);
+						void						resolveMaterialReference(PxU32 materialTableIndex, PxU16 materialIndex);
+//~PX_SERIALIZATION
+
+													ShapeCore(const PxGeometry& geometry, 
+															  PxShapeFlags shapeFlags,
+															  const PxU16* materialIndices, 
+															  PxU16 materialCount);
+
+													~ShapeCore();
+
+		PX_FORCE_INLINE	PxGeometryType::Enum		getGeometryType()							const	{ return mCore.geometry.getType();			}
+						PxShape*					getPxShape();
+						const PxShape*				getPxShape()								const;
+
+		PX_FORCE_INLINE	const Gu::GeometryUnion&	getGeometryUnion()							const	{ return mCore.geometry;					}
+		PX_FORCE_INLINE	const PxGeometry&			getGeometry()								const	{ return mCore.geometry.getGeometry();		}
+						void						setGeometry(const PxGeometry& geom);
+
+						PxU16						getNbMaterialIndices()						const;
+						const PxU16*				getMaterialIndices()						const;
+						void						setMaterialIndices(const PxU16* materialIndices, PxU16 materialIndexCount);
+
+		PX_FORCE_INLINE	const PxTransform&			getShape2Actor()							const	{ return mCore.transform;					}
+		PX_FORCE_INLINE	void						setShape2Actor(const PxTransform& s2b)				{ mCore.transform = s2b;					}
+		
+		PX_FORCE_INLINE	const PxFilterData&			getSimulationFilterData()					const	{ return mSimulationFilterData;				}
+		PX_FORCE_INLINE	void						setSimulationFilterData(const PxFilterData& data)	{ mSimulationFilterData = data;				}
+
+		// PT: this one doesn't need double buffering
+		PX_FORCE_INLINE	const PxFilterData&			getQueryFilterData()						const	{ return mQueryFilterData;					}
+		PX_FORCE_INLINE	void						setQueryFilterData(const PxFilterData& data)		{ mQueryFilterData = data;					}
+
+		PX_FORCE_INLINE	PxReal						getContactOffset()							const	{ return mCore.contactOffset;				}
+		PX_FORCE_INLINE	void						setContactOffset(PxReal offset)						{ mCore.contactOffset = offset;				}
+
+		PX_FORCE_INLINE	PxReal						getRestOffset()								const	{ return mRestOffset;						}
+		PX_FORCE_INLINE	void						setRestOffset(PxReal offset)						{ mRestOffset = offset;						}
+
+		PX_FORCE_INLINE	PxReal						getTorsionalPatchRadius()					const	{ return mTorsionalRadius;					}
+		PX_FORCE_INLINE	void						setTorsionalPatchRadius(PxReal tpr)					{ mTorsionalRadius = tpr;					}
+
+		PX_FORCE_INLINE PxReal						getMinTorsionalPatchRadius()				const	{return mMinTorsionalPatchRadius;			}
+		PX_FORCE_INLINE	void						setMinTorsionalPatchRadius(PxReal radius)			{ mMinTorsionalPatchRadius = radius;		}
+
+		PX_FORCE_INLINE	PxShapeFlags				getFlags()									const	{ return PxShapeFlags(mCore.mShapeFlags);	}
+		PX_FORCE_INLINE	void						setFlags(PxShapeFlags f)							{ mCore.mShapeFlags = f;					}
+
+		static PX_FORCE_INLINE size_t				getCoreOffset()										{ return PX_OFFSET_OF(ShapeCore, mCore);	}
+
+		PX_FORCE_INLINE const PxsShapeCore&			getCore()									const	{ return mCore;								}
+
+		static PX_FORCE_INLINE ShapeCore&			getCore(PxsShapeCore& core)
+		{
+			return *reinterpret_cast<ShapeCore*>(reinterpret_cast<PxU8*>(&core) - getCoreOffset()); 
+		}
+
+	protected:
+						PxFilterData				mQueryFilterData;		// Query filter data PT: TODO: consider moving this to SceneQueryShapeData
+						PxFilterData				mSimulationFilterData;	// Simulation filter data
+						PxsShapeCore				PX_ALIGN(16, mCore);	
+						PxReal						mRestOffset;			// same as the API property of the same name
+						PxReal						mTorsionalRadius;
+						PxReal						mMinTorsionalPatchRadius;
+	};
+
+} // namespace Sc
+
+
+}
+
+#endif

physx/source/simulationcontroller/include/ScStaticCore.h

@@ -0,0 +1,79 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef PX_PHYSICS_SCP_STATIC_CORE
+#define PX_PHYSICS_SCP_STATIC_CORE
+
+#include "ScRigidCore.h"
+#include "PxvDynamics.h"
+
+namespace physx
+{
+namespace Sc
+{
+	
+	class StaticSim;
+
+	class StaticCore: public RigidCore
+	{
+	//= ATTENTION! =====================================================================================
+	// Changing the data layout of this class breaks the binary serialization format.  See comments for 
+	// PX_BINARY_SERIAL_VERSION.  If a modification is required, please adjust the getBinaryMetaData 
+	// function.  If the modification is made on a custom branch, please change PX_BINARY_SERIAL_VERSION
+	// accordingly.
+	//==================================================================================================
+	public:
+											StaticCore(const PxTransform& actor2World): RigidCore(PxActorType::eRIGID_STATIC)	
+											{
+												mCore.body2World = actor2World;
+												mCore.mFlags = PxRigidBodyFlags();
+											}
+							
+		PX_FORCE_INLINE	const PxTransform&	getActor2World() const	{ return mCore.body2World;	}
+						void				setActor2World(const PxTransform& actor2World);
+
+		PX_FORCE_INLINE	PxsRigidCore&		getCore()				{ return mCore;				}
+		static PX_FORCE_INLINE size_t		getCoreOffset()			{ return PX_OFFSET_OF_RT(StaticCore, mCore); }
+
+											StaticCore(const PxEMPTY) :	RigidCore(PxEmpty), mCore(PxEmpty) {}
+		static			void				getBinaryMetaData(PxOutputStream& stream);
+
+						StaticSim*			getSim() const;
+
+		PX_FORCE_INLINE	void				onOriginShift(const PxVec3& shift)	{ mCore.body2World.p -= shift; }
+	private:
+						PxsRigidCore		mCore;
+	};
+
+} // namespace Sc
+
+}
+
+#endif

physx/source/simulationcontroller/src/ScActorCore.cpp

@@ -0,0 +1,87 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "ScActorCore.h"
+#include "ScActorSim.h"
+#include "ScShapeCore.h"
+#include "ScShapeSim.h"
+#include "ScBodySim.h"
+
+using namespace physx;
+
+Sc::ActorCore::ActorCore(PxActorType::Enum actorType, PxU8 actorFlags, PxClientID owner, PxDominanceGroup dominanceGroup) :
+	mSim					(NULL),
+	mAggregateIDOwnerClient	((PxU32(owner)<<24)|0x00ffffff),
+	mActorFlags				(actorFlags),
+	mActorType				(PxU8(actorType)),
+	mDominanceGroup			(dominanceGroup)
+{
+	PX_ASSERT((actorType & 0xff) == actorType);
+}
+
+Sc::ActorCore::~ActorCore()
+{
+}
+
+void Sc::ActorCore::setActorFlags(PxActorFlags af)	
+{ 
+	const PxActorFlags old = mActorFlags;
+	if(af!=old)
+	{
+		mActorFlags = af;
+
+		if(mSim)
+			mSim->postActorFlagChange(old, af);
+	}
+}	
+
+void Sc::ActorCore::setDominanceGroup(PxDominanceGroup g)
+{
+	PX_ASSERT(g<128);
+	mDominanceGroup = g;
+	if(mSim)
+	{
+		//force all related interactions to refresh, so they fetch new dominance values.
+		mSim->setActorsInteractionsDirty(InteractionDirtyFlag::eDOMINANCE, NULL, InteractionFlag::eRB_ELEMENT);
+	}
+}
+
+void Sc::ActorCore::reinsertShapes()
+{
+	PX_ASSERT(mSim);
+	if(!mSim)
+		return;
+
+	ElementSim* current = mSim->getElements_();
+	while(current)
+	{
+		static_cast<ShapeSim*>(current)->reinsertBroadPhase();
+		current = current->mNextInActor;
+	}
+}

physx/source/simulationcontroller/src/ScActorPair.h

@@ -0,0 +1,217 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef PX_COLLISION_ACTORPAIR
+#define PX_COLLISION_ACTORPAIR
+
+#include "ScRigidSim.h"
+#include "ScContactStream.h"
+#include "ScNPhaseCore.h"
+
+namespace physx
+{
+namespace Sc
+{
+	class ActorPairContactReportData
+	{
+	public:
+		ActorPairContactReportData() : 
+			mStrmResetStamp			(0xffffffff),
+			mActorAID				(0xffffffff),
+			mActorBID				(0xffffffff),
+			mPxActorA				(NULL),
+			mPxActorB				(NULL)
+			{}
+
+		ContactStreamManager	mContactStreamManager;
+		PxU32					mStrmResetStamp;
+		PxU32					mActorAID;
+		PxU32					mActorBID;
+		PxActor*				mPxActorA;
+		PxActor*				mPxActorB;
+	};
+
+	/**
+	\brief Class shared by all shape interactions for a pair of actors.
+
+	This base class is used if no shape pair of an actor pair has contact reports requested.
+	*/
+	class ActorPair
+	{
+	public:
+
+		enum ActorPairFlags
+		{
+			eIS_REPORT_PAIR	= (1<<0),
+			eNEXT_FREE		= (1<<1)
+		};
+
+		PX_FORCE_INLINE					ActorPair() : mInternalFlags(0), mTouchCount(0), mRefCount(0) {}
+		PX_FORCE_INLINE					~ActorPair() {}
+
+		PX_FORCE_INLINE	Ps::IntBool		isReportPair() const { return (mInternalFlags & eIS_REPORT_PAIR); }
+
+		PX_FORCE_INLINE	void			incTouchCount() { mTouchCount++; PX_ASSERT(mTouchCount); }
+		PX_FORCE_INLINE	void			decTouchCount() { PX_ASSERT(mTouchCount); mTouchCount--; }
+		PX_FORCE_INLINE	PxU32			getTouchCount() const { return mTouchCount; }
+
+		PX_FORCE_INLINE	void			incRefCount() { ++mRefCount; PX_ASSERT(mRefCount>0); }
+		PX_FORCE_INLINE	PxU32			decRefCount() { PX_ASSERT(mRefCount>0); return --mRefCount; }
+		PX_FORCE_INLINE	PxU32			getRefCount() const { return mRefCount; }
+
+	private:
+		ActorPair& operator=(const ActorPair&);
+
+	protected:
+						PxU16			mInternalFlags;
+						PxU16			mTouchCount;
+						PxU16			mRefCount;
+						PxU16			mPad;  // instances of this class are stored in a pool which needs an item size of at least size_t
+	};
+
+	/**
+	\brief Class shared by all shape interactions for a pair of actors if contact reports are requested.
+
+	This class is used if at least one shape pair of an actor pair has contact reports requested.
+
+	\note If a pair of actors had contact reports requested for some of the shape interactions but all of them switch to not wanting contact reports
+	any longer, then the ActorPairReport instance is kept being used and won't get replaced by a simpler ActorPair instance.
+	*/
+	class ActorPairReport : public ActorPair
+	{
+	public:
+
+		enum ActorPairReportFlags
+		{
+			eIS_IN_CONTACT_REPORT_ACTOR_PAIR_SET = ActorPair::eNEXT_FREE	// PT: whether the pair is already stored in the 'ContactReportActorPairSet' or not
+		};
+
+		PX_FORCE_INLINE					ActorPairReport(RigidSim&, RigidSim&);
+		PX_FORCE_INLINE					~ActorPairReport();
+
+		PX_INLINE ContactStreamManager&	createContactStreamManager(NPhaseCore&);
+		PX_FORCE_INLINE ContactStreamManager& getContactStreamManager() const { PX_ASSERT(mReportData); return mReportData->mContactStreamManager; }
+		PX_FORCE_INLINE	RigidSim&		getActorA() const { return mActorA; }
+		PX_FORCE_INLINE	RigidSim&		getActorB() const { return mActorB; }
+		PX_INLINE		PxU32			getActorAID() const { PX_ASSERT(mReportData); return mReportData->mActorAID; }
+		PX_INLINE		PxU32			getActorBID() const { PX_ASSERT(mReportData); return mReportData->mActorBID; }
+		PX_INLINE		PxActor*		getPxActorA() const { PX_ASSERT(mReportData); return mReportData->mPxActorA; }
+		PX_INLINE		PxActor*		getPxActorB() const { PX_ASSERT(mReportData); return mReportData->mPxActorB; }
+		PX_FORCE_INLINE	bool			streamResetNeeded(PxU32 cmpStamp) const;
+		PX_INLINE		bool			streamResetStamp(PxU32 cmpStamp);
+
+		PX_FORCE_INLINE	PxU16			isInContactReportActorPairSet() const { return PxU16(mInternalFlags & eIS_IN_CONTACT_REPORT_ACTOR_PAIR_SET); }
+		PX_FORCE_INLINE	void			setInContactReportActorPairSet() { mInternalFlags |= eIS_IN_CONTACT_REPORT_ACTOR_PAIR_SET; }
+		PX_FORCE_INLINE	void			clearInContactReportActorPairSet() { mInternalFlags &= ~eIS_IN_CONTACT_REPORT_ACTOR_PAIR_SET; }
+
+		PX_FORCE_INLINE void			createContactReportData(NPhaseCore&);
+		PX_FORCE_INLINE void			releaseContactReportData(NPhaseCore&);
+		PX_FORCE_INLINE const ActorPairContactReportData* hasReportData() const { return mReportData; }
+
+		PX_FORCE_INLINE	void			convert(ActorPair& aPair) { PX_ASSERT(!aPair.isReportPair()); mTouchCount = PxU16(aPair.getTouchCount()); mRefCount = PxU16(aPair.getRefCount()); }
+
+		PX_FORCE_INLINE static ActorPairReport& cast(ActorPair& aPair) { PX_ASSERT(aPair.isReportPair()); return static_cast<ActorPairReport&>(aPair); }
+
+	private:
+		ActorPairReport& operator=(const ActorPairReport&);
+
+						RigidSim&		mActorA;
+						RigidSim&		mActorB;
+
+			ActorPairContactReportData* mReportData;
+	};
+
+} // namespace Sc
+
+PX_FORCE_INLINE Sc::ActorPairReport::ActorPairReport(RigidSim& actor0, RigidSim& actor1) : ActorPair(),
+mActorA			(actor0),
+mActorB			(actor1),
+mReportData		(NULL)
+{
+	PX_ASSERT(mInternalFlags == 0);
+	mInternalFlags = ActorPair::eIS_REPORT_PAIR;
+}
+
+PX_FORCE_INLINE Sc::ActorPairReport::~ActorPairReport()
+{
+	PX_ASSERT(mReportData == NULL);
+}
+
+PX_FORCE_INLINE bool Sc::ActorPairReport::streamResetNeeded(PxU32 cmpStamp) const
+{
+	return (cmpStamp != mReportData->mStrmResetStamp);
+}
+
+PX_INLINE bool Sc::ActorPairReport::streamResetStamp(PxU32 cmpStamp) 
+{
+	PX_ASSERT(mReportData);
+	const bool ret = streamResetNeeded(cmpStamp);
+	mReportData->mStrmResetStamp = cmpStamp; 
+	return ret; 
+}
+
+PX_INLINE Sc::ContactStreamManager&	Sc::ActorPairReport::createContactStreamManager(NPhaseCore& npCore)
+{
+	// Lazy create report data
+	if(!mReportData)
+		createContactReportData(npCore);
+
+	return mReportData->mContactStreamManager;
+}
+
+PX_FORCE_INLINE void Sc::ActorPairReport::createContactReportData(NPhaseCore& npCore)
+{
+	PX_ASSERT(!mReportData);
+	Sc::ActorPairContactReportData* reportData = npCore.createActorPairContactReportData(); 
+	mReportData = reportData;
+
+	if(reportData)
+	{
+		reportData->mActorAID = mActorA.getRigidID();
+		reportData->mActorBID = mActorB.getRigidID();
+
+		reportData->mPxActorA = mActorA.getPxActor();
+		reportData->mPxActorB = mActorB.getPxActor();
+	}
+}
+
+PX_FORCE_INLINE void Sc::ActorPairReport::releaseContactReportData(NPhaseCore& npCore)
+{
+	// Can't take the NPhaseCore (scene) reference from the actors since they're already gone on scene release
+
+	if(mReportData)
+	{
+		npCore.releaseActorPairContactReportData(mReportData);
+		mReportData = NULL;
+	}
+}
+
+}
+
+#endif

physx/source/simulationcontroller/src/ScActorSim.cpp

@@ -0,0 +1,144 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "CmPhysXCommon.h"
+#include "ScActorSim.h"
+#include "ScActorCore.h"
+#include "ScElementSim.h"
+#include "ScScene.h"
+#include "ScInteraction.h"
+
+using namespace physx;
+
+Sc::ActorSim::ActorSim(Scene& scene, ActorCore& core) :
+	mFirstElement	(NULL),
+	mElementCount	(0),
+	mScene			(scene),
+	mCore			(core)
+{
+	core.setSim(this);
+}
+
+Sc::ActorSim::~ActorSim()
+{
+	mInteractions.releaseMem(*this);
+}
+
+void Sc::ActorSim::registerInteractionInActor(Interaction* interaction)
+{
+	const PxU32 id = mInteractions.size();
+	mInteractions.pushBack(interaction, *this);
+	interaction->setActorId(this, id);
+}
+
+void Sc::ActorSim::unregisterInteractionFromActor(Interaction* interaction)
+{
+	const PxU32 i = interaction->getActorId(this);
+	PX_ASSERT(i < mInteractions.size());
+	mInteractions.replaceWithLast(i); 
+	if (i<mInteractions.size())
+		mInteractions[i]->setActorId(this, i);
+}
+
+void Sc::ActorSim::onElementAttach(ElementSim& element)
+{
+	element.mNextInActor = mFirstElement;
+	mFirstElement = &element;
+	mElementCount++;
+}
+
+void Sc::ActorSim::onElementDetach(ElementSim& element)
+{
+	PX_ASSERT(mFirstElement);	// PT: else we shouldn't be called
+	ElementSim* currentElem = mFirstElement;
+	ElementSim* previousElem = NULL;
+	while(currentElem)
+	{
+		if(currentElem==&element)
+		{
+			if(previousElem)
+				previousElem->mNextInActor = currentElem->mNextInActor;
+			else
+				mFirstElement = currentElem->mNextInActor;
+			mElementCount--;
+			return;
+		}
+		previousElem = currentElem;
+		currentElem = currentElem->mNextInActor;
+	}
+	PX_ASSERT(0);
+}
+
+// PT: TODO: refactor with Sc::ParticlePacketShape::reallocInteractions - sschirm: particles are gone
+void Sc::ActorSim::reallocInteractions(Sc::Interaction**& mem, PxU32& capacity, PxU32 size, PxU32 requiredMinCapacity)
+{
+	Interaction** newMem;
+	PxU32 newCapacity;
+
+	if(requiredMinCapacity==0)
+	{
+		newCapacity = 0;
+		newMem = 0;
+	}
+	else if(requiredMinCapacity<=INLINE_INTERACTION_CAPACITY)
+	{
+		newCapacity = INLINE_INTERACTION_CAPACITY;
+		newMem = mInlineInteractionMem;
+	}
+	else
+	{
+		newCapacity = Ps::nextPowerOfTwo(requiredMinCapacity-1);
+		newMem = reinterpret_cast<Interaction**>(mScene.allocatePointerBlock(newCapacity));
+	}
+
+	PX_ASSERT(newCapacity >= requiredMinCapacity && requiredMinCapacity>=size);
+
+	if(mem)
+	{
+		PxMemCopy(newMem, mem, size*sizeof(Interaction*));
+
+		if(mem!=mInlineInteractionMem)
+			mScene.deallocatePointerBlock(reinterpret_cast<void**>(mem), capacity);
+	}
+	
+	capacity = newCapacity;
+	mem = newMem;
+}
+
+void Sc::ActorSim::setActorsInteractionsDirty(InteractionDirtyFlag::Enum flag, const ActorSim* other, PxU8 interactionFlag)
+{
+	PxU32 size = getActorInteractionCount();
+	Interaction** interactions = getActorInteractions();
+	while(size--)
+	{
+		Interaction* interaction = *interactions++;
+		if((!other || other == &interaction->getActorSim0() || other == &interaction->getActorSim1()) && (interaction->readInteractionFlag(interactionFlag)))
+			interaction->setDirty(flag);
+	}
+}

physx/source/simulationcontroller/src/ScActorSim.h

@@ -0,0 +1,123 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef PX_PHYSICS_SCP_ACTOR_SIM
+#define PX_PHYSICS_SCP_ACTOR_SIM
+
+#include "PsUserAllocated.h"
+#include "CmPhysXCommon.h"
+#include "CmUtils.h"
+#include "PxActor.h"
+#include "ScInteractionFlags.h"
+#include "ScActorCore.h"
+
+namespace physx
+{
+
+class PxActor;
+
+namespace Sc
+{
+	class Interaction;
+	class ElementSim;
+	
+	class ActorSim : public Ps::UserAllocated
+	{
+		friend class Scene;  // the scene is allowed to set the scene array index
+		friend class Interaction;
+		PX_NOCOPY(ActorSim)
+
+	public:
+		enum ActivityChangeInfoFlag
+		{
+			AS_PART_OF_CREATION				= (1 << 0),
+			AS_PART_OF_ISLAND_GEN			= (1 << 1)
+		};
+
+											ActorSim(Scene&, ActorCore&);
+		virtual								~ActorSim();
+
+		// Get the scene the actor resides in
+		PX_FORCE_INLINE	Scene&				getScene()					const	{ return mScene; }
+
+		// Get the number of interactions connected to the actor
+		PX_FORCE_INLINE	PxU32				getActorInteractionCount()	const	{ return mInteractions.size(); }
+
+		// Get an iterator to the interactions connected to the actor
+		PX_FORCE_INLINE	Interaction**		getActorInteractions()		const	{ return mInteractions.begin();	}
+
+		// Get first element in the actor (linked list)
+		PX_FORCE_INLINE	ElementSim*			getElements_()						{ return mFirstElement;		}
+		PX_FORCE_INLINE	const ElementSim*	getElements_()				const	{ return mFirstElement;		}
+
+		// Get the type ID of the actor
+		PX_FORCE_INLINE	PxActorType::Enum	getActorType()				const	{ return mCore.getActorCoreType();	}
+
+		// Returns true if the actor is a dynamic rigid body (including articulation links)
+		PX_FORCE_INLINE	bool				isDynamicRigid()			const	{ const PxActorType::Enum type = getActorType(); return type == PxActorType::eRIGID_DYNAMIC || type == PxActorType::eARTICULATION_LINK; }
+		PX_FORCE_INLINE	bool				isStaticRigid()				const   { const PxActorType::Enum type = getActorType(); return type == PxActorType::eRIGID_STATIC; }
+
+						void				onElementAttach(ElementSim& element);
+						void				onElementDetach(ElementSim& element);
+
+		virtual			void				postActorFlagChange(PxU32, PxU32) {}
+
+						void				setActorsInteractionsDirty(InteractionDirtyFlag::Enum flag, const ActorSim* other, PxU8 interactionFlag);
+
+		PX_FORCE_INLINE	ActorCore&			getActorCore() const { return mCore; }
+
+	private:
+		//These are called from interaction creation/destruction
+						void				registerInteractionInActor(Interaction* interaction);
+						void				unregisterInteractionFromActor(Interaction* interaction);
+
+						void				reallocInteractions(Sc::Interaction**& mem, PxU32& capacity, PxU32 size, PxU32 requiredMinCapacity);
+	protected:
+		// dsequeira: interaction arrays are a major cause of small allocations, so we don't want to delegate them to the heap allocator
+		// it's not clear this inline array is really needed, we should take it out and see whether the cache perf is worse
+
+		static const PxU32 INLINE_INTERACTION_CAPACITY = 4;
+						Interaction*		mInlineInteractionMem[INLINE_INTERACTION_CAPACITY];
+
+		Cm::OwnedArray<Sc::Interaction*, Sc::ActorSim, PxU32, &Sc::ActorSim::reallocInteractions>
+											mInteractions;
+
+						ElementSim*			mFirstElement;
+						PxU32				mElementCount;
+
+						Scene&				mScene;
+
+						ActorCore&			mCore;
+	};
+
+} // namespace Sc
+
+}
+
+#endif

physx/source/simulationcontroller/src/ScArticulationCore.cpp

@@ -0,0 +1,305 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#include "ScArticulationCore.h"
+
+#include "PsFoundation.h"
+#include "ScPhysics.h"
+#include "ScBodyCore.h"
+#include "ScBodySim.h"
+#include "ScArticulationSim.h"
+#include "DyArticulation.h"
+
+using namespace physx;
+
+Sc::ArticulationCore::ArticulationCore(bool reducedCoordinate) :
+	mSim(NULL), 
+	mIsReducedCoordinate(reducedCoordinate)
+{
+	const PxTolerancesScale& scale = Physics::getInstance().getTolerancesScale();
+
+	mCore.internalDriveIterations	= 4;
+	mCore.externalDriveIterations	= 4;
+	mCore.maxProjectionIterations	= 4;
+	mCore.solverIterationCounts		= 1<<8 | 4;
+	mCore.separationTolerance		= 0.1f * scale.length;
+	mCore.sleepThreshold			= 5e-5f * scale.speed * scale.speed;
+	mCore.freezeThreshold			= 5e-6f * scale.speed * scale.speed;
+	mCore.wakeCounter				= Physics::sWakeCounterOnCreation;
+}
+
+Sc::ArticulationCore::~ArticulationCore()
+{
+}
+
+//--------------------------------------------------------------
+//
+// ArticulationCore interface implementation
+//
+//--------------------------------------------------------------
+
+void Sc::ArticulationCore::setWakeCounter(const PxReal v)
+{
+	mCore.wakeCounter = v;
+
+#ifdef _DEBUG
+	if(mSim)
+		mSim->debugCheckWakeCounterOfLinks(v);
+#endif
+}
+
+bool Sc::ArticulationCore::isSleeping() const
+{
+	return mSim ? mSim->isSleeping() : (mCore.wakeCounter == 0.0f);
+}
+
+void Sc::ArticulationCore::wakeUp(PxReal wakeCounter)
+{
+	mCore.wakeCounter = wakeCounter;
+
+#ifdef _DEBUG
+	if(mSim)
+		mSim->debugCheckSleepStateOfLinks(false);
+#endif
+}
+
+void Sc::ArticulationCore::putToSleep()
+{
+	mCore.wakeCounter = 0.0f;
+
+#ifdef _DEBUG
+	if(mSim)
+		mSim->debugCheckSleepStateOfLinks(true);
+#endif
+}
+
+PxArticulationBase* Sc::ArticulationCore::getPxArticulationBase()
+{
+	return gOffsetTable.convertScArticulation2Px(this, isReducedCoordinate());
+}
+
+const PxArticulationBase* Sc::ArticulationCore::getPxArticulationBase() const
+{
+	return gOffsetTable.convertScArticulation2Px(this, isReducedCoordinate());
+}
+
+Sc::ArticulationDriveCache* Sc::ArticulationCore::createDriveCache(PxReal compliance, PxU32 driveIterations) const
+{
+	return mSim ? mSim->createDriveCache(compliance, driveIterations) : NULL;
+}
+
+void Sc::ArticulationCore::updateDriveCache(ArticulationDriveCache& cache, PxReal compliance, PxU32 driveIterations) const
+{
+	if(mSim)
+		mSim->updateDriveCache(cache, compliance, driveIterations);
+}
+
+void Sc::ArticulationCore::releaseDriveCache(Sc::ArticulationDriveCache& driveCache) const
+{
+	if(mSim)
+		mSim->releaseDriveCache(driveCache);
+}
+
+PxU32 Sc::ArticulationCore::getCacheLinkCount(const ArticulationDriveCache& cache) const
+{
+	return Dy::PxvArticulationDriveCache::getLinkCount(cache);
+}
+
+void Sc::ArticulationCore::applyImpulse(Sc::BodyCore& link,
+										const Sc::ArticulationDriveCache& driveCache,
+										const PxVec3& force,
+										const PxVec3& torque)
+{
+	if(mSim)
+		mSim->applyImpulse(link, driveCache, force, torque);
+}
+
+void Sc::ArticulationCore::computeImpulseResponse(Sc::BodyCore& link,
+												  PxVec3& linearResponse, 
+												  PxVec3& angularResponse,
+												  const Sc::ArticulationDriveCache& driveCache,
+												  const PxVec3& force,
+												  const PxVec3& torque) const
+{
+	if(mSim)
+		mSim->computeImpulseResponse(link, linearResponse, angularResponse, driveCache, force, torque);
+}
+
+void Sc::ArticulationCore::setArticulationFlags(PxArticulationFlags flags)
+{
+	mCore.flags = flags;
+	if (mSim)
+	{
+		const bool isKinematicLink = flags & PxArticulationFlag::eFIX_BASE;
+		mSim->setKinematicLink(isKinematicLink);
+	}
+}
+
+PxU32 Sc::ArticulationCore::getDofs() const
+{
+	return mSim ? mSim->getDofs() : 0;
+}
+
+PxArticulationCache* Sc::ArticulationCore::createCache() const
+{
+	return mSim ? mSim->createCache() : NULL;
+}
+
+PxU32 Sc::ArticulationCore::getCacheDataSize() const
+{
+	return mSim ? mSim->getCacheDataSize() : 0;
+}
+
+void Sc::ArticulationCore::zeroCache(PxArticulationCache& cache) const
+{
+	if(mSim)
+		mSim->zeroCache(cache);
+}
+
+void Sc::ArticulationCore::applyCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag) const
+{
+	if(mSim)
+		mSim->applyCache(cache, flag);
+}
+
+void Sc::ArticulationCore::copyInternalStateToCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag) const
+{
+	if(mSim)
+		mSim->copyInternalStateToCache(cache, flag);
+}
+
+void Sc::ArticulationCore::releaseCache(PxArticulationCache& cache) const
+{
+	if(mSim)
+		mSim->releaseCache(cache);
+}
+
+void Sc::ArticulationCore::packJointData(const PxReal* maximum, PxReal* reduced) const
+{
+	if(mSim)
+		mSim->packJointData(maximum, reduced);
+}
+
+void Sc::ArticulationCore::unpackJointData(const PxReal* reduced, PxReal* maximum) const
+{
+	if(mSim)
+		mSim->unpackJointData(reduced, maximum);
+}
+
+void Sc::ArticulationCore::commonInit() const
+{
+	if(mSim)
+		mSim->commonInit();
+}
+
+void Sc::ArticulationCore::computeGeneralizedGravityForce(PxArticulationCache& cache) const
+{
+	if(mSim)
+		mSim->computeGeneralizedGravityForce(cache);
+}
+
+void Sc::ArticulationCore::computeCoriolisAndCentrifugalForce(PxArticulationCache& cache) const
+{
+	if(mSim)
+		mSim->computeCoriolisAndCentrifugalForce(cache);
+}
+
+void Sc::ArticulationCore::computeGeneralizedExternalForce(PxArticulationCache& cache) const
+{
+	if(mSim)
+		mSim->computeGeneralizedExternalForce(cache);
+}
+
+void Sc::ArticulationCore::computeJointAcceleration(PxArticulationCache& cache) const
+{
+	if(mSim)
+		mSim->computeJointAcceleration(cache);
+}
+
+void Sc::ArticulationCore::computeJointForce(PxArticulationCache& cache) const
+{
+	if(mSim)
+		mSim->computeJointForce(cache);
+}
+
+void Sc::ArticulationCore::computeDenseJacobian(PxArticulationCache& cache, PxU32& nRows, PxU32& nCols) const
+{
+	if(mSim)
+		mSim->computeDenseJacobian(cache, nRows, nCols);
+}
+
+void Sc::ArticulationCore::computeCoefficientMatrix(PxArticulationCache& cache) const
+{
+	if(mSim)
+		mSim->computeCoefficientMatrix(cache);
+}
+
+bool Sc::ArticulationCore::computeLambda(PxArticulationCache& cache, PxArticulationCache& initialState, const PxReal* const jointTorque, const PxVec3 gravity, const PxU32 maxIter) const
+{
+	return mSim ? mSim->computeLambda(cache, initialState, jointTorque, gravity, maxIter) : false;
+}
+
+void Sc::ArticulationCore::computeGeneralizedMassMatrix(PxArticulationCache& cache) const
+{
+	if(mSim)
+		mSim->computeGeneralizedMassMatrix(cache);
+}
+
+PxU32 Sc::ArticulationCore::getCoefficientMatrixSize() const
+{
+	return mSim ? mSim->getCoefficientMatrixSize() : 0;
+}
+
+PxSpatialVelocity Sc::ArticulationCore::getLinkVelocity(const PxU32 linkId) const
+{
+	return mSim ? mSim->getLinkVelocity(linkId) : PxSpatialVelocity();
+}
+
+PxSpatialVelocity Sc::ArticulationCore::getLinkAcceleration(const PxU32 linkId) const
+{
+	return mSim ? mSim->getLinkAcceleration(linkId) : PxSpatialVelocity();
+}
+
+IG::NodeIndex Sc::ArticulationCore::getIslandNodeIndex() const
+{
+	return mSim ? mSim->getIslandNodeIndex() : IG::NodeIndex(IG_INVALID_NODE);
+}
+
+void Sc::ArticulationCore::setGlobalPose()
+{
+	if(mSim)
+		mSim->setGlobalPose();
+}
+
+void Sc::ArticulationCore::setDirty(const bool dirty)
+{
+	if(mSim)
+		mSim->setDirty(dirty);
+}

physx/source/simulationcontroller/src/ScArticulationJointCore.cpp

@@ -0,0 +1,318 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "ScArticulationJointCore.h"
+#include "ScArticulationCore.h"
+#include "ScArticulationJointSim.h"
+#include "ScBodyCore.h"
+#include "ScPhysics.h"
+
+using namespace physx;
+
+Sc::ArticulationJointCore::ArticulationJointCore(const PxTransform& parentFrame,
+												 const PxTransform& childFrame,
+												 bool isReducedCoordinate) :
+	mSim	(NULL)
+{
+	PX_ASSERT(parentFrame.isValid());
+	PX_ASSERT(childFrame.isValid());
+
+	mCore.parentPose = parentFrame;
+	mCore.childPose = childFrame;
+
+	mCore.targetPosition = PxQuat(PxIdentity);
+	mCore.targetVelocity = PxVec3(0.f);
+
+	mCore.driveType = PxArticulationJointDriveType::eTARGET;
+
+	mCore.spring = 0.0f;
+	mCore.damping = 0.0f;
+
+	mCore.internalCompliance = 1.0f;
+	mCore.externalCompliance = 1.0f;
+
+	if (!isReducedCoordinate)
+	{
+		PxReal swingYLimit = PxPi / 4;
+		PxReal swingZLimit = PxPi / 4;
+		mCore.limits[PxArticulationAxis::eSWING1].low = swingYLimit;
+		mCore.limits[PxArticulationAxis::eSWING1].high = swingYLimit;
+		mCore.limits[PxArticulationAxis::eSWING2].low = swingZLimit;
+		mCore.limits[PxArticulationAxis::eSWING2].high = swingZLimit;
+		mCore.swingLimitContactDistance = 0.05f;
+
+		PxReal twistLimitLow = -PxPi / 4;
+		PxReal twistLimitHigh = PxPi / 4;
+
+		mCore.limits[PxArticulationAxis::eTWIST].low = twistLimitLow;
+		mCore.limits[PxArticulationAxis::eTWIST].high = twistLimitHigh;
+		mCore.twistLimitContactDistance = 0.05f;
+		mCore.tanQSwingY = PxTan(swingYLimit / 4);
+		mCore.tanQSwingZ = PxTan(swingZLimit / 4);
+		mCore.tanQSwingPad = PxTan(mCore.swingLimitContactDistance / 4);
+
+		mCore.tanQTwistHigh = PxTan(twistLimitHigh / 4);
+		mCore.tanQTwistLow = PxTan(twistLimitLow / 4);
+		mCore.tanQTwistPad = PxTan(mCore.twistLimitContactDistance / 4);
+	}
+	else
+	{
+		for (PxU32 i = 0; i < 6; ++i)
+		{
+			mCore.limits[i].low = 0.f;
+			mCore.limits[i].high = 0.f;
+			mCore.drives[i].stiffness = 0.f;
+			mCore.drives[i].damping = 0.f;
+			mCore.drives[i].maxForce = 0.f;
+			mCore.drives[i].driveType = PxArticulationDriveType::eNONE;
+			mCore.targetP[i] = 0.f;
+			mCore.targetV[i] = 0.f;
+		}
+
+		mCore.twistLimitContactDistance = 0.f;
+		mCore.tanQSwingY = 0.f;
+		mCore.tanQSwingZ = 0.f;
+		mCore.tanQSwingPad = 0.f;
+
+		mCore.tanQTwistHigh = 0.f;
+		mCore.tanQTwistLow = 0.f;
+		mCore.tanQTwistPad = 0.f;
+	}
+
+	mCore.swingLimited = false;
+	mCore.twistLimited = false;
+	mCore.tangentialStiffness = 0.0f;
+	mCore.tangentialDamping = 0.0f;
+
+	mCore.frictionCoefficient = 0.05f;
+
+	mCore.jointType = PxArticulationJointType::eUNDEFINED;
+
+	for(PxU32 i = 0; i < PxArticulationAxis::eCOUNT; ++i)
+		mCore.motion[i] = PxArticulationMotion::eLOCKED;
+}
+
+Sc::ArticulationJointCore::~ArticulationJointCore()
+{
+	PX_ASSERT(getSim() == 0);
+}
+
+//--------------------------------------------------------------
+//
+// ArticulationJointCore interface implementation
+//
+//--------------------------------------------------------------
+
+void Sc::ArticulationJointCore::setParentPose(const PxTransform& t)
+{
+	mCore.parentPose = t;
+	setDirty(Dy::ArticulationJointCoreDirtyFlag::ePOSE);
+}
+
+void Sc::ArticulationJointCore::setChildPose(const PxTransform& t)
+{
+	mCore.childPose = t;
+	setDirty(Dy::ArticulationJointCoreDirtyFlag::ePOSE);
+}
+
+void Sc::ArticulationJointCore::setTargetOrientation(const PxQuat& p)
+{
+	mCore.targetPosition = p;
+}
+
+void Sc::ArticulationJointCore::setTargetVelocity(const PxVec3& v)
+{
+	mCore.targetVelocity = v;
+}
+
+void Sc::ArticulationJointCore::setDriveType(PxArticulationJointDriveType::Enum type)
+{
+	mCore.driveType = PxU8(type);
+}
+
+void Sc::ArticulationJointCore::setJointType(PxArticulationJointType::Enum type)
+{
+	mCore.jointType = PxU8(type);
+	mArticulation->setDirty(true);
+}
+
+PxArticulationJointType::Enum Sc::ArticulationJointCore::getJointType() const 
+{ 
+	return PxArticulationJointType::Enum(mCore.jointType); 
+}
+
+void Sc::ArticulationJointCore::setMotion(PxArticulationAxis::Enum axis, PxArticulationMotion::Enum motion)
+{
+	mCore.motion[axis] = PxU8(motion);
+	setDirty(Dy::ArticulationJointCoreDirtyFlag::eMOTION);
+}
+
+PxArticulationMotion::Enum Sc::ArticulationJointCore::getMotion(PxArticulationAxis::Enum axis) const
+{
+	return PxArticulationMotion::Enum(mCore.motion[axis]);
+}
+
+void Sc::ArticulationJointCore::setFrictionCoefficient(const PxReal coefficient)
+{
+	mCore.frictionCoefficient = coefficient;
+}
+
+PxReal Sc::ArticulationJointCore::getFrictionCoefficient() const
+{
+	return mCore.frictionCoefficient;
+}
+
+void Sc::ArticulationJointCore::setMaxJointVelocity(const PxReal maxJointV)
+{
+	mCore.maxJointVelocity = maxJointV;
+}
+
+PxReal Sc::ArticulationJointCore::getMaxJointVelocity() const
+{
+	return mCore.maxJointVelocity;
+}
+
+void Sc::ArticulationJointCore::setStiffness(PxReal s)
+{
+	mCore.spring = s;
+}
+
+void Sc::ArticulationJointCore::setDamping(PxReal d)
+{
+	mCore.damping = d;
+}
+
+void Sc::ArticulationJointCore::setInternalCompliance(PxReal r)
+{
+	mCore.internalCompliance = r;
+}
+
+void Sc::ArticulationJointCore::setExternalCompliance(PxReal r)
+{
+	mCore.externalCompliance = r;
+}
+
+void Sc::ArticulationJointCore::setSwingLimit(PxReal yLimit, PxReal zLimit)
+{
+	mCore.limits[PxArticulationAxis::eSWING1].low = yLimit;
+	mCore.limits[PxArticulationAxis::eSWING2].low = zLimit;
+
+	mCore.tanQSwingY	= PxTan(yLimit/4);
+	mCore.tanQSwingZ	= PxTan(zLimit/4);
+}
+
+void Sc::ArticulationJointCore::setTangentialStiffness(PxReal s)
+{
+	mCore.tangentialStiffness = s;
+}
+
+void Sc::ArticulationJointCore::setTangentialDamping(PxReal d)
+{
+	mCore.tangentialDamping = d;
+}
+
+void Sc::ArticulationJointCore::setSwingLimitEnabled(bool e)
+{
+	mCore.swingLimited = e;
+}
+
+void Sc::ArticulationJointCore::setSwingLimitContactDistance(PxReal e)
+{
+	mCore.swingLimitContactDistance = e;
+	mCore.tanQSwingPad = PxTan(e/4);
+}
+
+void Sc::ArticulationJointCore::setTwistLimit(PxReal lower, PxReal upper)
+{
+	mCore.limits[PxArticulationAxis::eTWIST].low = lower;
+	mCore.limits[PxArticulationAxis::eTWIST].high = upper;
+
+	mCore.tanQTwistHigh	= PxTan(upper/4);
+	mCore.tanQTwistLow	= PxTan(lower/4);
+}
+
+void Sc::ArticulationJointCore::setTwistLimitEnabled(bool e)
+{
+	mCore.twistLimited = e;
+}
+
+void Sc::ArticulationJointCore::setTwistLimitContactDistance(PxReal e)
+{
+	mCore.twistLimitContactDistance = e;
+	mCore.tanQTwistPad = PxTan(e/4);
+}
+
+void Sc::ArticulationJointCore::setDirty(Dy::ArticulationJointCoreDirtyFlag::Enum dirtyFlag)
+{
+	mCore.dirtyFlag |= dirtyFlag;
+	Sc::ArticulationJointSim* sim = getSim();
+	if (sim)
+	{
+		sim->setDirty();
+	}
+}
+
+void Sc::ArticulationJointCore::setTargetP(PxArticulationAxis::Enum axis, PxReal targetP)
+{
+	mCore.targetP[axis] = targetP;
+	setDirty(Dy::ArticulationJointCoreDirtyFlag::eTARGETPOSE);
+}
+
+void Sc::ArticulationJointCore::setTargetV(PxArticulationAxis::Enum axis, PxReal targetV)
+{
+	mCore.targetV[axis] = targetV;
+	setDirty(Dy::ArticulationJointCoreDirtyFlag::eTARGETVELOCITY);
+}
+
+void Sc::ArticulationJointCore::setLimit(PxArticulationAxis::Enum axis, PxReal lower, PxReal upper)
+{
+	mCore.limits[axis].low = lower;
+	mCore.limits[axis].high = upper;
+	setDirty(Dy::ArticulationJointCoreDirtyFlag::eLIMIT);
+}
+
+void Sc::ArticulationJointCore::setDrive(PxArticulationAxis::Enum axis, PxReal stiffness, PxReal damping, PxReal maxForce, PxArticulationDriveType::Enum driveType)
+{
+	mCore.drives[axis].stiffness = stiffness;
+	mCore.drives[axis].damping = damping;
+	mCore.drives[axis].maxForce = maxForce;
+	mCore.drives[axis].driveType = driveType;
+
+	setDirty(Dy::ArticulationJointCoreDirtyFlag::eDRIVE);
+}
+
+PxArticulationJointBase* Sc::ArticulationJointCore::getPxArticulationJointBase()
+{
+	return gOffsetTable.convertScArticulationJoint2Px(this, getArticulation()->isReducedCoordinate());
+}
+
+const PxArticulationJointBase* Sc::ArticulationJointCore::getPxArticulationJointBase() const
+{
+	return gOffsetTable.convertScArticulationJoint2Px(this, getArticulation()->isReducedCoordinate());
+}

physx/source/simulationcontroller/src/ScArticulationJointSim.cpp

@@ -0,0 +1,100 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "ScArticulationJointSim.h"
+#include "ScArticulationJointCore.h"
+#include "ScBodySim.h"
+#include "ScScene.h"
+#include "PxsRigidBody.h"
+#include "DyArticulation.h"
+#include "ScArticulationSim.h"
+#include "PxsSimpleIslandManager.h"
+
+using namespace physx;
+
+Sc::ArticulationJointSim::ArticulationJointSim(ArticulationJointCore& joint, ActorSim& parent, ActorSim& child) :
+	Interaction	(parent, child, InteractionType::eARTICULATION, 0),
+	mCore		(joint)
+{
+	registerInActors();
+
+	BodySim& childBody = static_cast<BodySim&>(child),
+		   & parentBody = static_cast<BodySim&>(parent);
+
+	parentBody.getArticulation()->addBody(childBody, &parentBody, this);
+
+	mCore.setSim(this);
+}
+
+Sc::ArticulationJointSim::~ArticulationJointSim()
+{
+	// articulation interactions do not make use of the dirty flags yet. If they did, a setClean(true) has to be introduced here.
+	PX_ASSERT(!readInteractionFlag(InteractionFlag::eIN_DIRTY_LIST));
+	PX_ASSERT(!getDirtyFlags());
+
+	unregisterFromActors();
+
+	BodySim& child = getChild();
+	child.getArticulation()->removeBody(child);
+
+	mCore.setSim(NULL);
+}
+
+Sc::BodySim& Sc::ArticulationJointSim::getParent() const
+{
+	return static_cast<BodySim&>(getActorSim0());
+}
+
+Sc::BodySim& Sc::ArticulationJointSim::getChild() const
+{
+	return static_cast<BodySim&>(getActorSim1());
+}
+
+bool Sc::ArticulationJointSim::onActivate_(void*)
+{
+	if(!(getParent().isActive() && getChild().isActive()))
+		return false;
+
+	raiseInteractionFlag(InteractionFlag::eIS_ACTIVE);
+	return true; 
+}
+
+bool Sc::ArticulationJointSim::onDeactivate_()
+{
+	clearInteractionFlag(InteractionFlag::eIS_ACTIVE);
+	return true;
+}
+
+void Sc::ArticulationJointSim::setDirty()
+{
+	Dy::ArticulationJointCore& llCore = mCore.getCore();
+	ArticulationSim* sim = mCore.getArticulation()->getSim();
+	sim->setDirty(true); //Don't set the articulation dirty - only the joint is dirty!
+	sim->setJointDirty(llCore);
+}

physx/source/simulationcontroller/src/ScArticulationJointSim.h

@@ -0,0 +1,73 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef PX_PHYSICS_SCP_ARTICULATION_JOINT_SIM
+#define PX_PHYSICS_SCP_ARTICULATION_JOINT_SIM
+
+#include "ScInteraction.h"
+#include "DyArticulation.h"
+
+namespace physx
+{
+namespace Sc
+{
+	class ArticulationJointCore;
+	class BodySim;
+
+	class ArticulationJointSim : public Interaction
+	{
+		ArticulationJointSim&	operator=(const ArticulationJointSim &);
+
+	public:
+												ArticulationJointSim(ArticulationJointCore& joint, ActorSim& parent, ActorSim& child);
+												~ArticulationJointSim();
+
+						bool					onActivate_(void*);
+						bool					onDeactivate_();
+
+		PX_FORCE_INLINE	ArticulationJointCore&	getCore()	const	{ return mCore; }
+
+						BodySim&				getParent()	const;
+						BodySim&				getChild()	const;
+
+						void					setDirty();
+
+		//---------------------------------------------------------------------------------
+		// Low Level data access
+		//---------------------------------------------------------------------------------
+	private:
+						ArticulationJointCore&	mCore;
+	};
+
+} // namespace Sc
+
+}
+
+#endif

physx/source/simulationcontroller/src/ScArticulationSim.cpp

@@ -0,0 +1,798 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#include "ScArticulationSim.h"
+#include "ScArticulationCore.h"
+#include "ScArticulationJointSim.h"
+#include "ScArticulationJointCore.h"
+#include "ScBodySim.h"
+#include "ScConstraintSim.h"
+#include "ScScene.h"
+
+#include "DyArticulation.h"
+#include "DyConstraint.h"
+#include "DyFeatherstoneArticulation.h"
+#include "PxsContext.h"
+#include "CmSpatialVector.h"
+#include "PsVecMath.h"
+#include "PxsSimpleIslandManager.h"
+#include "ScShapeSim.h"
+#include "PxsSimulationController.h"
+
+using namespace physx;
+using namespace physx::Dy;
+
+Sc::ArticulationSim::ArticulationSim(ArticulationCore& core, Scene& scene, BodyCore& root) : 
+	mLLArticulation	(NULL),
+	mScene			(scene),
+	mCore			(core),
+	mLinks			(PX_DEBUG_EXP("ScArticulationSim::links")),
+	mBodies			(PX_DEBUG_EXP("ScArticulationSim::bodies")),
+	mJoints			(PX_DEBUG_EXP("ScArticulationSim::joints")),
+	mMaxDepth		(0)
+{
+	mLinks.reserve(16);
+	mJoints.reserve(16);
+	mBodies.reserve(16);
+
+	mLLArticulation = mScene.createLLArticulation(this);
+	
+	mIslandNodeIndex = scene.getSimpleIslandManager()->addArticulation(this, mLLArticulation, false);
+
+	if(!mLLArticulation)
+	{
+		Ps::getFoundation().error(PxErrorCode::eINTERNAL_ERROR, __FILE__, __LINE__, "Articulation: could not allocate low-level resources.");
+		return;
+	}
+
+	mLLArticulation->setDirty(true);
+
+	PX_ASSERT(root.getSim());
+
+	addBody(*root.getSim(), NULL, NULL);
+
+	mCore.setSim(this);
+
+	mLLArticulation->setDyContext(mScene.getDynamicsContext());
+	mLLArticulation->getSolverDesc().initData(&core.getCore(), NULL);
+
+	//mLLArticulation->onUpdateSolverDesc();
+}
+
+Sc::ArticulationSim::~ArticulationSim()
+{
+	if (!mLLArticulation)
+		return;
+
+	mScene.destroyLLArticulation(*mLLArticulation);
+
+	mScene.getSimpleIslandManager()->removeNode(mIslandNodeIndex);
+
+	mCore.setSim(NULL);
+}
+
+PxU32 Sc::ArticulationSim::findBodyIndex(BodySim& body) const
+{
+	for(PxU32 i=0; i<mBodies.size(); i++)
+	{
+		if(mBodies[i]==&body)
+			return i;
+	}
+	PX_ASSERT(0);
+	return 0x80000000;
+}
+
+void Sc::ArticulationSim::addLoopConstraint(ConstraintSim* constraintSim)
+{
+	const PxU32 size = mLoopConstraints.size();
+	if (size < mLoopConstraints.size())
+		mLoopConstraints.reserve(size*2 + 1);
+	
+	BodySim* bodySim0 = constraintSim->getBody(0);
+	BodySim* bodySim1 = constraintSim->getBody(1);
+
+	ArticulationLoopConstraint lConstraint;
+	if (bodySim0)
+		lConstraint.linkIndex0 = findBodyIndex(*bodySim0);
+	else
+		lConstraint.linkIndex0 = 0x80000000;
+
+	if(bodySim1)
+		lConstraint.linkIndex1 = findBodyIndex(*bodySim1);
+	else
+		lConstraint.linkIndex1 = 0x80000000;
+
+	lConstraint.constraint = &constraintSim->getLowLevelConstraint();
+
+	mLoopConstraints.pushBack(lConstraint);
+}
+
+void Sc::ArticulationSim::removeLoopConstraint(ConstraintSim* constraintSim)
+{
+	Dy::Constraint* constraint = &constraintSim->getLowLevelConstraint();
+
+	const PxU32 size = mLoopConstraints.size();
+	PxU32 index = 0;
+	while (index < size && mLoopConstraints[index].constraint != constraint)
+		++index;
+
+	if (index != size)
+		mLoopConstraints.replaceWithLast(index);
+}
+
+void Sc::ArticulationSim::updateCached(Cm::BitMapPinned* shapeChangedMap)
+{
+	for(PxU32 i=0; i<mBodies.size(); i++)
+		mBodies[i]->updateCached(shapeChangedMap);
+}
+
+void Sc::ArticulationSim::markShapesUpdated(Cm::BitMapPinned* shapeChangedMap)
+{
+	for (PxU32 a = 0; a < mBodies.size(); ++a)
+	{
+		Sc::ElementSim* current = mBodies[a]->getElements_();
+		while (current)
+		{
+			Sc::ShapeSim* sim = static_cast<Sc::ShapeSim*>(current);
+			if (sim->isInBroadPhase())
+				shapeChangedMap->growAndSet(sim->getElementID());
+			current = current->mNextInActor;
+		}
+	}
+}
+
+void Sc::ArticulationSim::updateContactDistance(PxReal* contactDistance, const PxReal dt, Bp::BoundsArray& boundsArray)
+{
+	for (PxU32 i = 0; i<mBodies.size(); i++)
+		mBodies[i]->updateContactDistance(contactDistance, dt, boundsArray);
+}
+
+ArticulationLinkHandle Sc::ArticulationSim::getLinkHandle(BodySim &body) const
+{
+	return reinterpret_cast<size_t>(mLLArticulation) | findBodyIndex(body);
+}
+
+void Sc::ArticulationSim::addBody(BodySim& body, BodySim* parent, ArticulationJointSim* joint)
+{
+	mBodies.pushBack(&body);
+	mJoints.pushBack(joint);
+	mLLArticulation->addBody();
+
+	const PxU32 index = mLinks.size();
+
+	PX_ASSERT((((index==0) && (joint == 0)) && (parent == 0)) ||
+			  (((index!=0) && joint) && (parent && (parent->getArticulation() == this))));
+
+	ArticulationLink& link = mLinks.insert();
+	link.bodyCore	= &body.getBodyCore().getCore();
+	link.children	= 0;
+	bool shouldSleep;
+	bool currentlyAsleep;
+	const bool bodyReadyForSleep = body.checkSleepReadinessBesidesWakeCounter();
+	const PxReal wakeCounter = getCore().getWakeCounter();
+
+	if(parent)
+	{
+		currentlyAsleep = !mBodies[0]->isActive();
+		shouldSleep = currentlyAsleep && bodyReadyForSleep;
+
+		PxU32 parentIndex = findBodyIndex(*parent);
+		link.parent = parentIndex;
+		link.pathToRoot = mLinks[parentIndex].pathToRoot | ArticulationBitField(1)<<index;
+		link.inboundJoint = &joint->getCore().getCore();
+		mLinks[parentIndex].children |= ArticulationBitField(1)<<index;
+	}
+	else
+	{
+		currentlyAsleep = (wakeCounter == 0.0f);
+		shouldSleep = currentlyAsleep && bodyReadyForSleep;
+
+		link.parent = DY_ARTICULATION_LINK_NONE;
+		link.pathToRoot = 1;
+		link.inboundJoint = NULL;
+	}
+	
+	const PxU32 low = PxU32(link.pathToRoot & 0xffffffff);
+	const PxU32 high = PxU32(link.pathToRoot >> 32);
+	const PxU32 depth = Ps::bitCount(low) + Ps::bitCount(high);
+	mMaxDepth = PxMax(depth, mMaxDepth);
+
+	mLLArticulation->setMaxDepth(mMaxDepth);
+
+	if(currentlyAsleep && !shouldSleep)
+	{
+		for(PxU32 i=0; i < (mBodies.size() - 1); i++)
+			mBodies[i]->internalWakeUpArticulationLink(wakeCounter);
+	}
+
+	body.setArticulation(this, wakeCounter, shouldSleep, index);
+}
+
+void Sc::ArticulationSim::removeBody(BodySim &body)
+{
+	PX_ASSERT(body.getArticulation() == this);
+	PxU32 index = findBodyIndex(body);
+	body.setArticulation(NULL, 0.0f, true, 0);
+
+	ArticulationLink &link0 = mLinks[index];
+
+	PX_ASSERT(link0.children == 0);
+	PX_UNUSED(link0);
+
+	// copy all the later links down by one
+	for(PxU32 i=index+1;i<mLinks.size();i++)
+	{
+		mLinks[i-1] = mLinks[i];
+		mBodies[i-1] = mBodies[i];
+		mJoints[i-1] = mJoints[i];
+		//setIslandHandle(*mBodies[i-1], i-1);
+	}
+
+	// adjust parent/child indices
+	ArticulationBitField fixedIndices = (ArticulationBitField(1)<<index)-1;
+	ArticulationBitField shiftIndices = ~(fixedIndices|(ArticulationBitField(1)<<index));
+
+	mMaxDepth = 0;
+	for(PxU32 i=0;i<mLinks.size();i++)
+	{
+		ArticulationLink &link = mLinks[i];
+
+		if(link.parent != DY_ARTICULATION_LINK_NONE && link.parent>index)
+			link.pathToRoot = (link.pathToRoot&fixedIndices) | (link.pathToRoot&shiftIndices)>>1;
+		link.children = (link.children&fixedIndices) | (link.children&shiftIndices)>>1;
+
+		const PxU32 low = PxU32(link.pathToRoot & 0xffffffff);
+		const PxU32 high = PxU32(link.pathToRoot >> 32);
+		const PxU32 depth = Ps::bitCount(low) + Ps::bitCount(high);
+		mMaxDepth = PxMax(depth, mMaxDepth);
+	}
+
+	mLinks.popBack();
+	mBodies.popBack();
+	mJoints.popBack();
+
+	mLLArticulation->setMaxDepth(mMaxDepth);
+	mLLArticulation->removeBody();
+}
+
+void Sc::ArticulationSim::checkResize() const
+{
+	if(!mBodies.size())
+		return;
+
+	mLLArticulation->setupLinks(mLinks.size(), const_cast<Dy::ArticulationLink*>(mLinks.begin()));
+}
+
+PxU32 Sc::ArticulationSim::getCCDLinks(BodySim** sims)
+{
+	PxU32 nbCCDBodies = 0;
+	for (PxU32 a = 0; a < mBodies.size(); ++a)
+	{
+		if (mBodies[a]->getLowLevelBody().getCore().mFlags & PxRigidBodyFlag::eENABLE_CCD)
+		{
+			sims[nbCCDBodies++] = mBodies[a];
+		}
+	}
+	return nbCCDBodies;
+}
+
+void Sc::ArticulationSim::sleepCheck(PxReal dt)
+{
+	if(!mBodies.size())
+		return;
+
+#if PX_CHECKED
+	{
+		PxReal maxTimer = 0.0f, minTimer = PX_MAX_F32;
+		bool allActive = true, noneActive = true;
+		for(PxU32 i=0;i<mLinks.size();i++)
+		{
+			PxReal timer = mBodies[i]->getBodyCore().getWakeCounter();
+			maxTimer = PxMax(maxTimer, timer);
+			minTimer = PxMin(minTimer, timer);
+			bool active = mBodies[i]->isActive();
+			allActive &= active;
+			noneActive &= !active;
+		}
+		// either all links are asleep, or no links are asleep
+		PX_ASSERT(maxTimer==0 || minTimer!=0);
+		PX_ASSERT(allActive || noneActive);
+	}
+
+#endif
+
+	if(!mBodies[0]->isActive())
+		return;
+
+	const PxReal sleepThreshold = getCore().getCore().sleepThreshold;
+
+	PxReal maxTimer = 0.0f, minTimer = PX_MAX_F32;
+
+	for(PxU32 i=0;i<mLinks.size();i++)
+	{
+		const Cm::SpatialVector& motionVelocity = mLLArticulation->getMotionVelocity(i);
+		PxReal timer = mBodies[i]->updateWakeCounter(dt, sleepThreshold, motionVelocity);
+		maxTimer = PxMax(maxTimer, timer);
+		minTimer = PxMin(minTimer, timer);
+	}
+
+	mCore.setWakeCounterInternal(maxTimer);
+
+	if(maxTimer != 0.0f)
+	{
+		if(minTimer == 0.0f)
+		{
+			// make sure nothing goes to sleep unless everything does
+			for(PxU32 i=0;i<mLinks.size();i++)
+				mBodies[i]->getBodyCore().setWakeCounterFromSim(PxMax(1e-6f, mBodies[i]->getBodyCore().getWakeCounter()));
+		}
+		return;
+	}
+
+	for(PxU32 i=0;i<mLinks.size();i++)
+	{
+		mBodies[i]->notifyReadyForSleeping();
+		mBodies[i]->resetSleepFilter();
+	}
+
+	mScene.getSimpleIslandManager()->deactivateNode(mIslandNodeIndex);
+}
+
+bool Sc::ArticulationSim::isSleeping() const
+{
+	bool isActive = mBodies[0]->isActive();
+	PX_UNUSED(isActive);
+	return (mBodies.size() > 0) ? (!mBodies[0]->isActive()) : true;
+}
+
+void Sc::ArticulationSim::internalWakeUp(PxReal wakeCounter)
+{
+	if(mCore.getWakeCounter() < wakeCounter)
+	{
+		mCore.setWakeCounterInternal(wakeCounter);
+		for(PxU32 i=0;i<mLinks.size();i++)
+			mBodies[i]->internalWakeUpArticulationLink(wakeCounter);
+	}
+}
+
+void Sc::ArticulationSim::setActive(const bool b, const PxU32 infoFlag)
+{
+	for(PxU32 i=0;i<mBodies.size();i++)
+	{
+		if (i+1 < mBodies.size())
+		{
+			Ps::prefetchLine(mBodies[i+1],0);
+			Ps::prefetchLine(mBodies[i+1],128);
+		}
+		mBodies[i]->setActive(b, infoFlag);
+	}
+}
+
+void Sc::ArticulationSim::updateForces(PxReal dt, bool simUsesAdaptiveForce)
+{
+	PxU32 count = 0;
+
+	for(PxU32 i=0;i<mBodies.size();i++)
+	{
+		if (i+1 < mBodies.size())
+		{
+			Ps::prefetchLine(mBodies[i+1],128);
+			Ps::prefetchLine(mBodies[i+1],256);
+		}
+
+		PxU32 type = mLLArticulation->getType();
+		const bool useAccelerations = (type == Articulation::Enum::eReducedCoordinate);
+
+		mBodies[i]->updateForces(dt, NULL, NULL, count, &mLLArticulation->getSolverDesc().acceleration[i], 
+			useAccelerations, simUsesAdaptiveForce);
+	}
+}
+
+void Sc::ArticulationSim::saveLastCCDTransform()
+{
+	for(PxU32 i=0;i<mBodies.size();i++)
+	{
+		if (i+1 < mBodies.size())
+		{
+			Ps::prefetchLine(mBodies[i+1],128);
+			Ps::prefetchLine(mBodies[i+1],256);
+		}
+		mBodies[i]->getLowLevelBody().saveLastCCDTransform();
+	}
+}
+
+Sc::ArticulationDriveCache* Sc::ArticulationSim::createDriveCache(PxReal compliance, PxU32 driveIterations) const
+{
+	checkResize();
+	PxU32 solverDataSize, totalSize, scratchSize;
+	getLowLevelArticulation()->getDataSizes(mLinks.size(), solverDataSize, totalSize, scratchSize);
+
+	// In principle we should only need solverDataSize here. But right now prepareFsData generates the auxiliary data 
+	// for use in potential debugging, which takes up extra space. 
+	FsData* data = reinterpret_cast<FsData*>(PX_ALLOC(totalSize,"Articulation Drive Cache"));
+	PxvArticulationDriveCache::initialize(*data, Ps::to16(mLinks.size()), mLinks.begin(), compliance, driveIterations, mLLArticulation->getSolverDesc().scratchMemory, mLLArticulation->getSolverDesc().scratchMemorySize);
+	
+	return data;
+}
+
+void Sc::ArticulationSim::updateDriveCache(ArticulationDriveCache& cache, PxReal compliance, PxU32 driveIterations) const
+{
+	checkResize();
+	PxvArticulationDriveCache::initialize(cache,  Ps::to16(mLinks.size()), mLinks.begin(), compliance, driveIterations, 
+		mLLArticulation->getSolverDesc().scratchMemory, mLLArticulation->getSolverDesc().scratchMemorySize);
+}
+
+void Sc::ArticulationSim::releaseDriveCache(Sc::ArticulationDriveCache& driveCache) const
+{
+	PX_FREE(&driveCache);
+}
+
+void Sc::ArticulationSim::applyImpulse(Sc::BodyCore& link, const Sc::ArticulationDriveCache& driveCache, const PxVec3& force, const PxVec3& torque)
+{
+	Cm::SpatialVectorV v[DY_ARTICULATION_MAX_SIZE], z[DY_ARTICULATION_MAX_SIZE];
+	PxMemZero(z, mLinks.size()*sizeof(Cm::SpatialVector));
+	PxMemZero(v, mLinks.size()*sizeof(Cm::SpatialVector));
+
+	PxU32 bodyIndex = findBodyIndex(*link.getSim());
+	z[bodyIndex].linear = Ps::aos::V3LoadU(-force);
+	z[bodyIndex].angular = Ps::aos::V3LoadU(-torque);
+
+	PxvArticulationDriveCache::applyImpulses(driveCache, z, v);
+	for(PxU32 i=0;i<mLinks.size();i++)
+	{
+		Sc::BodyCore& body = mBodies[i]->getBodyCore();
+		PxVec3 lv, av;
+		Ps::aos::V3StoreU(v[i].linear, lv);
+		Ps::aos::V3StoreU(v[i].angular, av);
+
+		body.setLinearVelocity(body.getLinearVelocity()+lv);
+		body.setAngularVelocity(body.getAngularVelocity()+av);
+	}
+}
+
+void Sc::ArticulationSim::computeImpulseResponse(Sc::BodyCore& link,
+												  PxVec3& linearResponse, 
+												  PxVec3& angularResponse,
+												  const Sc::ArticulationDriveCache& driveCache,
+												  const PxVec3& force,
+												  const PxVec3& torque) const
+{
+	Cm::SpatialVectorV v;
+	PxvArticulationDriveCache::getImpulseResponse(driveCache, findBodyIndex(*link.getSim()), Cm::SpatialVectorV(Ps::aos::V3LoadU(force), Ps::aos::V3LoadU(torque)), v);
+	Ps::aos::V3StoreU(v.linear, linearResponse);
+	Ps::aos::V3StoreU(v.angular, angularResponse);
+}
+
+
+void Sc::ArticulationSim::setKinematicLink(const bool value)
+{
+	const PxU32 linkCount = mLinks.size();
+
+	if (linkCount > 0)
+	{
+		mLinks[0].bodyCore->kinematicLink = PxU8(value);
+	}
+}
+
+PxU32 Sc::ArticulationSim::getDofs() const
+{
+	return mLLArticulation->getDofs();
+}
+
+PxU32 Sc::ArticulationSim::getDof(const PxU32 linkID) const
+{
+	return mLLArticulation->getDof(linkID);
+}
+
+PX_COMPILE_TIME_ASSERT(sizeof(Cm::SpatialVector)==sizeof(PxSpatialForce));
+PxArticulationCache* Sc::ArticulationSim::createCache() const
+{
+	checkResize();
+
+	PxU32 totalSize = getCacheDataSize() + sizeof(PxArticulationCache);
+
+	const PxU32 linkCount = mLinks.size();
+	const PxU32 jointCount = linkCount - 1;
+
+	PxU8* tCache = reinterpret_cast<PxU8*>(PX_ALLOC(totalSize, "Articulation cache"));
+
+	PxMemZero(tCache, totalSize);
+
+	const PxU32 totalDofs = mLLArticulation->getDofs();
+	
+
+	PxArticulationCache* cache = reinterpret_cast<PxArticulationCache*>(tCache);
+
+	PxU32 offset = sizeof(PxArticulationCache);
+	cache->externalForces = reinterpret_cast<PxSpatialForce*>(tCache + offset);
+	offset += sizeof(PxSpatialForce) * linkCount;
+	
+	cache->denseJacobian = reinterpret_cast<PxReal*>(tCache + offset);
+	offset += sizeof(PxReal) * (6 + totalDofs) * ((1 + jointCount) * 6);				//size of dense jacobian assuming free floating base link.
+
+	cache->massMatrix = reinterpret_cast<PxReal*>(tCache + offset);
+
+	offset += sizeof(PxReal) *totalDofs * totalDofs;
+	cache->jointVelocity = reinterpret_cast<PxReal*>(tCache + offset);
+
+	offset += sizeof(PxReal) * totalDofs;
+	cache->jointAcceleration = reinterpret_cast<PxReal*>(tCache + offset);
+
+	offset += sizeof(PxReal) * totalDofs;
+	cache->jointPosition = reinterpret_cast<PxReal*>(tCache + offset);
+
+	offset += sizeof(PxReal) * totalDofs;
+	cache->jointForce = reinterpret_cast<PxReal*>(tCache + offset);
+
+	offset += sizeof(PxReal) * totalDofs;
+	cache->linkVelocity = reinterpret_cast<PxSpatialVelocity*>(tCache + offset);
+
+	offset += sizeof(PxSpatialVelocity) * linkCount;
+	cache->linkAcceleration = reinterpret_cast<PxSpatialVelocity*>(tCache + offset);
+
+	offset += sizeof(PxSpatialVelocity) * linkCount;
+	cache->rootLinkData = reinterpret_cast<PxArticulationRootLinkData*>(tCache + offset);
+
+	cache->coefficientMatrix = NULL;
+	cache->lambda =NULL;
+
+	const PxU32 scratchMemorySize = getScratchMemorySize();
+	void* scratchMemory = PX_ALLOC(scratchMemorySize, "Cache scratch memory");
+	cache->scratchMemory = scratchMemory;
+
+	cache->scratchAllocator = PX_PLACEMENT_NEW(PX_ALLOC(sizeof(PxcScratchAllocator), "PxScrachAllocator"), PxcScratchAllocator)();
+
+	reinterpret_cast<PxcScratchAllocator*>(cache->scratchAllocator)->setBlock(scratchMemory, scratchMemorySize);
+
+	return cache;
+}
+
+PxU32 Sc::ArticulationSim::getCacheDataSize() const
+{
+	const PxU32 totalDofs = mLLArticulation->getDofs();
+	const PxU32 linkCount = mLinks.size();
+	const PxU32 jointCount = linkCount - 1;
+	PxU32 totalSize =
+		sizeof(PxSpatialForce) * linkCount							//external force
+		+ sizeof(PxReal) * (6 + totalDofs) * ((1 + jointCount) * 6)		//offset to end of dense jacobian (assuming free floating base)
+		+ sizeof(PxReal) * totalDofs * totalDofs						//mass matrix
+		+ sizeof(PxReal) * totalDofs * 4								//jointVelocity, jointAcceleration, jointPosition, joint force
+		+ sizeof(PxSpatialVelocity) * linkCount * 2						//link velocity, link acceleration
+		+ sizeof(PxArticulationRootLinkData);							//root link data
+	
+	return totalSize;
+}
+
+PxU32 Sc::ArticulationSim::getScratchMemorySize() const
+{
+	const PxU32 totalDofs = mLLArticulation->getDofs();
+	const PxU32 linkCount = mLinks.size();
+
+	const PxU32 totalSize =
+		sizeof(Cm::SpatialVectorF) * linkCount * 5				//motionVelocity, motionAccelerations, coriolisVectors, spatialZAVectors, externalAccels;
+		+ sizeof(Dy::SpatialMatrix) * linkCount					//compositeSpatialInertias;
+		+ sizeof(PxReal) * totalDofs * 5;						//jointVelocity, jointAcceleration, jointForces, jointPositions, jointFrictionForces
+
+	return totalSize;
+}
+
+void Sc::ArticulationSim::zeroCache(PxArticulationCache& cache) const
+{
+	const PxU32 cacheDataSize = getCacheDataSize();
+
+	PxMemZero(cache.externalForces, cacheDataSize);
+}
+
+//copy external data to internal data
+void  Sc::ArticulationSim::applyCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag) const
+{
+	//checkResize();
+	if (mLLArticulation->applyCache(cache, flag))
+	{
+		mScene.getSimulationController()->updateArticulation(mLLArticulation, mIslandNodeIndex);
+	}
+}
+
+//copy internal data to external data
+void Sc::ArticulationSim::copyInternalStateToCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag) const
+{
+	mLLArticulation->copyInternalStateToCache(cache, flag);
+}
+
+//release cache
+void Sc::ArticulationSim::releaseCache(PxArticulationCache& cache) const
+{
+	if (cache.scratchAllocator)
+	{
+		PxcScratchAllocator* scratchAlloc = reinterpret_cast<PxcScratchAllocator*>(cache.scratchAllocator);
+		scratchAlloc->~PxcScratchAllocator();
+		PX_FREE_AND_RESET(cache.scratchAllocator);
+	}
+
+	if (cache.scratchMemory)
+		PX_FREE_AND_RESET(cache.scratchMemory);
+
+	PX_FREE(&cache);
+}
+
+void Sc::ArticulationSim::packJointData(const PxReal* maximum, PxReal* reduced) const
+{
+	mLLArticulation->packJointData(maximum, reduced);
+}
+
+void Sc::ArticulationSim::unpackJointData(const PxReal* reduced, PxReal* maximum) const
+{
+	mLLArticulation->unpackJointData(reduced, maximum);
+}
+
+void Sc::ArticulationSim::commonInit()
+{
+	mLLArticulation->initializeCommonData();
+}
+
+void Sc::ArticulationSim::computeGeneralizedGravityForce(PxArticulationCache& cache)
+{
+	mLLArticulation->getGeneralizedGravityForce(mScene.getGravityFast(), cache);
+}
+
+void Sc::ArticulationSim::computeCoriolisAndCentrifugalForce(PxArticulationCache& cache)
+{
+	mLLArticulation->getCoriolisAndCentrifugalForce(cache);
+}
+
+void Sc::ArticulationSim::computeGeneralizedExternalForce(PxArticulationCache& cache)
+{
+	mLLArticulation->getGeneralizedExternalForce(cache);
+}
+
+void Sc::ArticulationSim::computeJointAcceleration(PxArticulationCache& cache)
+{
+	mLLArticulation->getJointAcceleration(mScene.getGravityFast(), cache);
+}
+
+void Sc::ArticulationSim::computeJointForce(PxArticulationCache& cache)
+{
+	mLLArticulation->getJointForce(cache);
+}
+
+void Sc::ArticulationSim::computeDenseJacobian(PxArticulationCache& cache, PxU32& nRows, PxU32& nCols)
+{
+	mLLArticulation->getDenseJacobian(cache, nRows, nCols);
+}
+
+void Sc::ArticulationSim::computeCoefficientMatrix(PxArticulationCache& cache)
+{
+	mLLArticulation->getCoefficientMatrixWithLoopJoints(mLoopConstraints.begin(), mLoopConstraints.size(), cache);
+}
+
+bool Sc::ArticulationSim::computeLambda(PxArticulationCache& cache, PxArticulationCache& initialState,
+	const PxReal* const jointTorque, const PxVec3 gravity, const PxU32 maxIter)
+{
+	return mLLArticulation->getLambda(mLoopConstraints.begin(), mLoopConstraints.size(), cache, initialState, jointTorque, gravity, maxIter);
+}
+
+void Sc::ArticulationSim::computeGeneralizedMassMatrix(PxArticulationCache& cache)
+{
+	mLLArticulation->getGeneralizedMassMatrixCRB(cache);
+
+	/*const PxU32 totalDofs = mLLArticulation->getDofs();
+
+	PxReal* massMatrix = reinterpret_cast<PxReal*>(PX_ALLOC(sizeof(PxReal) * totalDofs * totalDofs, "MassMatrix"));
+	PxMemCopy(massMatrix, cache.massMatrix, sizeof(PxReal)*totalDofs * totalDofs);
+
+	mLLArticulation->getGeneralizedMassMatrix(cache);
+
+	PxReal* massMatrix1 = cache.massMatrix;
+	for (PxU32 i = 0; i < totalDofs; ++i)
+	{
+		PxReal* row = &massMatrix1[i * totalDofs];
+
+		for (PxU32 j = 0; j < totalDofs; ++j)
+		{
+			const PxReal dif = row[j] - massMatrix[j*totalDofs + i];
+			PX_ASSERT (PxAbs(dif) < 2e-4f)
+		}
+	}
+
+	PX_FREE(massMatrix);*/
+}
+
+PxU32 Sc::ArticulationSim::getCoefficientMatrixSize() const
+{
+	const PxU32 size = mLoopConstraints.size();
+	const PxU32 totalDofs = mLLArticulation->getDofs();
+	return sizeof(PxReal) * size * totalDofs;
+}
+
+PxSpatialVelocity Sc::ArticulationSim::getLinkVelocity(const PxU32 linkId) const
+{
+	Cm::SpatialVector vel = mLLArticulation->getMotionVelocity(linkId);
+	return reinterpret_cast<PxSpatialVelocity&>(vel);
+}
+
+PxSpatialVelocity Sc::ArticulationSim::getLinkAcceleration(const PxU32 linkId) const
+{
+	Cm::SpatialVector accel = mLLArticulation->getMotionAcceleration(linkId);
+	return reinterpret_cast<PxSpatialVelocity&>(accel);
+}
+
+// This method allows user teleport the root links and the articulation
+//system update all other links pose
+void Sc::ArticulationSim::setGlobalPose()
+{
+	checkResize();
+	mLLArticulation->teleportRootLink();
+}
+
+void Sc::ArticulationSim::setDirty(const bool dirty)
+{
+	mLLArticulation->setDirty(dirty);
+	if(dirty)
+		mScene.getSimulationController()->updateArticulation(mLLArticulation, mIslandNodeIndex);
+}
+
+void Sc::ArticulationSim::setJointDirty(Dy::ArticulationJointCore& jointCore)
+{
+	PX_UNUSED(jointCore);
+	mScene.getSimulationController()->updateArticulationJoint(mLLArticulation, mIslandNodeIndex);
+}
+
+void Sc::ArticulationSim::debugCheckWakeCounterOfLinks(PxReal wakeCounter) const
+{
+	PX_UNUSED(wakeCounter);
+
+#ifdef _DEBUG
+	// make sure the links are in sync with the articulation
+	for(PxU32 i=0; i < mBodies.size(); i++)
+	{
+		PX_ASSERT(mBodies[i]->getBodyCore().getWakeCounter() == wakeCounter);
+	}
+#endif
+}
+
+void Sc::ArticulationSim::debugCheckSleepStateOfLinks(bool isSleeping) const
+{
+	PX_UNUSED(isSleeping);
+
+#ifdef _DEBUG
+	// make sure the links are in sync with the articulation
+	for(PxU32 i=0; i < mBodies.size(); i++)
+	{
+		if (isSleeping)
+		{
+			PX_ASSERT(!mBodies[i]->isActive());
+			PX_ASSERT(mBodies[i]->getBodyCore().getWakeCounter() == 0.0f);
+			PX_ASSERT(mBodies[i]->checkSleepReadinessBesidesWakeCounter());
+		}
+		else
+			PX_ASSERT(mBodies[i]->isActive());
+	}
+#endif
+}

physx/source/simulationcontroller/src/ScArticulationSim.h

@@ -0,0 +1,223 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef PX_PHYSICS_ARTICULATION_SIM
+#define PX_PHYSICS_ARTICULATION_SIM
+
+
+#include "PsUserAllocated.h"
+#include "CmPhysXCommon.h"
+#include "DyArticulation.h"
+#include "ScArticulationCore.h" 
+#include "PxsSimpleIslandManager.h"
+
+namespace physx
+{
+
+namespace Dy
+{
+	class Articulation;
+}
+
+class PxsTransformCache;
+class PxsSimulationController;
+class PxJoint;
+
+namespace Cm
+{
+	class SpatialVector;
+	template <class Allocator> class BitMapBase;
+	typedef BitMapBase<Ps::NonTrackingAllocator> BitMap;
+}
+
+namespace Bp
+{
+	class BoundsArray;
+}
+
+namespace Sc
+{
+
+	class BodySim;
+	class ArticulationJointSim;
+	class ArticulationCore;
+	class Scene;
+	class ConstraintSim;
+
+
+	class ArticulationSim : public Ps::UserAllocated 
+	{
+	public:
+											ArticulationSim(ArticulationCore& core, 
+												Scene& scene,
+												BodyCore& root);
+
+											~ArticulationSim();
+
+		PX_INLINE	Dy::ArticulationV*		getLowLevelArticulation() const { return mLLArticulation; }
+		PX_INLINE	ArticulationCore&		getCore() const { return mCore; }
+
+								void		addBody(BodySim& body, 
+													BodySim* parent, 
+													ArticulationJointSim* joint);
+								void		removeBody(BodySim &sim);
+								
+								Dy::ArticulationLinkHandle	getLinkHandle(BodySim& body) const;
+								
+								void		checkResize() const;						// resize LL memory if necessary
+								
+								void		debugCheckWakeCounterOfLinks(PxReal wakeCounter) const;
+								void		debugCheckSleepStateOfLinks(bool isSleeping) const;
+
+								bool		isSleeping() const;
+								void		internalWakeUp(PxReal wakeCounter);	// called when sim sets sleep timer
+								void		sleepCheck(PxReal dt);
+								PxU32		getCCDLinks(BodySim** sims);
+								void		updateCached(Cm::BitMapPinned* shapehapeChangedMap);
+								void		markShapesUpdated(Cm::BitMapPinned* shapeChangedMap);
+								void		updateContactDistance(PxReal* contactDistance, const PxReal dt, Bp::BoundsArray& boundsArray);
+
+								void		setActive(const bool b, const PxU32 infoFlag=0);
+
+								void		updateForces(PxReal dt, bool simUsesAdaptiveForce);
+								void		saveLastCCDTransform();
+
+	// drive cache implementation
+	//
+						ArticulationDriveCache*		
+											createDriveCache(PxReal compliance,
+															 PxU32 driveIterations) const;
+
+						void				updateDriveCache(ArticulationDriveCache& cache,
+															 PxReal compliance,
+															 PxU32 driveIterations) const;
+
+						void				releaseDriveCache(ArticulationDriveCache& cache) const;
+						
+						void				applyImpulse(BodyCore& link,
+														 const ArticulationDriveCache& driveCache,
+														 const PxVec3& force,
+														 const PxVec3& torque);
+
+						void				computeImpulseResponse(BodyCore& link,
+																   PxVec3& linearResponse,
+																   PxVec3& angularResponse,
+																   const ArticulationDriveCache& driveCache,
+																   const PxVec3& force,
+																   const PxVec3& torque) const;
+
+
+					void					setKinematicLink(const bool value);
+					//external reduced coordinate implementation
+					PxU32					getDofs() const;
+
+					//This function return the dof of the inbound joint, which belong to a link with corresponding linkID
+					PxU32					getDof(const PxU32 linkID) const;
+
+					PxArticulationCache*	createCache() const;
+
+					PxU32					getCacheDataSize() const;
+
+					PxU32					getScratchMemorySize() const;
+
+					void					zeroCache(PxArticulationCache&) const;
+
+					void					applyCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag) const;
+
+					void					copyInternalStateToCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag) const;
+
+					void					releaseCache(PxArticulationCache&) const;
+
+					void					packJointData(const PxReal* maximum, PxReal* reduced) const;
+
+					void					unpackJointData(const PxReal* reduced, PxReal* maximum) const;
+
+					void					commonInit();
+
+					void					computeGeneralizedGravityForce(PxArticulationCache& cache);
+
+					void					computeCoriolisAndCentrifugalForce(PxArticulationCache& cache);
+
+					void					computeGeneralizedExternalForce(PxArticulationCache& cache);
+
+					void					computeJointAcceleration(PxArticulationCache& cache);
+
+					void					computeJointForce(PxArticulationCache& cache);
+
+					void					computeKinematicJacobian(const PxU32 linkID, PxArticulationCache& cache);
+
+					void					computeDenseJacobian(PxArticulationCache& cache, PxU32& nRows, PxU32& nCols);
+
+					void					computeCoefficientMatrix(PxArticulationCache& cache);
+
+					bool					computeLambda(PxArticulationCache& cache, PxArticulationCache& rollBackCache, const PxReal* jointTorque, const PxVec3 gravity, const PxU32 maxIter);
+
+					void					computeGeneralizedMassMatrix(PxArticulationCache& cache);
+
+					PxU32					getCoefficientMatrixSize() const;
+
+					PxSpatialVelocity		getLinkVelocity(const PxU32 linkId) const;
+
+					PxSpatialVelocity		getLinkAcceleration(const PxU32 linkId) const;
+
+					//internal method implementation
+					PX_FORCE_INLINE IG::NodeIndex		getIslandNodeIndex() const { return mIslandNodeIndex; }
+
+					void					setGlobalPose();
+
+					void					setDirty(const bool dirty);
+					PxU32					findBodyIndex(BodySim &body) const;
+
+					void					setJointDirty(Dy::ArticulationJointCore& jointCore);
+
+					void					addLoopConstraint(ConstraintSim* constraint);
+					void					removeLoopConstraint(ConstraintSim* constraint);
+
+					PxU32					getMaxDepth() { return mMaxDepth; }
+
+	private:
+					ArticulationSim&		operator=(const ArticulationSim&);
+
+					Dy::ArticulationV*								mLLArticulation;
+					Scene&											mScene;
+					ArticulationCore&								mCore;
+					Ps::Array<Dy::ArticulationLink>					mLinks;
+					Ps::Array<BodySim*>								mBodies;
+					Ps::Array<ArticulationJointSim*>				mJoints;
+					IG::NodeIndex									mIslandNodeIndex;
+					Ps::Array <Dy::ArticulationLoopConstraint>		mLoopConstraints;
+					PxU32											mMaxDepth;
+	};
+
+} // namespace Sc
+
+}
+
+#endif

physx/source/simulationcontroller/src/ScBodyCore.cpp

@@ -0,0 +1,687 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "ScBodyCore.h"
+#include "ScBodySim.h"
+#include "ScPhysics.h"
+#include "ScScene.h"
+#include "PxsSimulationController.h"
+#include "PsFoundation.h"
+
+using namespace physx;
+
+static void updateBodySim(Sc::BodySim* bodySim)
+{
+	const bool isArticulationLink = bodySim->isArticulationLink();
+	bodySim->getScene().getSimulationController()->updateDynamic(isArticulationLink, bodySim->getNodeIndex());
+}
+
+static void updateBodySim(Sc::BodyCore& bodyCore)
+{
+	Sc::BodySim* bodySim = bodyCore.getSim();
+	if(bodySim)
+		updateBodySim(bodySim);
+}
+
+Sc::BodyCore::BodyCore(PxActorType::Enum type, const PxTransform& bodyPose) :
+	RigidCore		(type),
+	mSimStateData	(NULL)
+{
+	const PxTolerancesScale& scale = Physics::getInstance().getTolerancesScale();
+
+	const bool isDynamic = type == PxActorType::eRIGID_DYNAMIC;
+	const float linearDamping = isDynamic ? 0.0f : 0.05f;
+	const float maxLinearVelocitySq = isDynamic ? 1e32f /*PX_MAX_F32*/ : 100.f * 100.f * scale.length * scale.length;
+	const float maxAngularVelocitySq = isDynamic ? 100.0f * 100.0f : 50.0f * 50.0f;
+
+	mCore.init(bodyPose, PxVec3(1.0f), 1.0f, Sc::Physics::sWakeCounterOnCreation, scale.speed, linearDamping, 0.05f, maxLinearVelocitySq, maxAngularVelocitySq);
+}
+
+Sc::BodyCore::~BodyCore()
+{
+	PX_ASSERT(getSim() == 0);
+	PX_ASSERT(!mSimStateData);
+}
+
+Sc::BodySim* Sc::BodyCore::getSim() const
+{
+	return static_cast<BodySim*>(Sc::ActorCore::getSim());
+}
+
+void Sc::BodyCore::restoreDynamicData()
+{
+	PX_ASSERT(mSimStateData && mSimStateData->isKine());
+	restore();
+}
+
+//--------------------------------------------------------------
+//
+// BodyCore interface implementation
+//
+//--------------------------------------------------------------
+
+void Sc::BodyCore::setBody2World(const PxTransform& p)
+{
+	mCore.body2World = p;
+	PX_ASSERT(p.p.isFinite());
+	PX_ASSERT(p.q.isFinite());
+
+	BodySim* sim = getSim();
+	if(sim)
+	{
+		sim->postBody2WorldChange();
+		updateBodySim(sim);
+	}
+}
+
+void Sc::BodyCore::setLinearVelocity(const PxVec3& v)
+{
+	mCore.linearVelocity = v;
+
+	updateBodySim(*this);
+}
+
+void Sc::BodyCore::setAngularVelocity(const PxVec3& v)
+{
+	mCore.angularVelocity = v;
+
+	updateBodySim(*this);
+}
+
+void Sc::BodyCore::setBody2Actor(const PxTransform& p)
+{
+	PX_ASSERT(p.p.isFinite());
+	PX_ASSERT(p.q.isFinite());
+
+	mCore.setBody2Actor(p);
+
+	BodySim* sim = getSim();
+	if(sim)
+	{
+		sim->notifyShapesOfTransformChange();
+		updateBodySim(sim);
+	}
+}
+
+void Sc::BodyCore::addSpatialAcceleration(Ps::Pool<SimStateData>* simStateDataPool, const PxVec3* linAcc, const PxVec3* angAcc)
+{
+	//The dirty flag is stored separately in the BodySim so that we query the dirty flag before going to 
+	//the expense of querying the simStateData for the velmod values.
+	BodySim* sim = getSim();
+	if(sim)
+		sim->notifyAddSpatialAcceleration();
+
+	if(!mSimStateData || !mSimStateData->isVelMod())
+		setupSimStateData(simStateDataPool, false);
+
+	VelocityMod* velmod = mSimStateData->getVelocityModData();
+	velmod->notifyAddAcceleration();
+	if(linAcc) velmod->accumulateLinearVelModPerSec(*linAcc);
+	if(angAcc) velmod->accumulateAngularVelModPerSec(*angAcc);
+}
+
+void Sc::BodyCore::setSpatialAcceleration(Ps::Pool<SimStateData>* simStateDataPool, const PxVec3* linAcc, const PxVec3* angAcc)
+{
+	//The dirty flag is stored separately in the BodySim so that we query the dirty flag before going to 
+	//the expense of querying the simStateData for the velmod values.
+	BodySim* sim = getSim();
+	if (sim)
+		sim->notifyAddSpatialAcceleration();
+
+	if (!mSimStateData || !mSimStateData->isVelMod())
+		setupSimStateData(simStateDataPool, false);
+
+	VelocityMod* velmod = mSimStateData->getVelocityModData();
+	velmod->notifyAddAcceleration();
+	if (linAcc) velmod->setLinearVelModPerSec(*linAcc);
+	if (angAcc) velmod->setAngularVelModPerSec(*angAcc);
+}
+
+void Sc::BodyCore::clearSpatialAcceleration(bool force, bool torque)
+{
+	PX_ASSERT(force || torque);
+
+	//The dirty flag is stored separately in the BodySim so that we query the dirty flag before going to 
+	//the expense of querying the simStateData for the velmod values.
+	BodySim* sim = getSim();
+	if(sim)
+		sim->notifyClearSpatialAcceleration();
+
+	if(mSimStateData)
+	{
+		PX_ASSERT(mSimStateData->isVelMod());
+		VelocityMod* velmod = mSimStateData->getVelocityModData();
+		velmod->notifyClearAcceleration();
+		if(force)
+			velmod->clearLinearVelModPerSec();
+		if(torque)
+			velmod->clearAngularVelModPerSec();
+	}
+}
+
+void Sc::BodyCore::addSpatialVelocity(Ps::Pool<SimStateData>* simStateDataPool, const PxVec3* linVelDelta, const PxVec3* angVelDelta)
+{
+	//The dirty flag is stored separately in the BodySim so that we query the dirty flag before going to 
+	//the expense of querying the simStateData for the velmod values.
+	BodySim* sim = getSim();
+	if(sim)
+		sim->notifyAddSpatialVelocity();
+
+	if(!mSimStateData || !mSimStateData->isVelMod())
+		setupSimStateData(simStateDataPool, false);
+
+	VelocityMod* velmod = mSimStateData->getVelocityModData();
+	velmod->notifyAddVelocity();
+	if(linVelDelta)
+		velmod->accumulateLinearVelModPerStep(*linVelDelta);
+	if(angVelDelta)
+		velmod->accumulateAngularVelModPerStep(*angVelDelta);
+}
+
+void Sc::BodyCore::clearSpatialVelocity(bool force, bool torque)
+{
+	PX_ASSERT(force || torque);
+
+	//The dirty flag is stored separately in the BodySim so that we query the dirty flag before going to 
+	//the expense of querying the simStateData for the velmod values.
+	BodySim* sim = getSim();
+	if(sim)
+		sim->notifyClearSpatialVelocity();
+
+	if(mSimStateData)
+	{
+		PX_ASSERT(mSimStateData->isVelMod());
+		VelocityMod* velmod = mSimStateData->getVelocityModData();
+		velmod->notifyClearVelocity();
+		if(force)
+			velmod->clearLinearVelModPerStep();
+		if(torque)
+			velmod->clearAngularVelModPerStep();
+	}
+}
+
+PxReal Sc::BodyCore::getInverseMass() const
+{
+	return mSimStateData && mSimStateData->isKine() ? mSimStateData->getKinematicData()->backupInvMass : mCore.inverseMass;
+}
+
+void Sc::BodyCore::setInverseMass(PxReal m)
+{
+	if(mSimStateData && mSimStateData->isKine())
+		mSimStateData->getKinematicData()->backupInvMass = m;
+	else
+	{
+		mCore.inverseMass = m;
+		updateBodySim(*this);
+	}
+}
+
+const PxVec3& Sc::BodyCore::getInverseInertia() const
+{
+	return mSimStateData && mSimStateData->isKine() ? mSimStateData->getKinematicData()->backupInverseInertia : mCore.inverseInertia;
+}
+
+void Sc::BodyCore::setInverseInertia(const PxVec3& i)
+{
+	if(mSimStateData && mSimStateData->isKine())
+		mSimStateData->getKinematicData()->backupInverseInertia = i;
+	else
+	{
+		mCore.inverseInertia = i;
+		updateBodySim(*this);
+	}
+}
+
+PxReal Sc::BodyCore::getLinearDamping() const
+{
+	return mSimStateData && mSimStateData->isKine() ? mSimStateData->getKinematicData()->backupLinearDamping : mCore.linearDamping;
+}
+
+void Sc::BodyCore::setLinearDamping(PxReal d)
+{
+	if(mSimStateData && mSimStateData->isKine())
+		mSimStateData->getKinematicData()->backupLinearDamping = d;
+	else
+	{
+		mCore.linearDamping = d;
+		updateBodySim(*this);
+	}
+}
+
+PxReal Sc::BodyCore::getAngularDamping() const
+{
+	return mSimStateData && mSimStateData->isKine() ? mSimStateData->getKinematicData()->backupAngularDamping : mCore.angularDamping;
+}
+
+void Sc::BodyCore::setAngularDamping(PxReal v)
+{
+	if(mSimStateData && mSimStateData->isKine())
+		mSimStateData->getKinematicData()->backupAngularDamping = v;
+	else
+	{
+		mCore.angularDamping = v;
+		updateBodySim(*this);
+	}
+}
+
+PxReal Sc::BodyCore::getMaxAngVelSq() const
+{
+	return mSimStateData && mSimStateData->isKine() ? mSimStateData->getKinematicData()->backupMaxAngVelSq : mCore.maxAngularVelocitySq;
+}
+
+void Sc::BodyCore::setMaxAngVelSq(PxReal v)
+{
+	if(mSimStateData && mSimStateData->isKine())
+		mSimStateData->getKinematicData()->backupMaxAngVelSq = v;
+	else
+	{
+		mCore.maxAngularVelocitySq = v;
+		updateBodySim(*this);
+	}
+}
+
+PxReal Sc::BodyCore::getMaxLinVelSq() const
+{
+	return mSimStateData && mSimStateData->isKine() ? mSimStateData->getKinematicData()->backupMaxLinVelSq : mCore.maxLinearVelocitySq;
+}
+
+void Sc::BodyCore::setMaxLinVelSq(PxReal v)
+{
+	if (mSimStateData && mSimStateData->isKine())
+		mSimStateData->getKinematicData()->backupMaxLinVelSq = v;
+	else
+	{
+		mCore.maxLinearVelocitySq = v;
+		updateBodySim(*this);
+	}
+}
+
+void Sc::BodyCore::setFlags(Ps::Pool<SimStateData>* simStateDataPool, PxRigidBodyFlags f)
+{
+	const PxRigidBodyFlags old = mCore.mFlags;
+	if(f != old)
+	{
+		const PxU32 wasKinematic = old & PxRigidBodyFlag::eKINEMATIC;
+		const PxU32 isKinematic = f & PxRigidBodyFlag::eKINEMATIC;
+		const bool switchToKinematic = ((!wasKinematic) && isKinematic);
+		const bool switchToDynamic = (wasKinematic && (!isKinematic));
+
+		mCore.mFlags = f;
+		BodySim* sim = getSim();
+		if (sim)
+		{
+			PX_ASSERT(simStateDataPool);
+
+			const PxU32 posePreviewFlag = f & PxRigidBodyFlag::eENABLE_POSE_INTEGRATION_PREVIEW;
+			if(PxU32(old & PxRigidBodyFlag::eENABLE_POSE_INTEGRATION_PREVIEW) != posePreviewFlag)
+				sim->postPosePreviewChange(posePreviewFlag);
+
+			// for those who might wonder about the complexity here:
+			// our current behavior is that you are not allowed to set a kinematic target unless the object is in a scene.
+			// Thus, the kinematic data should only be created/destroyed when we know for sure that we are in a scene.
+
+			if(switchToKinematic)
+			{
+				setupSimStateData(simStateDataPool, true, false);
+				sim->postSwitchToKinematic();
+			}
+			else if(switchToDynamic)
+			{
+				tearDownSimStateData(simStateDataPool, true);
+				sim->postSwitchToDynamic();
+			}
+
+			const PxU32 wasSpeculativeCCD = old & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD;
+			const PxU32 isSpeculativeCCD = f & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD;
+
+			if(wasSpeculativeCCD ^ isSpeculativeCCD)
+			{
+				if(wasSpeculativeCCD)
+				{
+					if(sim->isArticulationLink())
+						sim->getScene().resetSpeculativeCCDArticulationLink(sim->getNodeIndex().index());
+					else
+						sim->getScene().resetSpeculativeCCDRigidBody(sim->getNodeIndex().index());
+
+					sim->getLowLevelBody().mInternalFlags &= (~PxsRigidBody::eSPECULATIVE_CCD);
+				}
+				else
+				{
+					//Kinematic body switch puts the body to sleep, so we do not mark the speculative CCD bitmap for this actor to true in this case.
+					if (!switchToKinematic)
+					{
+						if (sim->isArticulationLink())
+							sim->getScene().setSpeculativeCCDArticulationLink(sim->getNodeIndex().index());
+						else
+							sim->getScene().setSpeculativeCCDRigidBody(sim->getNodeIndex().index());
+					}
+
+					sim->getLowLevelBody().mInternalFlags |= (PxsRigidBody::eSPECULATIVE_CCD);
+				}
+			}			
+		}
+
+		if(switchToKinematic)
+			putToSleep();
+
+		if(sim)
+		{
+			const PxRigidBodyFlags ktFlags(PxRigidBodyFlag::eUSE_KINEMATIC_TARGET_FOR_SCENE_QUERIES | PxRigidBodyFlag::eKINEMATIC);
+			const bool hadKt = (old & ktFlags) == ktFlags;
+			const bool hasKt = (f & ktFlags) == ktFlags;
+			if(hasKt && !hadKt)
+				sim->destroySqBounds();
+			else if(hadKt && !hasKt)
+				sim->createSqBounds();
+		}
+	}
+}
+
+void Sc::BodyCore::setMaxContactImpulse(PxReal m)	
+{ 
+	mCore.maxContactImpulse = m; 
+	updateBodySim(*this);
+}
+
+PxU32 Sc::BodyCore::getInternalIslandNodeIndex() const
+{
+	BodySim* sim = getSim();
+	if (sim)
+	{
+		return sim->getNodeIndex().index();
+	}
+
+	return IG_INVALID_NODE;
+}
+
+void Sc::BodyCore::setWakeCounter(PxReal wakeCounter, bool forceWakeUp)
+{
+	mCore.wakeCounter = wakeCounter;
+	BodySim* sim = getSim();
+	if(sim)
+	{
+		//wake counter change, we need to trigger dma pxgbodysim data again
+		updateBodySim(sim);
+		if ((wakeCounter > 0.0f) || forceWakeUp)
+			sim->wakeUp();
+		sim->postSetWakeCounter(wakeCounter, forceWakeUp);
+	}
+}
+
+void Sc::BodyCore::setSleepThreshold(PxReal t)
+{
+	mCore.sleepThreshold = t;
+	updateBodySim(*this);
+}
+
+void Sc::BodyCore::setFreezeThreshold(PxReal t)
+{
+	mCore.freezeThreshold = t;
+	updateBodySim(*this);
+}
+
+bool Sc::BodyCore::isSleeping() const
+{
+	BodySim* sim = getSim();
+	return sim ? !sim->isActive() : true;
+}
+
+void Sc::BodyCore::putToSleep()
+{
+	mCore.linearVelocity = PxVec3(0.0f);
+	mCore.angularVelocity = PxVec3(0.0f);
+
+	//The dirty flag is stored separately in the BodySim so that we query the dirty flag before going to 
+	//the expense of querying the simStateData for the velmod values.
+	BodySim* sim = getSim();
+	if(sim)
+	{
+		sim->notifyClearSpatialAcceleration();
+		sim->notifyClearSpatialVelocity();
+	}
+
+	//The velmod data is stored in a separate structure so we can record forces before scene insertion.
+	if(mSimStateData && mSimStateData->isVelMod())
+	{
+		VelocityMod* velmod = mSimStateData->getVelocityModData();
+		velmod->clear();
+	}
+
+	// important to clear all values before setting the wake counter because the values decide
+	// whether an object is ready to go to sleep or not.
+	setWakeCounter(0.0f);
+
+	if(sim)
+		sim->putToSleep();
+}
+
+void Sc::BodyCore::disableInternalCaching(bool disable)
+{
+	PX_ASSERT(!mSimStateData || mSimStateData->isKine());
+
+	if(mSimStateData)
+	{
+		PX_ASSERT(getFlags() & PxRigidBodyFlag::eKINEMATIC);
+		
+		if(disable)
+			restore();
+		else
+			backup(*mSimStateData);
+	}
+}
+
+bool Sc::BodyCore::setupSimStateData(Ps::Pool<SimStateData>* simStateDataPool, const bool isKinematic, const bool targetValid)
+{
+	SimStateData* data = mSimStateData;
+	if(!data)
+	{
+		data = simStateDataPool->construct();
+		if(!data)
+			return false;
+	}
+
+	if(isKinematic)
+	{
+		PX_ASSERT(!mSimStateData || !mSimStateData->isKine());
+
+		new(data) SimStateData(SimStateData::eKine);
+		Kinematic* kine = data->getKinematicData();
+		kine->targetValid = PxU8(targetValid ? 1 : 0);
+		backup(*data);
+	}
+	else
+	{
+		PX_ASSERT(!mSimStateData || !mSimStateData->isVelMod());
+		PX_ASSERT(!targetValid);
+
+		new(data) SimStateData(SimStateData::eVelMod);
+		VelocityMod* velmod = data->getVelocityModData();
+		velmod->clear();
+		velmod->flags = 0;
+	}
+	mSimStateData = data;
+	return true;
+}
+
+bool Sc::BodyCore::checkSimStateKinematicStatus(const bool isKinematic) const 
+{ 
+	PX_ASSERT(mSimStateData);
+	return mSimStateData->isKine() == isKinematic;
+}
+
+void Sc::BodyCore::tearDownSimStateData(Ps::Pool<SimStateData>* simStateDataPool, const bool isKinematic)
+{
+	PX_ASSERT(!mSimStateData || mSimStateData->isKine() == isKinematic);
+
+	if(mSimStateData)
+	{
+		if(isKinematic)
+			restore();
+
+		simStateDataPool->destroy(mSimStateData);
+		mSimStateData=NULL;
+	}
+}
+
+void Sc::BodyCore::backup(SimStateData& b)
+{
+	PX_ASSERT(b.isKine());
+
+	Kinematic* kine = b.getKinematicData();
+	kine->backupLinearDamping	= mCore.linearDamping;
+	kine->backupAngularDamping	= mCore.angularDamping;
+	kine->backupInverseInertia	= mCore.inverseInertia;
+	kine->backupInvMass			= mCore.inverseMass;
+	kine->backupMaxAngVelSq		= mCore.maxAngularVelocitySq;
+	kine->backupMaxLinVelSq		= mCore.maxLinearVelocitySq;
+
+	mCore.inverseMass			= 0.0f;
+	mCore.inverseInertia		= PxVec3(0.0f);
+	mCore.linearDamping			= 0.0f;
+	mCore.angularDamping		= 0.0f;
+	mCore.maxAngularVelocitySq	= PX_MAX_REAL;
+	mCore.maxLinearVelocitySq	= PX_MAX_REAL;
+}
+
+void Sc::BodyCore::restore()
+{
+	PX_ASSERT(mSimStateData && mSimStateData->isKine());
+
+	const Kinematic* kine		= mSimStateData->getKinematicData();
+	mCore.inverseMass			= kine->backupInvMass;
+	mCore.inverseInertia		= kine->backupInverseInertia;
+	mCore.linearDamping			= kine->backupLinearDamping;
+	mCore.angularDamping		= kine->backupAngularDamping;
+	mCore.maxAngularVelocitySq	= kine->backupMaxAngVelSq;
+	mCore.maxLinearVelocitySq	= kine->backupMaxLinVelSq;
+}
+
+void Sc::BodyCore::onOriginShift(const PxVec3& shift)
+{
+	mCore.body2World.p -= shift;
+	if(mSimStateData && (getFlags() & PxRigidBodyFlag::eKINEMATIC) && mSimStateData->getKinematicData()->targetValid)
+		mSimStateData->getKinematicData()->targetPose.p -= shift;
+
+	BodySim* b = getSim();
+	if(b)
+		b->onOriginShift(shift);  // BodySim might not exist if actor has simulation disabled (PxActorFlag::eDISABLE_SIMULATION)
+}
+
+// PT: TODO: isn't that the same as BodyCore->getPxActor() now?
+PxActor* Sc::getPxActorFromBodyCore(Sc::BodyCore* r, PxActorType::Enum& type)
+{
+	const PxActorType::Enum actorCoretype = r->getActorCoreType();
+	type = actorCoretype;
+	return Ps::pointerOffset<PxActor*>(r, Sc::gOffsetTable.scCore2PxActor[actorCoretype]);
+}
+
+// PT: TODO: why do we test againt NULL everywhere but not in 'isFrozen' ?
+Ps::IntBool Sc::BodyCore::isFrozen() const
+{
+	return getSim()->isFrozen();
+}
+
+void Sc::BodyCore::setSolverIterationCounts(PxU16 c)	
+{ 
+	mCore.solverIterationCounts = c;	
+	Sc::BodySim* sim = getSim();
+	if(sim)
+		sim->getLowLevelBody().solverIterationCounts = c;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+bool Sc::BodyCore::getKinematicTarget(PxTransform& p) const
+{
+	PX_ASSERT(mCore.mFlags & PxRigidBodyFlag::eKINEMATIC);
+
+	if(mSimStateData && mSimStateData->isKine() && mSimStateData->getKinematicData()->targetValid)
+	{
+		p = mSimStateData->getKinematicData()->targetPose;
+		return true;
+	}
+	else
+		return false;
+}
+
+bool Sc::BodyCore::getHasValidKinematicTarget() const
+{
+	//The use pattern for this is that we should only look for kinematic data if we know it is kinematic.
+	//We might look for velmod data even if it is kinematic.
+	PX_ASSERT(!mSimStateData || mSimStateData->isKine()); 
+	return mSimStateData && mSimStateData->isKine() && mSimStateData->getKinematicData()->targetValid;
+}
+
+void Sc::BodyCore::setKinematicTarget(Ps::Pool<SimStateData>* simStateDataPool, const PxTransform& p, PxReal wakeCounter)
+{
+	PX_ASSERT(mCore.mFlags & PxRigidBodyFlag::eKINEMATIC);
+	PX_ASSERT(!mSimStateData || mSimStateData->isKine());
+
+	if(mSimStateData)
+	{
+		Kinematic* kine = mSimStateData->getKinematicData();
+		kine->targetPose = p;
+		kine->targetValid = 1;
+		
+		Sc::BodySim* bSim = getSim();
+		if(bSim)
+			bSim->postSetKinematicTarget();
+	}
+	else
+	{
+		if(setupSimStateData(simStateDataPool, true, true))
+		{
+			PX_ASSERT(!getSim());  // covers the following scenario: kinematic gets added to scene while sim is running and target gets set (at that point the sim object does not yet exist)
+
+			Kinematic* kine = mSimStateData->getKinematicData();
+			kine->targetPose = p;
+			kine->targetValid = 1;
+		}
+		else
+			Ps::getFoundation().error(PxErrorCode::eOUT_OF_MEMORY, __FILE__, __LINE__, "PxRigidDynamic: setting kinematic target failed, not enough memory.");
+	}
+
+	wakeUp(wakeCounter);
+}
+
+void Sc::BodyCore::invalidateKinematicTarget()
+{ 
+	PX_ASSERT(mSimStateData && mSimStateData->isKine()); 
+	mSimStateData->getKinematicData()->targetValid = 0; 
+}
+
+void Sc::BodyCore::setKinematicLink(const bool value)
+{
+	BodySim* sim = getSim();
+
+	if (sim)
+		sim->getLowLevelBody().mCore->kinematicLink = PxU8(value);
+}
+
+///////////////////////////////////////////////////////////////////////////////

physx/source/simulationcontroller/src/ScBodySim.cpp

@@ -0,0 +1,993 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "ScBodySim.h"
+#include "ScShapeSim.h"
+#include "ScScene.h"
+#include "ScArticulationSim.h"
+#include "PxsContext.h"
+#include "PxsSimpleIslandManager.h"
+#include "PxsSimulationController.h"
+
+using namespace physx;
+using namespace physx::Dy;
+using namespace Sc;
+
+#define PX_FREEZE_INTERVAL 1.5f
+#define PX_FREE_EXIT_THRESHOLD 4.f
+#define PX_FREEZE_TOLERANCE 0.25f
+
+#define PX_SLEEP_DAMPING	0.5f
+#define PX_FREEZE_SCALE		0.9f
+
+BodySim::BodySim(Scene& scene, BodyCore& core, bool compound) :
+	RigidSim				(scene, core),
+	mLLBody					(&core.getCore(), PX_FREEZE_INTERVAL),
+	mNodeIndex				(IG_INVALID_NODE),
+	mInternalFlags			(0),
+	mVelModState			(VMF_GRAVITY_DIRTY),
+	mActiveListIndex		(SC_NOT_IN_SCENE_INDEX),
+	mActiveCompoundListIndex(SC_NOT_IN_SCENE_INDEX),
+	mArticulation			(NULL),
+	mConstraintGroup		(NULL)
+{
+	core.getCore().numCountedInteractions = 0;
+	core.getCore().numBodyInteractions = 0;
+	core.getCore().disableGravity = core.getActorFlags() & PxActorFlag::eDISABLE_GRAVITY;
+	if(core.getFlags() & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD)
+		mLLBody.mInternalFlags |= PxsRigidBody::eSPECULATIVE_CCD;
+
+	//If a body pending insertion was given a force/torque then it will have 
+	//the dirty flags stored in a separate structure.  Copy them across
+	//so we can use them now that the BodySim is constructed.
+	SimStateData* simStateData = core.getSimStateData(false);
+	bool hasPendingForce = false;
+	if(simStateData)
+	{
+		VelocityMod* velmod = simStateData->getVelocityModData();
+		hasPendingForce = (velmod->flags != 0) && 
+			(!velmod->getLinearVelModPerSec().isZero() || !velmod->getAngularVelModPerSec().isZero() ||
+			 !velmod->getLinearVelModPerStep().isZero() || !velmod->getAngularVelModPerStep().isZero());
+		mVelModState = velmod->flags;	
+		velmod->flags = 0;
+	}
+
+	// PT: don't read the core ptr we just wrote, use input param
+	// PT: at time of writing we get a big L2 here because even though bodycore has been prefetched, the wake counter is 160 bytes away
+	const bool isAwake =	(core.getWakeCounter() > 0) || 
+							(!core.getLinearVelocity().isZero()) ||
+							(!core.getAngularVelocity().isZero()) || 
+							hasPendingForce;
+
+	const bool isKine = isKinematic();
+
+	IG::SimpleIslandManager* simpleIslandManager = scene.getSimpleIslandManager();
+	if (!isArticulationLink())
+	{
+		mNodeIndex = simpleIslandManager->addRigidBody(&mLLBody, isKine, isAwake);
+	}
+	else
+	{
+		if(mArticulation)
+		{
+			const ArticulationLinkHandle articLinkhandle = mArticulation->getLinkHandle(*this);
+			IG::NodeIndex index = mArticulation->getIslandNodeIndex();
+			mNodeIndex.setIndices(index.index(), articLinkhandle & (DY_ARTICULATION_MAX_SIZE-1));
+		}
+	}
+
+	//If a user add force or torque before the body is inserted into the scene,
+	//this logic will make sure pre solver stage apply external force/torque to the body
+	if(hasPendingForce && !isArticulationLink())
+		scene.getVelocityModifyMap().growAndSet(mNodeIndex.index());
+
+	PX_ASSERT(mActiveListIndex == SC_NOT_IN_SCENE_INDEX);
+
+	// A.B. need to set the compound rigid flag early enough, so that we add the rigid into 
+	// active list and do not create the shape bounds
+	if(compound)
+		raiseInternalFlag(BF_IS_COMPOUND_RIGID);
+
+	setActive(isAwake, ActorSim::AS_PART_OF_CREATION);
+
+	if (isAwake)
+	{
+		scene.addToActiveBodyList(*this);
+		PX_ASSERT(isActive());
+	}
+	else
+	{
+		mActiveListIndex = SC_NOT_IN_ACTIVE_LIST_INDEX;
+		mActiveCompoundListIndex = SC_NOT_IN_ACTIVE_LIST_INDEX;
+		PX_ASSERT(!isActive());
+
+		simpleIslandManager->deactivateNode(mNodeIndex);
+	}
+
+	if (isKine)
+	{
+		initKinematicStateBase(core, true);
+
+		const SimStateData* kd = core.getSimStateData(true);
+		if (!kd)
+		{
+			core.setupSimStateData(scene.getSimStateDataPool(), true, false);
+			notifyPutToSleep();  // sleep state of kinematics is fully controlled by the simulation controller not the island manager
+		}
+		else
+		{
+			PX_ASSERT(kd->isKine());
+			PX_ASSERT(kd->getKinematicData()->targetValid);  // the only reason for the kinematic data to exist at that point already is if the target has been set
+			PX_ASSERT(isAwake);  // the expectation is that setting a target also sets the wake counter to a positive value
+			postSetKinematicTarget();
+		}
+	}
+}
+
+BodySim::~BodySim()
+{
+	Scene& scene = getScene();
+	const bool active = isActive();
+
+	getBodyCore().tearDownSimStateData(scene.getSimStateDataPool(), isKinematic() ? true : false);
+
+	PX_ASSERT(!readInternalFlag(BF_ON_DEATHROW)); // Before 3.0 it could happen that destroy could get called twice. Assert to make sure this is fixed.
+	raiseInternalFlag(BF_ON_DEATHROW);
+
+	scene.removeBody(*this);
+	PX_ASSERT(!getConstraintGroup());  // Removing from scene should erase constraint group node if it existed
+
+	if(mArticulation)
+		mArticulation->removeBody(*this);
+
+	//Articulations are represented by a single node, so they must only be removed by the articulation and not the links!
+	if(mArticulation == NULL && mNodeIndex.articulationLinkId() == 0) //If it wasn't an articulation link, then we can remove it
+		scene.getSimpleIslandManager()->removeNode(mNodeIndex);
+
+	PX_ASSERT(mActiveListIndex != SC_NOT_IN_SCENE_INDEX);
+
+	if (active)
+		scene.removeFromActiveBodyList(*this);
+
+	mActiveListIndex = SC_NOT_IN_SCENE_INDEX;
+	mActiveCompoundListIndex = SC_NOT_IN_SCENE_INDEX;
+
+	mCore.setSim(NULL);
+}
+
+void BodySim::updateCached(Cm::BitMapPinned* shapeChangedMap)
+{
+	if(!(mLLBody.mInternalFlags & PxsRigidBody::eFROZEN))
+	{
+		ElementSim* current = getElements_();
+		while(current)
+		{
+			static_cast<ShapeSim*>(current)->updateCached(0, shapeChangedMap);
+			current = current->mNextInActor;
+		}
+	}
+}
+
+void BodySim::updateCached(PxsTransformCache& transformCache, Bp::BoundsArray& boundsArray)
+{
+	PX_ASSERT(!(mLLBody.mInternalFlags & PxsRigidBody::eFROZEN));	// PT: should not be called otherwise
+
+	ElementSim* current = getElements_();
+	while(current)
+	{
+		static_cast<ShapeSim*>(current)->updateCached(transformCache, boundsArray);
+		current = current->mNextInActor;
+	}
+}
+
+void BodySim::updateContactDistance(PxReal* contactDistance, const PxReal dt, Bp::BoundsArray& boundsArray)
+{
+	if (getLowLevelBody().getCore().mFlags & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD
+		&& !(getLowLevelBody().mInternalFlags & PxsRigidBody::eFROZEN))
+	{
+		const PxVec3 linVel = getLowLevelBody().getLinearVelocity();
+		const PxVec3 aVel = getLowLevelBody().getAngularVelocity();
+		const PxReal inflation = linVel.magnitude() * dt;
+
+		ElementSim* current = getElements_();
+		while(current)
+		{
+			static_cast<ShapeSim*>(current)->updateContactDistance(contactDistance, inflation, aVel, dt, boundsArray);
+			current = current->mNextInActor;
+		}
+	}
+}
+
+//--------------------------------------------------------------
+//
+// BodyCore interface implementation
+//
+//--------------------------------------------------------------
+
+void BodySim::notifyAddSpatialAcceleration()
+{
+	//The dirty flag is stored separately in the BodySim so that we query the dirty flag before going to 
+	//the expense of querying the simStateData for the velmod values.
+	raiseVelocityModFlag(VMF_ACC_DIRTY);
+
+	if(!isArticulationLink())
+		getScene().getVelocityModifyMap().growAndSet(getNodeIndex().index());
+}
+
+void BodySim::notifyClearSpatialAcceleration()
+{
+	//The dirty flag is stored separately in the BodySim so that we query the dirty flag before going to 
+	//the expense of querying the simStateData for the velmod values.
+	raiseVelocityModFlag(VMF_ACC_DIRTY);
+	if (!isArticulationLink())
+		getScene().getVelocityModifyMap().growAndSet(getNodeIndex().index());
+}
+
+void BodySim::notifyAddSpatialVelocity()
+{
+	//The dirty flag is stored separately in the BodySim so that we query the dirty flag before going to 
+	//the expense of querying the simStateData for the velmod values.
+	raiseVelocityModFlag(VMF_VEL_DIRTY);
+	if (!isArticulationLink())
+		getScene().getVelocityModifyMap().growAndSet(getNodeIndex().index());
+}
+
+void BodySim::notifyClearSpatialVelocity()
+{
+	//The dirty flag is stored separately in the BodySim so that we query the dirty flag before going to 
+	//the expense of querying the simStateData for the velmod values.
+	raiseVelocityModFlag(VMF_VEL_DIRTY);
+	if (!isArticulationLink())
+		getScene().getVelocityModifyMap().growAndSet(getNodeIndex().index());
+}
+
+void BodySim::postActorFlagChange(PxU32 oldFlags, PxU32 newFlags)
+{
+	// PT: don't convert to bool if not needed
+	const PxU32 wasWeightless = oldFlags & PxActorFlag::eDISABLE_GRAVITY;
+	const PxU32 isWeightless = newFlags & PxActorFlag::eDISABLE_GRAVITY;
+
+	if (isWeightless != wasWeightless)
+	{
+		if (mVelModState == 0)
+			raiseVelocityModFlag(VMF_GRAVITY_DIRTY);
+
+		getBodyCore().getCore().disableGravity = isWeightless!=0;
+	}
+}
+
+void BodySim::postBody2WorldChange()
+{
+	mLLBody.saveLastCCDTransform();
+	notifyShapesOfTransformChange();
+}
+
+void BodySim::postSetWakeCounter(PxReal t, bool forceWakeUp)
+{
+	if ((t > 0.0f) || forceWakeUp)
+		notifyNotReadyForSleeping();
+	else
+	{
+		const bool readyForSleep = checkSleepReadinessBesidesWakeCounter();
+		if (readyForSleep)
+			notifyReadyForSleeping();
+	}
+}
+
+void BodySim::postSetKinematicTarget()
+{
+	PX_ASSERT(getBodyCore().getSimStateData(true));
+	PX_ASSERT(getBodyCore().getSimStateData(true)->isKine());
+	PX_ASSERT(getBodyCore().getSimStateData(true)->getKinematicData()->targetValid);
+
+	raiseInternalFlag(BF_KINEMATIC_MOVED);	// Important to set this here already because trigger interactions need to have this information when being activated.
+	clearInternalFlag(BF_KINEMATIC_SURFACE_VELOCITY);
+}
+
+static void updateBPGroup(ElementSim* current)
+{
+	while(current)
+	{
+		static_cast<ShapeSim*>(current)->updateBPGroup();
+		current = current->mNextInActor;
+	}
+}
+
+void BodySim::postSwitchToKinematic()
+{
+	initKinematicStateBase(getBodyCore(), false);
+
+	// - interactions need to get refiltered to make sure that kinematic-kinematic and kinematic-static pairs get suppressed
+	// - unlike postSwitchToDynamic(), constraint interactions are not marked dirty here because a transition to kinematic will put the object asleep which in turn 
+	//   triggers onDeactivate_() on the constraint pairs that are active. If such pairs get deactivated, they will get removed from the list of active breakable
+	//   constraints automatically.
+	setActorsInteractionsDirty(InteractionDirtyFlag::eBODY_KINEMATIC, NULL, InteractionFlag::eFILTERABLE);
+
+	getScene().getSimpleIslandManager()->setKinematic(mNodeIndex);
+
+	//
+	updateBPGroup(getElements_());
+}
+
+void BodySim::postSwitchToDynamic()
+{
+	mScene.getSimpleIslandManager()->setDynamic(mNodeIndex);
+
+	setForcesToDefaults(true);
+
+	if(getConstraintGroup())
+		getConstraintGroup()->markForProjectionTreeRebuild(mScene.getProjectionManager());
+
+	// - interactions need to get refiltered to make sure that former kinematic-kinematic and kinematic-static pairs get enabled
+	// - switching from kinematic to dynamic does not change the sleep state of the body. The constraint interactions are marked dirty
+	//   to check later whether they need to be activated plus potentially tracked for constraint break testing. This special treatment
+	//   is necessary because constraints between two kinematic bodies are considered inactive, no matter whether one of the kinematics
+	//   is active (has a target) or not.
+	setActorsInteractionsDirty(InteractionDirtyFlag::eBODY_KINEMATIC, NULL, InteractionFlag::eFILTERABLE | InteractionFlag::eCONSTRAINT);
+
+	clearInternalFlag(BF_KINEMATIC_MOVE_FLAGS);
+
+	if(isActive())
+		mScene.swapInActiveBodyList(*this);
+
+	//
+	updateBPGroup(getElements_());
+}
+
+void BodySim::postPosePreviewChange(const PxU32 posePreviewFlag)
+{
+	if (isActive())
+	{
+		if (posePreviewFlag & PxRigidBodyFlag::eENABLE_POSE_INTEGRATION_PREVIEW)
+			getScene().addToPosePreviewList(*this);
+		else
+			getScene().removeFromPosePreviewList(*this);
+	}
+	else
+		PX_ASSERT(!getScene().isInPosePreviewList(*this));
+}
+
+//--------------------------------------------------------------
+//
+// Sleeping
+//
+//--------------------------------------------------------------
+
+void BodySim::activate()
+{
+	// Activate body
+	{
+		PX_ASSERT((!isKinematic()) || notInScene() || readInternalFlag(InternalFlags(BF_KINEMATIC_MOVED | BF_KINEMATIC_SURFACE_VELOCITY)));	// kinematics should only get activated when a target is set.
+																								// exception: object gets newly added, then the state change will happen later
+		if(!isArticulationLink())
+		{
+			mLLBody.mInternalFlags &= (~PxsRigidBody::eFROZEN);
+			// Put in list of activated bodies. The list gets cleared at the end of a sim step after the sleep callbacks have been fired.
+			getScene().onBodyWakeUp(this);
+		}
+
+		BodyCore& core = getBodyCore();
+		if(core.getFlags() & PxRigidBodyFlag::eENABLE_POSE_INTEGRATION_PREVIEW)
+		{
+			PX_ASSERT(!getScene().isInPosePreviewList(*this));
+			getScene().addToPosePreviewList(*this);
+		}
+		createSqBounds();
+	}
+
+	// Activate interactions
+	{
+		const PxU32 nbInteractions = getActorInteractionCount();
+
+		for(PxU32 i=0; i<nbInteractions; ++i)
+		{
+			Ps::prefetchLine(mInteractions[PxMin(i+1,nbInteractions-1)]);
+			Interaction* interaction = mInteractions[i];
+
+			const bool isNotIGControlled = interaction->getType() != InteractionType::eOVERLAP &&
+				interaction->getType() != InteractionType::eMARKER;
+
+			if(!interaction->readInteractionFlag(InteractionFlag::eIS_ACTIVE) && (isNotIGControlled))
+			{
+				const bool proceed = activateInteraction(interaction, NULL);
+
+				if (proceed && (interaction->getType() < InteractionType::eTRACKED_IN_SCENE_COUNT))
+					getScene().notifyInteractionActivated(interaction);
+			}
+		}
+	}
+
+	//set speculative CCD bit map if speculative CCD flag is on
+	{
+		BodyCore& core = getBodyCore();
+		if (core.getFlags() & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD)
+		{
+			if (isArticulationLink())
+			{
+				if (getNodeIndex().isValid())
+					getScene().setSpeculativeCCDArticulationLink(getNodeIndex().index());
+			}
+			else
+				getScene().setSpeculativeCCDRigidBody(getNodeIndex().index());
+		}
+	}
+}
+
+void BodySim::deactivate()
+{
+	// Deactivate interactions
+	{
+		const PxU32 nbInteractions = getActorInteractionCount();
+
+		for(PxU32 i=0; i<nbInteractions; ++i)
+		{
+			Ps::prefetchLine(mInteractions[PxMin(i+1,nbInteractions-1)]);
+			Interaction* interaction = mInteractions[i];
+
+			const bool isNotIGControlled = interaction->getType() != InteractionType::eOVERLAP &&
+				interaction->getType() != InteractionType::eMARKER;
+
+			if (interaction->readInteractionFlag(InteractionFlag::eIS_ACTIVE) && isNotIGControlled)
+			{
+				const bool proceed = deactivateInteraction(interaction);
+				if (proceed && (interaction->getType() < InteractionType::eTRACKED_IN_SCENE_COUNT))
+					getScene().notifyInteractionDeactivated(interaction);
+			}
+		}
+	}
+
+	// Deactivate body
+	{
+		PX_ASSERT((!isKinematic()) || notInScene() || !readInternalFlag(BF_KINEMATIC_MOVED));	// kinematics should only get deactivated when no target is set.
+																								// exception: object gets newly added, then the state change will happen later
+		BodyCore& core = getBodyCore();
+		if(!readInternalFlag(BF_ON_DEATHROW))
+		{
+			// Set velocity to 0.
+			// Note: this is also fine if the method gets called because the user puts something to sleep (this behavior is documented in the API)
+			PX_ASSERT(core.getWakeCounter() == 0.0f);
+			const PxVec3 zero(0.0f);
+			core.setLinearVelocityInternal(zero);
+			core.setAngularVelocityInternal(zero);
+	
+			setForcesToDefaults(!core.getCore().disableGravity);
+		}
+
+		if(!isArticulationLink())  // Articulations have their own sleep logic.
+			getScene().onBodySleep(this);
+
+		if(core.getFlags() & PxRigidBodyFlag::eENABLE_POSE_INTEGRATION_PREVIEW)
+		{
+			PX_ASSERT(getScene().isInPosePreviewList(*this));
+			getScene().removeFromPosePreviewList(*this);
+		}
+		destroySqBounds();
+	}
+
+	// reset speculative CCD bit map if speculative CCD flag is on
+	{
+		BodyCore& core = getBodyCore();
+		if (core.getFlags() & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD)
+		{
+			if (isArticulationLink())
+			{
+				if (getNodeIndex().isValid())
+					getScene().resetSpeculativeCCDArticulationLink(getNodeIndex().index());
+			}
+			else
+				getScene().resetSpeculativeCCDRigidBody(getNodeIndex().index());
+		}
+	}
+}
+
+void BodySim::setActive(bool active, PxU32 infoFlag)
+{
+	PX_ASSERT(!active || isDynamicRigid());  // Currently there should be no need to activate an actor that does not take part in island generation
+
+	const PxU32 asPartOfCreation = infoFlag & ActorSim::AS_PART_OF_CREATION;
+	if (asPartOfCreation || isActive() != active)
+	{
+		PX_ASSERT(!asPartOfCreation || (getActorInteractionCount() == 0)); // On creation or destruction there should be no interactions
+
+		if (active)
+		{
+			if (!asPartOfCreation)
+			{
+				// Inactive => Active
+				getScene().addToActiveBodyList(*this);
+			}
+
+			activate();
+
+			PX_ASSERT(asPartOfCreation || isActive());
+		}
+		else
+		{
+			if (!asPartOfCreation)
+			{
+				// Active => Inactive
+				getScene().removeFromActiveBodyList(*this);
+			}
+
+			deactivate();
+
+			PX_ASSERT(asPartOfCreation || (!isActive()));
+		}
+	}
+}
+
+void BodySim::wakeUp()
+{
+	setActive(true);
+	notifyWakeUp(true);
+}
+
+void BodySim::putToSleep()
+{
+	PX_ASSERT(getBodyCore().getWakeCounter() == 0.0f);
+	PX_ASSERT(getBodyCore().getLinearVelocity().isZero());
+	PX_ASSERT(getBodyCore().getAngularVelocity().isZero());
+#ifdef _DEBUG
+	// pending forces should have been cleared at this point
+	const SimStateData* sd = getBodyCore().getSimStateData(false);
+	if (sd)
+	{
+		const VelocityMod* vm = sd->getVelocityModData();
+		PX_ASSERT(vm->linearPerSec.isZero() && vm->linearPerStep.isZero() && vm->angularPerSec.isZero() && vm->angularPerStep.isZero());
+	}
+#endif
+
+	setActive(false);
+	notifyPutToSleep();
+
+	clearInternalFlag(InternalFlags(BF_KINEMATIC_SETTLING | BF_KINEMATIC_SETTLING_2));	// putToSleep is used when a kinematic gets removed from the scene while the sim is running and then gets re-inserted immediately.
+												// We can move this code when we look into the open task of making buffered re-insertion more consistent with the non-buffered case.
+}
+
+void BodySim::internalWakeUp(PxReal wakeCounterValue)
+{
+	if(mArticulation)
+		mArticulation->internalWakeUp(wakeCounterValue);
+	else
+		internalWakeUpBase(wakeCounterValue);
+}
+
+void BodySim::internalWakeUpArticulationLink(PxReal wakeCounterValue)
+{
+	PX_ASSERT(mArticulation);
+	internalWakeUpBase(wakeCounterValue);
+}
+
+void BodySim::internalWakeUpBase(PxReal wakeCounterValue)	//this one can only increase the wake counter, not decrease it, so it can't be used to put things to sleep!
+{
+	if ((!isKinematic()) && (getBodyCore().getWakeCounter() < wakeCounterValue))
+	{
+		PX_ASSERT(wakeCounterValue > 0.0f);
+		getBodyCore().setWakeCounterFromSim(wakeCounterValue);
+
+		//we need to update the gpu body sim because we reset the wake counter for the body core
+		mScene.getSimulationController()->updateDynamic(isArticulationLink(), mNodeIndex);
+		setActive(true);
+		notifyWakeUp(false);
+		mLLBody.mInternalFlags &= (~PxsRigidBody::eFROZEN);
+	}
+}
+
+void BodySim::notifyReadyForSleeping()
+{
+	if(mArticulation == NULL)
+		getScene().getSimpleIslandManager()->deactivateNode(mNodeIndex);
+}
+
+void BodySim::notifyNotReadyForSleeping()
+{
+	getScene().getSimpleIslandManager()->activateNode(mNodeIndex);
+}
+
+void BodySim::notifyWakeUp(bool /*wakeUpInIslandGen*/)
+{
+	getScene().getSimpleIslandManager()->activateNode(mNodeIndex);
+}
+
+void BodySim::notifyPutToSleep()
+{
+	getScene().getSimpleIslandManager()->putNodeToSleep(mNodeIndex);
+}
+
+void BodySim::resetSleepFilter()
+{
+	mLLBody.sleepAngVelAcc = PxVec3(0.0f);
+	mLLBody.sleepLinVelAcc = PxVec3(0.0f);
+}
+
+//This function will be called by CPU sleepCheck code
+PxReal BodySim::updateWakeCounter(PxReal dt, PxReal energyThreshold, const Cm::SpatialVector& motionVelocity)
+{
+	// update the body's sleep state and 
+	BodyCore& core = getBodyCore();
+
+	const PxReal wakeCounterResetTime = ScInternalWakeCounterResetValue;
+
+	PxReal wc = core.getWakeCounter();
+	
+	{
+		PxVec3 bcSleepLinVelAcc = mLLBody.sleepLinVelAcc;
+		PxVec3 bcSleepAngVelAcc = mLLBody.sleepAngVelAcc;
+
+		if(wc < wakeCounterResetTime * 0.5f || wc < dt)
+		{
+			const PxTransform& body2World = getBody2World();
+
+			// calculate normalized energy: kinetic energy divided by mass
+			const PxVec3 t = core.getInverseInertia();
+			const PxVec3 inertia(t.x > 0.0f ? 1.0f/t.x : 1.0f, t.y > 0.0f ? 1.0f/t.y : 1.0f, t.z > 0.0f ? 1.0f/t.z : 1.0f);
+
+			PxVec3 sleepLinVelAcc =motionVelocity.linear;
+			PxVec3 sleepAngVelAcc = body2World.q.rotateInv(motionVelocity.angular);
+
+			bcSleepLinVelAcc += sleepLinVelAcc;
+			bcSleepAngVelAcc += sleepAngVelAcc;
+
+			PxReal invMass = core.getInverseMass();
+			if(invMass == 0.0f)
+				invMass = 1.0f;
+
+			const PxReal angular = bcSleepAngVelAcc.multiply(bcSleepAngVelAcc).dot(inertia) * invMass;
+			const PxReal linear = bcSleepLinVelAcc.magnitudeSquared();
+			PxReal normalizedEnergy = 0.5f * (angular + linear);
+
+			// scale threshold by cluster factor (more contacts => higher sleep threshold)
+			const PxReal clusterFactor = PxReal(1 + getNumCountedInteractions());
+			const PxReal threshold = clusterFactor*energyThreshold;
+		
+			if (normalizedEnergy >= threshold)
+			{
+				PX_ASSERT(isActive());
+				resetSleepFilter();
+				const float factor = threshold == 0.0f ? 2.0f : PxMin(normalizedEnergy/threshold, 2.0f);
+				PxReal oldWc = wc;
+				wc = factor * 0.5f * wakeCounterResetTime + dt * (clusterFactor - 1.0f);
+				core.setWakeCounterFromSim(wc);
+				if (oldWc == 0.0f)  // for the case where a sleeping body got activated by the system (not the user) AND got processed by the solver as well
+					notifyNotReadyForSleeping();
+				
+				return wc;
+			}
+		}
+
+		mLLBody.sleepLinVelAcc = bcSleepLinVelAcc;
+		mLLBody.sleepAngVelAcc = bcSleepAngVelAcc;
+	}
+
+	wc = PxMax(wc-dt, 0.0f);
+	core.setWakeCounterFromSim(wc);
+	return wc;
+}
+
+//--------------------------------------------------------------
+//
+// Kinematics
+//
+//--------------------------------------------------------------
+
+PX_FORCE_INLINE void BodySim::initKinematicStateBase(BodyCore&, bool asPartOfCreation)
+{
+	PX_ASSERT(!readInternalFlag(BF_KINEMATIC_MOVED));
+
+	if (!asPartOfCreation && isActive())
+		getScene().swapInActiveBodyList(*this);
+
+	//mLLBody.setAccelerationV(Cm::SpatialVector::zero());
+
+	// Need to be before setting setRigidBodyFlag::KINEMATIC
+
+	if (getConstraintGroup())
+		getConstraintGroup()->markForProjectionTreeRebuild(getScene().getProjectionManager());
+}
+
+void BodySim::calculateKinematicVelocity(PxReal oneOverDt)
+{
+	PX_ASSERT(isKinematic());
+	
+	/*------------------------------------------------\
+	| kinematic bodies are moved directly by the user and are not influenced by external forces
+	| we simply determine the distance moved since the last simulation frame and 
+	| assign the appropriate delta to the velocity. This vel will be used to shove dynamic
+	| objects in the solver.
+	| We have to do this like so in a delayed way, because when the user sets the target pos the dt is not
+	| yet known.
+	\------------------------------------------------*/
+	PX_ASSERT(isActive());
+
+	BodyCore& core = getBodyCore();
+
+	if (readInternalFlag(BF_KINEMATIC_MOVED))
+	{
+		clearInternalFlag(InternalFlags(BF_KINEMATIC_SETTLING | BF_KINEMATIC_SETTLING_2));
+		const SimStateData* kData = core.getSimStateData(true);
+		PX_ASSERT(kData);
+		PX_ASSERT(kData->isKine());
+		PX_ASSERT(kData->getKinematicData()->targetValid);
+		PxVec3 linVelLL, angVelLL;
+		const PxTransform targetPose = kData->getKinematicData()->targetPose;
+		const PxTransform& currBody2World = getBody2World();
+
+		//the kinematic target pose is now the target of the body (CoM) and not the actor.
+
+		PxVec3 deltaPos = targetPose.p;
+		deltaPos -= currBody2World.p;
+		linVelLL = deltaPos * oneOverDt;
+
+		PxQuat q = targetPose.q * currBody2World.q.getConjugate();
+
+		if (q.w < 0)	//shortest angle.
+			q = -q;
+
+		PxReal angle;
+ 		PxVec3 axis;
+		q.toRadiansAndUnitAxis(angle, axis);
+		angVelLL = axis * angle * oneOverDt;
+
+		core.getCore().linearVelocity = linVelLL;
+		core.getCore().angularVelocity = angVelLL;
+
+		// Moving a kinematic should trigger a wakeUp call on a higher level.
+		PX_ASSERT(core.getWakeCounter()>0);
+		PX_ASSERT(isActive());
+		
+	}
+	else if (!readInternalFlag(BF_KINEMATIC_SURFACE_VELOCITY))
+	{
+		core.setLinearVelocity(PxVec3(0));
+		core.setAngularVelocity(PxVec3(0));
+	}
+}
+
+void BodySim::updateKinematicPose()
+{
+	/*------------------------------------------------\
+	| kinematic bodies are moved directly by the user and are not influenced by external forces
+	| we simply determine the distance moved since the last simulation frame and 
+	| assign the appropriate delta to the velocity. This vel will be used to shove dynamic
+	| objects in the solver.
+	| We have to do this like so in a delayed way, because when the user sets the target pos the dt is not
+	| yet known.
+	\------------------------------------------------*/
+
+	PX_ASSERT(isKinematic());
+	PX_ASSERT(isActive());
+
+	BodyCore& core = getBodyCore();
+
+	if (readInternalFlag(BF_KINEMATIC_MOVED))
+	{
+		clearInternalFlag(InternalFlags(BF_KINEMATIC_SETTLING | BF_KINEMATIC_SETTLING_2));
+		const SimStateData* kData = core.getSimStateData(true);
+		PX_ASSERT(kData);
+		PX_ASSERT(kData->isKine());
+		PX_ASSERT(kData->getKinematicData()->targetValid);
+		
+		const PxTransform targetPose = kData->getKinematicData()->targetPose;
+		getBodyCore().getCore().body2World = targetPose;
+	}
+}
+
+bool BodySim::deactivateKinematic()
+{
+	BodyCore& core = getBodyCore();
+	if(readInternalFlag(BF_KINEMATIC_SETTLING_2))
+	{
+		clearInternalFlag(BF_KINEMATIC_SETTLING_2);
+		core.setWakeCounterFromSim(0);	// For sleeping objects the wake counter must be 0. This needs to hold for kinematics too.
+		notifyReadyForSleeping();
+		notifyPutToSleep();
+		setActive(false);
+		return true;
+	}
+	else if (readInternalFlag(BF_KINEMATIC_SETTLING))
+	{
+		clearInternalFlag(BF_KINEMATIC_SETTLING);
+		raiseInternalFlag(BF_KINEMATIC_SETTLING_2);
+	}
+	else if (!readInternalFlag(BF_KINEMATIC_SURFACE_VELOCITY))
+	{
+		clearInternalFlag(BF_KINEMATIC_MOVED);
+		raiseInternalFlag(BF_KINEMATIC_SETTLING);
+	}
+	return false;
+}
+
+//--------------------------------------------------------------
+//
+// Miscellaneous
+//
+//--------------------------------------------------------------
+
+void BodySim::updateForces(PxReal dt, PxsRigidBody** updatedBodySims, PxU32* updatedBodyNodeIndices, PxU32& index, Cm::SpatialVector* acceleration, const bool useAccelerations, bool simUsesAdaptiveForce)
+{
+	PxVec3 linAcc(0.0f), angAcc(0.0f);
+
+	const bool accDirty = readVelocityModFlag(VMF_ACC_DIRTY);
+	const bool velDirty = readVelocityModFlag(VMF_VEL_DIRTY);
+
+	BodyCore& bodyCore = getBodyCore();
+	SimStateData* simStateData = NULL;
+
+	//if we change the logic like this, which means we don't need to have two seperate variables in the pxgbodysim to represent linAcc and angAcc. However, this
+	//means angAcc will be always 0
+	if( (accDirty || velDirty) &&  ((simStateData = bodyCore.getSimStateData(false)) != NULL) )
+	{
+		VelocityMod* velmod = simStateData->getVelocityModData();
+
+		//we don't have support for articulation yet
+		if (updatedBodySims)
+		{
+			updatedBodySims[index] = &getLowLevelBody();
+			updatedBodyNodeIndices[index++] = getNodeIndex().index();
+		}
+
+		if(velDirty)
+		{
+			linAcc = velmod->getLinearVelModPerStep();
+			angAcc = velmod->getAngularVelModPerStep();
+
+			if (useAccelerations)
+			{
+				acceleration->linear = linAcc / dt;
+				acceleration->angular = angAcc / dt;
+
+			}
+			else
+			{
+				getBodyCore().updateVelocities(linAcc, angAcc);
+			}
+		}
+		
+		if (accDirty)
+		{
+			linAcc = velmod->getLinearVelModPerSec();
+			angAcc = velmod->getAngularVelModPerSec();
+
+			if (acceleration)
+			{
+				acceleration->linear = linAcc;
+				acceleration->angular = angAcc;
+			}
+			else
+			{
+				PxReal scale = dt;
+				if (simUsesAdaptiveForce)
+				{
+					if (getScene().getSimpleIslandManager()->getAccurateIslandSim().getIslandStaticTouchCount(mNodeIndex) != 0)
+					{
+						scale *= mLLBody.accelScale;
+					}
+				}
+				getBodyCore().updateVelocities(linAcc*scale, angAcc*scale);
+			}
+		}		
+	}
+
+	setForcesToDefaults(readVelocityModFlag(VMF_ACC_DIRTY));
+}
+
+void BodySim::onConstraintDetach()
+{
+	PX_ASSERT(readInternalFlag(BF_HAS_CONSTRAINTS));
+
+	PxU32 size = getActorInteractionCount();
+	Interaction** interactions = getActorInteractions();
+	unregisterCountedInteraction();
+
+	while(size--)
+	{
+		const Interaction* interaction = *interactions++;
+		if(interaction->getType() == InteractionType::eCONSTRAINTSHADER)
+			return;
+	}
+
+	clearInternalFlag(BF_HAS_CONSTRAINTS);  // There are no other constraint interactions left
+}
+
+void BodySim::setArticulation(ArticulationSim* a, PxReal wakeCounter, bool asleep, PxU32 bodyIndex)
+{
+	mArticulation = a; 
+	if(a)
+	{
+		IG::NodeIndex index = mArticulation->getIslandNodeIndex();
+		mNodeIndex.setIndices(index.index(), bodyIndex);
+		getBodyCore().setWakeCounterFromSim(wakeCounter);
+
+		if (getFlagsFast() & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD)
+			getScene().setSpeculativeCCDArticulationLink(mNodeIndex.index());
+
+		if (!asleep)
+		{
+			setActive(true);
+			notifyWakeUp(false);
+		}
+		else
+		{
+			notifyReadyForSleeping();
+			notifyPutToSleep();
+			setActive(false);
+		}
+	}
+	else
+	{
+		//Setting a 1 in the articulation ID to avoid returning the node Index to the node index
+		//manager
+		mNodeIndex.setIndices(IG_INVALID_NODE, 1);
+	}
+}
+
+void BodySim::createSqBounds()
+{
+	if(!isActive() || usingSqKinematicTarget() || readInternalFlag(BF_IS_COMPOUND_RIGID))
+		return;
+
+	PX_ASSERT(!isFrozen());
+	
+	ElementSim* current = getElements_();
+	while(current)
+	{
+		static_cast<ShapeSim*>(current)->createSqBounds();
+		current = current->mNextInActor;
+	}
+}
+
+void BodySim::destroySqBounds()
+{
+	ElementSim* current = getElements_();
+	while(current)
+	{
+		static_cast<ShapeSim*>(current)->destroySqBounds();
+		current = current->mNextInActor;
+	}
+}
+
+void BodySim::freezeTransforms(Cm::BitMapPinned* shapeChangedMap)
+{
+	ElementSim* current = getElements_();
+	while(current)
+	{
+		ShapeSim* sim = static_cast<ShapeSim*>(current);
+		sim->updateCached(PxsTransformFlag::eFROZEN, shapeChangedMap);
+		sim->destroySqBounds();
+		current = current->mNextInActor;
+	}
+}
+
+void BodySim::disableCompound()
+{
+	if(isActive())
+		getScene().removeFromActiveCompoundBodyList(*this);
+	clearInternalFlag(BF_IS_COMPOUND_RIGID);
+}
+

physx/source/simulationcontroller/src/ScBodySim.h

@@ -0,0 +1,346 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef PX_PHYSICS_SCP_BODYSIM
+#define PX_PHYSICS_SCP_BODYSIM
+
+#include "PsUtilities.h"
+#include "PsIntrinsics.h"
+#include "ScRigidSim.h"
+#include "PxvDynamics.h"
+#include "ScBodyCore.h"
+#include "ScSimStateData.h"
+#include "ScConstraintGroupNode.h"
+#include "PxRigidDynamic.h"
+#include "DyArticulation.h"
+#include "PxsRigidBody.h"
+#include "PxsSimpleIslandManager.h"
+
+namespace physx
+{
+namespace Bp
+{
+	class BoundsArray;
+}
+	class PxsTransformCache;
+	class PxsSimulationController;
+namespace Sc
+{
+	#define SC_NOT_IN_SCENE_INDEX		0xffffffff  // the body is not in the scene yet
+	#define SC_NOT_IN_ACTIVE_LIST_INDEX	0xfffffffe  // the body is in the scene but not in the active list
+
+	class Scene;
+	class ArticulationSim;
+
+	static const PxReal ScInternalWakeCounterResetValue = 20.0f*0.02f;
+
+#if PX_VC 
+    #pragma warning(push)   
+	#pragma warning( disable : 4324 ) // Padding was added at the end of a structure because of a __declspec(align) value.
+#endif
+
+	class BodySim : public RigidSim
+	{
+	public:
+		enum InternalFlags
+		{
+			//BF_DISABLE_GRAVITY		= 1 << 0,	// Don't apply the scene's gravity
+
+			BF_HAS_STATIC_TOUCH		= 1 << 1,	// Set when a body is part of an island with static contacts. Needed to be able to recalculate adaptive force if this changes
+			BF_KINEMATIC_MOVED		= 1 << 2,	// Set when the kinematic was moved
+
+			BF_ON_DEATHROW			= 1 << 3,	// Set when the body is destroyed
+
+			BF_IS_IN_SLEEP_LIST		= 1 << 4,	// Set when the body is added to the list of bodies which were put to sleep
+			BF_IS_IN_WAKEUP_LIST	= 1 << 5,	// Set when the body is added to the list of bodies which were woken up
+			BF_SLEEP_NOTIFY			= 1 << 6,	// A sleep notification should be sent for this body (and not a wakeup event, even if the body is part of the woken list as well)
+			BF_WAKEUP_NOTIFY		= 1 << 7,	// A wake up notification should be sent for this body (and not a sleep event, even if the body is part of the sleep list as well)
+
+			BF_HAS_CONSTRAINTS		= 1 << 8,	// Set if the body has one or more constraints
+			BF_KINEMATIC_SETTLING	= 1 << 9,	// Set when the body was moved kinematically last frame
+			BF_KINEMATIC_SETTLING_2 = 1 << 10,
+			BF_KINEMATIC_MOVE_FLAGS = BF_KINEMATIC_MOVED | BF_KINEMATIC_SETTLING | BF_KINEMATIC_SETTLING_2, //Used to clear kinematic masks in 1 call
+			BF_KINEMATIC_SURFACE_VELOCITY = 1 << 11, //Set when the application calls setKinematicVelocity. Actor remains awake until application calls clearKinematicVelocity. 
+			BF_IS_COMPOUND_RIGID	= 1 << 12	// Set when the body is a compound actor, we dont want to set the sq bounds
+
+			// PT: WARNING: flags stored on 16-bits now.
+		};
+
+	public:
+												BodySim(Scene&, BodyCore&, bool);
+		virtual									~BodySim();
+
+						void					notifyAddSpatialAcceleration();
+						void					notifyClearSpatialAcceleration();
+						void					notifyAddSpatialVelocity();
+						void					notifyClearSpatialVelocity();
+						void					updateCached(Cm::BitMapPinned* shapeChangedMap);
+						void					updateCached(PxsTransformCache& transformCache, Bp::BoundsArray& boundsArray);
+						void					updateContactDistance(PxReal* contactDistance, const PxReal dt, Bp::BoundsArray& boundsArray);
+
+		// hooks for actions in body core when it's attached to a sim object. Generally
+		// we get called after the attribute changed.
+			
+		virtual			void					postActorFlagChange(PxU32 oldFlags, PxU32 newFlags);
+						void					postBody2WorldChange();
+						void					postSetWakeCounter(PxReal t, bool forceWakeUp);
+						void					postSetKinematicTarget();
+						void					postSwitchToKinematic();
+						void					postSwitchToDynamic();
+						void					postPosePreviewChange(const PxU32 posePreviewFlag);  // called when PxRigidBodyFlag::eENABLE_POSE_INTEGRATION_PREVIEW changes
+
+		PX_FORCE_INLINE const PxTransform&		getBody2World()		const	{ return getBodyCore().getCore().body2World;		}
+		PX_FORCE_INLINE const PxTransform&		getBody2Actor()		const	{ return getBodyCore().getCore().getBody2Actor();	}
+		PX_FORCE_INLINE const PxsRigidBody&		getLowLevelBody()	const	{ return mLLBody;									}
+		PX_FORCE_INLINE	PxsRigidBody&			getLowLevelBody()			{ return mLLBody;									}
+						void					wakeUp();  // note: for user API call purposes only, i.e., use from BodyCore. For simulation internal purposes there is internalWakeUp().
+						void					putToSleep();
+
+						void					disableCompound();
+
+		static			PxU32					getRigidBodyOffset()		{ return  PxU32(PX_OFFSET_OF_RT(BodySim, mLLBody));}
+
+	private:
+						void					activate();
+						void					deactivate();
+
+		//---------------------------------------------------------------------------------
+		// Constraint projection
+		//---------------------------------------------------------------------------------
+	public:
+		PX_FORCE_INLINE	ConstraintGroupNode*	getConstraintGroup()								{ return mConstraintGroup; }
+		PX_FORCE_INLINE	void					setConstraintGroup(ConstraintGroupNode* node)		{ mConstraintGroup = node; }
+
+		//// A list of active projection trees in the scene might be better
+		//PX_FORCE_INLINE void					projectPose() { PX_ASSERT(mConstraintGroup); ConstraintGroupNode::projectPose(*mConstraintGroup); }
+
+		//---------------------------------------------------------------------------------
+		// Kinematics
+		//---------------------------------------------------------------------------------
+	public:
+		PX_FORCE_INLINE bool					isKinematic()								const	{ return getBodyCore().getFlags() & PxRigidBodyFlag::eKINEMATIC;	}
+		PX_FORCE_INLINE bool					isArticulationLink()						const	{ return getActorType() == PxActorType::eARTICULATION_LINK; }
+		PX_FORCE_INLINE bool					hasForcedKinematicNotif()					const
+												{
+													return getBodyCore().getFlags() & (PxRigidBodyFlag::eFORCE_KINE_KINE_NOTIFICATIONS|PxRigidBodyFlag::eFORCE_STATIC_KINE_NOTIFICATIONS);
+												}
+						void					calculateKinematicVelocity(PxReal oneOverDt);
+						void					updateKinematicPose();
+						bool					deactivateKinematic();
+	private:
+		PX_FORCE_INLINE void					initKinematicStateBase(BodyCore&, bool asPartOfCreation);
+
+		//---------------------------------------------------------------------------------
+		// Sleeping
+		//---------------------------------------------------------------------------------
+	public:
+		PX_FORCE_INLINE	bool					isActive() const { return (mActiveListIndex < SC_NOT_IN_ACTIVE_LIST_INDEX); }
+						void					setActive(bool active, PxU32 infoFlag=0);  // see ActivityChangeInfoFlag
+
+		PX_FORCE_INLINE PxU32					getActiveListIndex() const { return mActiveListIndex; }  // if the body is active, the index is smaller than SC_NOT_IN_ACTIVE_LIST_INDEX
+		PX_FORCE_INLINE void					setActiveListIndex(PxU32 index) { mActiveListIndex = index; }
+
+		PX_FORCE_INLINE PxU32					getActiveCompoundListIndex() const { return mActiveCompoundListIndex; }  // if the body is active and is compound, the index is smaller than SC_NOT_IN_ACTIVE_LIST_INDEX
+		PX_FORCE_INLINE void					setActiveCompoundListIndex(PxU32 index) { mActiveCompoundListIndex = index; }
+
+						void					internalWakeUp(PxReal wakeCounterValue=ScInternalWakeCounterResetValue);
+						void					internalWakeUpArticulationLink(PxReal wakeCounterValue);	// called by ArticulationSim to wake up this link
+
+						PxReal					updateWakeCounter(PxReal dt, PxReal energyThreshold, const Cm::SpatialVector& motionVelocity);
+
+						void					resetSleepFilter();
+						void					notifyReadyForSleeping();			// inform the sleep island generation system that the body is ready for sleeping
+						void					notifyNotReadyForSleeping();		// inform the sleep island generation system that the body is not ready for sleeping
+		PX_FORCE_INLINE bool					checkSleepReadinessBesidesWakeCounter();  // for API triggered changes to test sleep readiness
+
+		PX_FORCE_INLINE void					registerCountedInteraction() { mLLBody.getCore().numCountedInteractions++; PX_ASSERT(mLLBody.getCore().numCountedInteractions); }
+		PX_FORCE_INLINE void					unregisterCountedInteraction() { PX_ASSERT(mLLBody.getCore().numCountedInteractions); mLLBody.getCore().numCountedInteractions--;}
+		PX_FORCE_INLINE PxU32					getNumCountedInteractions()	const { return mLLBody.getCore().numCountedInteractions; }
+
+		PX_FORCE_INLINE Ps::IntBool				isFrozen()	const	{ return Ps::IntBool(mLLBody.mInternalFlags & PxsRigidBody::eFROZEN);	}
+	private:
+		PX_FORCE_INLINE	void					notifyWakeUp(bool wakeUpInIslandGen = false);					// inform the sleep island generation system that the object got woken up
+		PX_FORCE_INLINE	void					notifyPutToSleep();				// inform the sleep island generation system that the object was put to sleep
+		PX_FORCE_INLINE void					internalWakeUpBase(PxReal wakeCounterValue);
+
+		//---------------------------------------------------------------------------------
+		// External velocity changes
+		//---------------------------------------------------------------------------------
+	public:
+						void					updateForces(PxReal dt, PxsRigidBody** updatedBodySims, PxU32* updatedBodyNodeIndices, 
+													PxU32& index, Cm::SpatialVector* acceleration, const bool useAcceleration, bool simUsesAdaptiveForce);
+	private:
+		PX_FORCE_INLINE void					raiseVelocityModFlag(VelocityModFlags f)				{ mVelModState |= f;					}
+		PX_FORCE_INLINE void					clearVelocityModFlag(VelocityModFlags f)				{ mVelModState &= ~f;					}
+		PX_FORCE_INLINE bool					readVelocityModFlag(VelocityModFlags f)					{ return (mVelModState & f) != 0;		}
+		PX_FORCE_INLINE void					setForcesToDefaults(bool enableGravity);
+
+		//---------------------------------------------------------------------------------
+		// Miscellaneous
+		//---------------------------------------------------------------------------------
+	public:
+		PX_FORCE_INLINE	PxU16					getInternalFlag()								const	{ return mInternalFlags;  }
+		PX_FORCE_INLINE PxU16					readInternalFlag(InternalFlags flag)			const	{ return PxU16(mInternalFlags & flag);	}
+		PX_FORCE_INLINE void					raiseInternalFlag(InternalFlags flag)					{ mInternalFlags |= flag;				}
+		PX_FORCE_INLINE void					clearInternalFlag(InternalFlags flag)					{ mInternalFlags &= ~flag;				}
+		PX_FORCE_INLINE PxU32					getFlagsFast()									const	{ return getBodyCore().getFlags();		}
+
+		PX_FORCE_INLINE void					incrementBodyConstraintCounter()						{ mLLBody.mCore->numBodyInteractions++;					}
+		PX_FORCE_INLINE void					decrementBodyConstraintCounter()						{ PX_ASSERT(mLLBody.mCore->numBodyInteractions>0); mLLBody.mCore->numBodyInteractions--; }
+
+		PX_FORCE_INLINE	BodyCore&				getBodyCore()									const	{ return static_cast<BodyCore&>(getRigidCore());		}
+
+		PX_INLINE		ArticulationSim*		getArticulation()								const	{ return mArticulation; }
+						void 					setArticulation(ArticulationSim* a, PxReal wakeCounter, bool asleep, PxU32 bodyIndex);
+
+		PX_FORCE_INLINE IG::NodeIndex			getNodeIndex() const									{ return mNodeIndex; }
+
+		PX_FORCE_INLINE void					onConstraintAttach()									{ raiseInternalFlag(BF_HAS_CONSTRAINTS); registerCountedInteraction(); }
+						void					onConstraintDetach();
+
+		PX_FORCE_INLINE	void					onOriginShift(const PxVec3& shift)						{ mLLBody.mLastTransform.p -= shift; }
+
+		PX_FORCE_INLINE	bool					notInScene()									const	{ return mActiveListIndex == SC_NOT_IN_SCENE_INDEX; }
+
+		PX_FORCE_INLINE bool					usingSqKinematicTarget()						const	
+		{ 	
+			PxU32 ktFlags(PxRigidBodyFlag::eUSE_KINEMATIC_TARGET_FOR_SCENE_QUERIES | PxRigidBodyFlag::eKINEMATIC);
+			return (getFlagsFast()&ktFlags) == ktFlags;
+		}
+
+		PX_FORCE_INLINE	PxU32					getNbShapes()									const	{ return mElementCount; }
+
+						void					createSqBounds();
+						void					destroySqBounds();
+						void					freezeTransforms(Cm::BitMapPinned* shapeChangedMap);
+						void					invalidateSqBounds();
+	private:
+
+		//---------------------------------------------------------------------------------
+		// Base body
+		//---------------------------------------------------------------------------------
+						PxsRigidBody			mLLBody;
+
+		//---------------------------------------------------------------------------------
+		// Island manager
+		//---------------------------------------------------------------------------------
+						IG::NodeIndex			mNodeIndex;
+
+		//---------------------------------------------------------------------------------
+		// External velocity changes
+		//---------------------------------------------------------------------------------
+		// VelocityMod data allocated on the fly when the user applies velocity changes
+		// which need to be accumulated.
+		// VelMod dirty flags stored in BodySim so we can save ourselves the expense of looking at 
+		// the separate velmod data if no forces have been set.
+						PxU16					mInternalFlags;
+						PxU8					mVelModState;
+
+		//---------------------------------------------------------------------------------
+		// Sleeping
+		//---------------------------------------------------------------------------------
+						PxU32					mActiveListIndex;	// Used by Scene to track active bodies
+						PxU32					mActiveCompoundListIndex;	// Used by Scene to track active compound bodies
+
+		//---------------------------------------------------------------------------------
+		// Articulation
+		//---------------------------------------------------------------------------------
+						ArticulationSim*		mArticulation;				// NULL if not in an articulation
+
+		//---------------------------------------------------------------------------------
+		// Joints & joint groups
+		//---------------------------------------------------------------------------------
+
+		// This is a tree data structure that gives us the projection order of joints in which this body is the tree root.
+		// note: the link of the root body is not necces. the root link due to the re-rooting of the articulation!
+						ConstraintGroupNode*	mConstraintGroup;
+	};
+
+#if PX_VC 
+     #pragma warning(pop) 
+#endif
+
+} // namespace Sc
+
+PX_FORCE_INLINE void Sc::BodySim::setForcesToDefaults(bool enableGravity)
+{
+	if (!(mLLBody.mCore->mFlags & PxRigidBodyFlag::eRETAIN_ACCELERATIONS))
+	{
+		SimStateData* simStateData = getBodyCore().getSimStateData(false);
+		if(simStateData) 
+		{
+			VelocityMod* velmod = simStateData->getVelocityModData();
+			velmod->clear();
+		}
+
+		if (enableGravity)
+			mVelModState = VMF_GRAVITY_DIRTY;	// We want to keep the gravity flag to make sure the acceleration gets changed to gravity-only
+												// in the next step (unless the application adds new forces of course)
+		else
+			mVelModState = 0;
+	}
+	else
+	{
+		SimStateData* simStateData = getBodyCore().getSimStateData(false);
+		if (simStateData)
+		{
+			VelocityMod* velmod = simStateData->getVelocityModData();
+			velmod->clearPerStep();
+		}
+
+		mVelModState &= (~(VMF_VEL_DIRTY));
+	}
+}
+
+PX_FORCE_INLINE bool Sc::BodySim::checkSleepReadinessBesidesWakeCounter()
+{
+	const BodyCore& bodyCore = getBodyCore();
+	const SimStateData* simStateData = bodyCore.getSimStateData(false);
+	const VelocityMod* velmod = simStateData ? simStateData->getVelocityModData() : NULL;
+
+	bool readyForSleep = bodyCore.getLinearVelocity().isZero() && bodyCore.getAngularVelocity().isZero();
+	if (readVelocityModFlag(VMF_ACC_DIRTY))
+	{
+		readyForSleep = readyForSleep && (!velmod || velmod->getLinearVelModPerSec().isZero());
+		readyForSleep = readyForSleep && (!velmod || velmod->getAngularVelModPerSec().isZero());
+	}
+	if (readVelocityModFlag(VMF_VEL_DIRTY))
+	{
+		readyForSleep = readyForSleep && (!velmod || velmod->getLinearVelModPerStep().isZero());
+		readyForSleep = readyForSleep && (!velmod || velmod->getAngularVelModPerStep().isZero());
+	}
+
+	return readyForSleep;
+}
+
+
+}
+
+#endif

physx/source/simulationcontroller/src/ScClient.h

@@ -0,0 +1,53 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef PX_PHYSICS_CLIENT
+#define PX_PHYSICS_CLIENT
+
+#include "PxScene.h"
+#include "CmPhysXCommon.h"
+#include "PsUserAllocated.h"
+
+namespace physx
+{
+namespace Sc
+{
+	class Client : public Ps::UserAllocated
+	{
+	public:
+		Client()
+		{}
+
+	};
+
+} // namespace Sc
+
+}
+
+#endif

physx/source/simulationcontroller/src/ScConstraintCore.cpp

@@ -0,0 +1,149 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#include "PsFoundation.h"
+
+#include "ScPhysics.h"
+#include "ScBodyCore.h"
+#include "ScConstraintCore.h"
+#include "ScConstraintSim.h"
+
+using namespace physx;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+Sc::ConstraintCore::ConstraintCore(PxConstraintConnector& connector, const PxConstraintShaderTable& shaders, PxU32 dataSize)
+:	mFlags(PxConstraintFlag::eDRIVE_LIMITS_ARE_FORCES)
+,	mAppliedForce(PxVec3(0))
+,	mAppliedTorque(PxVec3(0))
+,	mConnector(&connector)
+,	mProject(shaders.project)
+,	mSolverPrep(shaders.solverPrep)
+,	mVisualize(shaders.visualize)
+,	mDataSize(dataSize)
+,	mLinearBreakForce(PX_MAX_F32)
+,	mAngularBreakForce(PX_MAX_F32)
+,	mMinResponseThreshold(0)
+,	mSim(NULL)
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+Sc::ConstraintCore::~ConstraintCore()
+{
+}
+
+void Sc::ConstraintCore::setFlags(PxConstraintFlags flags)
+{
+	PxConstraintFlags old = mFlags;
+	flags = flags | (old & PxConstraintFlag::eGPU_COMPATIBLE);
+	if(flags != old)
+	{		
+		mFlags = flags;
+		if(getSim())
+			getSim()->postFlagChange(old, flags);
+	}
+}
+
+void Sc::ConstraintCore::getForce(PxVec3& force, PxVec3& torque) const
+{
+	if(!mSim)
+	{
+		force = PxVec3(0,0,0);
+		torque = PxVec3(0,0,0);
+	}
+	else
+		mSim->getForce(force, torque);
+
+}
+
+void Sc::ConstraintCore::setBodies(RigidCore* r0v, RigidCore* r1v)
+{
+	if(mSim)
+		mSim->postBodiesChange(r0v, r1v);
+}
+
+bool Sc::ConstraintCore::updateConstants(void* addr)
+{
+	if (getSim())
+	{
+		getSim()->setConstantsLL(addr);
+		return true;
+	}
+	return false;
+}
+
+void Sc::ConstraintCore::setBreakForce(PxReal linear, PxReal angular)
+{
+	mLinearBreakForce = linear;
+	mAngularBreakForce = angular;
+
+	if (getSim())
+		getSim()->setBreakForceLL(linear, angular);
+}
+
+void Sc::ConstraintCore::getBreakForce(PxReal& linear, PxReal& angular) const
+{
+	linear = mLinearBreakForce;
+	angular = mAngularBreakForce;
+}
+
+void Sc::ConstraintCore::setMinResponseThreshold(PxReal threshold)
+{
+	mMinResponseThreshold = threshold;
+
+	if (getSim())
+		getSim()->setMinResponseThresholdLL(threshold);
+}
+
+PxConstraint* Sc::ConstraintCore::getPxConstraint()
+{
+	return gOffsetTable.convertScConstraint2Px(this);
+}
+
+const PxConstraint* Sc::ConstraintCore::getPxConstraint() const
+{
+	return gOffsetTable.convertScConstraint2Px(this);
+}
+
+void Sc::ConstraintCore::breakApart()
+{
+	// TODO: probably want to do something with the interaction here
+	// as well as remove the constraint from LL.
+
+	mFlags |= PxConstraintFlag::eBROKEN;
+}
+
+void Sc::ConstraintCore::prepareForSetBodies()
+{
+	if(mSim)
+		mSim->preBodiesChange();
+}

physx/source/simulationcontroller/src/ScConstraintGroupNode.cpp

@@ -0,0 +1,138 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#include "ScConstraintGroupNode.h"
+#include "ScConstraintProjectionManager.h"
+#include "PsFoundation.h"
+#include "ScBodySim.h"
+#include "ScConstraintSim.h"
+#include "ScConstraintInteraction.h"
+
+using namespace physx;
+
+Sc::ConstraintGroupNode::ConstraintGroupNode(BodySim& b) : 
+	body(&b),
+	parent(this),
+	tail(this),
+	rank(0),
+	next(NULL),
+
+	projectionFirstRoot(NULL),
+	projectionNextRoot(NULL),
+	projectionParent(NULL),
+	projectionFirstChild(NULL),
+	projectionNextSibling(NULL),
+	projectionConstraint(NULL),
+
+	flags(0)
+{
+}
+
+
+//
+// Implementation of FIND of 
+// UNION-FIND algo.
+//
+Sc::ConstraintGroupNode& Sc::ConstraintGroupNode::getRoot()
+{
+	PX_ASSERT(parent);
+
+	ConstraintGroupNode* root = parent;
+
+	if (root->parent == root)
+		return *root;
+	else
+	{
+		PxU32 nbHops = 1;
+		root = root->parent;
+
+		while(root != root->parent)
+		{
+			root = root->parent;
+			nbHops++;
+		}
+
+		// Write root to all nodes on the path
+		ConstraintGroupNode* curr = this;
+		while(nbHops)
+		{
+			ConstraintGroupNode* n = curr->parent;
+			curr->parent = root;
+			curr = n;
+			nbHops--;
+		}
+
+		return *root;
+	}
+}
+
+
+void Sc::ConstraintGroupNode::markForProjectionTreeRebuild(ConstraintProjectionManager& cpManager)
+{
+	ConstraintGroupNode& root = getRoot();
+	if (!root.readFlag(ConstraintGroupNode::ePENDING_TREE_UPDATE))
+	{
+		cpManager.addToPendingTreeUpdates(root);
+	}
+}
+
+
+void Sc::ConstraintGroupNode::initProjectionData(ConstraintGroupNode* parent_, ConstraintSim* c)
+{
+	projectionConstraint = c;
+
+	//add us to parent's child list:
+	if (parent_)
+	{
+		projectionNextSibling = parent_->projectionFirstChild;
+		parent_->projectionFirstChild = this;
+
+		projectionParent = parent_;
+	}
+}
+
+
+void Sc::ConstraintGroupNode::clearProjectionData()
+{
+	projectionFirstRoot = NULL;
+	projectionNextRoot = NULL;
+	projectionParent = NULL;
+	projectionFirstChild = NULL;
+	projectionNextSibling = NULL;
+	projectionConstraint = NULL;
+}
+
+
+void Sc::ConstraintGroupNode::projectPose(ConstraintGroupNode& node, Ps::Array<BodySim*>& projectedBodies)
+{
+	PX_ASSERT(node.hasProjectionTreeRoot());
+
+	Sc::ConstraintProjectionTree::projectPose(node, projectedBodies);
+}

physx/source/simulationcontroller/src/ScConstraintGroupNode.h

@@ -0,0 +1,178 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef PX_PHYSICS_SCP_CONSTRAINT_GROUP_NODE
+#define PX_PHYSICS_SCP_CONSTRAINT_GROUP_NODE
+
+#include "ScConstraintProjectionTree.h"
+#include "PsUtilities.h"  // for Ps::to8()
+
+namespace physx
+{
+namespace Sc
+{
+	class ConstraintSim;
+	class BodySim;
+	class ConstraintProjectionManager;
+
+	// A 'simulation island' of constraints. Created by a union-find algorithm every time a new constraint is added to any of the involved bodies.
+	struct ConstraintGroupNode : public Ps::UserAllocated
+	{
+		enum StateFlags
+		{
+			eDISCOVERED						= 1 << 0,	// Used during projection tree generation to mark processed nodes.
+			eIN_PROJECTION_PASS_LIST		= 1 << 1,	// Temporarily used to avoid duplicate entries in the list of nodes that should project the pose after the solver
+			ePENDING_TREE_UPDATE			= 1 << 2,	// Marks the constraint groups that need their projection trees updated. Must only be set on the root group node.
+			eNEXT_FREE_SHIFT				= 3,
+			eNEXT_FREE						= 1 << eNEXT_FREE_SHIFT
+		};
+
+		// these flags should give a rough hint how many projecting constraints to expect in the constraint group. This will be used for
+		// load balancing when running projection in parallel. The intervals were chosen somewhat arbitrarily but the general motivation was
+		// to cover very simple constraint setups, simple ragdolls, complex ragdolls and very complex projection setups. Note that the load
+		// balancing is not waterproof since at the end it is the projection shader from the external constraint implementer (for example, a joint)
+		// which decides based on some thresholds whether projection runs or not.
+		enum ProjectionCountHintFlags
+		{
+			e1_TO_4							= eNEXT_FREE,
+			e5_TO_16						= eNEXT_FREE << 1,
+			e17_TO_64						= eNEXT_FREE << 2,
+			e65_TO_INF						= eNEXT_FREE << 3,
+			eCLEAR_MASK						= ~(0xffffffff << eNEXT_FREE_SHIFT)
+		};
+
+		ConstraintGroupNode(BodySim& b);
+		~ConstraintGroupNode()
+		{
+			PX_ASSERT(!readFlag(ePENDING_TREE_UPDATE));
+			PX_ASSERT(projectionFirstRoot == NULL);
+		}
+
+		PX_FORCE_INLINE		void					raiseFlag(StateFlags f) { flags |= f; }
+		PX_FORCE_INLINE		void					clearFlag(StateFlags f) { flags &= ~f; }
+		PX_FORCE_INLINE		bool					readFlag(StateFlags f) const { return (flags & f) != 0; }
+		PX_FORCE_INLINE		PxU32					getProjectionCountHint() const;
+		PX_FORCE_INLINE		void					setProjectionCountHint(PxU32 constraintsToProjectCount);
+
+							ConstraintGroupNode&	getRoot();
+
+		PX_FORCE_INLINE		void					buildProjectionTrees(); //build the projection trees for a constraint group.
+							void					markForProjectionTreeRebuild(ConstraintProjectionManager&);
+		PX_FORCE_INLINE		void					purgeProjectionTrees();
+		PX_FORCE_INLINE		bool					hasProjectionTreeRoot() { return projectionFirstRoot != NULL; }
+		PX_FORCE_INLINE		void					setProjectionTreeRoot(ConstraintGroupNode* root) { projectionFirstRoot = root; }
+
+							void					initProjectionData(ConstraintGroupNode* parent, ConstraintSim* c);
+							void					clearProjectionData();
+
+		static				void					projectPose(ConstraintGroupNode& root, Ps::Array<BodySim*>& projectedBodies);
+
+
+		BodySim*					body;		//the owner body of this node
+
+		//tree for union/find:
+		ConstraintGroupNode*		parent;
+		ConstraintGroupNode*		tail;		//only valid if this is root of group, points to LList tail node.
+		PxU32						rank;		//rank counter for union/find. Initially zero. Is number of hops from root to furthest leaf in tree. This is just a hint to create more balanced trees.
+		
+		//linked list for traversal:
+		ConstraintGroupNode*		next;		//next in list, NULL at tail.
+
+		//projection tree information
+		ConstraintGroupNode*		projectionFirstRoot;	//pointer to first projection tree root node. Only set for constraint group roots
+		ConstraintGroupNode*		projectionNextRoot;		//pointer to next projection root node. Only set for constraint group roots
+															//a constraint group can consist of multiple projection trees if kinematics are involved! Because a kinematic doesn't split
+															//the constraint group as a static anchor does.
+		ConstraintGroupNode*		projectionParent;		//node to project to
+		ConstraintGroupNode*		projectionFirstChild;	//first node which gets projected to this one
+		ConstraintGroupNode*		projectionNextSibling;	//the next sibling which gets projected to the same node as this one. NULL if projectionParent is NULL.
+		ConstraintSim*				projectionConstraint;	//the constraint to project (constraint to projection parent)
+
+	private:
+		PxU8						flags;
+	};
+
+} // namespace Sc
+
+
+PX_FORCE_INLINE PxU32 Sc::ConstraintGroupNode::getProjectionCountHint() const
+{
+	// return the mean of the upper and lower bound
+
+	if (flags & ConstraintGroupNode::e65_TO_INF)
+		return 128;
+	else if (flags & ConstraintGroupNode::e17_TO_64)
+		return 40;
+	else if (flags & ConstraintGroupNode::e5_TO_16)
+		return 10;
+	else if (flags & ConstraintGroupNode::e1_TO_4)
+		return 2;
+
+	return 0;
+}
+
+
+PX_FORCE_INLINE void Sc::ConstraintGroupNode::setProjectionCountHint(PxU32 constraintsToProjectCount)
+{
+	PxU8 tmpFlags = flags;
+	tmpFlags &= PxU8(ConstraintGroupNode::eCLEAR_MASK);
+
+	if (constraintsToProjectCount >= 65)
+		tmpFlags |= ConstraintGroupNode::e65_TO_INF;
+	else if (constraintsToProjectCount >= 17)
+		tmpFlags |= ConstraintGroupNode::e17_TO_64;
+	else if (constraintsToProjectCount >= 5)
+		tmpFlags |= ConstraintGroupNode::e5_TO_16;
+	else if (constraintsToProjectCount >= 1)
+		tmpFlags |= ConstraintGroupNode::e1_TO_4;
+
+	flags = tmpFlags;
+}
+
+
+PX_FORCE_INLINE void Sc::ConstraintGroupNode::buildProjectionTrees()
+{
+	PX_ASSERT(this == parent);  // Only call for group roots
+	PX_ASSERT(!hasProjectionTreeRoot());
+
+	ConstraintProjectionTree::buildProjectionTrees(*this);
+}
+
+
+PX_FORCE_INLINE void Sc::ConstraintGroupNode::purgeProjectionTrees()
+{
+	PX_ASSERT(this == parent);  // Only call for group roots
+	PX_ASSERT(hasProjectionTreeRoot());
+	ConstraintProjectionTree::purgeProjectionTrees(*this);
+}
+
+}
+
+#endif

physx/source/simulationcontroller/src/ScConstraintInteraction.cpp

@@ -0,0 +1,169 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "ScConstraintInteraction.h"
+#include "ScConstraintSim.h"
+#include "ScBodySim.h"
+#include "ScScene.h"
+#include "PxsRigidBody.h"
+#include "PxsSimpleIslandManager.h"
+
+using namespace physx;
+using namespace Sc;
+
+ConstraintInteraction::ConstraintInteraction(ConstraintSim* constraint, RigidSim& r0, RigidSim& r1) :
+	Interaction	(r0, r1, InteractionType::eCONSTRAINTSHADER, InteractionFlag::eCONSTRAINT),
+	mConstraint	(constraint)
+{
+	registerInActors();
+
+	BodySim* b0 = mConstraint->getBody(0);
+	BodySim* b1 = mConstraint->getBody(1);
+
+	if(b0)
+		b0->onConstraintAttach();
+	if(b1)
+		b1->onConstraintAttach();
+
+	IG::SimpleIslandManager* simpleIslandManager = getScene().getSimpleIslandManager();
+	mEdgeIndex = simpleIslandManager->addConstraint(&mConstraint->getLowLevelConstraint(), b0 ? b0->getNodeIndex() : IG::NodeIndex(), b1 ? b1->getNodeIndex() : IG::NodeIndex(), this);
+}
+
+ConstraintInteraction::~ConstraintInteraction()
+{
+	PX_ASSERT(!readInteractionFlag(InteractionFlag::eIN_DIRTY_LIST));
+	PX_ASSERT(!getDirtyFlags());
+	PX_ASSERT(!mConstraint->readFlag(ConstraintSim::eCHECK_MAX_FORCE_EXCEEDED));
+}
+
+static PX_FORCE_INLINE void	removeFromActiveBreakableList(ConstraintSim* constraint, Scene& s)
+{
+	if(constraint->readFlag(ConstraintSim::eBREAKABLE | ConstraintSim::eCHECK_MAX_FORCE_EXCEEDED) == (ConstraintSim::eBREAKABLE | ConstraintSim::eCHECK_MAX_FORCE_EXCEEDED))
+		s.removeActiveBreakableConstraint(constraint);
+}
+
+void ConstraintInteraction::destroy()
+{
+	setClean(true);  // removes the pair from the dirty interaction list etc.
+
+	Scene& scene = getScene();
+
+	removeFromActiveBreakableList(mConstraint, scene);
+
+	if(mEdgeIndex != IG_INVALID_EDGE)
+		scene.getSimpleIslandManager()->removeConnection(mEdgeIndex);
+	mEdgeIndex = IG_INVALID_EDGE;
+
+	unregisterFromActors();
+
+	BodySim* b0 = mConstraint->getBody(0);
+	BodySim* b1 = mConstraint->getBody(1);
+
+	if(b0)
+		b0->onConstraintDetach();  // Note: Has to be done AFTER the interaction has unregistered from the actors
+	if(b1)
+		b1->onConstraintDetach();  // Note: Has to be done AFTER the interaction has unregistered from the actors
+
+	clearInteractionFlag(InteractionFlag::eIS_ACTIVE);  // ensures that broken constraints do not go into the list of active breakable constraints anymore
+}
+
+void ConstraintInteraction::updateState()
+{
+	PX_ASSERT(!mConstraint->isBroken());
+	PX_ASSERT(getDirtyFlags() & InteractionDirtyFlag::eBODY_KINEMATIC);  // at the moment this should be the only reason for this method being called
+
+	// at least one of the bodies got switched from kinematic to dynamic. This will not have changed the sleep state of the interactions, so the
+	// constraint interactions are just marked dirty and processed as part of the dirty interaction update system.
+	// 
+	// -> need to check whether to activate the constraint and whether constraint break testing
+	//    is now necessary
+	//
+	// the transition from dynamic to kinematic will always trigger an onDeactivate_() (because the body gets deactivated)
+	// and thus there is no need to consider that case here.
+	//
+
+	onActivate_(NULL);	// note: this will not activate if the necessary conditions are not met, so it can be called even if the pair has been deactivated again before the
+					//       simulation step started
+}
+
+bool ConstraintInteraction::onActivate_(void*)
+{
+	PX_ASSERT(!mConstraint->isBroken());
+
+	BodySim* b0 = mConstraint->getBody(0);
+	BodySim* b1 = mConstraint->getBody(1);
+
+	const bool b0Vote = !b0 || b0->isActive();
+	const bool b1Vote = !b1 || b1->isActive();
+
+	const bool b0Dynamic = b0 && (!b0->isKinematic());
+	const bool b1Dynamic = b1 && (!b1->isKinematic());
+
+	//
+	// note: constraints between kinematics and kinematics/statics are always inactive and must not be activated
+	//
+	if((b0Vote || b1Vote) && (b0Dynamic || b1Dynamic))
+	{
+		raiseInteractionFlag(InteractionFlag::eIS_ACTIVE);
+
+		if(mConstraint->readFlag(ConstraintSim::eBREAKABLE | ConstraintSim::eCHECK_MAX_FORCE_EXCEEDED) == ConstraintSim::eBREAKABLE)
+			getScene().addActiveBreakableConstraint(mConstraint, this);
+
+		return true;
+	}
+	else
+		return false;
+}
+
+bool ConstraintInteraction::onDeactivate_()
+{
+	const BodySim* b0 = mConstraint->getBody(0);
+	const BodySim* b1 = mConstraint->getBody(1);
+
+	const bool b0Dynamic = b0 && (!b0->isKinematic());
+	const bool b1Dynamic = b1 && (!b1->isKinematic());
+
+	PX_ASSERT(	(!b0 && b1 && !b1->isActive()) || 
+				(!b1 && b0 && !b0->isActive()) ||
+				((b0 && b1 && (!b0->isActive() || !b1->isActive()))) );
+
+	//
+	// note: constraints between kinematics and kinematics/statics should always get deactivated
+	//
+	if(((!b0 || !b0->isActive()) && (!b1 || !b1->isActive())) || (!b0Dynamic && !b1Dynamic))
+	{
+		removeFromActiveBreakableList(mConstraint, getScene());
+
+		clearInteractionFlag(InteractionFlag::eIS_ACTIVE);
+		
+		return true;
+	}
+	else
+		return false;
+}

physx/source/simulationcontroller/src/ScConstraintInteraction.h

@@ -0,0 +1,66 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef PX_PHYSICS_SCP_CONSTRAINTSHADERINTERACTION
+#define PX_PHYSICS_SCP_CONSTRAINTSHADERINTERACTION
+
+#include "ScInteraction.h"
+
+namespace physx
+{
+namespace Sc
+{
+	class ConstraintSim;
+	class RigidSim;
+
+	class ConstraintInteraction : public Interaction
+	{
+	public:
+										ConstraintInteraction(ConstraintSim* shader, RigidSim& r0, RigidSim& r1);
+										~ConstraintInteraction();
+
+						bool			onActivate_(void* data);
+						bool			onDeactivate_();
+
+						void			updateState();
+						void			destroy();  // disables the interaction and unregisters from the system. Does NOT delete the object. This is used on destruction but also when a constraint breaks.
+
+		PX_FORCE_INLINE	ConstraintSim*	getConstraint()			{ return mConstraint;	}
+		PX_FORCE_INLINE	PxU32			getEdgeIndex()	const	{ return mEdgeIndex;	}
+
+	private:
+						ConstraintSim*	mConstraint;
+						PxU32			mEdgeIndex;
+	};
+
+} // namespace Sc
+
+}
+
+#endif

physx/source/simulationcontroller/src/ScConstraintProjectionManager.cpp

@@ -0,0 +1,505 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#include "PxcScratchAllocator.h"
+#include "ScConstraintProjectionManager.h"
+#include "ScBodySim.h"
+#include "ScConstraintSim.h"
+#include "ScConstraintInteraction.h"
+
+using namespace physx;
+
+
+namespace physx
+{
+namespace Sc
+{
+
+template<typename T, const PxU32 elementsPerBlock>
+class ScratchAllocatorList
+{
+private:
+	struct ElementBlock
+	{
+		PX_FORCE_INLINE ElementBlock() {}
+		PX_FORCE_INLINE void init(PxU32 countAtStart) { next = NULL; count = countAtStart; }
+
+		ElementBlock* next;
+		PxU32 count;
+		T elements[elementsPerBlock];
+	};
+
+	PX_FORCE_INLINE const ScratchAllocatorList& operator=(const ScratchAllocatorList&) {}
+
+
+public:
+	class Iterator
+	{
+		friend class ScratchAllocatorList;
+
+	public:
+		T const* getNext()
+		{
+			if (mCurrentBlock)
+			{
+				if (mIndex < mCurrentBlock->count)
+				{
+					return &mCurrentBlock->elements[mIndex++];
+				}
+				else
+				{
+					if (mCurrentBlock->next)
+					{
+						PX_ASSERT(mCurrentBlock->count == elementsPerBlock);
+						mCurrentBlock = mCurrentBlock->next;
+						PX_ASSERT(mCurrentBlock->count > 0);
+
+						mIndex = 1;
+						return &mCurrentBlock->elements[0];
+					}
+					else
+						return NULL;
+				}
+			}
+			else
+				return NULL;
+		}
+
+	private:
+		Iterator(const ElementBlock* startBlock) : mCurrentBlock(startBlock), mIndex(0) {}
+
+	private:
+		const ElementBlock* mCurrentBlock;
+		PxU32 mIndex;
+	};
+
+	PX_FORCE_INLINE ScratchAllocatorList(PxcScratchAllocator& scratchAllocator) : mScratchAllocator(scratchAllocator)
+	{
+		mFirstBlock = reinterpret_cast<ElementBlock*>(scratchAllocator.alloc(sizeof(ElementBlock), true));
+		if (mFirstBlock)
+			mFirstBlock->init(0);
+
+		mCurrentBlock = mFirstBlock;
+	}
+
+	PX_FORCE_INLINE ~ScratchAllocatorList()
+	{
+		freeMemory();
+	}
+
+	PX_FORCE_INLINE bool add(const T& element)
+	{
+		if (mCurrentBlock)
+		{
+			if (mCurrentBlock->count < elementsPerBlock)
+			{
+				mCurrentBlock->elements[mCurrentBlock->count] = element;
+				mCurrentBlock->count++;
+				return true;
+			}
+			else
+			{
+				PX_ASSERT(mCurrentBlock->next == NULL);
+				PX_ASSERT(mCurrentBlock->count == elementsPerBlock);
+
+				ElementBlock* newBlock = reinterpret_cast<ElementBlock*>(mScratchAllocator.alloc(sizeof(ElementBlock), true));
+				if (newBlock)
+				{
+					newBlock->init(1);
+					newBlock->elements[0] = element;
+					mCurrentBlock->next = newBlock;
+					mCurrentBlock = newBlock;
+					return true;
+				}
+				else
+					return false;
+			}
+		}
+		else
+			return false;
+	}
+
+	PX_FORCE_INLINE Iterator getIterator() const
+	{
+		return Iterator(mFirstBlock);
+	}
+
+	PX_FORCE_INLINE void freeMemory()
+	{
+		ElementBlock* block = mFirstBlock;
+
+		while(block)
+		{
+			ElementBlock* blockToFree = block;
+			block = block->next;
+
+			mScratchAllocator.free(blockToFree);
+		}
+	}
+
+
+private:
+	PxcScratchAllocator& mScratchAllocator;
+	ElementBlock* mFirstBlock;
+	ElementBlock* mCurrentBlock;
+};
+
+}
+}
+
+
+Sc::ConstraintProjectionManager::ConstraintProjectionManager() : 
+	mNodePool(PX_DEBUG_EXP("projectionNodePool"))
+{
+}
+
+
+void Sc::ConstraintProjectionManager::addToPendingGroupUpdates(Sc::ConstraintSim& s)
+{
+	PX_ASSERT(!s.readFlag(ConstraintSim::ePENDING_GROUP_UPDATE));
+	bool isNew = mPendingGroupUpdates.insert(&s);
+	PX_UNUSED(isNew);
+	PX_ASSERT(isNew);
+
+	s.setFlag(ConstraintSim::ePENDING_GROUP_UPDATE);
+}
+
+
+void Sc::ConstraintProjectionManager::removeFromPendingGroupUpdates(Sc::ConstraintSim& s)
+{
+	PX_ASSERT(s.readFlag(ConstraintSim::ePENDING_GROUP_UPDATE));
+	bool didExist = mPendingGroupUpdates.erase(&s);
+	PX_UNUSED(didExist);
+	PX_ASSERT(didExist);
+
+	s.clearFlag(ConstraintSim::ePENDING_GROUP_UPDATE);
+}
+
+
+void Sc::ConstraintProjectionManager::addToPendingTreeUpdates(ConstraintGroupNode& n)
+{
+	PX_ASSERT(&n == &n.getRoot());
+	PX_ASSERT(!n.readFlag(ConstraintGroupNode::ePENDING_TREE_UPDATE));
+	bool isNew = mPendingTreeUpdates.insert(&n);
+	PX_UNUSED(isNew);
+	PX_ASSERT(isNew);
+
+	n.raiseFlag(ConstraintGroupNode::ePENDING_TREE_UPDATE);
+}
+
+
+void Sc::ConstraintProjectionManager::removeFromPendingTreeUpdates(ConstraintGroupNode& n)
+{
+	PX_ASSERT(&n == &n.getRoot());
+	PX_ASSERT(n.readFlag(ConstraintGroupNode::ePENDING_TREE_UPDATE));
+	bool didExist = mPendingTreeUpdates.erase(&n);
+	PX_UNUSED(didExist);
+	PX_ASSERT(didExist);
+
+	n.clearFlag(ConstraintGroupNode::ePENDING_TREE_UPDATE);
+}
+
+
+PX_INLINE Sc::ConstraintGroupNode* Sc::ConstraintProjectionManager::createGroupNode(BodySim& b)
+{
+	ConstraintGroupNode* n = mNodePool.construct(b);
+	b.setConstraintGroup(n);
+	return n;
+}
+
+
+//
+// Implementation of UNION of 
+// UNION-FIND algo.
+// It also updates the group traversal
+// linked list.
+//
+void Sc::ConstraintProjectionManager::groupUnion(ConstraintGroupNode& root0, ConstraintGroupNode& root1)
+{
+	// Should only get called for the roots
+	PX_ASSERT(&root0 == root0.parent);
+	PX_ASSERT(&root1 == root1.parent);
+
+	if (&root0 != &root1)	//different groups?  If not, its already merged.
+	{
+		//UNION(this, other);	//union-find algo unites groups.
+		ConstraintGroupNode* newRoot;
+		ConstraintGroupNode* otherRoot;
+		if (root0.rank > root1.rank)
+		{
+			//hisGroup appended to mygroup.
+			newRoot = &root0;
+			otherRoot = &root1;
+		}
+		else
+		{
+			//myGroup appended to hisGroup. 
+			newRoot = &root1;
+			otherRoot = &root0;
+			//there is a chance that the two ranks were equal, in which case the tree depth just increased.
+			root1.rank++;
+		}
+
+		PX_ASSERT(newRoot->parent == newRoot);
+		otherRoot->parent = newRoot;
+		
+		//update traversal linked list:
+		newRoot->tail->next = otherRoot;
+		newRoot->tail = otherRoot->tail;
+	}
+}
+
+
+//
+// Add a body to a constraint projection group.
+//
+void Sc::ConstraintProjectionManager::addToGroup(BodySim& b, BodySim* other, ConstraintSim& c)
+{
+	// If both bodies of the constraint are defined, we want to fetch the reference to the group root
+	// from body 0 by default (allows to avoid checking both)
+	PX_ASSERT(&b == c.getBody(0) || (c.getBody(0) == NULL && &b == c.getBody(1)));
+	PX_UNUSED(c);
+
+	ConstraintGroupNode* myRoot;
+	if (!b.getConstraintGroup())
+		myRoot = createGroupNode(b);
+	else
+	{
+		myRoot = &b.getConstraintGroup()->getRoot();
+		if (myRoot->hasProjectionTreeRoot())
+			myRoot->purgeProjectionTrees();  // If a new constraint gets added to a constraint group, projection trees need to be recreated
+	}
+
+	if (other)
+	{
+		ConstraintGroupNode* otherRoot;
+		if (!other->getConstraintGroup())
+			otherRoot = createGroupNode(*other);
+		else
+		{
+			otherRoot = &other->getConstraintGroup()->getRoot();
+			if (otherRoot->hasProjectionTreeRoot())
+				otherRoot->purgeProjectionTrees();  // If a new constraint gets added to a constraint group, projection trees need to be recreated
+		}
+
+		//merge the two groups, if disjoint.
+		groupUnion(*myRoot, *otherRoot);
+	}
+}
+
+
+//
+// Add all projection constraints connected to the specified body to the pending update list but
+// ignore the specified constraint.
+//
+void Sc::ConstraintProjectionManager::markConnectedConstraintsForUpdate(BodySim& b, ConstraintSim* c)
+{
+	PxU32 size = b.getActorInteractionCount();
+	Interaction** interactions = b.getActorInteractions();
+	while(size--)
+	{
+		Interaction* interaction = *interactions++;
+		if (interaction->getType() == InteractionType::eCONSTRAINTSHADER)
+		{
+			ConstraintSim* ct = static_cast<ConstraintInteraction*>(interaction)->getConstraint();
+
+			if ((ct != c) && ct->needsProjection() && (!ct->readFlag(ConstraintSim::ePENDING_GROUP_UPDATE)))
+			{
+				//mark constraint for pending update:
+				addToPendingGroupUpdates(*ct);
+			}
+		}
+	}
+}
+
+
+//
+// Add all constraints connected to the specified body to an array but
+// ignore the specified constraint.
+//
+PX_FORCE_INLINE static void dumpConnectedConstraints(Sc::BodySim& b, Sc::ConstraintSim* c, Sc::ScratchAllocatorList<Sc::ConstraintSim*>& constraintList)
+{
+	PxU32 size = b.getActorInteractionCount();
+	Sc::Interaction** interactions = b.getActorInteractions();
+	while(size--)
+	{
+		Sc::Interaction* interaction = *interactions++;
+		if (interaction->getType() == Sc::InteractionType::eCONSTRAINTSHADER)
+		{
+			Sc::ConstraintSim* ct = static_cast<Sc::ConstraintInteraction*>(interaction)->getConstraint();
+
+			if ((ct != c) && (!ct->readFlag(Sc::ConstraintSim::ePENDING_GROUP_UPDATE)))
+			{
+				bool success = constraintList.add(ct);
+				PX_UNUSED(success);
+				PX_ASSERT(success);
+			}
+		}
+	}
+}
+
+
+PX_FORCE_INLINE void Sc::ConstraintProjectionManager::processConstraintForGroupBuilding(ConstraintSim* c, ScratchAllocatorList<ConstraintSim*>& constraintList)
+{
+	c->clearFlag(ConstraintSim::ePENDING_GROUP_UPDATE);
+
+	// Find all constraints connected to the two bodies of the dirty constraint.
+	// - Constraints to static anchors are ignored (note: kinematics can't be ignored because they might get switched to dynamics any time which
+	//   does trigger a projection tree rebuild but not a constraint tree rebuild
+	// - Already processed bodies are ignored as well
+	BodySim* b0 = c->getBody(0);
+	if (b0 && !b0->getConstraintGroup())
+	{
+		dumpConnectedConstraints(*b0, c, constraintList);
+	}
+	BodySim* b1 = c->getBody(1);
+	if (b1 && !b1->getConstraintGroup())
+	{
+		dumpConnectedConstraints(*b1, c, constraintList);
+	}
+
+	BodySim* b = c->getAnyBody();
+	PX_ASSERT(b);
+
+	addToGroup(*b, c->getOtherBody(b), *c);  //this will eventually merge some body's constraint groups.
+}
+
+
+void Sc::ConstraintProjectionManager::processPendingUpdates(PxcScratchAllocator& scratchAllocator)
+{
+	//
+	// if there are dirty projection trees, then rebuild them
+	//
+	const PxU32 nbProjectionTreesToUpdate = mPendingTreeUpdates.size();
+	if (nbProjectionTreesToUpdate)
+	{
+		ConstraintGroupNode* const* projectionTreesToUpdate = mPendingTreeUpdates.getEntries();
+		for(PxU32 i=0; i < nbProjectionTreesToUpdate; i++)
+		{
+			ConstraintGroupNode* n = projectionTreesToUpdate[i];
+
+			PX_ASSERT(n == &n->getRoot());  // only root nodes should be in that list
+			PX_ASSERT(n->readFlag(ConstraintGroupNode::ePENDING_TREE_UPDATE));
+
+			n->clearFlag(ConstraintGroupNode::ePENDING_TREE_UPDATE);
+
+			// note: it is valid to get here and not have a projection root. This is the case if all nodes of a constraint graph are kinematic
+			//       at some point (hence no projection root) and later some of those get switched to dynamic.
+			if (n->hasProjectionTreeRoot())
+				n->purgeProjectionTrees();
+			n->buildProjectionTrees();
+		}
+
+		mPendingTreeUpdates.clear();
+	}
+
+	//
+	// if there are new/dirty constraints, update groups
+	//
+	const PxU32 nbProjectionConstraintsToUpdate = mPendingGroupUpdates.size();
+
+	if (nbProjectionConstraintsToUpdate)
+	{
+		ScratchAllocatorList<ConstraintSim*> nonProjectionConstraintList(scratchAllocator);
+
+		ConstraintSim* const* projectionConstraintsToUpdate = mPendingGroupUpdates.getEntries();
+
+#if PX_DEBUG
+		// At the beginning the list should only contain constraints with projection.
+		// Further below other constraints, connected to the constraints with projection, will be added too.
+		for(PxU32 i=0; i < nbProjectionConstraintsToUpdate; i++)
+		{
+			PX_ASSERT(projectionConstraintsToUpdate[i]->needsProjection());
+		}
+#endif
+		for(PxU32 i=0; i < nbProjectionConstraintsToUpdate; i++)
+		{
+			processConstraintForGroupBuilding(projectionConstraintsToUpdate[i], nonProjectionConstraintList);
+		}
+
+		ScratchAllocatorList<ConstraintSim*>::Iterator iter = nonProjectionConstraintList.getIterator();
+		ConstraintSim* const* nextConstraint = iter.getNext();
+		while(nextConstraint)
+		{
+			processConstraintForGroupBuilding(*nextConstraint, nonProjectionConstraintList);
+
+			nextConstraint = iter.getNext();
+		}
+
+		// Now find all the newly made groups and build projection trees.
+		// Don't need to iterate over the additionally constraints since the roots are supposed to be
+		// fetchable from any node.
+		for (PxU32 i=0; i < nbProjectionConstraintsToUpdate; i++)
+		{
+			ConstraintSim* c = projectionConstraintsToUpdate[i];
+			BodySim* b = c->getAnyBody();
+			PX_ASSERT(b);
+			PX_ASSERT(b->getConstraintGroup());
+
+			ConstraintGroupNode& root = b->getConstraintGroup()->getRoot();
+			if (!root.hasProjectionTreeRoot())  // Build projection tree only once
+				root.buildProjectionTrees();
+		}
+
+		mPendingGroupUpdates.clear();
+	}
+}
+
+
+//
+// Called if a body or a constraint gets deleted. All projecting constraints of the
+// group (except the deleted one) are moved to the dirty list and all group nodes are destroyed.
+//
+void Sc::ConstraintProjectionManager::invalidateGroup(ConstraintGroupNode& node, ConstraintSim* deletedConstraint)
+{
+	ConstraintGroupNode* n = &node.getRoot();
+
+	if (n->readFlag(ConstraintGroupNode::ePENDING_TREE_UPDATE))
+	{
+		removeFromPendingTreeUpdates(*n);
+	}
+
+	while (n) //go through nodes in constraint group
+	{
+		markConnectedConstraintsForUpdate(*n->body, deletedConstraint);
+
+		//destroy the body's constraint group information
+
+		ConstraintGroupNode* next = n->next;	//save next node ptr before we destroy it!
+
+		BodySim* b = n->body;
+		b->setConstraintGroup(NULL);
+		if (n->hasProjectionTreeRoot())
+			n->purgeProjectionTrees();
+		mNodePool.destroy(n);
+
+		n = next;
+	}
+}

physx/source/simulationcontroller/src/ScConstraintProjectionManager.h

@@ -0,0 +1,84 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef PX_PHYSICS_SCP_CONSTRAINT_PROJECTION_MANAGER
+#define PX_PHYSICS_SCP_CONSTRAINT_PROJECTION_MANAGER
+
+#include "PsPool.h"
+#include "PsHashSet.h"
+#include "ScConstraintGroupNode.h"
+
+namespace physx
+{
+	class PxcScratchAllocator;
+
+namespace Sc
+{
+	class ConstraintSim;
+	class BodySim;
+	template<typename T, const PxU32 elementsPerBlock = 64> class ScratchAllocatorList;
+
+	class ConstraintProjectionManager : public Ps::UserAllocated
+	{
+	public:
+		ConstraintProjectionManager();
+		~ConstraintProjectionManager() {}
+
+		void addToPendingGroupUpdates(ConstraintSim& s);
+		void removeFromPendingGroupUpdates(ConstraintSim& s);
+
+		void addToPendingTreeUpdates(ConstraintGroupNode& n);
+		void removeFromPendingTreeUpdates(ConstraintGroupNode& n);
+
+		void processPendingUpdates(PxcScratchAllocator&);
+		void invalidateGroup(ConstraintGroupNode& node, ConstraintSim* constraintDeleted);
+
+	private:
+		PX_INLINE Sc::ConstraintGroupNode* createGroupNode(BodySim& b);
+
+		void addToGroup(BodySim& b, BodySim* other, ConstraintSim& c);
+		void groupUnion(ConstraintGroupNode& root0, ConstraintGroupNode& root1);
+		void markConnectedConstraintsForUpdate(BodySim& b, ConstraintSim* c);
+		PX_FORCE_INLINE void processConstraintForGroupBuilding(ConstraintSim* c, ScratchAllocatorList<ConstraintSim*>&);
+
+
+	private:
+		Ps::Pool<ConstraintGroupNode>				mNodePool;
+		Ps::CoalescedHashSet<ConstraintSim*>		mPendingGroupUpdates; //list of constraints for which constraint projection groups need to be generated/updated
+		Ps::CoalescedHashSet<ConstraintGroupNode*>	mPendingTreeUpdates;	//list of constraint groups that need their projection trees rebuilt. Note: non of the
+																			//constraints in those groups are allowed to be in mPendingGroupUpdates at the same time
+																			//because a group update will automatically trigger tree rebuilds.
+	};
+
+} // namespace Sc
+
+}
+
+#endif

physx/source/simulationcontroller/src/ScConstraintProjectionTree.cpp

@@ -0,0 +1,567 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#include "ScConstraintProjectionTree.h"
+#include "ScScene.h"
+#include "ScBodySim.h"
+#include "ScConstraintCore.h"
+#include "ScConstraintSim.h"
+#include "ScConstraintInteraction.h"
+
+#include "PsFoundation.h"
+#include "PsBasicTemplates.h"
+#include "PsSort.h"
+#include "PsArray.h"
+
+using namespace physx;
+
+//------------------------------------------------------------------------------------------
+//
+// The projection tree related code
+// 
+// Projection trees are built out of a constraint group/graph. The constraint group just tracks
+// the constraint connectivity while the projection trees define the projection root and
+// the projection order.
+// A constraint group can contain multiple projection trees.
+//
+//------------------------------------------------------------------------------------------
+
+class Sc::BodyRank		
+{
+public:
+	PX_INLINE bool operator>(const BodyRank & b) const
+	{
+		return rank > b.rank;
+	}
+
+	Sc::ConstraintGroupNode* startingNode;
+	Sc::ConstraintSim* constraintToFixedAnchor;
+	PxU32 rank;
+
+	//
+	// The following weights are defined to fulfill the projection priorities described further below
+	//
+	static const PxU32 sOneWayProjection = PxU32(1 << 31);
+	static const PxU32 sAttachedToStatic = (1 << 30);
+	static const PxU32 sAttachedToKinematic = (1 << 29);
+	static const PxU32 sAllDominantDynamic = (1 << 28);  // if for a dynamic body all connections with projection, are one-way towards the body
+	static const PxU32 sDominantDynamic = (1 << 27);  // almost the same as above but there is at least one two-way projection
+	static const PxU32 sAttachedToDynamic = 1;
+	static const PxU32 sPrimaryTreeRootMinRank = sOneWayProjection | sAllDominantDynamic;
+};
+
+
+PX_INLINE bool isFixedBody(const Sc::BodySim* b)
+{
+	return (!b || (b->isKinematic()));
+}
+
+
+void Sc::ConstraintProjectionTree::getConstraintStatus(const ConstraintSim& c, const BodySim* b, BodySim*& otherBody, PxU32& projectToBody, PxU32& projectToOtherBody)
+{
+	const PxU32 isBroken = c.isBroken() ? 0 : 0xffffffff;
+	const PxU32 projFlags = c.getCore().getFlags() & PxConstraintFlag::ePROJECTION;
+	
+	if (b == c.getBody(0))
+	{
+		projectToBody = isBroken & (projFlags & PxConstraintFlag::ePROJECT_TO_ACTOR0);
+		projectToOtherBody = isBroken & (projFlags & PxConstraintFlag::ePROJECT_TO_ACTOR1);
+
+		otherBody = c.getBody(1);
+	}
+	else
+	{
+		projectToBody = isBroken & (projFlags & PxConstraintFlag::ePROJECT_TO_ACTOR1);
+		projectToOtherBody = isBroken & (projFlags & PxConstraintFlag::ePROJECT_TO_ACTOR0);
+
+		otherBody = c.getBody(0);
+	}
+}
+
+
+void Sc::ConstraintProjectionTree::rankConstraint(ConstraintSim& c, BodyRank& br, PxU32& dominanceTracking, PxU32& constraintsToProjectCount)
+{
+	PxU32 projectToBody, projectToOtherBody;
+	BodySim* otherB;
+	getConstraintStatus(c, br.startingNode->body, otherB, projectToBody, projectToOtherBody);
+
+	if (isFixedBody(otherB))	// joint to fixed anchor
+	{
+		PxU32 rank;
+		if (projectToOtherBody)
+		{
+			dominanceTracking = 0;  // makes sure that the flags below will never get raised again for the body
+			br.rank &= ~(BodyRank::sAllDominantDynamic | BodyRank::sDominantDynamic);
+			rank = BodyRank::sOneWayProjection;  //we should prefer picking projected constraints as the root over non-projected ones.
+			constraintsToProjectCount++;
+		}
+		else
+			rank = 0;
+
+		if (!otherB)
+			rank |= BodyRank::sAttachedToStatic;
+		else
+		{
+			PX_ASSERT(otherB->isKinematic());
+			rank |= BodyRank::sAttachedToKinematic;
+		}
+
+		// the highest ranked fixed anchor constraint should get tracked
+		if ((!br.constraintToFixedAnchor) || (rank > br.rank))
+			br.constraintToFixedAnchor = &c;
+
+		br.rank |= rank;
+	}
+	else
+	{
+		if (projectToBody && projectToOtherBody)
+		{
+			dominanceTracking &= ~BodyRank::sAllDominantDynamic;  // makes sure that from now on this will never get raised again for the body
+			br.rank &= ~BodyRank::sAllDominantDynamic;
+			constraintsToProjectCount++;
+		}
+		else if (projectToOtherBody)
+		{
+			dominanceTracking &= ~(BodyRank::sAllDominantDynamic | BodyRank::sDominantDynamic);  // makes sure that from now on these will never get raised again for the body
+			br.rank &= ~(BodyRank::sAllDominantDynamic | BodyRank::sDominantDynamic);
+			constraintsToProjectCount++;
+		}
+		else if (projectToBody)
+		{
+			br.rank |= BodyRank::sOneWayProjection | (dominanceTracking & (BodyRank::sAllDominantDynamic | BodyRank::sDominantDynamic));
+			constraintsToProjectCount++;
+		}
+
+		br.rank += BodyRank::sAttachedToDynamic;
+	}
+}
+
+
+/*
+the goal here is to take the constraint group whose root is passed, and create one or more projection trees.
+
+At the moment, the group has to be acyclic and have at most 1 constraint with the ground to be accepted
+without being broken up into multiple trees.
+
+We 'flood fill' the constraint graph several times, starting at bodies where projection trees can be rooted. 
+Projection tree roots are always dynamic bodies which either need to get projected to a fixed anchor directly 
+or have projecting constraints between dynamics some way along the tree branches. Static and kinematic actors
+are never roots and will not be explicitly part of any tree (but a tree root can project to at most one such fixed node).
+
+The algo looks like this:
+
+for all bodies
+mark body as undiscovered
+rank this body
+
+The rank of a body depends on the constraints it's connected to. It defines the projection priority which
+should be (highest first):
+- dynamic attached to static/world with projection
+- dynamic attached to kinematic with projection
+- all dominant dynamic (has projecting constraints and all of them are one-way towards this dynamic)
+---- all the ones above are guaranteed tree roots
+- dominant dynamic (same as above but there is at least one projecting two-way constraint as well)
+- partially dominant dynamic (has at least one projecting one-way constraints towards this dynamic and at least one projecting one-way constraints towards an other body)
+- dynamic attached to static/world without projection
+- dynamic attached to kinematic without projection
+- dynamic with or without two-way projecting constraints to other dynamics (among these, the one with the highest connectivity count wins)
+
+for the first three priority types sorted according to rank:
+create a projection tree root and grow the tree one connectivity layer at a time
+
+do the same for dominant dynamic bodies that have not been visited/discovered yet
+
+for all remaining bodies sorted according to rank:
+if the body still hasn't been visited/discovered start a projection tree there and build the whole tree in one go
+before moving to the next potential root.
+*/
+void Sc::ConstraintProjectionTree::buildProjectionTrees(ConstraintGroupNode& root)
+{
+	PX_ASSERT(&root == root.parent);
+	PX_ASSERT(!root.hasProjectionTreeRoot());
+
+	Ps::InlineArray<BodyRank, 64> bodyRankArray PX_DEBUG_EXP("bodyRankArray");
+	BodyRank br;
+	PxU32 constraintsToProjectCount = 0;
+	ConstraintGroupNode* node0 = &root;
+	while (node0)	//for all nodes in group
+	{
+		PX_ASSERT(node0->body);
+		if (!node0->body->isKinematic())
+		{
+			node0->clearFlag(ConstraintGroupNode::eDISCOVERED);
+
+			//rank 
+			br.startingNode = node0;
+			br.rank = 0;
+			br.constraintToFixedAnchor = 0;
+			PxU32 dominanceTracking = BodyRank::sAllDominantDynamic | BodyRank::sDominantDynamic;
+
+			//go through all constraints connected to body
+			PxU32 size = node0->body->getActorInteractionCount();
+			Sc::Interaction** interactions = node0->body->getActorInteractions();
+			while(size--)
+			{
+				Interaction* interaction = *interactions++;
+				if (interaction->getType() == InteractionType::eCONSTRAINTSHADER)
+				{
+					ConstraintSim* c = static_cast<ConstraintInteraction*>(interaction)->getConstraint();
+					rankConstraint(*c, br, dominanceTracking, constraintsToProjectCount);
+				}
+			}
+
+			PX_ASSERT(br.rank);	//if it has no constraints then why is it in the constraint group?
+
+			if (br.rank >= BodyRank::sPrimaryTreeRootMinRank)
+				node0->raiseFlag(ConstraintGroupNode::eDISCOVERED);	// we create a tree for each node attached to a fixed anchor, or a node which is an all dominating dynamic
+																	// -> make sure they do not include each other
+
+			bodyRankArray.pushBack(br);
+		}
+		else
+			node0->raiseFlag(ConstraintGroupNode::eDISCOVERED);  // a kinematic does not get projected, it might only get projected to and it is never part of a tree.
+
+		node0 = node0->next;
+	}
+
+	root.setProjectionCountHint(constraintsToProjectCount);
+
+	if (bodyRankArray.size())  // all of the bodies might have been switched to kinematic in which case there will be no ranked body
+	{
+		//sort bodyRankArray
+
+		Ps::sort(&bodyRankArray.front(), bodyRankArray.size(), Ps::Greater<BodyRank>());
+
+		ConstraintGroupNode** nodeQueue = reinterpret_cast<ConstraintGroupNode**>(PX_ALLOC_TEMP(sizeof(ConstraintGroupNode*)*bodyRankArray.size(), "ProjectionNodeQueue"));
+		if (nodeQueue)
+		{
+			//build the projectionTree
+
+			ConstraintGroupNode* firstProjectionTreeRoot = NULL;
+
+			//go through it in sorted order
+
+			//
+			// bodies attached to fixed anchors with projecting constraints or all dominant rigid dynamics should get processed first.
+			// For each of those we create a projection tree for sure, by extending one connectivity level from the root at a time. 
+			// This way we make sure that scenarios like a bridge that is attached to fixed anchors at both ends breaks in the middle 
+			// and not at one of the fixed anchors.
+			//
+			// this gets repeated for dominant dynamics. The reason for this is to cover cases where a dominant dynamic is connected to
+			// a higher ranked node by a chain of two-way constraints. In such a case the two-way constraint should project the dominant 
+			// dynamic towards the higher ranked node and not start a tree on its own.
+			//
+			PxU32 brIdx = 0;
+			PxU32 stopIdx = bodyRankArray.size();
+			PxU32 skipCount = 0;
+			PxU32 ranksToProcess = BodyRank::sPrimaryTreeRootMinRank;
+			ConstraintGroupNode** nodeQueueEnd;
+			ConstraintGroupNode** nodeQueueCurrent;
+			for(PxU32 i=0; i < 2; i++)
+			{
+				nodeQueueEnd = nodeQueue;
+				while((brIdx < stopIdx) && (bodyRankArray[brIdx].rank >= ranksToProcess))
+				{
+					BodyRank& bRank = bodyRankArray[brIdx];
+					PX_ASSERT((brIdx == 0) || (bRank.rank <= bodyRankArray[brIdx-1].rank));
+
+					ConstraintGroupNode& node = *bRank.startingNode;
+					PX_ASSERT(node.readFlag(ConstraintGroupNode::eDISCOVERED));
+
+					node.initProjectionData(NULL, bRank.constraintToFixedAnchor);
+
+					if (bRank.rank & (BodyRank::sAttachedToStatic | BodyRank::sAttachedToKinematic))
+					{
+						// for static/kinematic attached, the current node is already a child, so we must not traverse the neighborhood yet
+						// but rather add the current node to the queue.
+						PX_ASSERT(bRank.constraintToFixedAnchor);
+						*nodeQueueEnd = &node;
+						nodeQueueEnd++;
+					}
+					else
+					{
+						PX_ASSERT(!bRank.constraintToFixedAnchor);
+						PxU32 addedNodeCount = projectionTreeBuildStep(node, bRank.constraintToFixedAnchor, nodeQueueEnd);
+						nodeQueueEnd += addedNodeCount;
+					}
+
+					node.projectionNextRoot = firstProjectionTreeRoot;
+					firstProjectionTreeRoot = &node;
+
+					brIdx++;
+				}
+
+				// first neighbor connectivity level has been pushed to a queue for all chosen tree roots. Now extend the trees one level at a time.
+				nodeQueueCurrent = nodeQueue;
+				while(nodeQueueCurrent != nodeQueueEnd)
+				{
+					ConstraintGroupNode* node = *nodeQueueCurrent;
+					PX_ASSERT(node->readFlag(ConstraintGroupNode::eDISCOVERED));
+					nodeQueueCurrent++;
+
+					PxU32 addedNodeCount = projectionTreeBuildStep(*node, node->projectionConstraint, nodeQueueEnd);
+					nodeQueueEnd += addedNodeCount;
+				}
+
+				brIdx += skipCount;
+				skipCount = 0;
+
+				// find dominant dynamics that have not been discovered yet and arrange them in a consecutive block
+				ranksToProcess = BodyRank::sOneWayProjection | BodyRank::sDominantDynamic;
+				stopIdx = brIdx;
+				PxU32 k = brIdx;
+				while((k < bodyRankArray.size()) && (bodyRankArray[k].rank >= ranksToProcess))
+				{
+					ConstraintGroupNode* node = bodyRankArray[k].startingNode;
+					if (!node->readFlag(ConstraintGroupNode::eDISCOVERED))
+					{
+						node->raiseFlag(ConstraintGroupNode::eDISCOVERED);
+						bodyRankArray[stopIdx] = bodyRankArray[k];
+						stopIdx++;
+					}
+					else
+						skipCount++;
+					
+					k++;
+				}
+			}
+
+			//
+			// for every body that has not been discovered yet, we build a tree. Here we do not advance one connectivity level 
+			// at a time because there should be no fight over the nodes among equal roots anymore (or rather no fight that could
+			// break one-way projection in an unfair way).
+			//
+			PX_ASSERT((brIdx == 0) || (brIdx == bodyRankArray.size()) || (bodyRankArray[brIdx].rank < bodyRankArray[brIdx-1].rank));
+			for(PxU32 i=brIdx; i < bodyRankArray.size(); i++)
+			{
+				nodeQueueEnd = nodeQueue;
+
+				BodyRank& bRank = bodyRankArray[i];
+				PX_ASSERT((i == brIdx) || (bRank.rank <= bodyRankArray[i-1].rank));
+#ifdef _DEBUG
+				if (bRank.rank & (BodyRank::sAttachedToStatic | BodyRank::sAttachedToKinematic))
+				{ PX_ASSERT(bRank.constraintToFixedAnchor); }
+				else
+				{ PX_ASSERT(!bRank.constraintToFixedAnchor); }
+#endif
+
+				ConstraintGroupNode& node = *bRank.startingNode;
+				if (!node.readFlag(ConstraintGroupNode::eDISCOVERED))
+				{
+					node.raiseFlag(ConstraintGroupNode::eDISCOVERED);
+
+					PxU32 addedNodeCount = projectionTreeBuildStep(node, bRank.constraintToFixedAnchor, nodeQueueEnd);
+					nodeQueueEnd += addedNodeCount;
+
+					nodeQueueCurrent = nodeQueue;
+					while(nodeQueueCurrent != nodeQueueEnd)
+					{
+						ConstraintGroupNode* n = *nodeQueueCurrent;
+						PX_ASSERT(n->readFlag(ConstraintGroupNode::eDISCOVERED));
+						PX_ASSERT(n->projectionConstraint);
+						nodeQueueCurrent++;
+
+						PxU32 nodeCount = projectionTreeBuildStep(*n, n->projectionConstraint, nodeQueueEnd);
+						nodeQueueEnd += nodeCount;
+					}
+
+					node.projectionNextRoot = firstProjectionTreeRoot;
+					firstProjectionTreeRoot = &node;
+				}
+			}
+
+			root.setProjectionTreeRoot(firstProjectionTreeRoot);
+
+			PX_FREE(nodeQueue);
+		}
+		else
+			Ps::getFoundation().error(PxErrorCode::eOUT_OF_MEMORY, __FILE__, __LINE__, "Allocating projection node queue failed!");
+	}
+}
+
+
+PxU32 Sc::ConstraintProjectionTree::projectionTreeBuildStep(ConstraintGroupNode& node, ConstraintSim* cToParent, ConstraintGroupNode** nodeQueue)
+{
+	PX_ASSERT(node.readFlag(ConstraintGroupNode::eDISCOVERED));
+
+	PxU32 nodeQueueFillCount = 0;
+
+	//go through all constraints attached to the body.
+	BodySim* body = node.body;
+	PxU32 size = body->getActorInteractionCount();
+	Sc::Interaction** interactions = body->getActorInteractions();
+	while(size--)
+	{
+		Interaction* interaction = *interactions++;
+		if (interaction->getType() == InteractionType::eCONSTRAINTSHADER)
+		{
+			ConstraintSim* c = static_cast<ConstraintInteraction*>(interaction)->getConstraint();
+
+			if (c != cToParent)	//don't go back along the edge we came from (not really necessary I guess since the ConstraintGroupNode::eDISCOVERED marker should solve this)
+			{
+				PxU32 projectToBody, projectToOtherBody;
+				BodySim* neighbor;
+				getConstraintStatus(*c, body, neighbor, projectToBody, projectToOtherBody);
+
+				if(!isFixedBody(neighbor) && (!projectToOtherBody || projectToBody))	//just ignore the eventual constraint with environment over here. Body might be attached to multiple fixed anchors.
+																						//Also make sure to ignore one-way projection that goes the opposite way.
+				{
+					ConstraintGroupNode* neighborNode = neighbor->getConstraintGroup();
+					PX_ASSERT(neighborNode);
+
+					if (!neighborNode->readFlag(ConstraintGroupNode::eDISCOVERED))
+					{
+						*nodeQueue = neighborNode;
+
+						neighborNode->initProjectionData(&node, c);
+						neighborNode->raiseFlag(ConstraintGroupNode::eDISCOVERED);	//flag body nodes that we process so we can detect loops
+
+						nodeQueueFillCount++;
+						nodeQueue++;
+					}
+				}
+			}
+		}
+	}
+
+	return nodeQueueFillCount;
+}
+
+
+void Sc::ConstraintProjectionTree::purgeProjectionTrees(ConstraintGroupNode& root)
+{
+	PX_ASSERT(&root == root.parent);
+	PX_ASSERT(root.hasProjectionTreeRoot());
+
+	// CA: New code (non recursive: recursive calls can cause stack overflow with huge trees)
+	ConstraintGroupNode* projRoot = root.projectionFirstRoot;
+	do
+	{
+		ConstraintGroupNode* currentNode = projRoot;
+		projRoot = projRoot->projectionNextRoot;  // need to do it here because the info might get cleared below
+
+		do
+		{
+			// Go down the tree until we find a leaf
+			if (currentNode->projectionFirstChild)
+			{
+				currentNode = currentNode->projectionFirstChild;
+				continue;
+			}
+
+			// Delete current node and go to next sibling or parent
+			ConstraintGroupNode* nodeToDelete = currentNode;
+			ConstraintGroupNode* parent = currentNode->projectionParent;
+			currentNode = currentNode->projectionNextSibling;
+
+			// Mark parent as leaf
+			if (nodeToDelete->projectionParent)
+				nodeToDelete->projectionParent->projectionFirstChild = NULL;
+
+			// Clear projection info
+			nodeToDelete->clearProjectionData();
+
+			if (currentNode != NULL)
+				continue;
+
+			// No more siblings jump back to parent
+			currentNode = parent;
+
+		} while (currentNode != NULL);
+
+	} while (projRoot != NULL);
+
+	root.projectionFirstRoot = NULL;	// it can happen that the constraint graph root is not part of a projection tree (if it is a kinematic, for example) but it still points to the
+										// first projection tree root and that needs to get cleaned up as well.
+	PX_ASSERT(!root.projectionNextRoot);
+	PX_ASSERT(!root.projectionParent);
+	PX_ASSERT(!root.projectionFirstChild);
+	PX_ASSERT(!root.projectionNextSibling);
+	PX_ASSERT(!root.projectionConstraint);
+}
+
+
+void Sc::ConstraintProjectionTree::projectPoseForTree(ConstraintGroupNode& node, Ps::Array<BodySim*>& projectedBodies)
+{
+	// create a dummy node to keep the loops compact while covering the special case of the first node
+	PX_ASSERT(node.body);
+	ConstraintGroupNode dummyNode(*node.body);
+	dummyNode.projectionNextSibling = &node;
+	ConstraintGroupNode* currentNode = &dummyNode;
+
+	// non recursive: recursive calls can cause stack overflow with huge trees
+	do
+	{
+		ConstraintGroupNode* nextSiblingNode = currentNode->projectionNextSibling;
+
+		while (nextSiblingNode)
+		{
+			currentNode = nextSiblingNode;
+			ConstraintGroupNode* nextChildNode = currentNode;
+
+			do
+			{
+				currentNode = nextChildNode;
+
+				//-----------------------------------------------------------------------------
+				ConstraintSim* c = currentNode->projectionConstraint;
+
+				if (c && c->hasDynamicBody() && c->needsProjection())
+				{
+					c->projectPose(currentNode->body, projectedBodies);
+				}
+				//-----------------------------------------------------------------------------
+
+				nextChildNode = currentNode->projectionFirstChild;
+
+			} while (nextChildNode);
+
+			nextSiblingNode = currentNode->projectionNextSibling;
+		}
+
+		currentNode = currentNode->projectionParent;
+
+	} while (currentNode != NULL);
+}
+
+
+void Sc::ConstraintProjectionTree::projectPose(ConstraintGroupNode& root, Ps::Array<BodySim*>& projectedBodies)
+{
+	PX_ASSERT(&root == root.parent);
+	PX_ASSERT(root.hasProjectionTreeRoot());
+
+	ConstraintGroupNode* projRoot = root.projectionFirstRoot;
+	do
+	{
+		projectPoseForTree(*projRoot, projectedBodies);
+		projRoot = projRoot->projectionNextRoot;
+
+	} while (projRoot != NULL);
+}

physx/source/simulationcontroller/src/ScConstraintProjectionTree.h

@@ -0,0 +1,75 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef PX_PHYSICS_SCP_CONSTRAINT_PROJECTION_TREE
+#define PX_PHYSICS_SCP_CONSTRAINT_PROJECTION_TREE
+
+#include "PsArray.h"
+#include "PsUserAllocated.h"
+#include "CmPhysXCommon.h"
+
+namespace physx
+{
+namespace Sc
+{
+	struct ConstraintGroupNode;
+	class ConstraintSim;
+	class BodySim;
+	class BodyRank;
+
+	class ConstraintProjectionTree
+	{
+		/**
+		 This class serves both the static administration of an articulation and the actual articulation itself.
+		 An Articulation object holds several articulation root nodes which make up a simulation island that
+		 is further connected with lagrange joints.
+		*/
+		public:
+			ConstraintProjectionTree() {}
+			~ConstraintProjectionTree() {}
+
+			static	void		buildProjectionTrees(ConstraintGroupNode& root);	
+			static	void		purgeProjectionTrees(ConstraintGroupNode& root);
+
+			static void			projectPose(ConstraintGroupNode& root, Ps::Array<BodySim*>& projectedBodies);
+
+		private:
+			static	PxU32					projectionTreeBuildStep(ConstraintGroupNode& node, ConstraintSim* cToParent, ConstraintGroupNode** nodeStack);
+
+			static	void					getConstraintStatus(const ConstraintSim& c, const BodySim* b, BodySim*& otherBody, PxU32& projectToBody, PxU32& projectToOtherBody);
+			static	void					rankConstraint(ConstraintSim&, BodyRank&, PxU32& dominanceTracking, PxU32& constraintsToProjectCount);
+			static	void					projectPoseForTree(ConstraintGroupNode& node, Ps::Array<BodySim*>& projectedBodies);
+	};
+
+} // namespace Sc
+
+}
+
+#endif

physx/source/simulationcontroller/src/ScConstraintSim.cpp

@@ -0,0 +1,490 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "ScScene.h"
+#include "ScConstraintProjectionManager.h"
+#include "ScBodySim.h"
+#include "ScStaticSim.h"
+#include "PxsContext.h"
+#include "ScConstraintCore.h"
+#include "ScConstraintSim.h"
+#include "ScConstraintInteraction.h"
+#include "ScElementSimInteraction.h"
+#include "CmVisualization.h"
+#include "ScObjectIDTracker.h"
+#include "DyContext.h"
+
+using namespace physx;
+
+PX_FORCE_INLINE void invalidateConstraintGroupsOnAdd(Sc::ConstraintProjectionManager& cpm, Sc::BodySim* b0, Sc::BodySim* b1, Sc::ConstraintSim& constraint)
+{
+	// constraint groups get built by starting from dirty constraints that need projection. If a non-projecting constraint gets added
+	// we need to restart the whole process (we do not want to track dirty non-projecting constraints because of a scenario where
+	// all constraints of a group get switched to non-projecting which should kill the group and not rebuild a new one).
+	if (b0 && b0->getConstraintGroup())
+		cpm.invalidateGroup(*b0->getConstraintGroup(), &constraint);
+	if (b1 && b1->getConstraintGroup())
+		cpm.invalidateGroup(*b1->getConstraintGroup(), &constraint);
+}
+
+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;
+	
+	mLowLevelConstraint.index = scene.getConstraintIDTracker().createID();
+	Ps::Array<Dy::ConstraintWriteback, Ps::VirtualAllocator>& writeBackPool = scene.getDynamicsContext()->getConstraintWriteBackPool();
+	if (mLowLevelConstraint.index >= writeBackPool.capacity())
+	{
+		writeBackPool.reserve(writeBackPool.capacity() * 2);
+	}
+
+	writeBackPool.resize(PxMax(writeBackPool.size(), mLowLevelConstraint.index + 1));
+	writeBackPool[mLowLevelConstraint.index].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);
+
+	ConstraintProjectionManager& cpm = scene.getProjectionManager();
+	if (!needsProjection())
+		invalidateConstraintGroupsOnAdd(cpm, mBodies[0], mBodies[1], *this);
+	else
+		cpm.addToPendingGroupUpdates(*this);
+
+	ConstraintSim* cs = this;  // to make the Wii U compiler happy
+	mInteraction = mScene.getConstraintInteractionPool()->construct(cs, 
+																	r0 ? *r0->getSim() : scene.getStaticAnchor(), 
+																	r1 ? *r1->getSim() : scene.getStaticAnchor());
+
+	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
+
+	if (readFlag(ConstraintSim::ePENDING_GROUP_UPDATE))
+		mScene.getProjectionManager().removeFromPendingGroupUpdates(*this);
+
+	if (!isBroken())
+		mInteraction->destroy();
+
+	mScene.getConstraintIDTracker().releaseID(mLowLevelConstraint.index);
+	mScene.getConstraintInteractionPool()->destroy(mInteraction);
+
+	destroyLLConstraint();
+
+	mCore.setSim(NULL);
+}
+
+bool Sc::ConstraintSim::createLLConstraint()
+{
+	Dy::Constraint& llc = mLowLevelConstraint;
+	ConstraintCore& core = getCore();
+	PxU32 constantBlockSize = core.getConstantBlockSize();
+
+	void* constantBlock = mScene.allocateConstraintBlock(constantBlockSize);
+	if(!constantBlock)
+	{
+		Ps::getFoundation().error(PxErrorCode::eINTERNAL_ERROR, __FILE__, __LINE__, "Constraint: could not allocate low-level resources.");
+		return false;
+	}
+
+	//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);
+
+	core.getBreakForce(llc.linBreakForce, llc.angBreakForce);
+	llc.flags					= core.getFlags();
+	llc.constantBlockSize		= PxU16(constantBlockSize);
+
+	llc.solverPrep				= core.getSolverPrep();
+	llc.project					= core.getProject();
+	llc.constantBlock			= constantBlock;
+
+	//llc.index = mLowLevelConstraint.index;
+	llc.body0					= mBodies[0] ? &mBodies[0]->getLowLevelBody() : 0;
+	llc.body1					= mBodies[1] ? &mBodies[1]->getLowLevelBody() : 0;
+	llc.bodyCore0				= mBodies[0] ? &llc.body0->getCore() : NULL;
+	llc.bodyCore1				= mBodies[1] ? &llc.body1->getCore() : NULL;
+
+	llc.minResponseThreshold	= core.getMinResponseThreshold();
+
+	return true;
+}
+
+void Sc::ConstraintSim::destroyLLConstraint()
+{
+	if(mLowLevelConstraint.constantBlock)
+	{
+		mScene.deallocateConstraintBlock(mLowLevelConstraint.constantBlock,
+										 mLowLevelConstraint.constantBlockSize);
+	}
+}
+
+void Sc::ConstraintSim::preBodiesChange()
+{
+	PX_ASSERT(mInteraction);
+
+	BodySim* b = getConstraintGroupBody();
+	if (b)
+		mScene.getProjectionManager().invalidateGroup(*b->getConstraintGroup(), this);
+
+	if (!isBroken())
+		mInteraction->destroy();
+
+	mScene.getConstraintInteractionPool()->destroy(mInteraction);
+	mInteraction = NULL;
+}
+
+void Sc::ConstraintSim::postBodiesChange(RigidCore* r0, RigidCore* r1)
+{
+	PX_ASSERT(mInteraction == NULL);
+
+	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;
+
+	ConstraintProjectionManager& cpm = mScene.getProjectionManager();
+	PxConstraintFlags::InternalType projectionNeeded = getCore().getFlags() & PxConstraintFlag::ePROJECTION;  // can not use "needsProjection()" because that takes into account whether the constraint is broken
+	if (!projectionNeeded)
+		invalidateConstraintGroupsOnAdd(cpm, b0, b1, *this);
+	else if (!readFlag(ConstraintSim::ePENDING_GROUP_UPDATE))
+		cpm.addToPendingGroupUpdates(*this);
+
+	Dy::Constraint& c = mLowLevelConstraint;
+
+	c.body0 = b0 ? &b0->getLowLevelBody() : NULL;
+	c.body1 = b1 ? &b1->getLowLevelBody() : NULL;
+
+	c.bodyCore0 = c.body0 ? &c.body0->getCore() : NULL;
+	c.bodyCore1 = c.body1 ? &c.body1->getCore() : NULL;
+
+	mBodies[0] = b0;
+	mBodies[1] = b1;
+
+	ConstraintSim* cs = this;  // to make the Wii U compiler happy
+	mInteraction = mScene.getConstraintInteractionPool()->construct(cs, 
+																	r0 ? *r0->getSim() : mScene.getStaticAnchor(), 
+																	r1 ? *r1->getSim() : mScene.getStaticAnchor());
+}
+
+void Sc::ConstraintSim::checkMaxForceExceeded()
+{
+	PX_ASSERT(readFlag(eCHECK_MAX_FORCE_EXCEEDED));
+
+	Dy::ConstraintWriteback& solverOutput = mScene.getDynamicsContext()->getConstraintWriteBackPool()[mLowLevelConstraint.index];
+	if(solverOutput.broken)
+	{
+		setFlag(ConstraintSim::eBROKEN);
+		mScene.addBrokenConstraint(&mCore);
+		mCore.breakApart();
+		mInteraction->destroy();
+
+		// update related SIPs
+		{
+			ActorSim& a0 = mInteraction->getActorSim0();
+			ActorSim& a1 = mInteraction->getActorSim1();
+			ActorSim& actor = (a0.getActorInteractionCount()<  a1.getActorInteractionCount()) ? a0 : a1;
+
+			actor.setActorsInteractionsDirty(InteractionDirtyFlag::eFILTER_STATE, NULL, InteractionFlag::eRB_ELEMENT);
+			// because broken constraints can re-enable contact response between the two bodies
+		}
+
+		PX_ASSERT(!readFlag(eCHECK_MAX_FORCE_EXCEEDED));
+	}
+}
+
+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;
+
+	// PT: don't convert to bool if not needed
+	const PxU32 hadProjection   = (oldFlags & PxConstraintFlag::ePROJECTION);
+	const PxU32 needsProjection = (newFlags & PxConstraintFlag::ePROJECTION);
+
+	if(needsProjection && !hadProjection)
+	{
+		PX_ASSERT(!readFlag(ConstraintSim::ePENDING_GROUP_UPDATE)); // Non-projecting constrainst should not be part of the update list
+
+		Sc::BodySim* b0 = getBody(0);
+		Sc::BodySim* b1 = getBody(1);
+		if ((!b0 || b0->getConstraintGroup()) && (!b1 || b1->getConstraintGroup()))
+		{
+			// Already part of a constraint group but not as a projection constraint -> re-generate projection tree
+			PX_ASSERT(b0 != NULL || b1 != NULL);
+			if (b0)
+				b0->getConstraintGroup()->markForProjectionTreeRebuild(mScene.getProjectionManager());
+			else
+				b1->getConstraintGroup()->markForProjectionTreeRebuild(mScene.getProjectionManager());
+		}
+		else
+		{
+			// Not part of a constraint group yet
+			mScene.getProjectionManager().addToPendingGroupUpdates(*this);
+		}
+	}
+	else if(!needsProjection && hadProjection)
+	{
+		if (!readFlag(ConstraintSim::ePENDING_GROUP_UPDATE))
+		{
+			Sc::BodySim* b = getConstraintGroupBody();
+			if (b)
+			{
+				PX_ASSERT(b->getConstraintGroup());
+				mScene.getProjectionManager().invalidateGroup(*b->getConstraintGroup(), NULL);
+			}
+			// This is conservative but it could be the case that this constraint with projection was the only
+			// one in the group and thus the whole group must be killed. If we had a counter for the number of
+			// projecting constraints per group, we could just update the projection tree if the counter was
+			// larger than 1. But switching the projection flag does not seem likely anyway.
+		}
+		else
+			mScene.getProjectionManager().removeFromPendingGroupUpdates(*this);  // Was part of a group which got invalidated
+
+		PX_ASSERT(!readFlag(ConstraintSim::ePENDING_GROUP_UPDATE));  // make sure the expected post-condition is met for all paths
+	}
+}
+
+Sc::RigidSim& Sc::ConstraintSim::getRigid(PxU32 i)
+{
+	PX_ASSERT(mInteraction);
+
+	if (i == 0)
+		return static_cast<RigidSim&>(mInteraction->getActorSim0());
+	else
+		return static_cast<RigidSim&>(mInteraction->getActorSim1());
+}
+
+bool Sc::ConstraintSim::hasDynamicBody()
+{
+	return (mBodies[0] && (!mBodies[0]->isKinematic())) || (mBodies[1] && (!mBodies[1]->isKinematic()));
+}
+
+static void constrainMotion(PxsRigidBody* body, PxTransform& targetPose)
+{
+	//Now constraint deltaPos and deltaRot
+	const PxU32 lockFlags = body->mCore->lockFlags;
+
+	if (lockFlags)
+	{
+		const PxTransform& currBody2World = body->mCore->body2World;
+
+		PxVec3 deltaPos = targetPose.p - currBody2World.p;
+
+		PxQuat deltaQ = targetPose.q * currBody2World.q.getConjugate();
+
+		if (deltaQ.w < 0)	//shortest angle.
+			deltaQ = -deltaQ;
+
+		PxReal angle;
+		PxVec3 axis;
+		deltaQ.toRadiansAndUnitAxis(angle, axis);
+		PxVec3 deltaRot = axis * angle;
+
+		if (lockFlags & PxRigidDynamicLockFlag::eLOCK_LINEAR_X)
+			deltaPos.x = 0.0f;
+		if (lockFlags & PxRigidDynamicLockFlag::eLOCK_LINEAR_Y)
+			deltaPos.y = 0.0f;
+		if (lockFlags & PxRigidDynamicLockFlag::eLOCK_LINEAR_Z)
+			deltaPos.z = 0.0f;
+		if (lockFlags & PxRigidDynamicLockFlag::eLOCK_ANGULAR_X)
+			deltaRot.x = 0.0f;
+		if (lockFlags & PxRigidDynamicLockFlag::eLOCK_ANGULAR_Y)
+			deltaRot.y = 0.0f;
+		if (lockFlags & PxRigidDynamicLockFlag::eLOCK_ANGULAR_Z)
+			deltaRot.z = 0.0f;
+
+		targetPose.p = currBody2World.p + deltaPos;
+
+		PxReal w2 = deltaRot.magnitudeSquared();
+		if (w2 != 0.0f)
+		{
+			PxReal w = PxSqrt(w2);
+
+			const PxReal v = w * 0.5f;
+			PxReal s, q;
+			Ps::sincos(v, s, q);
+			s /= w;
+
+			const PxVec3 pqr = deltaRot * s;
+			const PxQuat quatVel(pqr.x, pqr.y, pqr.z, 0);
+			PxQuat result = quatVel * currBody2World.q;
+
+			result += currBody2World.q * q;
+
+			targetPose.q = result.getNormalized();
+		}
+		else
+		{
+			targetPose.q = currBody2World.q;
+		}
+	}
+}
+
+void Sc::ConstraintSim::projectPose(BodySim* childBody, Ps::Array<BodySim*>& projectedBodies)
+{
+#if PX_DEBUG
+	// We expect bodies in low level constraints to have same order as high level counterpart
+	PxsRigidBody* b0 = mLowLevelConstraint.body0;
+	PxsRigidBody* b1 = mLowLevelConstraint.body1;
+	PX_ASSERT(	(childBody == getBody(0) && &childBody->getLowLevelBody() == b0) ||
+				(childBody == getBody(1) && &childBody->getLowLevelBody() == b1) );
+#endif
+
+	Dy::Constraint& constraint = getLowLevelConstraint();
+	bool projectToBody0 = childBody == getBody(1);
+
+	PxsRigidBody* body0 = constraint.body0, 
+				* body1 = constraint.body1;
+
+	PxTransform body0ToWorld = body0 ? body0->getPose() : PxTransform(PxIdentity);
+	PxTransform body1ToWorld = body1 ? body1->getPose() : PxTransform(PxIdentity);
+
+	(*constraint.project)(constraint.constantBlock, body0ToWorld, body1ToWorld, projectToBody0);
+
+	if(projectToBody0)
+	{
+		PX_ASSERT(body1);
+		//Constrain new pose to valid world motion
+		constrainMotion(body1, body1ToWorld);
+		body1->setPose(body1ToWorld);
+		projectedBodies.pushBack(getBody(1));
+	}
+	else
+	{
+		PX_ASSERT(body0);
+		//Constrain new pose to valid world motion
+		constrainMotion(body0, body0ToWorld);
+		body0->setPose(body0ToWorld);
+		projectedBodies.pushBack(getBody(0));
+	}
+}
+
+bool Sc::ConstraintSim::needsProjection()
+{
+	const Dy::ConstraintWriteback& solverOutput = mScene.getDynamicsContext()->getConstraintWriteBackPool()[mLowLevelConstraint.index];
+	return (getCore().getFlags() & PxConstraintFlag::ePROJECTION ) && !solverOutput.broken;
+}
+
+PX_INLINE Sc::BodySim* Sc::ConstraintSim::getConstraintGroupBody()
+{
+	BodySim* b = NULL;
+	if (mBodies[0] && mBodies[0]->getConstraintGroup())
+		b = mBodies[0];
+	else if (mBodies[1] && mBodies[1]->getConstraintGroup())
+		b = mBodies[1];
+
+	return b;
+}
+
+void Sc::ConstraintSim::visualize(PxRenderBuffer& output)
+{
+	if(!(getCore().getFlags() & PxConstraintFlag::eVISUALIZATION))
+		return;
+
+	PxsRigidBody* b0 = mLowLevelConstraint.body0;
+	PxsRigidBody* b1 = mLowLevelConstraint.body1;
+	
+	const PxTransform idt(PxIdentity);
+	const PxTransform& t0 = b0 ? b0->getPose() : idt;
+	const PxTransform& t1 = b1 ? b1->getPose() : idt;
+
+	const PxReal frameScale = mScene.getVisualizationScale() * mScene.getVisualizationParameter(PxVisualizationParameter::eJOINT_LOCAL_FRAMES);
+	const PxReal limitScale = mScene.getVisualizationScale() * mScene.getVisualizationParameter(PxVisualizationParameter::eJOINT_LIMITS);
+
+	Cm::RenderOutput renderOut(static_cast<Cm::RenderBuffer &>(output));
+	Cm::ConstraintImmediateVisualizer viz(frameScale, limitScale, renderOut);
+
+	PxU32 flags = 0;
+	if(frameScale!=0.0f)
+		flags |= PxConstraintVisualizationFlag::eLOCAL_FRAMES;
+	if(limitScale!=0.0f)
+		flags |= PxConstraintVisualizationFlag::eLIMITS;
+
+	mCore.getVisualize()(viz, mLowLevelConstraint.constantBlock, t0, t1, flags);
+}
+
+void Sc::ConstraintSim::setConstantsLL(void* addr)
+{
+	PxMemCopy(mLowLevelConstraint.constantBlock, addr, mLowLevelConstraint.constantBlockSize);
+
+	getAnyBody()->getScene().getSimulationController()->updateJoint(mInteraction->getEdgeIndex(), &mLowLevelConstraint);
+}

physx/source/simulationcontroller/src/ScConstraintSim.h

@@ -0,0 +1,156 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef PX_PHYSICS_CONSTRAINT_SIM
+#define PX_PHYSICS_CONSTRAINT_SIM
+
+#include "PxSimulationEventCallback.h"
+#include "DyConstraint.h"
+
+namespace physx
+{
+namespace Sc
+{
+
+	class Scene;
+	class ConstraintInteraction;
+	class ConstraintCore;
+	class RigidCore;
+	class BodySim;
+	class RigidSim;
+
+	class ConstraintSim : public Ps::UserAllocated 
+	{
+	public:
+		enum Enum
+		{
+			ePENDING_GROUP_UPDATE		=	(1<<0),	// For constraint projection an island of the bodies connected by constraints is generated.
+													// Schedule generation/update of the island this constraint is a part of.
+			eBREAKABLE					=	(1<<1),	// The constraint can break
+			eCHECK_MAX_FORCE_EXCEEDED	=	(1<<2),	// This constraint will get tested for breakage at the end of the sim step
+			eBROKEN						=	(1<<3)
+		};
+
+												ConstraintSim(ConstraintCore& core, 
+													RigidCore* r0,
+													RigidCore* r1,
+													Scene& scene);
+
+												~ConstraintSim();
+
+						void					preBodiesChange();
+						void					postBodiesChange(RigidCore* r0, RigidCore* r1);
+
+						void					checkMaxForceExceeded();
+
+						void					setBreakForceLL(PxReal linear, PxReal angular);
+		PX_INLINE		void					setMinResponseThresholdLL(PxReal threshold);
+		PX_INLINE		void					setAngularConstraintLinearCoefficientLL(PxReal coefficient);
+						void					setConstantsLL(void* addr);
+		PX_INLINE		const void*				getConstantsLL() const;
+
+						void					postFlagChange(PxConstraintFlags oldFlags, PxConstraintFlags newFlags);
+
+		PX_FORCE_INLINE	const Dy::Constraint&	getLowLevelConstraint()	const	{ return mLowLevelConstraint;	}
+		PX_FORCE_INLINE	Dy::Constraint&			getLowLevelConstraint()			{ return mLowLevelConstraint;	}
+		PX_FORCE_INLINE	ConstraintCore&			getCore()				const	{ return mCore;					}
+		PX_FORCE_INLINE	BodySim*				getBody(PxU32 i) const  // for static actors or world attached constraints NULL is returned
+												{
+													return mBodies[i];
+												}
+
+						RigidSim&				getRigid(PxU32 i);
+
+						void					getForce(PxVec3& force, PxVec3& torque);
+
+		PX_FORCE_INLINE	PxU8					readFlag(PxU8 flag)	const	{ return PxU8(mFlags & flag);						}
+		PX_FORCE_INLINE	void					setFlag(PxU8 flag)			{ mFlags |= flag;									}
+		PX_FORCE_INLINE	void					clearFlag(PxU8 flag)		{ mFlags &= ~flag;									}
+		PX_FORCE_INLINE	PxU32					isBroken()			const	{ return PxU32(mFlags) & ConstraintSim::eBROKEN;	}
+
+
+		//------------------------------------ Projection trees -----------------------------------------
+	private:
+		PX_INLINE		BodySim*				getConstraintGroupBody();
+
+	public:
+						bool					hasDynamicBody();
+
+						void					projectPose(BodySim* childBody, Ps::Array<BodySim*>& projectedBodies);
+		PX_INLINE		BodySim*				getOtherBody(BodySim*);
+		PX_INLINE		BodySim*				getAnyBody();
+
+						bool					needsProjection();
+		//-----------------------------------------------------------------------------------------------
+
+						void					visualize(PxRenderBuffer &out);
+	private:
+						ConstraintSim&			operator=(const ConstraintSim&);
+						bool					createLLConstraint();
+						void					destroyLLConstraint();
+	private:
+						Dy::Constraint			mLowLevelConstraint;
+						Scene&					mScene;
+						ConstraintCore&			mCore;
+						ConstraintInteraction*	mInteraction;
+						BodySim*				mBodies[2];
+						PxU8					mFlags;
+	};
+} // namespace Sc
+
+
+PX_INLINE void Sc::ConstraintSim::setMinResponseThresholdLL(PxReal threshold)
+{
+	mLowLevelConstraint.minResponseThreshold = threshold;
+}
+
+PX_INLINE const void* Sc::ConstraintSim::getConstantsLL()	const
+{
+	return mLowLevelConstraint.constantBlock;
+}
+
+
+PX_INLINE Sc::BodySim* Sc::ConstraintSim::getOtherBody(BodySim* b)
+{
+	return (b == mBodies[0]) ? mBodies[1] : mBodies[0];
+}
+
+
+PX_INLINE Sc::BodySim* Sc::ConstraintSim::getAnyBody()
+{
+	if (mBodies[0]) 
+		return mBodies[0];
+	else 
+		return mBodies[1];
+}
+
+}
+
+#endif

physx/source/simulationcontroller/src/ScContactReportBuffer.h

@@ -0,0 +1,174 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef PX_PHYSICS_SCP_CONTACTREPORTBUFFER
+#define PX_PHYSICS_SCP_CONTACTREPORTBUFFER
+
+#include "foundation/Px.h"
+#include "common/PxProfileZone.h"
+
+namespace physx
+{
+	namespace Sc
+	{
+		class ContactReportBuffer
+		{
+		public:
+			PX_FORCE_INLINE ContactReportBuffer(PxU32 initialSize, bool noResizeAllowed)
+				: mBuffer(NULL)
+				 ,mCurrentBufferIndex(0)
+				 ,mCurrentBufferSize(initialSize)
+				 ,mDefaultBufferSize(initialSize)
+				 ,mLastBufferIndex(0)
+				 ,mAllocationLocked(noResizeAllowed)
+			{
+				mBuffer = allocateBuffer(initialSize);
+				PX_ASSERT(mBuffer);
+			}
+
+			~ContactReportBuffer()
+			{
+				PX_FREE(mBuffer);
+			}
+
+			PX_FORCE_INLINE void					reset();
+			PX_FORCE_INLINE void					flush();
+
+			PX_FORCE_INLINE PxU8*					allocateNotThreadSafe(PxU32 size, PxU32& index, PxU32 alignment= 16);
+			PX_FORCE_INLINE PxU8*					reallocateNotThreadSafe(PxU32 size, PxU32& index, PxU32 alignment= 16, PxU32 lastIndex = 0xFFFFFFFF);
+			PX_FORCE_INLINE	PxU8*					getData(const PxU32& index) const { return mBuffer+index; }
+
+			PX_FORCE_INLINE PxU32					getDefaultBufferSize() const {return mDefaultBufferSize;}
+
+		private:
+			PX_FORCE_INLINE PxU8* allocateBuffer(PxU32 size);
+
+		private:
+			PxU8*			mBuffer;
+			PxU32			mCurrentBufferIndex;
+			PxU32			mCurrentBufferSize;
+			PxU32			mDefaultBufferSize;
+			PxU32			mLastBufferIndex;
+			bool			mAllocationLocked;
+		};
+
+	} // namespace Sc
+
+	//////////////////////////////////////////////////////////////////////////
+
+	PX_FORCE_INLINE void Sc::ContactReportBuffer::reset()
+	{
+		mCurrentBufferIndex = 0;	
+		mLastBufferIndex = 0xFFFFFFFF;
+	}
+
+	//////////////////////////////////////////////////////////////////////////
+
+	void Sc::ContactReportBuffer::flush()
+	{
+		mCurrentBufferIndex = 0;
+		mLastBufferIndex = 0xFFFFFFFF;
+
+		if(mCurrentBufferSize != mDefaultBufferSize)
+		{
+			PX_FREE(mBuffer);
+
+			mBuffer = allocateBuffer(mDefaultBufferSize);
+			PX_ASSERT(mBuffer);
+
+			mCurrentBufferSize = mDefaultBufferSize;			
+		}
+	}
+
+	//////////////////////////////////////////////////////////////////////////
+
+	PxU8* Sc::ContactReportBuffer::allocateNotThreadSafe(PxU32 size, PxU32& index ,PxU32 alignment/* =16 */)
+	{
+		PX_ASSERT(shdfnd::isPowerOfTwo(alignment));
+		
+		// padding for alignment
+		PxU32 pad = ((mCurrentBufferIndex+alignment-1)&~(alignment-1)) - mCurrentBufferIndex;
+
+		index = mCurrentBufferIndex + pad;
+
+		if (index + size > mCurrentBufferSize)
+		{		
+			PX_PROFILE_ZONE("ContactReportBuffer::Resize", 0);
+			if(mAllocationLocked)
+				return NULL;
+
+			PxU32 oldBufferSize = mCurrentBufferSize;
+			while(index + size > mCurrentBufferSize)
+			{
+				mCurrentBufferSize *= 2;
+			}
+			
+			PxU8* tempBuffer = allocateBuffer(mCurrentBufferSize);
+
+			PxMemCopy(tempBuffer,mBuffer,oldBufferSize);
+
+			PX_FREE(mBuffer);
+
+			mBuffer = tempBuffer;
+		}
+
+		
+		PxU8* ptr = mBuffer + index;
+		mLastBufferIndex = index;
+		PX_ASSERT((reinterpret_cast<size_t>(ptr)&(alignment-1)) == 0);	
+		mCurrentBufferIndex += size + pad;
+		return ptr;
+	}
+
+	//////////////////////////////////////////////////////////////////////////
+
+	PxU8* Sc::ContactReportBuffer::reallocateNotThreadSafe(PxU32 size, PxU32& index ,PxU32 alignment/* =16 */, PxU32 lastIndex)
+	{		
+		if(lastIndex != mLastBufferIndex)
+		{
+			return allocateNotThreadSafe(size,index,alignment);
+		}
+		else
+		{
+			mCurrentBufferIndex = mLastBufferIndex;
+			return allocateNotThreadSafe(size,index,alignment);
+		}
+	}
+
+	//////////////////////////////////////////////////////////////////////////
+
+	PX_FORCE_INLINE PxU8* Sc::ContactReportBuffer::allocateBuffer(PxU32 size)
+	{
+		return (static_cast<PxU8*>(PX_ALLOC(size, "ContactReportBuffer")));
+	}
+
+} // namespace physx
+
+#endif // PX_PHYSICS_SCP_CONTACTREPORTBUFFER

physx/source/simulationcontroller/src/ScContactStream.h

@@ -0,0 +1,413 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef PX_PHYSICS_SCP_CONTACTSTREAM
+#define PX_PHYSICS_SCP_CONTACTSTREAM
+
+#include "foundation/Px.h"
+#include "PxSimulationEventCallback.h"
+#include "ScObjectIDTracker.h"
+#include "ScRigidSim.h"
+#include "ScStaticSim.h"
+#include "ScBodySim.h"
+
+namespace physx
+{
+	class PxShape;
+
+namespace Sc
+{
+	class ActorPair;
+
+
+	// Internal counterpart of PxContactPair
+	struct ContactShapePair
+	{
+	public:
+		PxShape*				shapes[2];
+		const PxU8*				contactPatches;
+		const PxU8*				contactPoints;
+		const PxReal*			contactForces;
+		PxU32					requiredBufferSize;
+		PxU8					contactCount;
+		PxU8					patchCount;
+		PxU16					constraintStreamSize;
+		PxU16					flags;
+		PxU16					events;
+		PxU32					shapeID[2];
+		//26 (or 38 on 64bit)
+	};
+	PX_COMPILE_TIME_ASSERT(sizeof(ContactShapePair) == sizeof(PxContactPair));
+
+	struct ContactStreamManagerFlag
+	{
+		enum Enum
+		{
+			/**
+			\brief Need to test stream for shapes that were removed from the actor/scene
+
+			Usually this is the case when a shape gets removed from the scene, however, other operations that remove the
+			broadphase volume of a pair object have to be considered as well since the shape might get removed later after such an
+			operation. The scenarios to consider are:
+
+			\li shape gets removed (this includes raising PxActorFlag::eDISABLE_SIMULATION)
+			\li shape switches to eSCENE_QUERY_SHAPE only
+			\li shape switches to eTRIGGER_SHAPE
+			\li resetFiltering()
+			\li actor gets removed from an aggregate
+
+			*/
+			eTEST_FOR_REMOVED_SHAPES	= (1<<0),
+
+			/**
+			\brief Invalid stream memory not allocated
+			*/
+			eINVALID_STREAM				= (1<<1),
+
+			/**
+			\brief Incomplete stream will be reported
+			*/
+			eINCOMPLETE_STREAM			= (1<<2),
+
+			/**
+			\brief The stream contains extra data with PxContactPairVelocity items where the post solver velocity needs to get written to.
+			       Only valid for discrete collision (in CCD the post response velocity is available immediately).
+			*/
+			eNEEDS_POST_SOLVER_VELOCITY	= (1<<3),
+
+			/**
+			\brief Contains pairs that lost touch
+			
+			This info is used as an optimization to only parse the stream and check for removed shapes if there is a potential for
+			having removed shapes in the stream that won't get detected in any other way. For example, there is the scenario where
+			during the simulation a pair loses AABB touch and gets deleted. At that point a lost touch event might get written to the
+			stream. If at fetchResults a buffered shape removal takes place, and that shape was part of the mentioned pair, there is
+			no way any longer to make the connection to the corresponding event stream (since the pair has been deleted during the sim).
+			*/
+			eHAS_PAIRS_THAT_LOST_TOUCH	= (1<<4),
+
+			/**
+			\brief Marker for the next available free flag
+			*/
+			eNEXT_FREE_FLAG				= (1<<5)
+		};
+	};
+
+	struct ContactStreamHeader
+	{
+		PxU16 contactPass;  // marker for extra data to know when a new collison pass started (discrete collision -> CCD pass 1 -> CCD pass 2 -> ...)
+		PxU16 pad;  // to keep the stream 4byte aligned
+	};
+
+	/**
+	\brief Contact report logic and data management.
+
+	The internal contact report stream has the following format:
+
+	ContactStreamHeader | PxContactPairIndex0 | (PxContactPairPose0, PxContactPairVelocity0) | ... | PxContactPairIndexN | (PxContactPairPoseN, PxContactPairVelocityN) | (unused memory up to maxExtraDataSize ) |
+	PxContactPair0 | ... | PxContactPairM | (unsued pairs up to maxPairCount)
+	*/
+	class ContactStreamManager
+	{
+	public:
+		PX_FORCE_INLINE ContactStreamManager() : maxPairCount(0), flags_and_maxExtraDataBlocks(0) {}
+		PX_FORCE_INLINE ~ContactStreamManager() {}
+
+		PX_FORCE_INLINE void reset();
+
+		PX_FORCE_INLINE PxU16 getFlags() const;
+		PX_FORCE_INLINE void raiseFlags(PxU16 flags);
+		PX_FORCE_INLINE void clearFlags(PxU16 flags);
+
+		PX_FORCE_INLINE PxU32 getMaxExtraDataSize() const;
+		PX_FORCE_INLINE void setMaxExtraDataSize(PxU32 size);  // size in bytes (will translate into blocks internally)
+
+		PX_FORCE_INLINE Sc::ContactShapePair* getShapePairs(PxU8* contactReportPairData) const;
+
+		PX_FORCE_INLINE static void convertDeletedShapesInContactStream(ContactShapePair*, PxU32 pairCount, const ObjectIDTracker&);
+		
+		PX_FORCE_INLINE static PxU32 computeExtraDataBlockCount(PxU32 extraDataSize);
+		PX_FORCE_INLINE static PxU32 computeExtraDataBlockSize(PxU32 extraDataSize);
+		PX_FORCE_INLINE static PxU16 computeContactReportExtraDataSize(PxU32 extraDataFlags, bool addHeader);
+		PX_FORCE_INLINE static void fillInContactReportExtraData(PxContactPairVelocity*, PxU32 index, const RigidSim&, bool isCCDPass);
+		PX_FORCE_INLINE static void fillInContactReportExtraData(PxContactPairPose*, PxU32 index, const RigidSim&, bool isCCDPass, const bool useCurrentTransform);
+		PX_FORCE_INLINE void fillInContactReportExtraData(PxU8* stream, PxU32 extraDataFlags, const RigidSim&, const RigidSim&, PxU32 ccdPass, const bool useCurrentTransform, PxU32 pairIndex, PxU32 sizeOffset);
+		PX_FORCE_INLINE void setContactReportPostSolverVelocity(PxU8* stream, const RigidSim&, const RigidSim&);
+
+		PxU32				bufferIndex;  // marks the start of the shape pair stream of the actor pair (byte offset with respect to global contact buffer stream)
+		PxU16				maxPairCount;  // used to reserve the same amount of memory as in the last frame (as an initial guess)
+		PxU16				currentPairCount;  // number of shape pairs stored in the buffer
+		PxU16				extraDataSize;  // size of the extra data section in the stream
+	private:
+		PxU16				flags_and_maxExtraDataBlocks;  // used to reserve the same amount of memory as in the last frame (as an initial guess)
+
+	public:
+		static const PxU32	sExtraDataBlockSizePow2 = 4;  // extra data gets allocated as a multiple of 2^sExtraDataBlockSizePow2 to keep memory low of this struct.
+		static const PxU32	sFlagMask = (ContactStreamManagerFlag::eNEXT_FREE_FLAG - 1);
+		static const PxU32	sMaxExtraDataShift = 5;  // shift necessary to extract the maximum number of blocks allocated for extra data
+
+		PX_COMPILE_TIME_ASSERT(ContactStreamManagerFlag::eNEXT_FREE_FLAG == (1 << sMaxExtraDataShift));
+	};
+
+} // namespace Sc
+
+
+PX_FORCE_INLINE void Sc::ContactStreamManager::reset()
+{
+	currentPairCount = 0;
+	extraDataSize = 0;
+	flags_and_maxExtraDataBlocks &= ~sFlagMask;
+}
+
+
+PX_FORCE_INLINE PxU16 Sc::ContactStreamManager::getFlags() const
+{
+	return (flags_and_maxExtraDataBlocks & sFlagMask);
+}
+
+
+PX_FORCE_INLINE void Sc::ContactStreamManager::raiseFlags(PxU16 flags)
+{
+	PX_ASSERT(flags < ContactStreamManagerFlag::eNEXT_FREE_FLAG);
+
+	flags_and_maxExtraDataBlocks |= flags;
+}
+
+
+PX_FORCE_INLINE void Sc::ContactStreamManager::clearFlags(PxU16 flags)
+{
+	PX_ASSERT(flags < ContactStreamManagerFlag::eNEXT_FREE_FLAG);
+
+	PxU16 tmpFlags = getFlags();
+	tmpFlags &= ~flags;
+	flags_and_maxExtraDataBlocks &= ~sFlagMask;
+	raiseFlags(tmpFlags);
+}
+
+
+PX_FORCE_INLINE PxU32 Sc::ContactStreamManager::getMaxExtraDataSize() const
+{
+	return PxU32((flags_and_maxExtraDataBlocks >> sMaxExtraDataShift) << sExtraDataBlockSizePow2);
+}
+
+
+PX_FORCE_INLINE void Sc::ContactStreamManager::setMaxExtraDataSize(PxU32 size)
+{
+	PxU32 nbBlocks = computeExtraDataBlockCount(size);
+	flags_and_maxExtraDataBlocks = Ps::to16((flags_and_maxExtraDataBlocks & sFlagMask) | (nbBlocks << sMaxExtraDataShift));
+}
+
+
+PX_FORCE_INLINE Sc::ContactShapePair* Sc::ContactStreamManager::getShapePairs(PxU8* contactReportPairData) const
+{
+	return reinterpret_cast<Sc::ContactShapePair*>(contactReportPairData + getMaxExtraDataSize());
+}
+
+
+PX_FORCE_INLINE void Sc::ContactStreamManager::convertDeletedShapesInContactStream(ContactShapePair* shapePairs, PxU32 pairCount, const ObjectIDTracker& tracker)
+{
+	for(PxU32 i=0; i < pairCount; i++)
+	{
+		ContactShapePair& csp = shapePairs[i];
+
+		PxU32 shape0ID = csp.shapeID[0];
+		PxU32 shape1ID = csp.shapeID[1];
+
+		PxU16 flags = csp.flags;
+		PX_COMPILE_TIME_ASSERT(sizeof(flags) == sizeof((reinterpret_cast<ContactShapePair*>(0))->flags));
+
+		if (tracker.isDeletedID(shape0ID))
+			flags |= PxContactPairFlag::eREMOVED_SHAPE_0;
+		if (tracker.isDeletedID(shape1ID))
+			flags |= PxContactPairFlag::eREMOVED_SHAPE_1;
+
+		csp.flags = flags;
+	}
+}
+
+
+PX_FORCE_INLINE PxU32 Sc::ContactStreamManager::computeExtraDataBlockCount(PxU32 extraDataSize_)
+{
+	PxU32 nbBlocks;
+	if (extraDataSize_ & ((1 << sExtraDataBlockSizePow2) - 1))  // not a multiple of block size -> need one block more
+		nbBlocks = (extraDataSize_ >> sExtraDataBlockSizePow2) + 1;
+	else
+		nbBlocks = (extraDataSize_ >> sExtraDataBlockSizePow2);
+
+	return nbBlocks;
+}
+
+
+PX_FORCE_INLINE PxU32 Sc::ContactStreamManager::computeExtraDataBlockSize(PxU32 extraDataSize_)
+{
+	return (computeExtraDataBlockCount(extraDataSize_) << sExtraDataBlockSizePow2);
+}
+
+
+PX_FORCE_INLINE PxU16 Sc::ContactStreamManager::computeContactReportExtraDataSize(PxU32 extraDataFlags, bool addHeader)
+{
+	PX_ASSERT(extraDataFlags);
+
+	PxU16 extraDataSize_ = sizeof(PxContactPairIndex);
+	if (extraDataFlags & PxPairFlag::ePRE_SOLVER_VELOCITY)
+		extraDataSize_ += sizeof(PxContactPairVelocity);
+	if (extraDataFlags & PxPairFlag::ePOST_SOLVER_VELOCITY)
+		extraDataSize_ += sizeof(PxContactPairVelocity);
+	if (extraDataFlags & PxPairFlag::eCONTACT_EVENT_POSE)
+		extraDataSize_ += sizeof(PxContactPairPose);
+	if (addHeader)
+		extraDataSize_ += sizeof(ContactStreamHeader);
+	return extraDataSize_;
+}
+
+
+PX_FORCE_INLINE void Sc::ContactStreamManager::fillInContactReportExtraData(PxContactPairVelocity* cpVel, PxU32 index, const RigidSim& rs, bool isCCDPass)
+{
+	if (rs.getActorType() != PxActorType::eRIGID_STATIC)
+	{
+		const BodySim& bs = static_cast<const BodySim&>(rs);
+		if ((!isCCDPass) || (cpVel->type == PxContactPairExtraDataType::ePOST_SOLVER_VELOCITY))
+		{
+			const BodyCore& bc = bs.getBodyCore();
+			cpVel->linearVelocity[index] = bc.getLinearVelocity();
+			cpVel->angularVelocity[index] = bc.getAngularVelocity();
+		}
+		else
+		{
+			PX_ASSERT(cpVel->type == PxContactPairExtraDataType::ePRE_SOLVER_VELOCITY);
+			const Cm::SpatialVector& vel = bs.getLowLevelBody().getPreSolverVelocities();
+			cpVel->linearVelocity[index] = vel.linear;
+			cpVel->angularVelocity[index] = vel.angular;
+		}
+	}
+	else
+	{
+		cpVel->linearVelocity[index] = PxVec3(0.0f);
+		cpVel->angularVelocity[index] = PxVec3(0.0f);
+	}
+}
+
+
+PX_FORCE_INLINE void Sc::ContactStreamManager::fillInContactReportExtraData(PxContactPairPose* cpPose, PxU32 index, const RigidSim& rs, bool isCCDPass, const bool useCurrentTransform)
+{
+	if(rs.getActorType() != PxActorType::eRIGID_STATIC)
+	{
+		const BodySim& bs = static_cast<const BodySim&>(rs);
+		const BodyCore& bc = bs.getBodyCore();
+		const PxTransform& src = (!isCCDPass && useCurrentTransform) ? bc.getBody2World() : bs.getLowLevelBody().getLastCCDTransform();
+		cpPose->globalPose[index] = src * bc.getBody2Actor().getInverse();
+	}
+	else
+	{
+		const StaticSim& ss = static_cast<const StaticSim&>(rs);
+		const StaticCore& sc = ss.getStaticCore();
+		cpPose->globalPose[index] = sc.getActor2World();
+	}
+}
+
+
+PX_FORCE_INLINE void Sc::ContactStreamManager::fillInContactReportExtraData(PxU8* stream, PxU32 extraDataFlags, const RigidSim& rs0, const RigidSim& rs1, PxU32 ccdPass, const bool useCurrentTransform,
+	PxU32 pairIndex, PxU32 sizeOffset)
+{
+	ContactStreamHeader* strHeader = reinterpret_cast<ContactStreamHeader*>(stream);
+	strHeader->contactPass = Ps::to16(ccdPass);
+
+	stream += sizeOffset;
+	PxU8* edStream = stream;
+	bool isCCDPass = (ccdPass != 0);
+
+	{
+		PxContactPairIndex* cpIndex = reinterpret_cast<PxContactPairIndex*>(edStream);
+		cpIndex->type = PxContactPairExtraDataType::eCONTACT_PAIR_INDEX;
+		cpIndex->index = Ps::to16(pairIndex);
+		edStream += sizeof(PxContactPairIndex);
+
+		PX_ASSERT(edStream <= reinterpret_cast<PxU8*>(getShapePairs(stream)));
+	}
+
+	// Important: make sure this one is the first after the PxContactPairIndex item for discrete contacts as it needs to get filled in before the reports get sent
+	//            (post solver velocity is not available when it gets created)
+	if (extraDataFlags & PxPairFlag::ePOST_SOLVER_VELOCITY)
+	{
+		PxContactPairVelocity* cpVel = reinterpret_cast<PxContactPairVelocity*>(edStream);
+		cpVel->type = PxContactPairExtraDataType::ePOST_SOLVER_VELOCITY;
+		edStream += sizeof(PxContactPairVelocity);
+
+		if (!isCCDPass)
+			raiseFlags(ContactStreamManagerFlag::eNEEDS_POST_SOLVER_VELOCITY);  // don't know the post solver velocity yet
+		else
+		{
+			ContactStreamManager::fillInContactReportExtraData(cpVel, 0, rs0, true);
+			ContactStreamManager::fillInContactReportExtraData(cpVel, 1, rs1, true);
+		}
+
+		PX_ASSERT(edStream <= reinterpret_cast<PxU8*>(getShapePairs(stream)));
+	}
+	if (extraDataFlags & PxPairFlag::ePRE_SOLVER_VELOCITY)
+	{
+		PxContactPairVelocity* cpVel = reinterpret_cast<PxContactPairVelocity*>(edStream);
+		cpVel->type = PxContactPairExtraDataType::ePRE_SOLVER_VELOCITY;
+		ContactStreamManager::fillInContactReportExtraData(cpVel, 0, rs0, isCCDPass);
+		ContactStreamManager::fillInContactReportExtraData(cpVel, 1, rs1, isCCDPass);
+		edStream += sizeof(PxContactPairVelocity);
+
+		PX_ASSERT(edStream <= reinterpret_cast<PxU8*>(getShapePairs(stream)));
+	}
+	if (extraDataFlags & PxPairFlag::eCONTACT_EVENT_POSE)
+	{
+		PxContactPairPose* cpPose = reinterpret_cast<PxContactPairPose*>(edStream);
+		cpPose->type = PxContactPairExtraDataType::eCONTACT_EVENT_POSE;
+		ContactStreamManager::fillInContactReportExtraData(cpPose, 0, rs0, isCCDPass, useCurrentTransform);
+		ContactStreamManager::fillInContactReportExtraData(cpPose, 1, rs1, isCCDPass, useCurrentTransform);
+		edStream += sizeof(PxContactPairPose);
+
+		PX_ASSERT(edStream <= reinterpret_cast<PxU8*>(getShapePairs(stream)));
+	}
+
+	extraDataSize = Ps::to16(sizeOffset + PxU32(edStream - stream));
+}
+
+
+PX_FORCE_INLINE void Sc::ContactStreamManager::setContactReportPostSolverVelocity(PxU8* stream, const RigidSim& rs0, const RigidSim& rs1)
+{
+	PX_ASSERT(extraDataSize > (sizeof(ContactStreamHeader) + sizeof(PxContactPairIndex)));
+	PxContactPairVelocity* cpVel = reinterpret_cast<PxContactPairVelocity*>(stream + sizeof(ContactStreamHeader) + sizeof(PxContactPairIndex));
+	PX_ASSERT(cpVel->type == PxContactPairExtraDataType::ePOST_SOLVER_VELOCITY);
+
+	fillInContactReportExtraData(cpVel, 0, rs0, false);
+	fillInContactReportExtraData(cpVel, 1, rs1, false);
+
+	clearFlags(ContactStreamManagerFlag::eNEEDS_POST_SOLVER_VELOCITY);
+}
+
+
+}
+
+#endif

physx/source/simulationcontroller/src/ScElementInteractionMarker.cpp

@@ -0,0 +1,45 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "ScElementInteractionMarker.h"
+#include "ScNPhaseCore.h"
+
+using namespace physx;
+
+Sc::ElementInteractionMarker::~ElementInteractionMarker()
+{
+	if(isRegistered())
+	{
+		Scene& scene = getScene();
+		scene.unregisterInteraction(this);
+		scene.getNPhaseCore()->unregisterInteraction(this);
+	}
+	unregisterFromActors();
+}
+

physx/source/simulationcontroller/src/ScElementInteractionMarker.h

@@ -0,0 +1,68 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef PX_COLLISION_ELEMENT_INTERACTION_MARKER
+#define PX_COLLISION_ELEMENT_INTERACTION_MARKER
+
+#include "ScElementSimInteraction.h"
+#include "ScNPhaseCore.h"
+
+namespace physx
+{
+namespace Sc
+{
+	class ElementInteractionMarker : public ElementSimInteraction
+	{
+	public:
+		PX_INLINE		ElementInteractionMarker(ElementSim& element0, ElementSim& element1, bool createParallel/* = false*/);
+						~ElementInteractionMarker();
+
+				bool	onActivate_(void*)	{ return false;	}
+				bool	onDeactivate_()		{ return true;	}
+	};
+
+} // namespace Sc
+
+
+PX_INLINE Sc::ElementInteractionMarker::ElementInteractionMarker(ElementSim& element0, ElementSim& element1, bool createParallel) :
+	ElementSimInteraction(element0, element1, InteractionType::eMARKER, InteractionFlag::eRB_ELEMENT|InteractionFlag::eFILTERABLE)
+{
+	if(!createParallel)
+	{
+		bool active = registerInActors();
+		PX_UNUSED(active);
+		PX_ASSERT(!active);
+		getScene().registerInteraction(this, false);
+		getScene().getNPhaseCore()->registerInteraction(this);
+	}
+}
+
+}
+
+#endif //PX_COLLISION_SHAPEINTERACTIONMARKER

physx/source/simulationcontroller/src/ScElementSim.cpp

@@ -0,0 +1,131 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#include "PsFoundation.h"
+#include "PxsContext.h"
+#include "ScElementSim.h"
+#include "ScElementSimInteraction.h"
+#include "ScSqBoundsManager.h"
+#include "ScSimStats.h"
+
+using namespace physx;
+using namespace Sc;
+
+static PX_FORCE_INLINE bool interactionHasElement(const Interaction* it, const ElementSim* elem)
+{
+	if(it->readInteractionFlag(InteractionFlag::eRB_ELEMENT))
+	{
+		PX_ASSERT(	(it->getType() == InteractionType::eMARKER) ||
+					(it->getType() == InteractionType::eOVERLAP) ||
+					(it->getType() == InteractionType::eTRIGGER) );
+
+		const ElementSimInteraction* ei = static_cast<const ElementSimInteraction*>(it);
+		if((&ei->getElement0() == elem) || (&ei->getElement1() == elem))
+			return true;
+	}
+	return false;
+}
+
+Sc::ElementSimInteraction* Sc::ElementSim::ElementInteractionIterator::getNext()
+{
+	while(mInteractions!=mInteractionsLast)
+	{
+		Interaction* it = *mInteractions++;
+		if(interactionHasElement(it, mElement))
+			return static_cast<ElementSimInteraction*>(it);
+	}
+	return NULL;
+}
+
+Sc::ElementSimInteraction* Sc::ElementSim::ElementInteractionReverseIterator::getNext()
+{
+	while(mInteractions!=mInteractionsLast)
+	{
+		Interaction* it = *--mInteractionsLast;
+		if(interactionHasElement(it, mElement))
+			return static_cast<ElementSimInteraction*>(it);
+	}
+	return NULL;
+}
+
+Sc::ElementSim::ElementSim(ActorSim& actor) :
+	mNextInActor	(NULL),
+	mActor			(actor),
+	mInBroadPhase	(false)
+{
+	initID();
+
+	actor.onElementAttach(*this);
+}
+
+Sc::ElementSim::~ElementSim()
+{
+	PX_ASSERT(!mInBroadPhase);
+	releaseID();
+	mActor.onElementDetach(*this);
+}
+
+void Sc::ElementSim::setElementInteractionsDirty(InteractionDirtyFlag::Enum flag, PxU8 interactionFlag)
+{
+	ElementSim::ElementInteractionIterator iter = getElemInteractions();
+	ElementSimInteraction* interaction = iter.getNext();
+	while(interaction)
+	{
+		if(interaction->readInteractionFlag(interactionFlag))
+			interaction->setDirty(flag);
+
+		interaction = iter.getNext();
+	}
+}
+
+void Sc::ElementSim::addToAABBMgr(PxReal contactDistance, Bp::FilterGroup::Enum group, Ps::IntBool isTrigger)
+{
+	Sc::Scene& scene = getScene();
+	if(!scene.getAABBManager()->addBounds(mElementID, contactDistance, group, this, mActor.getActorCore().getAggregateID(), isTrigger ? Bp::ElementType::eTRIGGER : Bp::ElementType::eSHAPE))
+		return;
+
+	mInBroadPhase = true;
+#if PX_ENABLE_SIM_STATS
+	scene.getStatsInternal().incBroadphaseAdds();
+#endif
+}
+
+void Sc::ElementSim::removeFromAABBMgr()
+{
+	PX_ASSERT(mInBroadPhase);
+	Sc::Scene& scene = getScene();
+	scene.getAABBManager()->removeBounds(mElementID);
+	scene.getAABBManager()->getChangedAABBMgActorHandleMap().growAndReset(mElementID);
+
+	mInBroadPhase = false;
+#if PX_ENABLE_SIM_STATS
+	scene.getStatsInternal().incBroadphaseRemoves();
+#endif
+}

physx/source/simulationcontroller/src/ScElementSim.h

@@ -0,0 +1,130 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef PX_PHYSICS_SCP_ELEMENT_SIM
+#define PX_PHYSICS_SCP_ELEMENT_SIM
+
+#include "PsUserAllocated.h"
+#include "PxFiltering.h"
+#include "PxvConfig.h"
+#include "ScActorSim.h"
+#include "ScInteraction.h"
+#include "BpAABBManager.h"
+#include "ScObjectIDTracker.h"
+
+namespace physx
+{
+namespace Sc
+{
+	class ElementSimInteraction;
+
+	/*
+	A ElementSim is a part of a ActorSim. It contributes to the activation framework by adding its 
+	interactions to the actor. */
+	class ElementSim : public Ps::UserAllocated
+	{
+		PX_NOCOPY(ElementSim)
+
+	public:
+		class ElementInteractionIterator
+		{
+			public:
+				PX_FORCE_INLINE			ElementInteractionIterator(const ElementSim& e, PxU32 nbInteractions, Interaction** interactions) :
+					mInteractions(interactions), mInteractionsLast(interactions + nbInteractions), mElement(&e) {}
+				ElementSimInteraction*	getNext();
+
+			private:
+				Interaction**					mInteractions;
+				Interaction**					mInteractionsLast;
+				const ElementSim*				mElement;
+		};
+
+		class ElementInteractionReverseIterator
+		{
+			public:
+				PX_FORCE_INLINE			ElementInteractionReverseIterator(const ElementSim& e, PxU32 nbInteractions, Interaction** interactions) :
+					mInteractions(interactions), mInteractionsLast(interactions + nbInteractions), mElement(&e) {}
+				ElementSimInteraction*	getNext();
+
+			private:
+				Interaction**					mInteractions;
+				Interaction**					mInteractionsLast;
+				const ElementSim*				mElement;
+		};
+
+												ElementSim(ActorSim& actor);
+		protected:
+												~ElementSim();
+		public:
+
+		// Get an iterator to the interactions connected to the element
+		PX_FORCE_INLINE	ElementInteractionIterator getElemInteractions()	const	{ return ElementInteractionIterator(*this, mActor.getActorInteractionCount(), mActor.getActorInteractions()); }
+		PX_FORCE_INLINE	ElementInteractionReverseIterator getElemInteractionsReverse()	const	{ return ElementInteractionReverseIterator(*this, mActor.getActorInteractionCount(), mActor.getActorInteractions()); }
+
+		PX_FORCE_INLINE	ActorSim&				getActor()					const	{ return mActor; }
+
+		PX_FORCE_INLINE	Scene&					getScene()					const	{ return mActor.getScene();	}
+
+		PX_FORCE_INLINE PxU32					getElementID()				const	{ return mElementID;	}
+		PX_FORCE_INLINE bool					isInBroadPhase()			const	{ return mInBroadPhase;	}
+		
+						void					addToAABBMgr(PxReal contactDistance, Bp::FilterGroup::Enum group, Ps::IntBool isTrigger);
+						void					removeFromAABBMgr();
+
+						void					setElementInteractionsDirty(InteractionDirtyFlag::Enum flag, PxU8 interactionFlag);
+
+		PX_FORCE_INLINE	void					initID()
+												{
+													Scene& scene = getScene();
+													mElementID = scene.getElementIDPool().createID();
+													scene.getBoundsArray().initEntry(mElementID);
+												}
+
+		PX_FORCE_INLINE	void					releaseID()
+												{
+													getScene().getElementIDPool().releaseID(mElementID);
+												}
+	public:
+						ElementSim*				mNextInActor;
+	private:
+						ActorSim&				mActor;
+
+						PxU32					mElementID : 31;
+						PxU32					mInBroadPhase : 1;
+	};
+
+	PX_FORCE_INLINE void setFilterObjectAttributeType(PxFilterObjectAttributes& attr, PxFilterObjectType::Enum type)
+	{
+		PX_ASSERT((attr & (PxFilterObjectType::eMAX_TYPE_COUNT-1)) == 0);
+		attr |= type;
+	}
+} // namespace Sc
+}
+
+#endif

physx/source/simulationcontroller/src/ScElementSimInteraction.h

@@ -0,0 +1,77 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef PX_PHYSICS_SCP_ELEMENT_SIM_INTERACTION
+#define PX_PHYSICS_SCP_ELEMENT_SIM_INTERACTION
+
+#include "ScInteraction.h"
+#include "ScElementSim.h"
+
+namespace physx
+{
+namespace Sc
+{
+	class ElementSimInteraction : public Interaction
+	{
+	public:
+		PX_FORCE_INLINE	ElementSim&	getElement0()						const	{ return mElement0;						}
+		PX_FORCE_INLINE	ElementSim&	getElement1()						const	{ return mElement1;						}
+
+		PX_FORCE_INLINE	void		setFilterPairIndex(PxU32 filterPairIndex)	{ mFilterPairIndex = filterPairIndex;	}
+		PX_FORCE_INLINE	PxU32		getFilterPairIndex()				const	{ return mFilterPairIndex;				}
+
+	protected:
+		PX_INLINE					ElementSimInteraction(ElementSim& element0, ElementSim& element1, InteractionType::Enum type, PxU8 flags);
+		virtual						~ElementSimInteraction() {}
+
+		ElementSimInteraction& operator=(const ElementSimInteraction&);
+
+	private:
+						ElementSim&	mElement0;
+						ElementSim&	mElement1;
+						PxU32		mFilterPairIndex;
+	};
+
+} // namespace Sc
+
+//////////////////////////////////////////////////////////////////////////
+
+PX_INLINE Sc::ElementSimInteraction::ElementSimInteraction(ElementSim& element0, ElementSim& element1, InteractionType::Enum type, PxU8 flags) :
+	Interaction	(element0.getActor(), element1.getActor(), type, flags),
+	mElement0	(element0),
+	mElement1	(element1),
+	mFilterPairIndex(INVALID_FILTER_PAIR_INDEX)
+{
+}
+
+
+}
+
+#endif

physx/source/simulationcontroller/src/ScInteraction.cpp

@@ -0,0 +1,71 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "foundation/Px.h"
+
+#include "ScInteraction.h"
+#include "ScNPhaseCore.h"
+
+using namespace physx;
+
+Sc::Interaction::Interaction(ActorSim& actor0, ActorSim& actor1, InteractionType::Enum type, PxU8 flags) :
+	mActor0				(actor0),
+	mActor1				(actor1), 
+	mSceneId			(PX_INVALID_INTERACTION_SCENE_ID), 
+	mActorId0			(PX_INVALID_INTERACTION_ACTOR_ID),
+	mActorId1			(PX_INVALID_INTERACTION_ACTOR_ID), 
+	mInteractionType	(Ps::to8(type)),
+	mInteractionFlags	(flags),
+	mDirtyFlags			(0)
+{
+	PX_ASSERT_WITH_MESSAGE(&actor0.getScene() == &actor1.getScene(),"Cannot create an interaction between actors belonging to different scenes.");
+	PX_ASSERT(PxU32(type)<256);	// PT: type is now stored on a byte
+}
+
+void Sc::Interaction::addToDirtyList()
+{
+	getActorSim0().getScene().getNPhaseCore()->addToDirtyInteractionList(this);		
+}
+
+void Sc::Interaction::removeFromDirtyList()
+{
+	getActorSim0().getScene().getNPhaseCore()->removeFromDirtyInteractionList(this);
+}
+
+void Sc::Interaction::setClean(bool removeFromList)
+{
+	if (readInteractionFlag(InteractionFlag::eIN_DIRTY_LIST))
+	{
+		if (removeFromList)  // if we process all dirty interactions anyway, then we can just clear the list at the end and save the work here.
+			removeFromDirtyList();
+		clearInteractionFlag(InteractionFlag::eIN_DIRTY_LIST);
+	}
+
+	mDirtyFlags = 0;
+}

physx/source/simulationcontroller/src/ScInteraction.h

@@ -0,0 +1,210 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef PX_PHYSICS_SCP_INTERACTION
+#define PX_PHYSICS_SCP_INTERACTION
+
+#include "foundation/Px.h"
+#include "ScInteractionFlags.h"
+#include "ScScene.h"
+#include "ScActorSim.h"
+#include "PsUserAllocated.h"
+#include "PsUtilities.h"
+#include "PsFoundation.h"
+
+namespace physx
+{
+#define PX_INVALID_INTERACTION_ACTOR_ID 0xffffffff
+#define PX_INVALID_INTERACTION_SCENE_ID 0xffffffff
+
+namespace Sc
+{
+	// Interactions are used for connecting actors into activation groups. An interaction always connects exactly two actors. 
+	// An interaction is implicitly active if at least one of the two actors it connects is active.
+
+	class Interaction : public Ps::UserAllocated
+	{
+		PX_NOCOPY(Interaction)
+
+	protected:
+										Interaction(ActorSim& actor0, ActorSim& actor1, InteractionType::Enum interactionType, PxU8 flags);
+										~Interaction() { PX_ASSERT(!readInteractionFlag(InteractionFlag::eIN_DIRTY_LIST)); }
+
+	public:
+		// Interactions automatically register themselves in the actors here
+		PX_FORCE_INLINE bool			registerInActors(void* data = NULL);
+
+		// Interactions automatically unregister themselves from the actors here
+		PX_FORCE_INLINE void			unregisterFromActors();
+
+		PX_FORCE_INLINE	ActorSim&		getActorSim0()	const	{ return mActor0; }
+		PX_FORCE_INLINE	ActorSim&		getActorSim1()	const	{ return mActor1; }
+
+		PX_FORCE_INLINE Scene&			getScene() const { return mActor0.getScene(); }
+
+		PX_FORCE_INLINE	InteractionType::Enum getType() const { return InteractionType::Enum(mInteractionType); }
+
+		PX_FORCE_INLINE PxU8			readInteractionFlag(PxU8 flag) const { return PxU8(mInteractionFlags & flag); }
+		PX_FORCE_INLINE void			raiseInteractionFlag(InteractionFlag::Enum flag) { mInteractionFlags |= flag; }
+		PX_FORCE_INLINE void			clearInteractionFlag(InteractionFlag::Enum flag) { mInteractionFlags &= ~flag; }
+
+		/**
+		\brief Mark the interaction as dirty. This will put the interaction into a list that is processed once per simulation step.
+
+		@see InteractionDirtyFlag
+		*/
+		PX_FORCE_INLINE void			setDirty(PxU32 dirtyFlags);
+
+		/**
+		\brief Clear all flags that mark the interaction as dirty and optionally remove the interaction from the list of dirty interactions.
+
+		@see InteractionDirtyFlag
+		*/
+		/*PX_FORCE_INLINE*/ void		setClean(bool removeFromList);
+
+		PX_FORCE_INLINE Ps::IntBool		needsRefiltering() const { return (getDirtyFlags() & InteractionDirtyFlag::eFILTER_STATE); }
+
+		PX_FORCE_INLINE Ps::IntBool		isElementInteraction() const;
+
+		// Called when an interaction is activated or created.
+		// Return true if activation should proceed else return false (for example: joint interaction between two kinematics should not get activated)
+//		virtual			bool			onActivate_(void* data) = 0;
+
+		// Called when an interaction is deactivated.
+		// Return true if deactivation should proceed else return false (for example: joint interaction between two kinematics can ignore deactivation because it always is deactivated)
+//		virtual			bool			onDeactivate_() = 0;
+
+		PX_FORCE_INLINE	void			setInteractionId(PxU32 id)	{ mSceneId = id;										}
+		PX_FORCE_INLINE	PxU32			getInteractionId()	const	{ return mSceneId;										}
+		PX_FORCE_INLINE	bool			isRegistered()		const	{ return mSceneId != PX_INVALID_INTERACTION_SCENE_ID;	}
+
+		PX_FORCE_INLINE	void			setActorId(ActorSim* actor, PxU32 id);
+		PX_FORCE_INLINE	PxU32			getActorId(const ActorSim* actor) const;
+
+		PX_FORCE_INLINE PxU8			getDirtyFlags() const { return mDirtyFlags; }
+
+	private:
+						void			addToDirtyList();
+						void			removeFromDirtyList();
+
+						ActorSim&		mActor0;
+						ActorSim&		mActor1;
+
+		// PT: TODO: merge the 6bits of the 3 PxU8s in the top bits of the 3 PxU32s
+						PxU32			mSceneId;	// PT: TODO: merge this with mInteractionType
+
+		// PT: TODO: are those IDs even worth caching? Since the number of interactions per actor is (or should be) small,
+		// we could just do a linear search and save memory here...
+						PxU32			mActorId0;	// PT: id of this interaction within mActor0's mInteractions array
+						PxU32			mActorId1;	// PT: id of this interaction within mActor1's mInteractions array
+	protected:
+						PxU8			mInteractionType;	// PT: stored on a byte to save space, should be InteractionType enum, 5/6 bits needed here
+						PxU8			mInteractionFlags;	// PT: 6 bits needed here, see InteractionFlag enum
+						PxU8			mDirtyFlags;		// PT: 6 bits needed here, see InteractionDirtyFlag enum
+						PxU8			mPadding;
+	};
+
+} // namespace Sc
+
+//////////////////////////////////////////////////////////////////////////
+
+PX_FORCE_INLINE bool Sc::Interaction::registerInActors(void* data)
+{
+	bool active = activateInteraction(this, data);
+
+	mActor0.registerInteractionInActor(this);
+	mActor1.registerInteractionInActor(this);
+
+	return active;
+}
+
+PX_FORCE_INLINE void Sc::Interaction::unregisterFromActors()
+{
+	mActor0.unregisterInteractionFromActor(this);
+	mActor1.unregisterInteractionFromActor(this);
+}
+
+PX_FORCE_INLINE	void Sc::Interaction::setActorId(ActorSim* actor, PxU32 id)
+{
+	PX_ASSERT(id != PX_INVALID_INTERACTION_ACTOR_ID);
+	PX_ASSERT(&mActor0 == actor || &mActor1 == actor);
+	if(&mActor0 == actor)
+		mActorId0 = id;
+	else
+		mActorId1 = id;
+}
+
+PX_FORCE_INLINE	PxU32 Sc::Interaction::getActorId(const ActorSim* actor) const
+{
+	PX_ASSERT(&mActor0 == actor || &mActor1 == actor);
+	return &mActor0 == actor ? mActorId0 : mActorId1;
+}
+
+PX_FORCE_INLINE Ps::IntBool Sc::Interaction::isElementInteraction() const
+{
+	const Ps::IntBool res = readInteractionFlag(InteractionFlag::eRB_ELEMENT);
+	PX_ASSERT(	(res && 
+				((getType() == InteractionType::eOVERLAP)	|| 
+				(getType() == InteractionType::eTRIGGER)	|| 
+				(getType() == InteractionType::eMARKER))) ||
+				(!res && 
+				((getType() == InteractionType::eCONSTRAINTSHADER)	|| 
+				(getType() == InteractionType::eARTICULATION))));
+
+	return res;
+}
+
+PX_FORCE_INLINE void Sc::Interaction::setDirty(PxU32 dirtyFlags)
+{
+	PX_ASSERT(getType() != InteractionType::eARTICULATION);
+
+	mDirtyFlags |= Ps::to8(dirtyFlags);
+	if(!readInteractionFlag(InteractionFlag::eIN_DIRTY_LIST))
+	{
+		addToDirtyList();
+		raiseInteractionFlag(InteractionFlag::eIN_DIRTY_LIST);
+	}
+}
+
+//PX_FORCE_INLINE void Sc::Interaction::setClean(bool removeFromList)
+//{
+//	if (readInteractionFlag(InteractionFlag::eIN_DIRTY_LIST))
+//	{
+//		if (removeFromList)  // if we process all dirty interactions anyway, then we can just clear the list at the end and save the work here.
+//			removeFromDirtyList();
+//		clearInteractionFlag(InteractionFlag::eIN_DIRTY_LIST);
+//	}
+//
+//	mDirtyFlags = 0;
+//}
+
+
+}
+
+#endif // PX_PHYSICS_SCP_INTERACTION

physx/source/simulationcontroller/src/ScInteractionFlags.h

@@ -0,0 +1,74 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef PX_PHYSICS_SCP_INTERACTION_FLAGS
+#define PX_PHYSICS_SCP_INTERACTION_FLAGS
+
+#include "foundation/Px.h"
+
+namespace physx
+{
+
+namespace Sc
+{
+	struct InteractionFlag	// PT: TODO: use PxFlags
+	{
+		enum Enum
+		{
+			eRB_ELEMENT			= (1 << 0), // Interactions between rigid body shapes
+			eCONSTRAINT			= (1 << 1),
+			eFILTERABLE			= (1 << 2), // Interactions that go through the filter code
+			eIN_DIRTY_LIST		= (1 << 3),	// The interaction is in the dirty list
+			eIS_FILTER_PAIR		= (1 << 4),	// The interaction is tracked by the filter callback mechanism
+			eIS_ACTIVE			= (1 << 5)
+		};
+	};
+
+	struct InteractionDirtyFlag
+	{
+		enum Enum
+		{
+			eFILTER_STATE		= (1 << 0), // All changes filtering related
+			eMATERIAL			= (1 << 1),
+			eBODY_KINEMATIC		= (1 << 2) | eFILTER_STATE,  // A transition between dynamic and kinematic (and vice versa) require a refiltering
+			eDOMINANCE			= (1 << 3),
+			eREST_OFFSET		= (1 << 4),
+			eVISUALIZATION		= (1 << 5)
+		};
+	};
+
+
+} // namespace Sc
+
+
+} // namespace physx
+
+
+#endif // PX_PHYSICS_SCP_INTERACTION_FLAGS

physx/source/simulationcontroller/src/ScIterators.cpp

@@ -0,0 +1,105 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+           
+#include "ScIterators.h"
+#include "ScBodySim.h"
+#include "ScShapeSim.h"
+#include "ScShapeInteraction.h"
+
+using namespace physx;
+
+///////////////////////////////////////////////////////////////////////////////
+
+Sc::ContactIterator::Pair::Pair(const void*& contactPatches, const void*& contactPoints, PxU32 /*contactDataSize*/, const PxReal*& forces, PxU32 numContacts, PxU32 numPatches,
+	ShapeSim& shape0, ShapeSim& shape1)
+: mIndex(0)
+, mNumContacts(numContacts)
+, mIter(reinterpret_cast<const PxU8*>(contactPatches), reinterpret_cast<const PxU8*>(contactPoints), reinterpret_cast<const PxU32*>(forces + numContacts), numPatches, numContacts)
+, mForces(forces)
+{	
+	mCurrentContact.shape0 = shape0.getPxShape();
+	mCurrentContact.shape1 = shape1.getPxShape();
+	mCurrentContact.normalForceAvailable = (forces != NULL);
+}
+
+Sc::ContactIterator::Pair* Sc::ContactIterator::getNextPair()
+{ 
+	if(mCurrent < mLast)
+	{
+		ShapeInteraction* si = static_cast<ShapeInteraction*>(*mCurrent);
+
+		const void* contactPatches = NULL;
+		const void* contactPoints = NULL;
+		PxU32 contactDataSize = 0;
+		const PxReal* forces = NULL;
+		PxU32 numContacts = 0;
+		PxU32 numPatches = 0;
+
+		PxU32 nextOffset = si->getContactPointData(contactPatches, contactPoints, contactDataSize, numContacts, numPatches, forces, mOffset, *mOutputs);
+
+		if (nextOffset == mOffset)
+			++mCurrent;
+		else
+			mOffset = nextOffset;
+
+		mCurrentPair = Pair(contactPatches, contactPoints, contactDataSize, forces, numContacts, numPatches, si->getShape0(), si->getShape1());
+		return &mCurrentPair;
+	}
+	else
+		return NULL;
+}
+
+Sc::Contact* Sc::ContactIterator::Pair::getNextContact()
+{
+	if(mIndex < mNumContacts)
+	{
+		if(!mIter.hasNextContact())
+		{
+			if(!mIter.hasNextPatch())
+				return NULL;
+			mIter.nextPatch();
+		}
+		PX_ASSERT(mIter.hasNextContact());
+		mIter.nextContact();
+
+		mCurrentContact.normal = mIter.getContactNormal();
+		mCurrentContact.point = mIter.getContactPoint();
+		mCurrentContact.separation = mIter.getSeparation();
+		mCurrentContact.normalForce = mForces ? mForces[mIndex] : 0;
+		mCurrentContact.faceIndex0 = mIter.getFaceIndex0();
+		mCurrentContact.faceIndex1 = mIter.getFaceIndex1();
+
+		mIndex++;
+		return &mCurrentContact;
+	}
+	return NULL;
+}
+
+///////////////////////////////////////////////////////////////////////////////

physx/source/simulationcontroller/src/ScMetaData.cpp

@@ -0,0 +1,421 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "foundation/PxIO.h"
+#include "ScActorCore.h"
+#include "ScActorSim.h"
+#include "ScBodyCore.h"
+#include "ScStaticCore.h"
+#include "ScConstraintCore.h"
+#include "ScMaterialCore.h"
+#include "ScShapeCore.h"
+#include "ScArticulationCore.h"
+#include "ScArticulationJointCore.h"
+
+using namespace physx;
+using namespace Ps;
+using namespace Cm;
+using namespace Sc;
+
+///////////////////////////////////////////////////////////////////////////////
+
+template <typename T> class PxMetaDataArray : public physx::shdfnd::Array<T>
+{
+public:
+    static PX_FORCE_INLINE physx::PxU32 getDataOffset()           { return PX_OFFSET_OF(PxMetaDataArray<T>, mData); }
+    static PX_FORCE_INLINE physx::PxU32 getDataSize()             { return PX_SIZE_OF(PxMetaDataArray<T>, mData); }
+    static PX_FORCE_INLINE physx::PxU32 getSizeOffset()           { return PX_OFFSET_OF(PxMetaDataArray<T>, mSize); }
+    static PX_FORCE_INLINE physx::PxU32 getSizeSize()             { return PX_SIZE_OF(PxMetaDataArray<T>, mSize); }
+    static PX_FORCE_INLINE physx::PxU32 getCapacityOffset()       { return PX_OFFSET_OF(PxMetaDataArray<T>, mCapacity); }
+    static PX_FORCE_INLINE physx::PxU32 getCapacitySize()         { return PX_SIZE_OF(PxMetaDataArray<T>, mCapacity); }
+};
+
+void Sc::ActorCore::getBinaryMetaData(PxOutputStream& stream)
+{
+	PX_DEF_BIN_METADATA_TYPEDEF(stream,	PxActorFlags,     PxU8)
+	PX_DEF_BIN_METADATA_TYPEDEF(stream,	PxDominanceGroup, PxU8)
+	PX_DEF_BIN_METADATA_TYPEDEF(stream,	PxClientID,       PxU8)
+
+	PX_DEF_BIN_METADATA_CLASS(stream,	Sc::ActorCore)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,	Sc::ActorCore, ActorSim,			mSim,						PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	Sc::ActorCore, PxU32,			    mAggregateIDOwnerClient,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	Sc::ActorCore, PxActorFlags,		mActorFlags,				0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	Sc::ActorCore, PxU8,				mActorType,					0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	Sc::ActorCore, PxU8,				mDominanceGroup,			0)
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+static void getBinaryMetaData_PxsRigidCore(PxOutputStream& stream)
+{
+	PX_DEF_BIN_METADATA_CLASS(stream,	PxsRigidCore)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,	PxsBodyCore, PxTransform,		body2World,				0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	PxsBodyCore, PxRigidBodyFlags,	mFlags,					0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	PxsBodyCore, PxU16,				solverIterationCounts,	0)
+}
+
+namespace
+{
+	class ShadowPxsBodyCore : public PxsBodyCore
+	{
+	public:
+		static void getBinaryMetaData(PxOutputStream& stream)
+		{
+			PX_DEF_BIN_METADATA_CLASS(stream,		ShadowPxsBodyCore)
+			PX_DEF_BIN_METADATA_BASE_CLASS(stream,	ShadowPxsBodyCore, PxsRigidCore)
+
+			PX_DEF_BIN_METADATA_ITEM(stream,		ShadowPxsBodyCore, PxTransform,	body2Actor,				0)
+			PX_DEF_BIN_METADATA_ITEM(stream,		ShadowPxsBodyCore, PxReal,		ccdAdvanceCoefficient,	0)
+			PX_DEF_BIN_METADATA_ITEM(stream,		ShadowPxsBodyCore, PxVec3,		linearVelocity,			0)
+			PX_DEF_BIN_METADATA_ITEM(stream,		ShadowPxsBodyCore, PxReal,		maxPenBias,				0)
+			PX_DEF_BIN_METADATA_ITEM(stream,		ShadowPxsBodyCore, PxVec3,		angularVelocity,		0)
+			PX_DEF_BIN_METADATA_ITEM(stream,		ShadowPxsBodyCore, PxReal,		contactReportThreshold,	0)
+			PX_DEF_BIN_METADATA_ITEM(stream,		ShadowPxsBodyCore, PxReal,		maxAngularVelocitySq,	0)
+			PX_DEF_BIN_METADATA_ITEM(stream,		ShadowPxsBodyCore, PxReal,		maxLinearVelocitySq,	0)
+			PX_DEF_BIN_METADATA_ITEM(stream,		ShadowPxsBodyCore, PxReal,		linearDamping,			0)
+			PX_DEF_BIN_METADATA_ITEM(stream,		ShadowPxsBodyCore, PxReal,		angularDamping,			0)
+			PX_DEF_BIN_METADATA_ITEM(stream,		ShadowPxsBodyCore, PxVec3,		inverseInertia,			0)
+			PX_DEF_BIN_METADATA_ITEM(stream,		ShadowPxsBodyCore, PxReal,		inverseMass,			0)
+			PX_DEF_BIN_METADATA_ITEM(stream,		ShadowPxsBodyCore, PxReal,		maxContactImpulse,		0)
+			PX_DEF_BIN_METADATA_ITEM(stream,		ShadowPxsBodyCore, PxReal,		sleepThreshold,			0)
+			PX_DEF_BIN_METADATA_ITEM(stream,		ShadowPxsBodyCore, PxReal,		freezeThreshold,		0)
+			PX_DEF_BIN_METADATA_ITEM(stream,		ShadowPxsBodyCore, PxReal,		wakeCounter,			0)
+			PX_DEF_BIN_METADATA_ITEM(stream,		ShadowPxsBodyCore, PxReal,		solverWakeCounter,		0)
+			PX_DEF_BIN_METADATA_ITEM(stream, 		ShadowPxsBodyCore, PxU32,		numCountedInteractions,	0)
+			PX_DEF_BIN_METADATA_ITEM(stream,		ShadowPxsBodyCore, PxU32,		numBodyInteractions,	0)
+			PX_DEF_BIN_METADATA_ITEM(stream,		ShadowPxsBodyCore, PxU8,		isFastMoving,			0)
+			PX_DEF_BIN_METADATA_ITEM(stream,		ShadowPxsBodyCore, PxU8,		disableGravity,			0)
+			PX_DEF_BIN_METADATA_ITEM(stream,		ShadowPxsBodyCore, PxU8,		lockFlags,				0)
+			PX_DEF_BIN_METADATA_ITEM(stream,		ShadowPxsBodyCore, PxU8,		kinematicLink,			0)
+		}
+	};
+}
+
+static void getBinaryMetaData_PxsBodyCore(PxOutputStream& stream)
+{
+	getBinaryMetaData_PxsRigidCore(stream);
+
+/*	PX_DEF_BIN_METADATA_CLASS(stream,		PxsBodyCore)
+	PX_DEF_BIN_METADATA_BASE_CLASS(stream,	PxsBodyCore, PxsRigidCore)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,		PxsBodyCore, PxTransform,	body2Actor,				0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		PxsBodyCore, PxReal,		ccdAdvanceCoefficient,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		PxsBodyCore, PxVec3,		linearVelocity,			0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		PxsBodyCore, PxReal,		maxPenBias,				0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		PxsBodyCore, PxVec3,		angularVelocity,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		PxsBodyCore, PxReal,		contactReportThreshold,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		PxsBodyCore, PxReal,		maxAngularVelocitySq,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		PxsBodyCore, PxReal,		maxLinearVelocitySq,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		PxsBodyCore, PxReal,		linearDamping,			0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		PxsBodyCore, PxReal,		angularDamping,			0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		PxsBodyCore, PxVec3,		inverseInertia,			0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		PxsBodyCore, PxReal,		inverseMass,			0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		PxsBodyCore, PxReal,		maxContactImpulse,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		PxsBodyCore, PxReal,		sleepThreshold,			0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		PxsBodyCore, PxReal,		freezeThreshold,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		PxsBodyCore, PxReal,		wakeCounter,			0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		PxsBodyCore, PxReal,		solverWakeCounter,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream, 		PxsBodyCore, PxU32,			numCountedInteractions,	0)*/
+
+	ShadowPxsBodyCore::getBinaryMetaData(stream);
+	PX_DEF_BIN_METADATA_TYPEDEF(stream,	PxsBodyCore, ShadowPxsBodyCore)
+}
+
+/*
+We need to fix the header deps by moving the API out of PhysXCore and into its own dir where other code can get to it.
+[25.08.2010 18:34:57] Dilip Sequeira: In the meantime, I think it's Ok to include PxSDK.h, but you're right, we need to be very careful about include deps in that direction.
+[25.08.2010 18:38:15] Dilip Sequeira: On the memory thing... PxsBodyCore has 28 bytes of padding at the end, for no reason. In addition, it has two words of padding after the velocity fields, to facilitate SIMD loads. But in fact, Vec3FromVec4 is fast enough such that unless you were using it in an inner loop (which we never are with PxsBodyCore) that padding isn't worth it.
+[25.08.2010 18:38:58] Dilip Sequeira: So, we should drop the end-padding, and move the damping values to replace the velocity padding. This probably requires a bit of fixup in the places where we do SIMD writes to the velocity.
+[25.08.2010 18:39:18] Dilip Sequeira: Then we're down to 92 bytes of data, and 4 bytes of padding I think.
+[25.08.2010 18:50:41] Dilip Sequeira: The reason we don't want to put the sleep data there explicitly is that it isn't LL data so I'd rather not have it in an LL interface struct.
+[25.08.2010 19:04:53] Gordon Yeoman nvidia: simd loads are faster when they are 16-byte aligned.  I think the padding might be to ensure the second vector is also 16-byte aligned.  We could drop the second 4-byte pad but dropping the 1st 4-byte pad will likely  have performance implications.
+[25.08.2010 19:06:22] Dilip Sequeira: We should still align the vec3s, as now - but we shouldn't use padding to do it, since there are a boatload of scalar data fields floating around in that struct too.
+*/
+void Sc::BodyCore::getBinaryMetaData(PxOutputStream& stream)
+{
+	getBinaryMetaData_PxsBodyCore(stream);
+	PX_DEF_BIN_METADATA_TYPEDEF(stream, PxRigidBodyFlags, PxU16)
+
+	PX_DEF_BIN_METADATA_CLASS(stream,		Sc::BodyCore)
+	PX_DEF_BIN_METADATA_BASE_CLASS(stream,	Sc::BodyCore, Sc::RigidCore)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,		Sc::BodyCore, PxsBodyCore,	mCore,					0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		Sc::BodyCore, SimStateData,	mSimStateData,			PxMetaDataFlag::ePTR)
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void Sc::ConstraintCore::getBinaryMetaData(PxOutputStream& stream)
+{   
+	PX_DEF_BIN_METADATA_TYPEDEF(stream, PxConstraintFlags, PxU16)
+
+	PX_DEF_BIN_METADATA_CLASS(stream,	ConstraintCore)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConstraintCore, PxConstraintFlags,		mFlags,					0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConstraintCore, PxU16,					mPaddingFromFlags,		PxMetaDataFlag::ePADDING)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConstraintCore, PxVec3,					mAppliedForce,			0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConstraintCore, PxVec3,					mAppliedTorque,			0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConstraintCore, PxConstraintConnector,	mConnector,				PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConstraintCore, PxConstraintProject,	mProject,				PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConstraintCore, PxConstraintSolverPrep,	mSolverPrep,			PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConstraintCore, PxConstraintVisualize,	mVisualize,				PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConstraintCore, PxU32,					mDataSize,				0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConstraintCore, PxReal,					mLinearBreakForce,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConstraintCore, PxReal,					mAngularBreakForce,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConstraintCore, PxReal,					mMinResponseThreshold,	0)
+		
+	PX_DEF_BIN_METADATA_ITEM(stream,	ConstraintCore, ConstraintSim,			mSim,					PxMetaDataFlag::ePTR)
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void Sc::MaterialCore::getBinaryMetaData(PxOutputStream& stream)
+{
+	PX_DEF_BIN_METADATA_TYPEDEF(stream,	PxCombineMode::Enum, PxU32)
+	PX_DEF_BIN_METADATA_TYPEDEF(stream,	PxMaterialFlags, PxU16)
+
+	PX_DEF_BIN_METADATA_CLASS(stream,	MaterialCore)
+
+	// MaterialData
+	PX_DEF_BIN_METADATA_ITEM(stream,	MaterialCore, PxReal,			dynamicFriction,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	MaterialCore, PxReal,			staticFriction,			0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	MaterialCore, PxReal,			restitution,			0)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,	MaterialCore, PxMaterialFlags,	flags,					0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	MaterialCore, PxU8,				fricRestCombineMode,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	MaterialCore, PxU8,				padding,				PxMetaDataFlag::ePADDING)
+
+	// MaterialCore
+	PX_DEF_BIN_METADATA_ITEM(stream,	MaterialCore, PxMaterial,		mNxMaterial,			PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	MaterialCore, PxU16,			mMaterialIndex,			PxMetaDataFlag::eHANDLE)
+	PX_DEF_BIN_METADATA_ITEM(stream,	MaterialCore, PxU16,			mPadding,				PxMetaDataFlag::ePADDING)
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void Sc::RigidCore::getBinaryMetaData(PxOutputStream& stream)
+{
+	PX_DEF_BIN_METADATA_CLASS(stream,		Sc::RigidCore)
+	PX_DEF_BIN_METADATA_BASE_CLASS(stream,	Sc::RigidCore, Sc::ActorCore)
+}
+
+
+///////////////////////////////////////////////////////////////////////////////
+
+void Sc::StaticCore::getBinaryMetaData(PxOutputStream& stream)
+{
+	PX_DEF_BIN_METADATA_CLASS(stream,		Sc::StaticCore)
+	PX_DEF_BIN_METADATA_BASE_CLASS(stream,	Sc::StaticCore, Sc::RigidCore)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,		Sc::StaticCore, PxsRigidCore,		mCore,					0)
+
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+static void getBinaryMetaData_PxFilterData(PxOutputStream& stream)
+{
+	PX_DEF_BIN_METADATA_CLASS(stream,	PxFilterData)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,	PxFilterData, PxU32,		word0,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	PxFilterData, PxU32,		word1,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	PxFilterData, PxU32,		word2,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	PxFilterData, PxU32,		word3,		0)
+}
+
+static void getBinaryMetaData_PxsShapeCore(PxOutputStream& stream)
+{
+	PX_DEF_BIN_METADATA_CLASS(stream,		PxsShapeCore)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,		PxsShapeCore,	PxTransform,		transform,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		PxsShapeCore,	Gu::GeometryUnion,	geometry,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		PxsShapeCore,	PxReal,				contactOffset,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		PxsShapeCore,	PxShapeFlags,		mShapeFlags,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		PxsShapeCore,	PxU8,				mOwnsMaterialIdxMemory,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		PxsShapeCore,	PxU16,				materialIndex,	PxMetaDataFlag::eHANDLE)
+}
+
+void Sc::ShapeCore::getBinaryMetaData(PxOutputStream& stream)
+{
+	getBinaryMetaData_PxFilterData(stream);
+	getBinaryMetaData_PxsShapeCore(stream);
+
+	PX_DEF_BIN_METADATA_TYPEDEF(stream, PxShapeFlags, PxU8)
+
+	PX_DEF_BIN_METADATA_CLASS(stream,	ShapeCore)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,	ShapeCore, PxFilterData,	mQueryFilterData,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ShapeCore, PxFilterData,	mSimulationFilterData,	0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ShapeCore, PxsShapeCore,	mCore,					0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ShapeCore, PxReal,		    mRestOffset,			0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ShapeCore, PxReal,			mTorsionalRadius,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ShapeCore, PxReal,			mMinTorsionalPatchRadius, 0)
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+static void getBinaryMetaData_ArticulationCore(PxOutputStream& stream)
+{
+	PX_DEF_BIN_METADATA_CLASS(stream, Dy::ArticulationCore)
+
+	PX_DEF_BIN_METADATA_TYPEDEF(stream, PxArticulationFlags, PxU8)
+
+	PX_DEF_BIN_METADATA_ITEM(stream, Dy::ArticulationCore, PxU32, internalDriveIterations, 0)
+	PX_DEF_BIN_METADATA_ITEM(stream, Dy::ArticulationCore, PxU32, externalDriveIterations, 0)
+	PX_DEF_BIN_METADATA_ITEM(stream, Dy::ArticulationCore, PxU32, maxProjectionIterations, 0)
+	PX_DEF_BIN_METADATA_ITEM(stream, Dy::ArticulationCore, PxU16, solverIterationCounts, 0)
+	PX_DEF_BIN_METADATA_ITEM(stream, Dy::ArticulationCore, PxReal, separationTolerance, 0)
+	PX_DEF_BIN_METADATA_ITEM(stream, Dy::ArticulationCore, PxReal, sleepThreshold, 0)
+	PX_DEF_BIN_METADATA_ITEM(stream, Dy::ArticulationCore, PxReal, freezeThreshold, 0)
+	PX_DEF_BIN_METADATA_ITEM(stream, Dy::ArticulationCore, PxReal, wakeCounter, 0)
+	PX_DEF_BIN_METADATA_ITEM(stream, Dy::ArticulationCore, PxArticulationFlags, flags, 0)
+}
+
+void Sc::ArticulationCore::getBinaryMetaData(PxOutputStream& stream)
+{
+	getBinaryMetaData_ArticulationCore(stream);
+
+	PX_DEF_BIN_METADATA_CLASS(stream,	ArticulationCore)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,	ArticulationCore, ArticulationSim,		mSim,					PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ArticulationCore, Dy::ArticulationCore,	mCore,					0)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ArticulationCore, bool,					mIsReducedCoordinate,	0)
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+static void getBinaryMetaData_ArticulationLimit(PxOutputStream& stream)
+{
+	PX_DEF_BIN_METADATA_CLASS(stream, PxArticulationLimit)
+	PX_DEF_BIN_METADATA_ITEM(stream, PxArticulationLimit, PxReal, low, 0)
+	PX_DEF_BIN_METADATA_ITEM(stream, PxArticulationLimit, PxReal, high, 0)
+}
+
+static void getBinaryMetaData_ArticulationDrive(PxOutputStream& stream)
+{
+	PX_DEF_BIN_METADATA_TYPEDEF(stream, PxArticulationDriveType::Enum, PxU32)
+
+	PX_DEF_BIN_METADATA_CLASS(stream, PxArticulationDrive)
+	PX_DEF_BIN_METADATA_ITEM(stream, PxArticulationDrive, PxReal, stiffness, 0)
+	PX_DEF_BIN_METADATA_ITEM(stream, PxArticulationDrive, PxReal, damping, 0)
+	PX_DEF_BIN_METADATA_ITEM(stream, PxArticulationDrive, PxReal, maxForce, 0)
+	PX_DEF_BIN_METADATA_ITEM(stream, PxArticulationDrive, PxArticulationDriveType::Enum, driveType, 0)
+}
+
+static void getBinaryMetaData_ArticulationJointCoreBase(PxOutputStream& stream)
+{
+	getBinaryMetaData_ArticulationLimit(stream);
+	getBinaryMetaData_ArticulationDrive(stream);
+
+	PX_DEF_BIN_METADATA_CLASS(stream, Dy::ArticulationJointCoreBase)	
+
+	PX_DEF_BIN_METADATA_TYPEDEF(stream, ArticulationJointCoreDirtyFlags, PxU8)
+
+	PX_DEF_BIN_METADATA_ITEM(stream, Dy::ArticulationJointCoreBase, PxTransform, parentPose, 0)
+	PX_DEF_BIN_METADATA_ITEM(stream, Dy::ArticulationJointCoreBase, PxTransform, childPose, 0)
+
+	PX_DEF_BIN_METADATA_ITEMS_AUTO(stream, Dy::ArticulationJointCoreBase, PxArticulationLimit, limits, 0)
+	PX_DEF_BIN_METADATA_ITEMS_AUTO(stream, Dy::ArticulationJointCoreBase, PxArticulationDrive, drives, 0)
+
+	PX_DEF_BIN_METADATA_ITEMS_AUTO(stream, Dy::ArticulationJointCoreBase, PxReal, targetP, 0)
+	PX_DEF_BIN_METADATA_ITEMS_AUTO(stream, Dy::ArticulationJointCoreBase, PxReal, targetV, 0)
+
+	PX_DEF_BIN_METADATA_ITEM(stream, Dy::ArticulationJointCoreBase, PxReal, frictionCoefficient, 0)
+
+	PX_DEF_BIN_METADATA_ITEMS_AUTO(stream, Dy::ArticulationJointCoreBase, PxU8, dofIds, 0)
+
+	PX_DEF_BIN_METADATA_ITEMS_AUTO(stream, Dy::ArticulationJointCoreBase, PxU8, motion, 0)
+
+	PX_DEF_BIN_METADATA_ITEM(stream, Dy::ArticulationJointCoreBase, PxReal, maxJointVelocity, 0)
+
+	PX_DEF_BIN_METADATA_ITEM(stream, Dy::ArticulationJointCoreBase, ArticulationJointCoreDirtyFlags, dirtyFlag, 0)
+
+	PX_DEF_BIN_METADATA_ITEM(stream, Dy::ArticulationJointCoreBase, PxU8, jointOffset, 0)
+
+	PX_DEF_BIN_METADATA_ITEM(stream, Dy::ArticulationJointCoreBase, PxU8, jointType, 0)
+}
+
+
+static void getBinaryMetaData_ArticulationJointCore(PxOutputStream& stream)
+{	
+	getBinaryMetaData_ArticulationJointCoreBase(stream);
+	PX_DEF_BIN_METADATA_CLASS(stream, Dy::ArticulationJointCore)
+	PX_DEF_BIN_METADATA_BASE_CLASS(stream, Dy::ArticulationJointCore, Dy::ArticulationJointCoreBase)
+	
+	PX_DEF_BIN_METADATA_ITEM(stream,		Dy::ArticulationJointCore, PxQuat, targetPosition,					0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		Dy::ArticulationJointCore, PxVec3, targetVelocity,					0)
+	
+	PX_DEF_BIN_METADATA_ITEM(stream,		Dy::ArticulationJointCore, PxReal, spring, 0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		Dy::ArticulationJointCore, PxReal, damping, 0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		Dy::ArticulationJointCore, PxReal, internalCompliance, 0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		Dy::ArticulationJointCore, PxReal, externalCompliance, 0)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,		Dy::ArticulationJointCore, PxReal, swingLimitContactDistance, 0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		Dy::ArticulationJointCore, PxReal, tangentialStiffness, 0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		Dy::ArticulationJointCore, PxReal, tangentialDamping, 0)
+
+	PX_DEF_BIN_METADATA_ITEM(stream,		Dy::ArticulationJointCore, bool, swingLimited, 0)
+	PX_DEF_BIN_METADATA_ITEM(stream,		Dy::ArticulationJointCore, bool, twistLimited, 0)
+	
+	PX_DEF_BIN_METADATA_ITEM(stream,		Dy::ArticulationJointCore, PxU8, driveType, 0)
+
+	PX_DEF_BIN_METADATA_ITEM(stream, Dy::ArticulationJointCore, PxReal, twistLimitContactDistance, 0)
+
+	PX_DEF_BIN_METADATA_ITEM(stream, Dy::ArticulationJointCore, PxReal, tanQSwingY, 0)
+	PX_DEF_BIN_METADATA_ITEM(stream, Dy::ArticulationJointCore, PxReal, tanQSwingZ, 0)
+	PX_DEF_BIN_METADATA_ITEM(stream, Dy::ArticulationJointCore, PxReal, tanQSwingPad, 0)
+	PX_DEF_BIN_METADATA_ITEM(stream, Dy::ArticulationJointCore, PxReal, tanQTwistHigh, 0)
+	PX_DEF_BIN_METADATA_ITEM(stream, Dy::ArticulationJointCore, PxReal, tanQTwistLow, 0)
+	PX_DEF_BIN_METADATA_ITEM(stream, Dy::ArticulationJointCore, PxReal, tanQTwistPad, 0)
+}
+
+void Sc::ArticulationJointCore::getBinaryMetaData(PxOutputStream& stream)
+{
+	getBinaryMetaData_ArticulationJointCore(stream);
+	PX_DEF_BIN_METADATA_CLASS(stream,	ArticulationJointCore)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ArticulationJointCore, ArticulationJointSim,		mSim,		PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream,	ArticulationJointCore, Dy::ArticulationJointCore,	mCore,		0)
+	PX_DEF_BIN_METADATA_ITEM(stream, ArticulationJointCore, Dy::ArticulationCore, mArticulation, PxMetaDataFlag::ePTR)
+	PX_DEF_BIN_METADATA_ITEM(stream, ArticulationJointCore, Dy::PxArticulationJointBase, mRootType, PxMetaDataFlag::ePTR)
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+#define PX_DEF_BIN_METADATA_ARRAY(stream, Class, type, array)	\
+{ PxMetaDataEntry tmp = {"void", #array".mData", PxU32(PX_OFFSET_OF(Class, array)) + PxMetaDataArray<type>::getDataOffset(), PxMetaDataArray<type>::getDataSize(), 1, 0, PxMetaDataFlag::ePTR, 0};							PX_STORE_METADATA(stream, tmp); } \
+{ PxMetaDataEntry tmp = {"PxU32", #array".mSize", PxU32(PX_OFFSET_OF(Class, array)) + PxMetaDataArray<type>::getSizeOffset(), PxMetaDataArray<type>::getSizeSize(), 1, 0, 0, 0};											PX_STORE_METADATA(stream, tmp);	} \
+{ PxMetaDataEntry tmp = {"PxU32", #array".mCapacity", PxU32(PX_OFFSET_OF(Class, array)) + PxMetaDataArray<type>::getCapacityOffset(), PxMetaDataArray<type>::getCapacitySize(), 1, 0, PxMetaDataFlag::eCOUNT_MASK_MSB, 0};	PX_STORE_METADATA(stream, tmp);	} \
+{ PxMetaDataEntry tmp = {#type, 0, PxU32(PX_OFFSET_OF(Class, array)) + PxMetaDataArray<type>::getSizeOffset(), PxMetaDataArray<type>::getSizeSize(), 0, 0, PxMetaDataFlag::eEXTRA_DATA, 0};									PX_STORE_METADATA(stream, tmp); }
+
+
+
+
+

physx/source/simulationcontroller/src/ScNPhaseCore.h

@@ -0,0 +1,383 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef PX_PHYSICS_SCP_NPHASE_CORE
+#define PX_PHYSICS_SCP_NPHASE_CORE
+
+#include "CmPhysXCommon.h"
+#include "CmRenderOutput.h"
+#include "PxPhysXConfig.h"
+#include "PsUserAllocated.h"
+#include "PsMutex.h"
+#include "PsAtomic.h"
+#include "PsPool.h"
+#include "PsHashSet.h"
+#include "PsHashMap.h"
+#include "PxSimulationEventCallback.h"
+#include "ScTriggerPairs.h"
+#include "ScScene.h"
+#include "ScContactReportBuffer.h"
+#include "PsHash.h"
+
+namespace physx
+{
+namespace Bp
+{
+	struct AABBOverlap;
+	struct BroadPhasePair;
+}
+
+namespace Sc
+{
+	class ActorSim;
+	class ElementSim;
+	class ShapeSim;
+
+	class Interaction;
+	class ElementSimInteraction;
+	class ElementInteractionMarker;
+	class TriggerInteraction;
+
+	class ShapeInteraction;
+	class ActorPair;
+	class ActorPairReport;
+
+	class ActorPairContactReportData;
+	struct ContactShapePair;
+
+	class NPhaseContext;
+	class ContactStreamManager;
+
+	struct FilterPair;
+	class FilterPairManager;
+
+	class RigidSim;
+
+	struct PairReleaseFlag
+	{
+		enum Enum
+		{
+			eBP_VOLUME_REMOVED			=	(1 << 0),	// the broadphase volume of one pair object has been removed.
+			eSHAPE_BP_VOLUME_REMOVED	=	(1 << 1) | eBP_VOLUME_REMOVED,  // the removed broadphase volume was from a rigid body shape.
+			eWAKE_ON_LOST_TOUCH			=	(1 << 2)
+		};
+	};
+
+	/*
+	Description: NPhaseCore encapsulates the near phase processing to allow multiple implementations(eg threading and non
+	threaded).
+
+	The broadphase inserts shape pairs into the NPhaseCore, which are then processed into contact point streams.
+	Pairs can then be processed into AxisConstraints by the GroupSolveCore.
+	*/
+
+	struct BodyPairKey
+	{
+		PxU32 mSim0;
+		PxU32 mSim1;
+
+		PX_FORCE_INLINE	bool operator == (const BodyPairKey& pair) const { return mSim0 == pair.mSim0 && mSim1 == pair.mSim1; }
+	};
+
+	PX_INLINE PxU32 hash(const BodyPairKey& key)
+	{
+		const PxU32 add0 = key.mSim0;
+		const PxU32 add1 = key.mSim1;
+
+		const PxU32 base = PxU32((add0 & 0xFFFF) | (add1 << 16));
+
+		return physx::shdfnd::hash(base);
+	}
+
+	struct ElementSimKey
+	{
+		ElementSim* mSim0, *mSim1;
+
+		ElementSimKey() : mSim0(NULL), mSim1(NULL)
+		{}
+
+		ElementSimKey(ElementSim* sim0, ElementSim* sim1)
+		{
+			if(sim0 > sim1)
+				Ps::swap(sim0, sim1);
+			 mSim0 = sim0;
+			 mSim1 = sim1;
+		}
+
+		PX_FORCE_INLINE	bool operator == (const ElementSimKey& pair) const { return mSim0 == pair.mSim0 && mSim1 == pair.mSim1; }
+	};
+
+	PX_INLINE PxU32 hash(const ElementSimKey& key)
+	{
+		PxU32 add0 = (size_t(key.mSim0)) & 0xFFFFFFFF;
+		PxU32 add1 = (size_t(key.mSim1)) & 0xFFFFFFFF;
+
+		//Clear the lower 2 bits, they will be 0s anyway
+		add0 = add0 >> 2;
+		add1 = add1 >> 2;
+
+		const PxU32 base = PxU32((add0 & 0xFFFF) | (add1 << 16));
+
+		return physx::shdfnd::hash(base);
+	}
+
+	class ContactReportAllocationManager
+	{
+		PxU8* mBuffer;
+		PxU32 mBufferSize;
+		PxU32 mCurrentBufferIndex;
+		PxU32 mCurrentOffset;
+		ContactReportBuffer& mReportBuffer;
+		Ps::Mutex& mMutex;
+		const PxU32 mBuferBlockSize;
+		PX_NOCOPY(ContactReportAllocationManager)
+	public:
+
+		ContactReportAllocationManager(ContactReportBuffer& buffer, Ps::Mutex& mutex, const PxU32 bufferBlockSize = 16384) : mBuffer(NULL), mBufferSize(0), mCurrentBufferIndex(0), 
+			mCurrentOffset(0), mReportBuffer(buffer), mMutex(mutex), mBuferBlockSize(bufferBlockSize)
+		{
+		}
+
+		PxU8* allocate(const PxU32 size, PxU32& index, PxU32 alignment = 16u)
+		{
+			//(1) fix up offsets...
+			PxU32 pad = ((mCurrentBufferIndex + alignment - 1)&~(alignment - 1)) - mCurrentBufferIndex;
+			PxU32 currOffset = mCurrentOffset + pad;
+
+			if ((currOffset + size) > mBufferSize)
+			{
+				const PxU32 allocSize = PxMax(size, mBuferBlockSize);
+
+				mMutex.lock();
+				mBuffer = mReportBuffer.allocateNotThreadSafe(allocSize, mCurrentBufferIndex, alignment);
+				mCurrentOffset = currOffset = 0;
+				mBufferSize = allocSize;
+				mMutex.unlock();
+			}
+
+			PxU8* ret = mBuffer + currOffset;
+			index = mCurrentBufferIndex + currOffset;
+			mCurrentOffset = currOffset + size;
+			return ret;
+		}
+	};
+
+	class NPhaseCore : public Ps::UserAllocated
+	{
+		PX_NOCOPY(NPhaseCore)
+
+	public:
+		NPhaseCore(Scene& scene, const PxSceneDesc& desc);
+		~NPhaseCore();
+
+		void onOverlapCreated(const Bp::AABBOverlap* PX_RESTRICT pairs, PxU32 pairCount);
+
+		void	runOverlapFilters(	PxU32 nbToProcess, const Bp::AABBOverlap* PX_RESTRICT pairs, PxFilterInfo* PX_RESTRICT filterInfo,
+										PxU32& nbToKeep, PxU32& nbToSuppress, PxU32& nbToCallback, PxU32* PX_RESTRICT keepMap, PxU32* PX_RESTRICT callbackMap);
+
+		ElementSimInteraction* onOverlapRemovedStage1(ElementSim* volume0, ElementSim* volume1);
+		void onOverlapRemoved(ElementSim* volume0, ElementSim* volume1, const PxU32 ccdPass, void* elemSim, PxsContactManagerOutputIterator& outputs, bool useAdaptiveForce);
+		void onVolumeRemoved(ElementSim* volume, PxU32 flags, PxsContactManagerOutputIterator& outputs, bool useAdaptiveForce);
+
+		void managerNewTouch(Sc::ShapeInteraction& interaction);
+
+		PxU32 getDefaultContactReportStreamBufferSize() const;
+
+		void fireCustomFilteringCallbacks(PxsContactManagerOutputIterator& outputs, bool useAdaptiveForce);
+
+		void addToDirtyInteractionList(Interaction* interaction);
+		void removeFromDirtyInteractionList(Interaction* interaction);
+		void updateDirtyInteractions(PxsContactManagerOutputIterator& outputs, bool useAdaptiveForce);
+
+		/*
+		Description: Perform trigger overlap tests.
+		*/
+		void processTriggerInteractions(PxBaseTask* continuation);
+
+		/*
+		Description: Gather results from trigger overlap tests and clean up.
+		*/
+		void mergeProcessedTriggerInteractions(PxBaseTask* continuation);
+
+		/*
+		Description: Check candidates for persistent touch contact events and create those events if necessary.
+		*/
+		void processPersistentContactEvents(PxsContactManagerOutputIterator& outputs, PxBaseTask* continuation);
+
+		/*
+		Description: Displays visualizations associated with the near phase.
+		*/
+		void visualize(Cm::RenderOutput& out, PxsContactManagerOutputIterator& outputs);
+
+		PX_FORCE_INLINE Scene& getScene() const	{ return mOwnerScene;	}
+
+		PX_FORCE_INLINE void addToContactReportActorPairSet(ActorPairReport* pair) { mContactReportActorPairSet.pushBack(pair); }
+		void clearContactReportActorPairs(bool shrinkToZero);
+		PX_FORCE_INLINE PxU32 getNbContactReportActorPairs() const { return mContactReportActorPairSet.size(); }
+		PX_FORCE_INLINE ActorPairReport* const* getContactReportActorPairs() const { return mContactReportActorPairSet.begin(); }
+
+		void addToPersistentContactEventPairs(ShapeInteraction*);
+		void addToPersistentContactEventPairsDelayed(ShapeInteraction*);
+		void removeFromPersistentContactEventPairs(ShapeInteraction*);
+		PX_FORCE_INLINE PxU32 getCurrentPersistentContactEventPairCount() const { return mNextFramePersistentContactEventPairIndex; }
+		PX_FORCE_INLINE ShapeInteraction* const* getCurrentPersistentContactEventPairs() const { return mPersistentContactEventPairList.begin(); }
+		PX_FORCE_INLINE PxU32 getAllPersistentContactEventPairCount() const { return mPersistentContactEventPairList.size(); }
+		PX_FORCE_INLINE ShapeInteraction* const* getAllPersistentContactEventPairs() const { return mPersistentContactEventPairList.begin(); }
+		PX_FORCE_INLINE void preparePersistentContactEventListForNextFrame();
+
+		void addToForceThresholdContactEventPairs(ShapeInteraction*);
+		void removeFromForceThresholdContactEventPairs(ShapeInteraction*);
+		PX_FORCE_INLINE PxU32 getForceThresholdContactEventPairCount() const { return mForceThresholdContactEventPairList.size(); }
+		PX_FORCE_INLINE ShapeInteraction* const* getForceThresholdContactEventPairs() const { return mForceThresholdContactEventPairList.begin(); }
+
+		PX_FORCE_INLINE PxU8* getContactReportPairData(const PxU32& bufferIndex) const { return mContactReportBuffer.getData(bufferIndex); }
+		PxU8* reserveContactReportPairData(PxU32 pairCount, PxU32 extraDataSize, PxU32& bufferIndex, ContactReportAllocationManager* alloc = NULL);
+		PxU8* resizeContactReportPairData(PxU32 pairCount, PxU32 extraDataSize, Sc::ContactStreamManager& csm);
+		PX_FORCE_INLINE void clearContactReportStream() { mContactReportBuffer.reset(); }  // Do not free memory at all
+		PX_FORCE_INLINE void freeContactReportStreamMemory() { mContactReportBuffer.flush(); }
+
+		ActorPairContactReportData* createActorPairContactReportData();
+		void releaseActorPairContactReportData(ActorPairContactReportData* data);
+
+		void registerInteraction(ElementSimInteraction* interaction);
+		void unregisterInteraction(ElementSimInteraction* interaction);
+		
+		ElementSimInteraction* createRbElementInteraction(const PxFilterInfo& fInfo, ShapeSim& s0, ShapeSim& s1, PxsContactManager* contactManager, Sc::ShapeInteraction* shapeInteraction, 
+			Sc::ElementInteractionMarker* interactionMarker, bool isTriggerPair);
+
+	private:
+		ElementSimInteraction* createRbElementInteraction(ShapeSim& s0, ShapeSim& s1, PxsContactManager* contactManager, Sc::ShapeInteraction* shapeInteraction, 
+			Sc::ElementInteractionMarker* interactionMarker);
+
+		void releaseElementPair(ElementSimInteraction* pair, PxU32 flags, const PxU32 ccdPass, bool removeFromDirtyList, PxsContactManagerOutputIterator& outputs, bool useAdaptiveForce);
+		void lostTouchReports(ShapeInteraction* pair, PxU32 flags, const PxU32 ccdPass, PxsContactManagerOutputIterator& outputs, bool useAdaptiveForce);
+
+		ShapeInteraction* createShapeInteraction(ShapeSim& s0, ShapeSim& s1, PxPairFlags pairFlags, PxsContactManager* contactManager, Sc::ShapeInteraction* shapeInteraction);
+		TriggerInteraction* createTriggerInteraction(ShapeSim& s0, ShapeSim& s1, PxPairFlags triggerFlags);
+		ElementInteractionMarker* createElementInteractionMarker(ElementSim& e0, ElementSim& e1, ElementInteractionMarker* marker);
+
+		//------------- Filtering -------------
+
+		ElementSimInteraction* refilterInteraction(ElementSimInteraction* pair, const PxFilterInfo* filterInfo, bool removeFromDirtyList, PxsContactManagerOutputIterator& outputs,
+			bool useAdaptiveForce);
+		//-------------------------------------
+
+		ElementSimInteraction* convert(ElementSimInteraction* pair, InteractionType::Enum type, PxFilterInfo& filterInfo, bool removeFromDirtyList, PxsContactManagerOutputIterator& outputs,
+			bool useAdaptiveForce);
+
+		ActorPair* findActorPair(ShapeSim* s0, ShapeSim* s1, Ps::IntBool isReportPair);
+		PX_FORCE_INLINE void destroyActorPairReport(ActorPairReport&);
+
+		Sc::ElementSimInteraction* findInteraction(ElementSim* _element0, ElementSim* _element1);
+
+		// Pooling
+		Scene&											mOwnerScene;
+
+		Ps::Array<ActorPairReport*>						mContactReportActorPairSet;
+		Ps::Array<ShapeInteraction*>					mPersistentContactEventPairList;	// Pairs which request events which do not get triggered by the sdk and thus need to be tested actively every frame.
+																							// May also contain force threshold event pairs (see mForceThresholdContactEventPairList)
+																							// This list is split in two, the elements in front are for the current frame, the elements at the
+																							// back will get added next frame.
+		PxU32											mNextFramePersistentContactEventPairIndex;  // start index of the pairs which need to get added to the persistent list for next frame
+
+		Ps::Array<ShapeInteraction*>					mForceThresholdContactEventPairList;	// Pairs which request force threshold contact events. A pair is only in this list if it does have contact.
+																								// Note: If a pair additionally requests PxPairFlag::eNOTIFY_TOUCH_PERSISTS events, then it
+																								// goes into mPersistentContactEventPairList instead. This allows to share the list index.
+		//
+		//  data layout:
+		//  ContactActorPair0_ExtraData, ContactShapePair0_0, ContactShapePair0_1, ... ContactShapePair0_N, 
+		//  ContactActorPair1_ExtraData, ContactShapePair1_0, ...
+		//
+		ContactReportBuffer								mContactReportBuffer;				// Shape pair information for contact reports
+
+		Ps::CoalescedHashSet<Interaction*>				mDirtyInteractions;
+		FilterPairManager*								mFilterPairManager;
+
+		// Pools
+		Ps::Pool<ActorPair>								mActorPairPool;
+		Ps::Pool<ActorPairReport>						mActorPairReportPool;
+		Ps::Pool<ShapeInteraction>						mShapeInteractionPool;
+		Ps::Pool<TriggerInteraction>					mTriggerInteractionPool;
+		Ps::Pool<ActorPairContactReportData>			mActorPairContactReportDataPool;
+		Ps::Pool<ElementInteractionMarker>				mInteractionMarkerPool;
+
+		Cm::DelegateTask<Sc::NPhaseCore, &Sc::NPhaseCore::mergeProcessedTriggerInteractions> mMergeProcessedTriggerInteractions;
+		void*											mTmpTriggerProcessingBlock;  // temporary memory block to process trigger pairs in parallel
+		Ps::Mutex										mTriggerWriteBackLock;
+		volatile PxI32									mTriggerPairsToDeactivateCount;
+		Ps::HashMap<BodyPairKey, ActorPair*>			mActorPairMap; 
+
+		Ps::HashMap<ElementSimKey, ElementSimInteraction*> mElementSimMap;
+
+		Ps::Mutex										mBufferAllocLock;
+		Ps::Mutex										mReportAllocLock;
+
+		friend class Sc::Scene;
+		friend class Sc::ShapeInteraction;
+	};
+
+	struct FilteringContext
+	{
+		PX_NOCOPY(FilteringContext)
+	public:
+		FilteringContext(const Sc::Scene& scene, FilterPairManager* filterPairManager) :
+			mFilterShader			(scene.getFilterShaderFast()),
+			mFilterShaderData		(scene.getFilterShaderDataFast()),
+			mFilterShaderDataSize	(scene.getFilterShaderDataSizeFast()),
+			mFilterCallback			(scene.getFilterCallbackFast()),
+			mFilterPairManager		(filterPairManager),
+			mKineKineFilteringMode	(scene.getKineKineFilteringMode()),
+			mStaticKineFilteringMode(scene.getStaticKineFilteringMode())
+		{
+		}
+
+		PxSimulationFilterShader			mFilterShader;
+		const void*							mFilterShaderData;
+		PxU32								mFilterShaderDataSize;
+		PxSimulationFilterCallback*			mFilterCallback;
+		FilterPairManager*					mFilterPairManager;
+		const PxPairFilteringMode::Enum		mKineKineFilteringMode;
+		const PxPairFilteringMode::Enum		mStaticKineFilteringMode;
+	};
+
+	// helper function to run the filter logic after some hardwired filter criteria have been passed successfully
+	PxFilterInfo filterRbCollisionPairSecondStage(const FilteringContext& context, const ShapeSim& s0, const ShapeSim& s1, const Sc::BodySim* b0, const Sc::BodySim* b1, PxU32 filterPairIndex, bool runCallbacks);
+
+} // namespace Sc
+
+
+PX_FORCE_INLINE void Sc::NPhaseCore::preparePersistentContactEventListForNextFrame()
+{
+	// reports have been processed -> "activate" next frame candidates for persistent contact events
+	mNextFramePersistentContactEventPairIndex = mPersistentContactEventPairList.size();
+}
+
+}
+
+#endif

physx/source/simulationcontroller/src/ScObjectIDTracker.h

@@ -0,0 +1,99 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef PX_PHYSICS_SC_OBJECT_ID_TRACKER
+#define PX_PHYSICS_SC_OBJECT_ID_TRACKER
+
+#include "CmPhysXCommon.h"
+#include "CmIDPool.h"
+#include "CmBitMap.h"
+#include "PsUserAllocated.h"
+
+namespace physx
+{
+namespace Sc
+{
+	// PT: TODO: this has no direct dependency on "Sc". It should really be a "Cm" class.
+	class ObjectIDTracker : public Ps::UserAllocated
+	{
+		PX_NOCOPY(ObjectIDTracker)
+	public:
+		ObjectIDTracker() : mPendingReleasedIDs(PX_DEBUG_EXP("objectIDTrackerIDs")) {}
+
+		PX_INLINE PxU32			createID()						{ return mIDPool.getNewID();								}
+		PX_INLINE void			releaseID(PxU32 id)				
+		{ 
+			markIDAsDeleted(id); 
+			mPendingReleasedIDs.pushBack(id);	
+		}
+		PX_INLINE Ps::IntBool	isDeletedID(PxU32 id)	const	{ return mDeletedIDsMap.boundedTest(id);					}
+		PX_FORCE_INLINE PxU32	getDeletedIDCount()		const	{ return mPendingReleasedIDs.size();						}
+		PX_INLINE void			clearDeletedIDMap()				{ mDeletedIDsMap.clear();									}
+		PX_INLINE void			resizeDeletedIDMap(PxU32 id, PxU32 numIds)	
+		{ 
+			mDeletedIDsMap.resize(id); 
+			mPendingReleasedIDs.reserve(numIds);
+		}
+		PX_INLINE void			processPendingReleases()
+		{
+			for(PxU32 i=0; i < mPendingReleasedIDs.size(); i++)
+			{
+				 mIDPool.freeID(mPendingReleasedIDs[i]);
+			}
+			mPendingReleasedIDs.clear();
+		}
+		PX_INLINE void reset()
+		{
+			processPendingReleases();
+			mPendingReleasedIDs.reset();
+
+			// Don't free stuff in IDPool, we still need the list of free IDs
+
+			// And it does not seem worth freeing the memory of the bitmap
+		}
+
+		PX_INLINE PxU32 getMaxID()
+		{
+			return mIDPool.getMaxID();
+		}
+	private:
+		PX_INLINE void markIDAsDeleted(PxU32 id) { PX_ASSERT(!isDeletedID(id)); mDeletedIDsMap.growAndSet(id); }
+
+
+	private:
+		Cm::IDPool					mIDPool;
+		Cm::BitMap					mDeletedIDsMap;
+		Ps::Array<PxU32>			mPendingReleasedIDs;  // Buffer for released IDs to make sure newly created objects do not re-use these IDs immediately
+	};
+
+}
+}
+
+#endif

physx/source/simulationcontroller/src/ScPhysics.cpp

@@ -0,0 +1,62 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#include "common/PxTolerancesScale.h"
+
+#include "ScPhysics.h"
+#include "ScScene.h"
+#include "PxvGlobals.h"
+
+using namespace physx;
+
+Sc::Physics* Sc::Physics::mInstance = NULL;
+const PxReal Sc::Physics::sWakeCounterOnCreation = 20.0f*0.02f;
+
+namespace physx
+{
+	namespace Sc
+	{
+		OffsetTable	gOffsetTable;
+	}
+}
+
+Sc::Physics::Physics(const PxTolerancesScale& scale, const PxvOffsetTable& pxvOffsetTable)
+: mScale(scale)
+{
+	mInstance = this;
+	PxvInit(pxvOffsetTable);
+}
+
+
+Sc::Physics::~Physics()
+{
+	PxvTerm();
+	mInstance = 0;
+}

physx/source/simulationcontroller/src/ScRigidCore.cpp

@@ -0,0 +1,125 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "ScBodyCore.h"
+#include "ScStaticCore.h"
+#include "ScRigidSim.h"
+#include "ScShapeSim.h"
+#include "ScScene.h"
+#include "ScPhysics.h"
+
+using namespace physx;
+using namespace Sc;
+
+static ShapeSim& getSimForShape(const ShapeCore& core, const ActorSim& actorSim)
+{
+	const ElementSim* current = actorSim.getElements_();
+	while(current)
+	{
+		const ShapeSim* sim = static_cast<const ShapeSim*>(current);
+		if(&sim->getCore() == &core)
+			return *const_cast<ShapeSim*>(sim);
+		current = current->mNextInActor;
+	}
+
+	PX_ASSERT(0); // should never fail
+	return *reinterpret_cast<ShapeSim*>(1);
+}
+
+RigidCore::RigidCore(const PxActorType::Enum type) 
+: ActorCore(type, PxActorFlag::eVISUALIZATION, PX_DEFAULT_CLIENT, 0)
+{
+}
+
+RigidCore::~RigidCore()
+{
+}
+
+void RigidCore::addShapeToScene(ShapeCore& shapeCore)
+{
+	RigidSim* sim = getSim();
+	PX_ASSERT(sim);
+	if(!sim)
+		return;
+	sim->getScene().addShape_(*sim, shapeCore);
+}
+
+void RigidCore::removeShapeFromScene(ShapeCore& shapeCore, bool wakeOnLostTouch)
+{
+	RigidSim* sim = getSim();
+	if(!sim)
+		return;
+	ShapeSim& s = getSimForShape(shapeCore, *sim);
+	sim->getScene().removeShape_(s, wakeOnLostTouch);
+}
+
+void RigidCore::onShapeChange(ShapeCore& shape, ShapeChangeNotifyFlags notifyFlags, PxShapeFlags oldShapeFlags, bool forceBoundsUpdate)
+{
+	// DS: We pass flags to avoid searching multiple times or exposing RigidSim outside SC, and this form is
+	// more convenient for the Scb::Shape::syncState method. If we start hitting this a lot we should do it
+	// a different way, but shape modification after insertion is rare. 
+
+	RigidSim* sim = getSim();
+	if(!sim)
+		return;
+	ShapeSim& s = getSimForShape(shape, *sim);
+
+	if(notifyFlags & ShapeChangeNotifyFlag::eGEOMETRY)
+		s.onVolumeOrTransformChange(forceBoundsUpdate);
+	if(notifyFlags & ShapeChangeNotifyFlag::eMATERIAL)
+		s.onMaterialChange();
+	if(notifyFlags & ShapeChangeNotifyFlag::eRESET_FILTERING)
+		s.onResetFiltering();
+	if(notifyFlags & ShapeChangeNotifyFlag::eSHAPE2BODY)
+		s.onVolumeOrTransformChange(forceBoundsUpdate);
+	if(notifyFlags & ShapeChangeNotifyFlag::eFILTERDATA)
+		s.onFilterDataChange();
+	if(notifyFlags & ShapeChangeNotifyFlag::eFLAGS)
+		s.onFlagChange(oldShapeFlags);
+	if(notifyFlags & ShapeChangeNotifyFlag::eCONTACTOFFSET)
+		s.onContactOffsetChange();
+	if(notifyFlags & ShapeChangeNotifyFlag::eRESTOFFSET)
+		s.onRestOffsetChange();
+}
+
+RigidSim* RigidCore::getSim() const
+{
+	return static_cast<RigidSim*>(ActorCore::getSim());
+}
+
+PxU32 RigidCore::getRigidID() const
+{
+	return static_cast<RigidSim*>(ActorCore::getSim())->getRigidID();
+}
+
+PxActor* RigidCore::getPxActor() const
+{
+	return Ps::pointerOffset<PxActor*>(const_cast<RigidCore*>(this), gOffsetTable.scCore2PxActor[getActorCoreType()]);
+}
+

physx/source/simulationcontroller/src/ScRigidSim.cpp

@@ -0,0 +1,88 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "ScScene.h"
+#include "ScRigidSim.h"
+#include "ScShapeSim.h"
+#include "PsFoundation.h"
+
+using namespace physx;
+using namespace Sc;
+
+/*
+	PT:
+
+	The BP group ID comes from a Cm::IDPool, and RigidSim is the only class releasing the ID.
+
+	The rigid tracker ID comes from a Cm::IDPool internal to an ObjectIDTracker, and RigidSim
+	is the only class using it.
+
+	Thus we should:
+	- promote the BP group ID stuff to a "tracker" object
+	- use the BP group ID as a rigid ID
+*/
+
+RigidSim::RigidSim(Scene& scene, RigidCore& core) : ActorSim(scene, core)
+{
+	mRigidId = scene.getRigidIDTracker().createID();
+}
+
+RigidSim::~RigidSim()
+{
+	Scene& scScene = getScene();
+	scScene.getRigidIDTracker().releaseID(mRigidId);
+}
+
+void notifyActorInteractionsOfTransformChange(ActorSim& actor);
+void RigidSim::notifyShapesOfTransformChange()
+{
+	if(0)
+	{
+		ElementSim* current = getElements_();
+		while(current)
+		{
+			ShapeSim* sim = static_cast<ShapeSim*>(current);
+			sim->onVolumeOrTransformChange(false);
+			current = current->mNextInActor;
+		}
+	}
+	else
+	{
+		ElementSim* current = getElements_();
+		while(current)
+		{
+			ShapeSim* sim = static_cast<ShapeSim*>(current);
+			sim->markBoundsForUpdate(false);
+			current = current->mNextInActor;
+		}
+
+		notifyActorInteractionsOfTransformChange(*this);
+	}
+}
+

physx/source/simulationcontroller/src/ScRigidSim.h

@@ -0,0 +1,63 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef PX_PHYSICS_SCP_RB_SIM
+#define PX_PHYSICS_SCP_RB_SIM
+
+#include "ScActorSim.h"
+#include "ScRigidCore.h"
+
+namespace physx
+{
+namespace Sc
+{
+	class Scene;
+
+	class RigidSim : public ActorSim
+	{
+	public:
+									RigidSim(Scene&, RigidCore&);
+		virtual						~RigidSim();
+
+		PX_FORCE_INLINE	RigidCore&	getRigidCore()	const	{ return static_cast<RigidCore&>(mCore);	}
+
+		PX_FORCE_INLINE	PxU32		getRigidID()	const	{ return mRigidId;							}
+
+						void		notifyShapesOfTransformChange();
+
+						PxActor*	getPxActor()	const	{ return getRigidCore().getPxActor();		}
+	private:
+						PxU32		mRigidId;
+	};
+
+} // namespace Sc
+
+}
+
+#endif

physx/source/simulationcontroller/src/ScShapeCore.cpp

@@ -0,0 +1,372 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "foundation/PxErrorCallback.h"
+#include "ScShapeSim.h"
+#include "ScPhysics.h"
+#include "GuConvexMesh.h"
+#include "GuTriangleMesh.h"
+#include "GuHeightField.h"
+#include "ScMaterialCore.h"
+
+using namespace physx;
+using namespace Sc;
+
+// djs: temporary cruft 
+
+static PxConvexMeshGeometryLL extendForLL(const PxConvexMeshGeometry& hlGeom)
+{
+	PxConvexMeshGeometryLL llGeom;
+	static_cast<PxConvexMeshGeometry&>(llGeom) = hlGeom;
+
+	Gu::ConvexMesh* cm = static_cast<Gu::ConvexMesh*>(hlGeom.convexMesh);
+
+	llGeom.hullData = &(cm->getHull());
+	llGeom.gpuCompatible = hlGeom.convexMesh->isGpuCompatible();
+
+	return llGeom;
+}
+
+static PxTriangleMeshGeometryLL extendForLL(const PxTriangleMeshGeometry& hlGeom)
+{
+	PxTriangleMeshGeometryLL llGeom;
+	static_cast<PxTriangleMeshGeometry&>(llGeom) = hlGeom;
+
+	Gu::TriangleMesh* tm = static_cast<Gu::TriangleMesh*>(hlGeom.triangleMesh);
+	llGeom.meshData = tm;
+	llGeom.materialIndices = tm->getMaterials();
+	llGeom.materials = static_cast<const PxTriangleMeshGeometryLL&>(hlGeom).materials;
+
+	return llGeom;
+}
+
+static PxHeightFieldGeometryLL extendForLL(const PxHeightFieldGeometry& hlGeom)
+{
+	PxHeightFieldGeometryLL llGeom;
+	static_cast<PxHeightFieldGeometry&>(llGeom) = hlGeom;
+
+	Gu::HeightField* hf = static_cast<Gu::HeightField*>(hlGeom.heightField);
+
+	llGeom.heightFieldData = &hf->getData();
+
+	llGeom.materials = static_cast<const PxHeightFieldGeometryLL&>(hlGeom).materials;
+
+	return llGeom;
+}
+
+ShapeCore::ShapeCore(const PxGeometry& geometry, PxShapeFlags shapeFlags, const PxU16* materialIndices, PxU16 materialCount) :
+	mRestOffset				(0.0f),
+	mTorsionalRadius		(0.0f),
+	mMinTorsionalPatchRadius(0.0f)
+{
+	mCore.mOwnsMaterialIdxMemory	= true;
+
+	PX_ASSERT(materialCount > 0);
+
+	const PxTolerancesScale& scale = Physics::getInstance().getTolerancesScale();
+	mCore.geometry.set(geometry);
+	mCore.transform			= PxTransform(PxIdentity);
+	mCore.contactOffset		= 0.02f * scale.length;
+
+	mCore.mShapeFlags		= shapeFlags;
+
+	setMaterialIndices(materialIndices, materialCount);
+}
+
+// PX_SERIALIZATION
+ShapeCore::ShapeCore(const PxEMPTY) : 
+	mQueryFilterData		(PxEmpty),
+	mSimulationFilterData	(PxEmpty),
+	mCore					(PxEmpty)
+{ 
+	mCore.mOwnsMaterialIdxMemory	= false;
+}
+//~PX_SERIALIZATION
+
+ShapeCore::~ShapeCore()
+{
+	if(mCore.geometry.getType() == PxGeometryType::eTRIANGLEMESH)
+	{
+		PxTriangleMeshGeometryLL& meshGeom = mCore.geometry.get<PxTriangleMeshGeometryLL>();
+		if(mCore.mOwnsMaterialIdxMemory)
+			meshGeom.materials.deallocate();
+	}
+	else if(mCore.geometry.getType() == PxGeometryType::eHEIGHTFIELD)
+	{
+		PxHeightFieldGeometryLL& hfGeom = mCore.geometry.get<PxHeightFieldGeometryLL>();
+		if(mCore.mOwnsMaterialIdxMemory)
+			hfGeom.materials.deallocate();
+	}
+}
+
+PxU16 Sc::ShapeCore::getNbMaterialIndices() const
+{
+	const PxGeometryType::Enum geomType = mCore.geometry.getType();
+
+	if((geomType != PxGeometryType::eTRIANGLEMESH) && (geomType != PxGeometryType::eHEIGHTFIELD))
+	{
+		return 1;
+	}
+	else if(geomType == PxGeometryType::eTRIANGLEMESH)
+	{
+		const PxTriangleMeshGeometryLL& meshGeom = mCore.geometry.get<PxTriangleMeshGeometryLL>();
+		return meshGeom.materials.numIndices;
+	}
+	else
+	{
+		PX_ASSERT(geomType == PxGeometryType::eHEIGHTFIELD);
+		const PxHeightFieldGeometryLL& hfGeom = mCore.geometry.get<PxHeightFieldGeometryLL>();
+		return hfGeom.materials.numIndices;
+	}
+}
+
+const PxU16* Sc::ShapeCore::getMaterialIndices() const
+{
+	const PxGeometryType::Enum geomType = mCore.geometry.getType();
+
+	if((geomType != PxGeometryType::eTRIANGLEMESH) && (geomType != PxGeometryType::eHEIGHTFIELD))
+	{
+		return &mCore.materialIndex;
+	}
+	else if(geomType == PxGeometryType::eTRIANGLEMESH)
+	{
+		const PxTriangleMeshGeometryLL& meshGeom = mCore.geometry.get<PxTriangleMeshGeometryLL>();
+		return meshGeom.materials.indices;
+	}
+	else
+	{
+		PX_ASSERT(geomType == PxGeometryType::eHEIGHTFIELD);
+		const PxHeightFieldGeometryLL& hfGeom = mCore.geometry.get<PxHeightFieldGeometryLL>();
+		return hfGeom.materials.indices;
+	}
+}
+
+PX_FORCE_INLINE void setMaterialsHelper(MaterialIndicesStruct& materials, const PxU16* materialIndices, PxU16 materialIndexCount, PxU8& ownsMemory)
+{
+	if(materials.numIndices < materialIndexCount)
+	{
+		if(materials.indices && ownsMemory)
+			materials.deallocate();
+		materials.allocate(materialIndexCount);
+		ownsMemory = true;
+	}
+	PxMemCopy(materials.indices, materialIndices, sizeof(PxU16)*materialIndexCount);
+	materials.numIndices = materialIndexCount;
+}
+
+void ShapeCore::setMaterialIndices(const PxU16* materialIndices, PxU16 materialIndexCount)
+{
+	const PxGeometryType::Enum geomType = mCore.geometry.getType();
+	mCore.materialIndex = materialIndices[0];
+
+	if(geomType == PxGeometryType::eTRIANGLEMESH)
+	{
+		PxTriangleMeshGeometryLL& meshGeom = mCore.geometry.get<PxTriangleMeshGeometryLL>();
+		setMaterialsHelper(meshGeom.materials, materialIndices, materialIndexCount, mCore.mOwnsMaterialIdxMemory);
+	}
+	else if(geomType == PxGeometryType::eHEIGHTFIELD)
+	{
+		PxHeightFieldGeometryLL& hfGeom = mCore.geometry.get<PxHeightFieldGeometryLL>();
+		setMaterialsHelper(hfGeom.materials, materialIndices, materialIndexCount, mCore.mOwnsMaterialIdxMemory);
+	}
+}
+
+void ShapeCore::setGeometry(const PxGeometry& geom)
+{
+	const PxGeometryType::Enum oldGeomType = mCore.geometry.getType();
+	const PxGeometryType::Enum newGeomType = geom.getType();
+
+	// copy material related data to restore it after the new geometry has been set
+	MaterialIndicesStruct materials;
+	PX_ASSERT(materials.numIndices == 0);
+	
+	if(oldGeomType == PxGeometryType::eTRIANGLEMESH)
+	{
+		PxTriangleMeshGeometryLL& meshGeom = mCore.geometry.get<PxTriangleMeshGeometryLL>();
+		materials = meshGeom.materials;
+	}
+	else if(oldGeomType == PxGeometryType::eHEIGHTFIELD)
+	{
+		PxHeightFieldGeometryLL& hfGeom = mCore.geometry.get<PxHeightFieldGeometryLL>();
+		materials = hfGeom.materials;
+	}
+
+	mCore.geometry.set(geom);
+
+	if((newGeomType == PxGeometryType::eTRIANGLEMESH) || (newGeomType == PxGeometryType::eHEIGHTFIELD))
+	{
+		MaterialIndicesStruct* newMaterials;
+
+		if(newGeomType == PxGeometryType::eTRIANGLEMESH)
+		{
+			PxTriangleMeshGeometryLL& meshGeom = mCore.geometry.get<PxTriangleMeshGeometryLL>();
+			newMaterials = &meshGeom.materials;
+		}
+		else
+		{
+			PX_ASSERT(newGeomType == PxGeometryType::eHEIGHTFIELD);
+			PxHeightFieldGeometryLL& hfGeom = mCore.geometry.get<PxHeightFieldGeometryLL>();
+			newMaterials = &hfGeom.materials;
+		}
+
+		if(materials.numIndices != 0)  // old type was mesh type
+			*newMaterials = materials;
+		else
+		{   // old type was non-mesh type
+			newMaterials->allocate(1);
+			*newMaterials->indices = mCore.materialIndex;
+			mCore.mOwnsMaterialIdxMemory = true;
+		}
+	}
+	else if((materials.numIndices != 0) && mCore.mOwnsMaterialIdxMemory)
+	{
+		// geometry changed to non-mesh type
+		materials.deallocate();
+	}
+}
+
+PxShape* ShapeCore::getPxShape()
+{
+	return Sc::gOffsetTable.convertScShape2Px(this);
+}
+
+const PxShape* ShapeCore::getPxShape() const
+{
+	return Sc::gOffsetTable.convertScShape2Px(this);
+}
+
+// PX_SERIALIZATION
+
+PX_FORCE_INLINE void exportExtraDataMaterials(PxSerializationContext& stream, const MaterialIndicesStruct& materials)
+{
+	stream.alignData(PX_SERIAL_ALIGN);
+	stream.writeData(materials.indices, sizeof(PxU16)*materials.numIndices);
+}
+
+void ShapeCore::exportExtraData(PxSerializationContext& stream)
+{
+	const PxGeometryType::Enum geomType = mCore.geometry.getType();
+
+	if(geomType == PxGeometryType::eTRIANGLEMESH)
+	{
+		PxTriangleMeshGeometryLL& meshGeom = mCore.geometry.get<PxTriangleMeshGeometryLL>();
+		exportExtraDataMaterials(stream, meshGeom.materials);
+	}
+	else if(geomType == PxGeometryType::eHEIGHTFIELD)
+	{
+		PxHeightFieldGeometryLL& hfGeom = mCore.geometry.get<PxHeightFieldGeometryLL>();
+		exportExtraDataMaterials(stream, hfGeom.materials);
+	}
+}
+
+void ShapeCore::importExtraData(PxDeserializationContext& context)
+{
+	const PxGeometryType::Enum geomType = mCore.geometry.getType();
+
+	if(geomType == PxGeometryType::eTRIANGLEMESH)
+	{
+		MaterialIndicesStruct& materials = mCore.geometry.get<PxTriangleMeshGeometryLL>().materials;
+		materials.indices = context.readExtraData<PxU16, PX_SERIAL_ALIGN>(materials.numIndices);
+	}
+	else if(geomType == PxGeometryType::eHEIGHTFIELD)
+	{
+		MaterialIndicesStruct& materials = mCore.geometry.get<PxHeightFieldGeometryLL>().materials;
+		materials.indices = context.readExtraData<PxU16, PX_SERIAL_ALIGN>(materials.numIndices);
+	}
+}
+
+void ShapeCore::resolveMaterialReference(PxU32 materialTableIndex, PxU16 materialIndex)
+{
+	if(materialTableIndex == 0)
+	{
+		mCore.materialIndex = materialIndex;
+	}
+
+	PxGeometry& geom = const_cast<PxGeometry&>(mCore.geometry.getGeometry());
+
+	if(geom.getType() == PxGeometryType::eHEIGHTFIELD)
+	{
+		PxHeightFieldGeometryLL& hfGeom = static_cast<PxHeightFieldGeometryLL&>(geom);
+		hfGeom.materials.indices[materialTableIndex] = materialIndex;
+	}
+	else if(geom.getType() == PxGeometryType::eTRIANGLEMESH)
+	{
+		PxTriangleMeshGeometryLL& meshGeom = static_cast<PxTriangleMeshGeometryLL&>(geom);
+		meshGeom.materials.indices[materialTableIndex] = materialIndex;
+	}
+}
+
+void ShapeCore::resolveReferences(PxDeserializationContext& context)
+{
+	// Resolve geometry pointers if needed
+	PxGeometry& geom = const_cast<PxGeometry&>(mCore.geometry.getGeometry());	
+	
+	switch(geom.getType())
+	{
+	case PxGeometryType::eCONVEXMESH:
+	{
+		PxConvexMeshGeometryLL& convexGeom = static_cast<PxConvexMeshGeometryLL&>(geom);
+		context.translatePxBase(convexGeom.convexMesh);		
+
+		// update the hullData pointer
+		static_cast<PxConvexMeshGeometryLL&>(geom) = extendForLL(convexGeom);
+	}
+	break;
+
+	case PxGeometryType::eHEIGHTFIELD:
+	{
+		PxHeightFieldGeometryLL& hfGeom = static_cast<PxHeightFieldGeometryLL&>(geom);
+		context.translatePxBase(hfGeom.heightField);
+
+		// update hf pointers
+		static_cast<PxHeightFieldGeometryLL&>(geom) = extendForLL(hfGeom);
+	}
+	break;
+
+	case PxGeometryType::eTRIANGLEMESH:
+	{
+		PxTriangleMeshGeometryLL& meshGeom = static_cast<PxTriangleMeshGeometryLL&>(geom);
+		context.translatePxBase(meshGeom.triangleMesh);		
+
+		// update mesh pointers
+		static_cast<PxTriangleMeshGeometryLL&>(geom) = extendForLL(meshGeom);
+	}
+	break;
+	case PxGeometryType::eSPHERE:
+	case PxGeometryType::ePLANE:
+	case PxGeometryType::eCAPSULE:
+	case PxGeometryType::eBOX:
+	case PxGeometryType::eGEOMETRY_COUNT:
+	case PxGeometryType::eINVALID:
+	break;
+
+	}	
+}
+
+//~PX_SERIALIZATION

physx/source/simulationcontroller/src/ScShapeInteraction.h

@@ -0,0 +1,412 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+          
+#ifndef PX_COLLISION_SHAPE_INTERACTION
+#define PX_COLLISION_SHAPE_INTERACTION
+
+#include "ScElementSimInteraction.h"
+#include "ScShapeSim.h"
+#include "ScActorPair.h"
+#include "ScScene.h"
+#include "ScBodySim.h"
+#include "PxsContactManager.h"
+#include "PxsContext.h"
+#include "PxsSimpleIslandManager.h"
+
+#define INVALID_REPORT_PAIR_ID	0xffffffff
+
+namespace physx
+{
+class PxsContactManagerOutputIterator;
+namespace Sc
+{
+	class ContactReportAllocationManager;
+	/*
+	Description: A ShapeInteraction represents a pair of objects which _may_ have contacts. Created by the broadphase
+	and processed by the NPhaseCore.
+	*/
+	class ShapeInteraction : public ElementSimInteraction
+	{
+		friend class NPhaseCore;
+		ShapeInteraction& operator=(const ShapeInteraction&);
+	public:
+		enum SiFlag
+		{
+			PAIR_FLAGS_MASK					= (PxPairFlag::eNEXT_FREE - 1),	// Bits where the PxPairFlags get stored
+			NEXT_FREE						= ((PAIR_FLAGS_MASK << 1) & ~PAIR_FLAGS_MASK),
+
+			HAS_TOUCH						= (NEXT_FREE << 0),		// Tracks the last know touch state
+			HAS_NO_TOUCH					= (NEXT_FREE << 1),		// Tracks the last know touch state
+			TOUCH_KNOWN						= (HAS_TOUCH | HAS_NO_TOUCH),  // If none of these flags is set, the touch state is not known (for example, this is true for pairs that never ran narrowphase
+
+			CONTACTS_COLLECT_POINTS			= (NEXT_FREE << 2),		// The user wants to get the contact points (includes debug rendering)
+			CONTACTS_RESPONSE_DISABLED		= (NEXT_FREE << 3),		// Collision response disabled (either by the user through PxPairFlag::eSOLVE_CONTACT or because the pair has two kinematics)
+
+			CONTACT_FORCE_THRESHOLD_PAIRS	= PxU32(PxPairFlag::eNOTIFY_THRESHOLD_FORCE_FOUND) | PxU32(PxPairFlag::eNOTIFY_THRESHOLD_FORCE_PERSISTS) | PxU32(PxPairFlag::eNOTIFY_THRESHOLD_FORCE_LOST),
+			CONTACT_REPORT_EVENTS			= PxU32(PxPairFlag::eNOTIFY_TOUCH_FOUND) | PxU32(PxPairFlag::eNOTIFY_TOUCH_PERSISTS) | PxU32(PxPairFlag::eNOTIFY_TOUCH_LOST) |
+												PxU32(PxPairFlag::eNOTIFY_THRESHOLD_FORCE_FOUND) | PxU32(PxPairFlag::eNOTIFY_THRESHOLD_FORCE_PERSISTS) | PxU32(PxPairFlag::eNOTIFY_THRESHOLD_FORCE_LOST),
+			CONTACT_REPORT_EXTRA_DATA		= PxU32(PxPairFlag::ePRE_SOLVER_VELOCITY) | PxU32(PxPairFlag::ePOST_SOLVER_VELOCITY) | PxU32(PxPairFlag::eCONTACT_EVENT_POSE),
+
+			FORCE_THRESHOLD_EXCEEDED_NOW	= (NEXT_FREE << 4),
+			FORCE_THRESHOLD_EXCEEDED_BEFORE	= (NEXT_FREE << 5),
+			FORCE_THRESHOLD_EXCEEDED_FLAGS	= FORCE_THRESHOLD_EXCEEDED_NOW | FORCE_THRESHOLD_EXCEEDED_BEFORE,
+
+			IS_IN_PERSISTENT_EVENT_LIST		= (NEXT_FREE << 6), // The pair is in the list of persistent contact events
+			WAS_IN_PERSISTENT_EVENT_LIST	= (NEXT_FREE << 7), // The pair is inactive but used to be in the list of persistent contact events
+			IN_PERSISTENT_EVENT_LIST		= IS_IN_PERSISTENT_EVENT_LIST | WAS_IN_PERSISTENT_EVENT_LIST,
+			IS_IN_FORCE_THRESHOLD_EVENT_LIST= (NEXT_FREE << 8), // The pair is in the list of force threshold contact events
+			IS_IN_CONTACT_EVENT_LIST		= IS_IN_PERSISTENT_EVENT_LIST | IS_IN_FORCE_THRESHOLD_EVENT_LIST,
+
+			LL_MANAGER_RECREATE_EVENT		= CONTACT_REPORT_EVENTS | CONTACTS_COLLECT_POINTS |
+											  CONTACTS_RESPONSE_DISABLED | PxU32(PxPairFlag::eMODIFY_CONTACTS)
+		};
+												ShapeInteraction(ShapeSim& s1, ShapeSim& s2, PxPairFlags pairFlags, PxsContactManager* contactManager);
+												~ShapeInteraction();
+
+		// Submits to contact stream
+						void					processUserNotification(PxU32 contactEvent, PxU16 infoFlags, bool touchLost, const PxU32 ccdPass, const bool useCurrentTransform, 
+												PxsContactManagerOutputIterator& outputs);  // ccdPass is 0 for discrete collision and then 1,2,... for the CCD passes
+
+						void					processUserNotificationSync();
+
+						void					processUserNotificationAsync(PxU32 contactEvent, PxU16 infoFlags, bool touchLost, const PxU32 ccdPass, const bool useCurrentTransform,
+							PxsContactManagerOutputIterator& outputs, ContactReportAllocationManager* alloc = NULL);  // ccdPass is 0 for discrete collision and then 1,2,... for the CCD passes
+
+						void					visualize(Cm::RenderOutput&, PxsContactManagerOutputIterator&);
+
+						PxU32					getContactPointData(const void*& contactPatches, const void*& contactPoints, PxU32& contactDataSize, PxU32& contactPointCount, PxU32& patchCount, const PxReal*& impulses, PxU32 startOffset, PxsContactManagerOutputIterator& outputs);
+
+						bool					managerLostTouch(const PxU32 ccdPass, bool adjustCounters, PxsContactManagerOutputIterator& outputs, bool useAdaptiveForce);
+						void					managerNewTouch(const PxU32 ccdPass, bool adjustCounters, PxsContactManagerOutputIterator& outputs, bool useAdaptiveForce);
+
+		PX_FORCE_INLINE	void					adjustCountersOnLostTouch(BodySim*, BodySim*, bool useAdaptiveForce);
+		PX_FORCE_INLINE	void					adjustCountersOnNewTouch(bool useAdaptiveForce);
+
+		PX_FORCE_INLINE	void					sendCCDRetouch(const PxU32 ccdPass, PxsContactManagerOutputIterator& outputs);
+						void					setContactReportPostSolverVelocity(ContactStreamManager& cs);
+		PX_FORCE_INLINE	void					sendLostTouchReport(bool shapeVolumeRemoved, const PxU32 ccdPass, PxsContactManagerOutputIterator& ouptuts);
+						void					resetManagerCachedState()	const;
+	
+		PX_FORCE_INLINE	ActorPair*				getActorPair()				const	{ return mActorPair;								}
+		PX_FORCE_INLINE	void					setActorPair(ActorPair& aPair)		{ mActorPair = &aPair;								}
+		PX_FORCE_INLINE	void					clearActorPair()					{ mActorPair = NULL;							}
+		PX_FORCE_INLINE	ActorPairReport&		getActorPairReport()		const	{ return ActorPairReport::cast(*mActorPair); }
+		PX_INLINE		Ps::IntBool				isReportPair()				const	{ /*PX_ASSERT(!(Ps::IntBool(getPairFlags() & CONTACT_REPORT_EVENTS)) || mActorPair->isReportPair());*/ return Ps::IntBool(getPairFlags() & CONTACT_REPORT_EVENTS);	}
+		PX_INLINE		Ps::IntBool				hasTouch()					const	{ return readFlag(HAS_TOUCH); }
+		PX_INLINE		Ps::IntBool				hasCCDTouch()				const	{ PX_ASSERT(mManager); return mManager->getHadCCDContact(); }
+		PX_INLINE		void					swapAndClearForceThresholdExceeded();
+
+		PX_FORCE_INLINE void					raiseFlag(SiFlag flag) { mFlags |= flag; }
+		PX_FORCE_INLINE	Ps::IntBool				readFlag(SiFlag flag) const { return Ps::IntBool(mFlags & flag); }
+		PX_FORCE_INLINE	PxU32					getPairFlags() const;
+
+		PX_FORCE_INLINE	void					removeFromReportPairList();
+
+						void					onShapeChangeWhileSleeping(bool shapeOfDynamicChanged);
+
+		PX_FORCE_INLINE	Ps::IntBool				hasKnownTouchState() const;
+
+						bool					onActivate_(void* data);
+						bool					onDeactivate_();
+
+						void					updateState(const PxU8 externalDirtyFlags);
+
+						const PxsContactManager*	getContactManager() const { return mManager; }
+
+						void					clearIslandGenData();
+
+		PX_FORCE_INLINE PxU32					getEdgeIndex() const { return mEdgeIndex;  }
+
+		PX_FORCE_INLINE	Sc::ShapeSim&			getShape0()	const	{ return static_cast<ShapeSim&>(getElement0());	}
+		PX_FORCE_INLINE	Sc::ShapeSim&			getShape1()	const	{ return static_cast<ShapeSim&>(getElement1());	}
+
+	private:
+						PxU32					mContactReportStamp;
+						PxU32					mFlags;
+						ActorPair*				mActorPair;
+						PxU32					mReportPairIndex;			// Owned by NPhaseCore for its report pair list
+
+						PxsContactManager*		mManager;
+
+						PxU32					mEdgeIndex;
+
+						PxU16					mReportStreamIndex;  // position of this pair in the contact report stream
+
+		// Internal functions:
+
+						void					createManager(void* contactManager);
+		PX_INLINE		void					resetManager();
+		PX_INLINE		bool					updateManager(void* contactManager);
+		PX_INLINE		void					destroyManager();
+		PX_FORCE_INLINE	bool					activeManagerAllowed() const;
+		PX_FORCE_INLINE	PxU32					getManagerContactState()		const	{ return mFlags & LL_MANAGER_RECREATE_EVENT; }
+
+		PX_FORCE_INLINE void					clearFlag(SiFlag flag)					{ mFlags &= ~flag;				}
+		PX_INLINE		void					setFlag(SiFlag flag, bool value)
+												{
+													if (value)
+														raiseFlag(flag);
+													else
+														clearFlag(flag);
+												}
+		PX_FORCE_INLINE void					setHasTouch() { clearFlag(HAS_NO_TOUCH); raiseFlag(HAS_TOUCH); }
+		PX_FORCE_INLINE void					setHasNoTouch() { clearFlag(HAS_TOUCH); raiseFlag(HAS_NO_TOUCH); }
+
+		PX_FORCE_INLINE	void					setPairFlags(PxPairFlags flags);
+
+		PX_FORCE_INLINE	void					processReportPairOnActivate();
+		PX_FORCE_INLINE	void					processReportPairOnDeactivate();
+
+		// Certain SiFlag cache properties of the pair. If these properties change then the flags have to be updated.
+		// For example: is collision enabled for this pair? are contact points requested for this pair?
+		PX_FORCE_INLINE	void					updateFlags(const Sc::Scene&, const Sc::BodySim*, const Sc::BodySim*, const PxU32 pairFlags);
+
+		friend class Sc::Scene;
+	};
+
+} // namespace Sc
+
+
+PX_FORCE_INLINE void Sc::ShapeInteraction::sendLostTouchReport(bool shapeVolumeRemoved, const PxU32 ccdPass, PxsContactManagerOutputIterator& outputs)
+{
+	PX_ASSERT(hasTouch());
+	PX_ASSERT(isReportPair());
+
+	PxU32 thresholdForceLost = readFlag(ShapeInteraction::FORCE_THRESHOLD_EXCEEDED_NOW) ? PxU32(PxPairFlag::eNOTIFY_THRESHOLD_FORCE_LOST) : 0;  // make sure to only send report if force is still above threshold
+	PxU32 triggeredFlags = getPairFlags() & (PxU32(PxPairFlag::eNOTIFY_TOUCH_LOST) | thresholdForceLost);
+	if (triggeredFlags)
+	{
+		PxU16 infoFlag = 0;
+		if (mActorPair->getTouchCount() == 1)  // this code assumes that the actor pair touch count does get decremented afterwards
+		{
+			infoFlag |= PxContactPairFlag::eACTOR_PAIR_LOST_TOUCH;
+		}
+
+		//Lost touch is processed after solver, so we should use the previous transform to update the pose for objects if user request eCONTACT_EVENT_POSE
+		processUserNotification(triggeredFlags, infoFlag, true, ccdPass, false, outputs);
+	}
+
+	ActorPairReport& apr = getActorPairReport();
+	if (apr.hasReportData() && !apr.streamResetNeeded(getScene().getTimeStamp()))
+	{
+		// If there has been no event recorded yet, there is no need to worry about events with shape pointers which might later reference
+		// removed shapes due to buffered removal, i.e., removal while the simulation was running.
+		// This is also correct for CCD scenarios where a touch could get lost and then found again etc. If in such a case there ever is a contact event
+		// recorded, there will always be another sendLostTouchReport() call at some later point (caused by the simulation or when the shape gets
+		// removed at fetchResults).
+		PxU16 flagsToRaise = ContactStreamManagerFlag::eHAS_PAIRS_THAT_LOST_TOUCH;
+
+		ContactStreamManager& cs = apr.getContactStreamManager();
+
+		if (shapeVolumeRemoved)
+		{
+			flagsToRaise |= ContactStreamManagerFlag::eTEST_FOR_REMOVED_SHAPES;
+
+			// if an actor gets deleted while the simulation is running and the actor has a pending contact report with post solver 
+			// velocity extra data, then the post solver velocity needs to get written now because it is too late when the reports
+			// get fired (the object will have been deleted already)
+
+			if (cs.getFlags() & ContactStreamManagerFlag::eNEEDS_POST_SOLVER_VELOCITY)
+				setContactReportPostSolverVelocity(cs);
+		}
+
+		cs.raiseFlags(flagsToRaise);
+	}
+}
+
+
+PX_FORCE_INLINE void Sc::ShapeInteraction::setPairFlags(PxPairFlags flags)
+{
+	PX_ASSERT(PxU32(flags) < PxPairFlag::eNEXT_FREE);  // to find out if a new PxPairFlag has been added after eLAST instead of in front
+
+	PxU32 newFlags = mFlags;
+	PxU32 fl = PxU32(flags) & PAIR_FLAGS_MASK;
+	newFlags &= (~PAIR_FLAGS_MASK);  // clear old flags
+	newFlags |= fl;
+
+	mFlags = newFlags;
+}
+
+
+// PT: returning PxU32 instead of PxPairFlags to remove LHS. Please do not undo this.
+PX_FORCE_INLINE PxU32 Sc::ShapeInteraction::getPairFlags() const
+{
+	return (mFlags & PAIR_FLAGS_MASK);
+}
+
+
+PX_INLINE void Sc::ShapeInteraction::swapAndClearForceThresholdExceeded()
+{
+	PxU32 flags = mFlags;
+
+	PX_COMPILE_TIME_ASSERT(FORCE_THRESHOLD_EXCEEDED_NOW == (FORCE_THRESHOLD_EXCEEDED_BEFORE >> 1));
+
+	PxU32 nowToBefore = (flags & FORCE_THRESHOLD_EXCEEDED_NOW) << 1;
+	flags &= ~(FORCE_THRESHOLD_EXCEEDED_NOW | FORCE_THRESHOLD_EXCEEDED_BEFORE);
+	flags |= nowToBefore;
+
+	mFlags = flags;
+}
+
+PX_FORCE_INLINE	void Sc::ShapeInteraction::removeFromReportPairList()
+{
+	// this method should only get called if the pair is in the list for
+	// persistent or force based contact reports
+	PX_ASSERT(mReportPairIndex != INVALID_REPORT_PAIR_ID);
+	PX_ASSERT(readFlag(IS_IN_CONTACT_EVENT_LIST));
+
+	Scene& scene = getScene();
+
+	if (readFlag(IS_IN_FORCE_THRESHOLD_EVENT_LIST))
+		scene.getNPhaseCore()->removeFromForceThresholdContactEventPairs(this);
+	else 
+	{
+		PX_ASSERT(readFlag(IS_IN_PERSISTENT_EVENT_LIST));
+		scene.getNPhaseCore()->removeFromPersistentContactEventPairs(this);
+	}
+}
+
+PX_INLINE bool Sc::ShapeInteraction::updateManager(void* contactManager)
+{
+	if (activeManagerAllowed())
+	{
+		if (mManager == 0)
+			createManager(contactManager);
+
+		return (mManager != NULL);  // creation might fail (pool reached limit, mem allocation failed etc.)
+	}
+	else
+		return false;
+}
+
+PX_INLINE void Sc::ShapeInteraction::destroyManager()
+{
+	PX_ASSERT(mManager);
+
+	Scene& scene = getScene();
+	
+	PxvNphaseImplementationContext* nphaseImplementationContext = scene.getLowLevelContext()->getNphaseImplementationContext();
+	PX_ASSERT(nphaseImplementationContext);
+	nphaseImplementationContext->unregisterContactManager(mManager);
+
+	/*if (mEdgeIndex != IG_INVALID_EDGE)
+		scene.getSimpleIslandManager()->clearEdgeRigidCM(mEdgeIndex);*/
+	scene.getLowLevelContext()->destroyContactManager(mManager);
+	mManager = 0;
+}
+
+
+PX_FORCE_INLINE bool Sc::ShapeInteraction::activeManagerAllowed() const
+{
+	PX_ASSERT(getShape0().getActor().isDynamicRigid() || getShape1().getActor().isDynamicRigid());
+	
+	const BodySim* bodySim0 = getShape0().getBodySim();
+	const BodySim* bodySim1 = getShape1().getBodySim();
+	PX_ASSERT(bodySim0);  // the first shape always belongs to a dynamic body
+
+	const IG::IslandSim& islandSim = getScene().getSimpleIslandManager()->getSpeculativeIslandSim();
+
+	//if((bodySim0->isActive()) || (bodySim1 && bodySim1->isActive()))
+	//check whether active in the speculative sim!
+	if (islandSim.getNode(bodySim0->getNodeIndex()).isActive() || 
+		(bodySim1 && islandSim.getNode(bodySim1->getNodeIndex()).isActive()))
+	{
+		return true;
+	}
+	else
+	{
+		//Sleeping kinematic 0 vs sleeping kinematic 1
+		return false;
+	}
+}
+
+
+PX_FORCE_INLINE void Sc::ShapeInteraction::sendCCDRetouch(const PxU32 ccdPass, PxsContactManagerOutputIterator& outputs)
+{
+	PxU32 pairFlags = getPairFlags();
+	if (pairFlags & PxPairFlag::eNOTIFY_TOUCH_CCD)
+	{
+		processUserNotification(PxPairFlag::eNOTIFY_TOUCH_CCD, 0, false, ccdPass, false, outputs);
+	}
+}
+
+
+PX_FORCE_INLINE void Sc::ShapeInteraction::adjustCountersOnLostTouch(BodySim* body0, BodySim* body1, bool useAdaptiveForce)
+{
+	PX_ASSERT(body0);  // the first shape always belongs to a dynamic body
+
+	PX_ASSERT(mActorPair->getTouchCount());
+
+	mActorPair->decTouchCount();
+
+	if (useAdaptiveForce || mActorPair->getTouchCount() == 0)
+	{
+		body0->decrementBodyConstraintCounter();
+		if (body1)
+			body1->decrementBodyConstraintCounter();
+	}
+}
+
+
+PX_FORCE_INLINE void Sc::ShapeInteraction::adjustCountersOnNewTouch(bool useAdaptiveForce)
+{
+	BodySim* body0 = getShape0().getBodySim();
+	BodySim* body1 = getShape1().getBodySim();
+	PX_ASSERT(body0);  // the first shape always belongs to a dynamic body
+
+	mActorPair->incTouchCount();
+	//If using adaptive force, always record a body constraint, otherwise only record if this is the first constraint
+	//with this pair of bodies (doubling up usage of this counter for both adaptive force and stabilization)
+	if (useAdaptiveForce || mActorPair->getTouchCount() == 1)
+	{
+		body0->incrementBodyConstraintCounter();
+		if (body1)
+			body1->incrementBodyConstraintCounter();
+	}
+}
+
+
+PX_FORCE_INLINE Ps::IntBool Sc::ShapeInteraction::hasKnownTouchState() const
+{
+	// For a pair where the bodies were added asleep, the touch state is not known until narrowphase runs on the pair for the first time.
+	// If such a pair looses AABB overlap before, the conservative approach is to wake the bodies up. This method provides an indicator that
+	// this is such a pair. Note: this might also wake up objects that do not touch but that's the price to pay (unless we want to run
+	// overlap tests on such pairs).
+	
+	if (mManager)
+		return mManager->touchStatusKnown();
+	else
+		return readFlag(TOUCH_KNOWN);
+}
+
+
+}
+
+#endif

physx/source/simulationcontroller/src/ScShapeSim.cpp

@@ -0,0 +1,495 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "ScBodySim.h"
+#include "ScStaticSim.h"
+#include "ScScene.h"
+#include "ScElementSimInteraction.h"
+#include "ScShapeInteraction.h"
+#include "ScTriggerInteraction.h"
+#include "ScSimStats.h"
+#include "ScObjectIDTracker.h"
+#include "GuHeightFieldUtil.h"
+#include "GuTriangleMesh.h"
+#include "GuConvexMeshData.h"
+#include "GuHeightField.h"
+#include "PxsContext.h"
+#include "PxsTransformCache.h"
+#include "CmTransformUtils.h"
+#include "GuBounds.h"
+#include "PxsRigidBody.h"
+#include "ScSqBoundsManager.h"
+#include "PxsSimulationController.h"
+#include "common/PxProfileZone.h"
+
+using namespace physx;
+using namespace Gu;
+using namespace Sc;
+
+// PT: keep local functions in cpp, no need to pollute the header. Don't force conversions to bool if not necessary.
+static PX_FORCE_INLINE PxU32 hasTriggerFlags(PxShapeFlags flags)	{ return PxU32(flags) & PxU32(PxShapeFlag::eTRIGGER_SHAPE);									}
+static PX_FORCE_INLINE PxU32 isBroadPhase(PxShapeFlags flags)		{ return PxU32(flags) & PxU32(PxShapeFlag::eTRIGGER_SHAPE|PxShapeFlag::eSIMULATION_SHAPE);	}
+
+static PX_FORCE_INLINE void resetElementID(Scene& scene, ShapeSim& shapeSim)
+{
+	PX_ASSERT(!shapeSim.isInBroadPhase());
+
+	scene.getDirtyShapeSimMap().reset(shapeSim.getElementID());
+
+	if(shapeSim.getSqBoundsId() != PX_INVALID_U32)
+		shapeSim.destroySqBounds();
+}
+
+void ShapeSim::initSubsystemsDependingOnElementID()
+{
+	Scene& scScene = getScene();
+
+	Bp::BoundsArray& boundsArray = scScene.getBoundsArray();
+	const PxU32 index = getElementID();
+
+	PX_ALIGN(16, PxTransform absPos);
+	getAbsPoseAligned(&absPos);
+
+	PxsTransformCache& cache = scScene.getLowLevelContext()->getTransformCache();
+	cache.initEntry(index);
+	cache.setTransformCache(absPos, 0, index);
+
+	boundsArray.updateBounds(absPos, mCore.getGeometryUnion(), index);
+	
+	{
+		PX_PROFILE_ZONE("API.simAddShapeToBroadPhase", scScene.getContextId());
+		if(isBroadPhase(mCore.getFlags()))
+			internalAddToBroadPhase();			
+		else
+			scScene.getAABBManager()->reserveSpaceForBounds(index);
+		scScene.updateContactDistance(index, getContactOffset());
+	}
+
+	if(scScene.getDirtyShapeSimMap().size() <= index)
+		scScene.getDirtyShapeSimMap().resize(PxMax(index+1, (scScene.getDirtyShapeSimMap().size()+1) * 2u));
+
+	RigidSim& owner = getRbSim();
+	if(owner.isDynamicRigid() && static_cast<BodySim&>(owner).isActive())
+		createSqBounds();
+
+	// Init LL shape
+	{
+		mLLShape.mElementIndex = index;
+		mLLShape.mShapeCore = const_cast<PxsShapeCore*>(&mCore.getCore());
+	
+		if(owner.getActorType()==PxActorType::eRIGID_STATIC)
+		{
+			mLLShape.mBodySimIndex = IG::NodeIndex(IG_INVALID_NODE);
+		}
+		else
+		{
+			BodySim& bodySim = static_cast<BodySim&>(getActor());
+			mLLShape.mBodySimIndex = bodySim.getNodeIndex();
+			//mLLShape.mLocalBound = computeBounds(mCore.getGeometry(), PxTransform(PxIdentity));
+		}
+	}
+}
+
+ShapeSim::ShapeSim(RigidSim& owner, const ShapeCore& core) :
+	ElementSim	(owner),
+	mCore		(core),
+	mSqBoundsId	(PX_INVALID_U32)
+{
+	// sizeof(ShapeSim) = 32 bytes
+	Scene& scScene = getScene();
+
+	mId = scScene.getShapeIDTracker().createID();
+
+	initSubsystemsDependingOnElementID();
+}
+
+ShapeSim::~ShapeSim()
+{
+	Scene& scScene = getScene();
+	resetElementID(scScene, *this);
+	scScene.getShapeIDTracker().releaseID(mId);
+}
+
+Bp::FilterGroup::Enum ShapeSim::getBPGroup() const
+{
+	BodySim* bs = getBodySim();
+
+	bool isKinematic = bs ? bs->isKinematic() : false;
+	if(isKinematic && bs->hasForcedKinematicNotif())
+		isKinematic = false;
+
+	RigidSim& rbSim = getRbSim();
+	return Bp::getFilterGroup(rbSim.getActorType()==PxActorType::eRIGID_STATIC, rbSim.getRigidID(), isKinematic);
+}
+
+PX_FORCE_INLINE void ShapeSim::internalAddToBroadPhase()
+{
+	PX_ASSERT(!isInBroadPhase());
+
+	addToAABBMgr(mCore.getContactOffset(), getBPGroup(), Ps::IntBool(mCore.getCore().mShapeFlags & PxShapeFlag::eTRIGGER_SHAPE));
+}
+
+PX_FORCE_INLINE void ShapeSim::internalRemoveFromBroadPhase(bool wakeOnLostTouch)
+{
+	PX_ASSERT(isInBroadPhase());
+	removeFromAABBMgr();
+
+	Scene& scene = getScene();
+	PxsContactManagerOutputIterator outputs = scene.getLowLevelContext()->getNphaseImplementationContext()->getContactManagerOutputs();
+	scene.getNPhaseCore()->onVolumeRemoved(this, wakeOnLostTouch ? PxU32(PairReleaseFlag::eWAKE_ON_LOST_TOUCH) : 0, outputs, scene.getPublicFlags() & PxSceneFlag::eADAPTIVE_FORCE);
+}
+
+void ShapeSim::removeFromBroadPhase(bool wakeOnLostTouch)
+{
+	if(isInBroadPhase())
+		internalRemoveFromBroadPhase(wakeOnLostTouch);
+}
+
+void ShapeSim::reinsertBroadPhase()
+{
+	if(isInBroadPhase())
+		internalRemoveFromBroadPhase();
+//	internalAddToBroadPhase();
+
+	Scene& scene = getScene();
+
+	// Scene::removeShape
+	{
+		//unregisterShapeFromNphase(shape.getCore());
+
+		// PT: "getID" is const but the addShape call used LLShape, which uses elementID, so....
+		scene.getSimulationController()->removeShape(getID());
+	}
+
+	// Call ShapeSim dtor
+	{
+		resetElementID(scene, *this);
+	}
+
+	// Call ElementSim dtor
+	{
+		releaseID();
+	}
+
+	// Call ElementSim ctor
+	{
+		initID();
+	}
+
+	// Call ShapeSim ctor
+	{
+		initSubsystemsDependingOnElementID();
+	}
+
+	// Scene::addShape
+	{
+		scene.getSimulationController()->addShape(&getLLShapeSim(), getID());
+		// PT: TODO: anything else needed here?
+		//registerShapeInNphase(shapeCore);
+	}
+}
+
+void ShapeSim::onFilterDataChange()
+{
+	setElementInteractionsDirty(InteractionDirtyFlag::eFILTER_STATE, InteractionFlag::eFILTERABLE);
+}
+
+void ShapeSim::onResetFiltering()
+{
+	if(isInBroadPhase())
+		reinsertBroadPhase();
+}
+
+void ShapeSim::onMaterialChange()
+{
+	setElementInteractionsDirty(InteractionDirtyFlag::eMATERIAL, InteractionFlag::eRB_ELEMENT);
+}
+
+void ShapeSim::onRestOffsetChange()
+{
+	setElementInteractionsDirty(InteractionDirtyFlag::eREST_OFFSET, InteractionFlag::eRB_ELEMENT);
+}
+
+void ShapeSim::onContactOffsetChange()
+{
+	if(isInBroadPhase())
+		getScene().getAABBManager()->setContactOffset(getElementID(), mCore.getContactOffset());
+}
+
+void ShapeSim::onFlagChange(PxShapeFlags oldFlags)
+{
+	const PxShapeFlags newFlags = mCore.getFlags();
+
+	const bool oldBp = isBroadPhase(oldFlags)!=0;
+	const bool newBp = isBroadPhase(newFlags)!=0;
+
+	// Change of collision shape flags requires removal/add to broadphase
+	if(oldBp != newBp)
+	{
+		if(!oldBp && newBp)
+		{
+			// A.B. if a trigger was removed and inserted within the same frame we need to reinsert
+			if(hasTriggerFlags(newFlags) && getScene().getAABBManager()->isMarkedForRemove(getElementID()))
+			{
+				reinsertBroadPhase();
+			}
+			else
+			{
+				internalAddToBroadPhase();
+			}
+		}
+		else
+			internalRemoveFromBroadPhase();
+	}
+	else
+	{
+		const bool wasTrigger = hasTriggerFlags(oldFlags)!=0;
+		const bool isTrigger = hasTriggerFlags(newFlags)!=0;
+		if(wasTrigger != isTrigger)
+			reinsertBroadPhase();  // re-insertion is necessary because trigger pairs get killed
+	}
+
+	const PxShapeFlags hadSq = oldFlags & PxShapeFlag::eSCENE_QUERY_SHAPE;
+	const PxShapeFlags hasSq = newFlags & PxShapeFlag::eSCENE_QUERY_SHAPE;
+	if(hasSq && !hadSq)
+	{
+		BodySim* body = getBodySim();
+		if(body &&  body->isActive())
+			createSqBounds();
+	}
+	else if(hadSq && !hasSq)
+		destroySqBounds();
+}
+
+void ShapeSim::getAbsPoseAligned(PxTransform* PX_RESTRICT globalPose) const
+{
+	const PxTransform& shape2Actor = mCore.getCore().transform;
+	const PxTransform* actor2World = NULL;
+	if(getActor().getActorType()==PxActorType::eRIGID_STATIC)
+	{
+		PxsRigidCore& core = static_cast<StaticSim&>(getActor()).getStaticCore().getCore();
+		actor2World = &core.body2World;
+	}
+	else
+	{
+		PxsBodyCore& core = static_cast<BodySim&>(getActor()).getBodyCore().getCore();
+		if(!core.hasIdtBody2Actor())
+		{
+			Cm::getDynamicGlobalPoseAligned(core.body2World, shape2Actor, core.getBody2Actor(), *globalPose);
+			return;
+		}
+		actor2World = &core.body2World;
+	}
+	Cm::getStaticGlobalPoseAligned(*actor2World, shape2Actor, *globalPose);
+}
+
+BodySim* ShapeSim::getBodySim() const
+{ 
+	ActorSim& a = getActor();
+	return a.isDynamicRigid() ? static_cast<BodySim*>(&a) : NULL;
+}
+
+PxsRigidCore& ShapeSim::getPxsRigidCore() const
+{
+	ActorSim& a = getActor();
+	return a.isDynamicRigid() ? static_cast<BodySim&>(a).getBodyCore().getCore()
+							  : static_cast<StaticSim&>(a).getStaticCore().getCore();
+}
+
+void ShapeSim::updateCached(PxU32 transformCacheFlags, Cm::BitMapPinned* shapeChangedMap)
+{
+	PX_ALIGN(16, PxTransform absPose);
+	getAbsPoseAligned(&absPose);
+
+	Scene& scene = getScene();
+	const PxU32 index = getElementID();
+
+	scene.getLowLevelContext()->getTransformCache().setTransformCache(absPose, transformCacheFlags, index);
+	scene.getBoundsArray().updateBounds(absPose, mCore.getGeometryUnion(), index);
+	if (shapeChangedMap && isInBroadPhase())
+		shapeChangedMap->growAndSet(index);
+}
+
+void ShapeSim::updateCached(PxsTransformCache& transformCache, Bp::BoundsArray& boundsArray)
+{
+	const PxU32 index = getElementID();
+
+	PxsCachedTransform& ct = transformCache.getTransformCache(index);
+	Ps::prefetchLine(&ct);
+
+	getAbsPoseAligned(&ct.transform);
+
+	ct.flags = 0;
+
+	PxBounds3& b = boundsArray.begin()[index];
+	Gu::computeBounds(b, mCore.getGeometryUnion().getGeometry(), ct.transform, 0.0f, NULL, 1.0f);
+}
+
+void ShapeSim::updateContactDistance(PxReal* contactDistance, const PxReal inflation, const PxVec3 angVel, const PxReal dt, Bp::BoundsArray& boundsArray)
+{
+	const PxU32 index = getElementID();
+
+	const PxBounds3& bounds = boundsArray.getBounds(index);
+
+	PxReal radius = bounds.getExtents().magnitude();
+
+	//Heuristic for angular velocity...
+	PxReal angularInflation = angVel.magnitude() * dt * radius;
+
+	contactDistance[index] = getContactOffset() + inflation + angularInflation;
+}
+
+Ps::IntBool ShapeSim::updateSweptBounds()
+{
+	Vec3p endOrigin, endExtent;
+	const ShapeCore& shapeCore = mCore;
+	const PxTransform& endPose = getScene().getLowLevelContext()->getTransformCache().getTransformCache(getElementID()).transform;
+	PxReal ccdThreshold = computeBoundsWithCCDThreshold(endOrigin, endExtent, shapeCore.getGeometry(), endPose, NULL);
+
+	PxBounds3 bounds = PxBounds3::centerExtents(endOrigin, endExtent);
+
+	BodySim* body = getBodySim();
+	const PxsRigidBody& rigidBody = body->getLowLevelBody();
+	const PxsBodyCore& bodyCore = body->getBodyCore().getCore();
+	PX_ALIGN(16, PxTransform shape2World);
+	Cm::getDynamicGlobalPoseAligned(rigidBody.mLastTransform, shapeCore.getShape2Actor(), bodyCore.getBody2Actor(), shape2World);
+	const PxBounds3 startBounds = computeBounds(shapeCore.getGeometry(), shape2World);
+
+	const Ps::IntBool isFastMoving = (startBounds.getCenter() - endOrigin).magnitudeSquared() >= ccdThreshold * ccdThreshold ? 1 : 0;
+
+	if (isFastMoving)
+		bounds.include(startBounds);
+
+	PX_ASSERT(bounds.minimum.x <= bounds.maximum.x
+		&&	  bounds.minimum.y <= bounds.maximum.y
+		&&	  bounds.minimum.z <= bounds.maximum.z);
+
+	getScene().getBoundsArray().setBounds(bounds, getElementID());
+
+	return isFastMoving;	
+}
+
+void ShapeSim::updateBPGroup()
+{
+	if(isInBroadPhase())
+	{
+		Sc::Scene& scene = getScene();
+		scene.getAABBManager()->setBPGroup(getElementID(), getBPGroup());
+
+		reinsertBroadPhase();
+//		internalRemoveFromBroadPhase();
+//		internalAddToBroadPhase();
+	}
+}
+
+void ShapeSim::markBoundsForUpdate(bool forceBoundsUpdate)
+{
+	Scene& scene = getScene();
+	if(forceBoundsUpdate)
+		updateCached(0, &scene.getAABBManager()->getChangedAABBMgActorHandleMap());
+	else if(isInBroadPhase())
+		scene.getDirtyShapeSimMap().growAndSet(getElementID());
+}
+
+static PX_FORCE_INLINE void updateInteraction(Scene& scene, Interaction* i, const bool isDynamic, const bool isAsleep)
+{
+	if(i->getType() == InteractionType::eOVERLAP)
+	{
+		ShapeInteraction* si = static_cast<ShapeInteraction*>(i);
+		si->resetManagerCachedState();
+			
+		if (isAsleep)
+			si->onShapeChangeWhileSleeping(isDynamic);
+	}
+	else if(i->getType() == InteractionType::eTRIGGER)
+		(static_cast<TriggerInteraction*>(i))->forceProcessingThisFrame(scene);  // trigger pairs need to be checked next frame
+}
+
+void ShapeSim::onVolumeOrTransformChange(bool forceBoundsUpdate)
+{
+	Scene& scene = getScene();
+	BodySim* body = getBodySim();
+	const bool isDynamic = (body != NULL);
+	const bool isAsleep = body ? !body->isActive() : true;
+
+	ElementSim::ElementInteractionIterator iter = getElemInteractions();
+	ElementSimInteraction* i = iter.getNext();
+	while(i)
+	{
+		updateInteraction(scene, i, isDynamic, isAsleep);
+		i = iter.getNext();
+	}
+
+	markBoundsForUpdate(forceBoundsUpdate);
+}
+
+void notifyActorInteractionsOfTransformChange(ActorSim& actor)
+{
+	bool isDynamic;
+	bool isAsleep;
+	if(actor.isDynamicRigid())
+	{
+		isDynamic = true;
+		isAsleep = !static_cast<BodySim&>(actor).isActive();
+	}
+	else
+	{
+		isDynamic = false;
+		isAsleep = true;
+	}
+
+	Scene& scene = actor.getScene();
+
+	PxU32 nbInteractions = actor.getActorInteractionCount();
+	Interaction** interactions = actor.getActorInteractions();
+	while(nbInteractions--)
+		updateInteraction(scene, *interactions++, isDynamic, isAsleep);
+}
+
+void ShapeSim::createSqBounds()
+{
+	if(mSqBoundsId!=PX_INVALID_U32)
+		return;
+
+	BodySim* bodySim = getBodySim();
+	PX_ASSERT(bodySim);
+
+	if(bodySim->usingSqKinematicTarget() || bodySim->isFrozen() || !bodySim->isActive() || bodySim->readInternalFlag(BodySim::BF_IS_COMPOUND_RIGID))
+		return;
+
+	if(mCore.getFlags() & PxShapeFlag::eSCENE_QUERY_SHAPE)
+		getScene().getSqBoundsManager().addShape(*this);
+}
+
+void ShapeSim::destroySqBounds()
+{
+	if(mSqBoundsId!=PX_INVALID_U32)
+		getScene().getSqBoundsManager().removeShape(*this);
+}
+

physx/source/simulationcontroller/src/ScShapeSim.h

@@ -0,0 +1,147 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef PX_PHYSICS_SCP_SHAPESIM
+#define PX_PHYSICS_SCP_SHAPESIM
+
+#include "ScElementSim.h"
+#include "ScShapeCore.h"
+#include "ScRigidSim.h"
+#include "PxsShapeSim.h"
+
+namespace physx
+{
+	class PxsTransformCache;
+namespace Gu
+{
+	class TriangleMesh;
+	class HeightField;
+}
+
+/** Simulation object corresponding to a shape core object. This object is created when
+    a ShapeCore object is added to the simulation, and destroyed when it is removed
+*/
+
+struct PxsRigidCore;
+
+namespace Sc
+{
+	class RigidSim;
+	class ShapeCore;
+	class Scene;
+	class BodySim;
+	class StaticSim;
+
+	class ShapeSim : public ElementSim
+	{
+		ShapeSim &operator=(const ShapeSim &);
+	public:
+
+		// passing in a pointer for the shape to output its bounds allows us not to have to compute them twice.
+		// A neater way to do this would be to ask the AABB Manager for the bounds after the shape has been 
+		// constructed, but there is currently no spec for what the AABBMgr is allowed to do with the bounds, 
+		// hence better not to assume anything.
+
+												ShapeSim(RigidSim&, const ShapeCore& core);
+												~ShapeSim();
+
+						void					reinsertBroadPhase();
+
+		PX_FORCE_INLINE	const ShapeCore&		getCore()				const	{ return mCore; }
+
+						// TODO: compile time coupling
+
+		PX_INLINE		PxGeometryType::Enum	getGeometryType()		const	{ return mCore.getGeometryType();	}
+
+		// This is just for getting a reference for the user, so we cast away const-ness
+
+		PX_INLINE		PxShape*				getPxShape()			const	{ return const_cast<PxShape*>(mCore.getPxShape());	}
+		
+		PX_FORCE_INLINE	PxReal					getRestOffset()			const	{ return mCore.getRestOffset();		}
+		PX_FORCE_INLINE	PxReal					getTorsionalPatchRadius() const { return mCore.getTorsionalPatchRadius(); }
+		PX_FORCE_INLINE	PxReal					getMinTorsionalPatchRadius() const { return mCore.getMinTorsionalPatchRadius(); }
+		PX_FORCE_INLINE	PxU32					getFlags()				const	{ return mCore.getFlags();			}
+		PX_FORCE_INLINE	PxReal					getContactOffset()		const	{ return mCore.getContactOffset();	}
+
+		PX_FORCE_INLINE	RigidSim&				getRbSim()				const	{ return static_cast<RigidSim&>(getActor());	}
+						BodySim*				getBodySim()			const;
+
+						PxsRigidCore&			getPxsRigidCore()		const;
+
+						void					onFilterDataChange();
+						void					onRestOffsetChange();
+						void					onFlagChange(PxShapeFlags oldFlags);
+						void					onResetFiltering();
+						void					onVolumeOrTransformChange(bool forceBoundsUpdate);
+						void					onMaterialChange();  // remove when material properties are gone from PxcNpWorkUnit
+						void					onContactOffsetChange();
+						void					markBoundsForUpdate(bool forceBoundsUpdate);
+
+						void					getAbsPoseAligned(PxTransform* PX_RESTRICT globalPose)	const;
+
+		PX_FORCE_INLINE	PxU32					getID()								const	{ return mId;		}
+		PX_FORCE_INLINE	PxU32					getTransformCacheID()				const	{ return getElementID();	}
+
+						void					removeFromBroadPhase(bool wakeOnLostTouch);
+
+						void					createSqBounds();
+						void					destroySqBounds();
+
+		PX_FORCE_INLINE PxU32					getSqBoundsId()						const	{ return mSqBoundsId; }
+		PX_FORCE_INLINE void					setSqBoundsId(PxU32 id)						{ mSqBoundsId = id; }
+
+						void					updateCached(PxU32 transformCacheFlags, Cm::BitMapPinned* shapeChangedMap);
+						void					updateCached(PxsTransformCache& transformCache, Bp::BoundsArray& boundsArray);
+						void					updateContactDistance(PxReal* contactDistance, const PxReal inflation, const PxVec3 angVel, const PxReal dt, Bp::BoundsArray& boundsArray);
+						Ps::IntBool				updateSweptBounds();
+						void					updateBPGroup();
+
+		PX_FORCE_INLINE PxsShapeSim&			getLLShapeSim() 							{ return mLLShape; }
+	private:
+						PxsShapeSim				mLLShape;
+						const ShapeCore&		mCore;
+						PxU32					mId;
+						PxU32					mSqBoundsId;
+
+		PX_FORCE_INLINE	void					internalAddToBroadPhase();
+		PX_FORCE_INLINE	void					internalRemoveFromBroadPhase(bool wakeOnLostTouch=true);
+						void					initSubsystemsDependingOnElementID();
+						Bp::FilterGroup::Enum	getBPGroup()	const;
+	};
+
+#if !PX_P64_FAMILY
+//	PX_COMPILE_TIME_ASSERT(32==sizeof(Sc::ShapeSim)); // after removing bounds from shapes
+//	PX_COMPILE_TIME_ASSERT((sizeof(Sc::ShapeSim) % 16) == 0); // aligned mem bounds are better for prefetching
+#endif
+
+} // namespace Sc
+
+}
+
+#endif

physx/source/simulationcontroller/src/ScSimStateData.h

@@ -0,0 +1,139 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef PX_SIMSTATEDATA
+#define PX_SIMSTATEDATA
+
+#include "foundation/PxMemory.h"
+#include "ScBodyCore.h"
+
+namespace physx
+{
+namespace Sc
+{
+	struct Kinematic : public KinematicTransform
+	{
+		// The following members buffer the original body data to restore them when switching back to dynamic body
+		// (for kinematics the corresponding LowLevel properties are set to predefined values)
+		PxVec3			backupInverseInertia;			// The inverse of the body space inertia tensor
+		PxReal			backupInvMass;					// The inverse of the body mass
+		PxReal			backupLinearDamping;			// The velocity is scaled by (1.0f - this * dt) inside integrateVelocity() every substep.
+		PxReal			backupAngularDamping;
+		PxReal			backupMaxAngVelSq;				// The angular velocity's magnitude is clamped to this maximum value.
+		PxReal			backupMaxLinVelSq;				// The angular velocity's magnitude is clamped to this maximum value	
+	};
+	PX_COMPILE_TIME_ASSERT(0 == (sizeof(Kinematic) & 0x0f));
+
+	// Important: Struct is reset in setForcesToDefaults.
+
+	enum VelocityModFlags
+	{
+		VMF_GRAVITY_DIRTY	= (1 << 0),
+		VMF_ACC_DIRTY		= (1 << 1),
+		VMF_VEL_DIRTY		= (1 << 2)
+	};
+
+	struct VelocityMod
+	{
+		PxVec3	linearPerSec;		// A request to change the linear velocity by this much each second. The velocity is changed by this * dt inside integrateVelocity().
+		PxU8	flags;
+		PxU8	pad0[3];
+		PxVec3	angularPerSec;
+		PxU8	pad1[3];
+		PxU8	type;
+		PxVec3	linearPerStep;		// A request to change the linear velocity by this much the next step. The velocity is changed inside updateForces().
+		PxU32	pad2;
+		PxVec3	angularPerStep;
+		PxU32	pad3;
+
+		PX_FORCE_INLINE	void					clear()													{ linearPerSec = angularPerSec = linearPerStep = angularPerStep = PxVec3(0.0f); }
+
+		PX_FORCE_INLINE	void					clearPerStep()											{ linearPerStep = angularPerStep = PxVec3(0.0f); }
+
+		PX_FORCE_INLINE const PxVec3&			getLinearVelModPerSec()							const	{ return linearPerSec;				}
+		PX_FORCE_INLINE void					accumulateLinearVelModPerSec(const PxVec3& v)			{ linearPerSec += v;				}
+		PX_FORCE_INLINE void					setLinearVelModPerSec(const PxVec3& v)					{ linearPerSec = v; }
+		PX_FORCE_INLINE void					clearLinearVelModPerSec()								{ linearPerSec = PxVec3(0.0f);		}
+
+		PX_FORCE_INLINE const PxVec3&			getLinearVelModPerStep()						const	{ return linearPerStep;				}
+		PX_FORCE_INLINE void					accumulateLinearVelModPerStep(const PxVec3& v)			{ linearPerStep += v;				}
+		PX_FORCE_INLINE void					clearLinearVelModPerStep()								{ linearPerStep = PxVec3(0.0f);		}
+
+		PX_FORCE_INLINE const PxVec3&			getAngularVelModPerSec()						const	{ return angularPerSec;				}
+		PX_FORCE_INLINE void					accumulateAngularVelModPerSec(const PxVec3& v)			{ angularPerSec += v;				}
+		PX_FORCE_INLINE void					setAngularVelModPerSec(const PxVec3& v)					{ angularPerSec = v; }
+		PX_FORCE_INLINE void					clearAngularVelModPerSec()								{ angularPerSec = PxVec3(0.0f);		}
+
+		PX_FORCE_INLINE const PxVec3&			getAngularVelModPerStep()						const	{ return angularPerStep;			}
+		PX_FORCE_INLINE void					accumulateAngularVelModPerStep(const PxVec3& v)			{ angularPerStep += v;				}
+		PX_FORCE_INLINE void					clearAngularVelModPerStep()								{ angularPerStep = PxVec3(0.0f);	}
+
+		PX_FORCE_INLINE void					notifyAddAcceleration()									{ flags |= VMF_ACC_DIRTY;			}
+		PX_FORCE_INLINE void					notifyClearAcceleration()								{ flags |= VMF_ACC_DIRTY;			}
+		PX_FORCE_INLINE void					notifyAddVelocity()										{ flags |= VMF_VEL_DIRTY;			}
+		PX_FORCE_INLINE void					notifyClearVelocity()									{ flags |= VMF_VEL_DIRTY;			}
+	};
+	PX_COMPILE_TIME_ASSERT(sizeof(VelocityMod) == sizeof(Kinematic));
+
+
+	// Structure to store data for kinematics (target pose etc.)
+	// note: we do not delete this object for kinematics even if no target is set.
+	struct SimStateData : public Ps::UserAllocated	// TODO: may want to optimize the allocation of this further.
+	{
+		PxU8 data[sizeof(Kinematic)];
+
+		enum Enum
+		{
+			eVelMod=0,
+			eKine
+		};
+
+		SimStateData(){}
+		SimStateData(const PxU8 type)
+		{
+			PxMemZero(data, sizeof(Kinematic));
+			Kinematic* kine = reinterpret_cast<Kinematic*>(data);
+			kine->type = type;
+		}
+
+		PX_FORCE_INLINE PxU32 getType() const { const Kinematic* kine = reinterpret_cast<const Kinematic*>(data); return kine->type;}
+		PX_FORCE_INLINE bool isKine() const {return eKine == getType();}
+		PX_FORCE_INLINE bool isVelMod() const {return eVelMod == getType();}
+
+		Kinematic* getKinematicData() { Kinematic* kine = reinterpret_cast<Kinematic*>(data); PX_ASSERT(eKine == kine->type);  return kine;}
+		VelocityMod* getVelocityModData() { VelocityMod* velmod = reinterpret_cast<VelocityMod*>(data); PX_ASSERT(eVelMod == velmod->type); return velmod;}
+		const Kinematic* getKinematicData() const { const Kinematic* kine = reinterpret_cast<const Kinematic*>(data); PX_ASSERT(eKine == kine->type);  return kine;}
+		const VelocityMod* getVelocityModData() const { const VelocityMod* velmod = reinterpret_cast<const VelocityMod*>(data); PX_ASSERT(eVelMod == velmod->type); return velmod;}
+	};
+
+} // namespace Sc
+
+}  // namespace physx
+
+#endif //PX_SIMSTATEDATA

physx/source/simulationcontroller/src/ScSimStats.cpp

@@ -0,0 +1,132 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "foundation/PxMemory.h"
+#include "ScSimStats.h"
+#include "PxvSimStats.h"
+
+using namespace physx;
+
+Sc::SimStats::SimStats()
+{
+	numBroadPhaseAdds = numBroadPhaseRemoves = 0;
+
+	clear();
+}
+
+void Sc::SimStats::clear()
+{
+#if PX_ENABLE_SIM_STATS
+	PxMemZero(const_cast<void*>(reinterpret_cast<volatile void*>(&numTriggerPairs)), sizeof(TriggerPairCounts));
+	numBroadPhaseAddsPending = numBroadPhaseRemovesPending = 0;
+#endif
+}
+
+void Sc::SimStats::simStart()
+{
+#if PX_ENABLE_SIM_STATS
+	// pending broadphase adds/removes are now the current ones
+	numBroadPhaseAdds = numBroadPhaseAddsPending;
+	numBroadPhaseRemoves = numBroadPhaseRemovesPending;
+	clear();
+#endif
+}
+
+void Sc::SimStats::readOut(PxSimulationStatistics& s, const PxvSimStats& simStats) const
+{
+#if PX_ENABLE_SIM_STATS
+	s = PxSimulationStatistics();  // clear stats
+
+	for(PxU32 i=0; i < PxGeometryType::eCONVEXMESH+1; i++)
+	{
+		for(PxU32 j=0; j < PxGeometryType::eGEOMETRY_COUNT; j++)
+		{
+			s.nbTriggerPairs[i][j] += PxU32(numTriggerPairs[i][j]);
+			if (i != j)
+				s.nbTriggerPairs[j][i] += PxU32(numTriggerPairs[i][j]);
+		}
+	}
+
+	s.nbBroadPhaseAdds = numBroadPhaseAdds;
+	s.nbBroadPhaseRemoves = numBroadPhaseRemoves;
+
+	for(PxU32 i=0; i < PxGeometryType::eGEOMETRY_COUNT; i++)
+	{
+		s.nbDiscreteContactPairs[i][i] = simStats.mNbDiscreteContactPairs[i][i];
+		s.nbModifiedContactPairs[i][i] = simStats.mNbModifiedContactPairs[i][i];
+		s.nbCCDPairs[i][i] = simStats.mNbCCDPairs[i][i];
+
+		for(PxU32 j=i+1; j < PxGeometryType::eGEOMETRY_COUNT; j++)
+		{
+			PxU32 c = simStats.mNbDiscreteContactPairs[i][j];
+			s.nbDiscreteContactPairs[i][j] = c;
+			s.nbDiscreteContactPairs[j][i] = c;
+
+			c = simStats.mNbModifiedContactPairs[i][j];
+			s.nbModifiedContactPairs[i][j] = c;
+			s.nbModifiedContactPairs[j][i] = c;
+
+			c = simStats.mNbCCDPairs[i][j];
+			s.nbCCDPairs[i][j] = c;
+			s.nbCCDPairs[j][i] = c;
+		}
+#if PX_DEBUG
+		for(PxU32 j=0; j < i; j++)
+		{
+			// PxvSimStats should only use one half of the matrix
+			PX_ASSERT(simStats.mNbDiscreteContactPairs[i][j] == 0);
+			PX_ASSERT(simStats.mNbModifiedContactPairs[i][j] == 0);
+			PX_ASSERT(simStats.mNbCCDPairs[i][j] == 0);
+		}
+#endif
+	}
+
+	s.nbDiscreteContactPairsTotal = simStats.mNbDiscreteContactPairsTotal;
+	s.nbDiscreteContactPairsWithCacheHits = simStats.mNbDiscreteContactPairsWithCacheHits;
+	s.nbDiscreteContactPairsWithContacts = simStats.mNbDiscreteContactPairsWithContacts;
+	s.nbActiveConstraints = simStats.mNbActiveConstraints;
+	s.nbActiveDynamicBodies = simStats.mNbActiveDynamicBodies;
+	s.nbActiveKinematicBodies = simStats.mNbActiveKinematicBodies;
+
+	s.nbAxisSolverConstraints = simStats.mNbAxisSolverConstraints;
+
+	s.peakConstraintMemory = simStats.mPeakConstraintBlockAllocations * 16 * 1024;
+	s.compressedContactSize = simStats.mTotalCompressedContactSize;
+	s.requiredContactConstraintMemory = simStats.mTotalConstraintSize;
+	s.nbNewPairs = simStats.mNbNewPairs;
+	s.nbLostPairs = simStats.mNbLostPairs;
+	s.nbNewTouches = simStats.mNbNewTouches;
+	s.nbLostTouches = simStats.mNbLostTouches;
+	s.nbPartitions = simStats.mNbPartitions;
+
+#else
+	PX_UNUSED(s);
+	PX_UNUSED(simStats);
+#endif
+}

physx/source/simulationcontroller/src/ScSimStats.h

@@ -0,0 +1,89 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef PX_PHYSICS_SCP_SIM_STATS
+#define PX_PHYSICS_SCP_SIM_STATS
+
+#include "geometry/PxGeometry.h"
+#include "PxSimulationStatistics.h"
+#include "PsAtomic.h"
+#include "PsUserAllocated.h"
+#include "CmPhysXCommon.h"
+
+namespace physx
+{
+
+struct PxvSimStats;
+
+namespace Sc
+{
+
+	/*
+	Description: contains statistics for the scene.
+	*/
+	class SimStats : public Ps::UserAllocated
+	{
+	public:
+		SimStats();
+
+		void clear();		//set counters to zero
+		void simStart();
+		void readOut(PxSimulationStatistics& dest, const PxvSimStats& simStats) const;
+
+		PX_INLINE void incBroadphaseAdds()
+		{
+			numBroadPhaseAddsPending++;
+		}
+
+		PX_INLINE void incBroadphaseRemoves()
+		{
+			numBroadPhaseRemovesPending++;
+		}
+
+	private:
+		// Broadphase adds/removes for the current simulation step
+		PxU32 numBroadPhaseAdds;
+		PxU32 numBroadPhaseRemoves;
+
+		// Broadphase adds/removes for the next simulation step
+		PxU32 numBroadPhaseAddsPending;
+		PxU32 numBroadPhaseRemovesPending;
+
+	public:
+		typedef PxI32 TriggerPairCountsNonVolatile[PxGeometryType::eCONVEXMESH+1][PxGeometryType::eGEOMETRY_COUNT];
+		typedef volatile TriggerPairCountsNonVolatile TriggerPairCounts;
+		TriggerPairCounts numTriggerPairs;
+	};
+
+} // namespace Sc
+
+}
+
+#endif

physx/source/simulationcontroller/src/ScSimulationController.cpp

@@ -0,0 +1,43 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "ScSimulationController.h"
+#include "PsAllocator.h"
+
+using namespace physx;
+
+PxsSimulationController* physx::createSimulationController(PxsSimulationControllerCallback* callback)
+{
+	return PX_PLACEMENT_NEW(PX_ALLOC(sizeof(Sc::SimulationController), PX_DEBUG_EXP("ScSimulationController")), Sc::SimulationController(callback)); 
+}
+
+void Sc::SimulationController::udpateScBodyAndShapeSim(PxsTransformCache& /*cache*/, Bp::BoundsArray& /*boundArray*/, PxBaseTask* continuation)
+{
+	mCallback->updateScBodyAndShapeSim(continuation);
+}

physx/source/simulationcontroller/src/ScSimulationController.h

@@ -0,0 +1,102 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved. 
+
+#include "PxsSimulationController.h"
+
+#ifndef SC_SIMULATION_CONTROLLER_H
+#define	SC_SIMULATION_CONTROLLER_H
+
+
+namespace physx
+{
+
+class PxsHeapMemoryAllocator;
+
+namespace Dy
+{
+	class FeatherstoneArticulation;
+	struct ArticulationJointCore;
+}
+
+namespace Sc
+{
+
+	class SimulationController : public PxsSimulationController
+	{
+		PX_NOCOPY(SimulationController)
+	public:
+		SimulationController(PxsSimulationControllerCallback* callback): PxsSimulationController(callback)
+		{
+		}
+	
+		virtual ~SimulationController(){}
+		virtual void addJoint(const PxU32 /*edgeIndex*/, Dy::Constraint* /*constraint*/, IG::IslandSim& /*islandSim*/, Ps::Array<PxU32>& /*jointIndices*/,
+		Ps::Array<PxgSolverConstraintManagerConstants, Ps::VirtualAllocator>& /*managerIter*/, PxU32 /*uniqueId*/){}
+		virtual void removeJoint(const PxU32 /*edgeIndex*/, Dy::Constraint* /*constraint*/, Ps::Array<PxU32>& /*jointIndices*/, IG::IslandSim& /*islandSim*/){}
+		virtual void addShape(PxsShapeSim* /*shapeSim*/, const PxU32 /*index*/){}
+		virtual void removeShape(const PxU32 /*index*/){}
+		virtual void addDynamic(PxsRigidBody* /*rigidBody*/, const IG::NodeIndex& /*nodeIndex*/){}
+		virtual void addDynamics(PxsRigidBody** /*rigidBody*/, const PxU32* /*nodeIndex*/, PxU32 /*nbBodies*/) {}
+		virtual void addArticulation(Dy::ArticulationV* /*articulation*/, const IG::NodeIndex& /*nodeIndex*/){}
+		virtual void releaseArticulation(Dy::ArticulationV* /*articulation*/, const IG::NodeIndex& /*nodeIndex*/) {}
+		virtual void releaseDeferredArticulationIds(){}
+		virtual void updateDynamic(const bool /*isArticulationLink*/, const IG::NodeIndex& /*nodeIndex*/) {}
+		virtual void updateJoint(const PxU32 /*edgeIndex*/, Dy::Constraint* /*constraint*/){}
+		virtual void updateBodies(PxsRigidBody** /*rigidBodies*/,  PxU32* /*nodeIndices*/, const PxU32 /*nbBodies*/) {}
+		virtual void updateBody(PxsRigidBody* /*rigidBody*/, const PxU32 /*nodeIndex*/) {}
+		virtual void updateBodiesAndShapes(PxBaseTask* /*continuation*/){}
+		virtual void update(const PxU32 /*bitMapWordCounts*/){}
+		virtual void gpuDmabackData(PxsTransformCache& /*cache*/, Bp::BoundsArray& /*boundArray*/, Cm::BitMapPinned&  /*changedAABBMgrHandles*/){}
+		virtual void udpateScBodyAndShapeSim(PxsTransformCache& cache, Bp::BoundsArray& boundArray, PxBaseTask* continuation);
+		virtual void updateArticulation(Dy::ArticulationV* /*articulation*/, const IG::NodeIndex& /*nodeIndex*/) {}
+		virtual void updateArticulationJoint(Dy::ArticulationV* /*articulation*/, const IG::NodeIndex& /*nodeIndex*/) {}
+		virtual PxU32* getActiveBodies() { return NULL; }
+		virtual PxU32* getDeactiveBodies() { return NULL; }
+		virtual void** getRigidBodies() { return NULL; }
+		virtual PxU32	getNbBodies() { return 0; }
+		virtual PxU32	getNbFrozenShapes() { return 0; }
+		virtual PxU32	getNbUnfrozenShapes() { return 0; }
+
+		virtual PxU32*	getUnfrozenShapes() { return NULL; }
+		virtual PxU32*	getFrozenShapes() { return NULL; }
+		virtual PxsShapeSim** getShapeSims() { return NULL; }
+		virtual PxU32	getNbShapes()	{ return 0; }
+
+		virtual void	clear() { }
+		virtual void	setBounds(Bp::BoundsArray* /*boundArray*/){}
+		virtual void	reserve(const PxU32 /*nbBodies*/) {}
+
+		virtual PxU32   getArticulationRemapIndex(const PxU32 /*nodeIndex*/) { return PX_INVALID_U32;}
+		
+	};
+}
+
+}
+
+#endif

physx/source/simulationcontroller/src/ScSqBoundsManager.cpp

@@ -0,0 +1,122 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "ScSqBoundsManager.h"
+#include "ScBodySim.h"
+#include "ScShapeSim.h"
+#include "common/PxProfileZone.h"
+
+using namespace physx;
+using namespace Sc;
+
+SqBoundsManager::SqBoundsManager() :
+	mShapes			(PX_DEBUG_EXP("SqBoundsManager::mShapes")),
+	mRefs			(PX_DEBUG_EXP("SqBoundsManager::mRefs")),				
+	mBoundsIndices	(PX_DEBUG_EXP("SqBoundsManager::mBoundsIndices")),
+	mRefless		(PX_DEBUG_EXP("SqBoundsManager::mRefless"))
+{
+}
+
+void SqBoundsManager::addShape(ShapeSim& shape)
+{
+	PX_ASSERT(shape.getFlags() & PxShapeFlag::eSCENE_QUERY_SHAPE);
+	PX_ASSERT(!shape.getBodySim()->usingSqKinematicTarget());
+	PX_ASSERT(!shape.getBodySim()->isFrozen());
+
+	const PxU32 id = mShapes.size();
+	PX_ASSERT(id == mRefs.size());
+	PX_ASSERT(id == mBoundsIndices.size());
+
+	shape.setSqBoundsId(id);
+
+	mShapes.pushBack(&shape);
+	mRefs.pushBack(PX_INVALID_U32);	// PT: TODO: should be INVALID_PRUNERHANDLE but cannot include SqPruner.h
+	mBoundsIndices.pushBack(shape.getElementID());
+	mRefless.pushBack(&shape);
+}
+
+void SqBoundsManager::removeShape(ShapeSim& shape)
+{
+	const PxU32 id = shape.getSqBoundsId();
+	PX_ASSERT(id!=PX_INVALID_U32);
+
+	shape.setSqBoundsId(PX_INVALID_U32);
+	mShapes[id] = mShapes.back();
+	mBoundsIndices[id] = mBoundsIndices.back();
+	mRefs[id] = mRefs.back();
+
+	if(id+1 != mShapes.size())
+		mShapes[id]->setSqBoundsId(id);
+
+	mShapes.popBack();
+	mRefs.popBack();
+	mBoundsIndices.popBack();
+}
+
+void SqBoundsManager::syncBounds(SqBoundsSync& sync, SqRefFinder& finder, const PxBounds3* bounds, PxU64 contextID, const Cm::BitMap& dirtyShapeSimMap)
+{
+	PX_PROFILE_ZONE("Sim.sceneQuerySyncBounds", contextID);
+	PX_UNUSED(contextID);
+
+#if PX_DEBUG
+	for(PxU32 i=0;i<mShapes.size();i++)
+	{
+		ShapeSim& shape = *mShapes[i];
+		PX_UNUSED(shape);
+		PX_ASSERT(shape.getFlags() & PxShapeFlag::eSCENE_QUERY_SHAPE);
+		PX_ASSERT(!shape.getBodySim()->usingSqKinematicTarget());
+		PX_ASSERT(!shape.getBodySim()->isFrozen());
+	}
+#endif
+
+	ShapeSim*const * shapes = mRefless.begin();
+	for(PxU32 i=0, size = mRefless.size();i<size;i++)
+	{
+		const PxU32 id = shapes[i]->getSqBoundsId();
+		// PT:
+		//
+		// If id == PX_INVALID_U32, the shape has been removed and not re-added. Nothing to do in this case, we just ignore it.
+		// This case didn't previously exist since mRefless only contained valid (added) shapes. But now we left removed shapes in the
+		// structure, and these have an id == PX_INVALID_U32.
+		//
+		// Now if the id is valid but mRefs[id] == PX_INVALID_U32, this is a regular shape that has been added and not processed yet.
+		// So we process it.
+		//
+		// Finally, if both id and mRefs[id] are not PX_INVALID_U32, this is a shape that has been added, removed, and re-added. The
+		// array contains the same shape twice and we only need to process it once.
+		if(id!=PX_INVALID_U32)
+		{
+			if(mRefs[id] == PX_INVALID_U32)	// PT: TODO: should be INVALID_PRUNERHANDLE but cannot include SqPruner.h
+				mRefs[id] = finder.find(static_cast<PxRigidBody*>(shapes[i]->getBodySim()->getPxActor()), shapes[i]->getPxShape());
+		}
+	}
+	mRefless.clear();
+
+	sync.sync(mRefs.begin(), mBoundsIndices.begin(), bounds, mShapes.size(), dirtyShapeSimMap);
+}

physx/source/simulationcontroller/src/ScSqBoundsManager.h

@@ -0,0 +1,73 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef PX_PHYSICS_SCP_SQ_BOUNDS_MANAGER
+#define PX_PHYSICS_SCP_SQ_BOUNDS_MANAGER
+
+#include "CmPhysXCommon.h"
+#include "foundation/PxBounds3.h"
+#include "PsArray.h"
+#include "PsUserAllocated.h"
+#include "PsHashSet.h"
+#include "CmBitMap.h"
+
+namespace physx
+{
+namespace Sq
+{
+typedef PxU32 PrunerHandle;	// PT: we should get this from SqPruner.h but it cannot be included from here
+}
+
+namespace Sc
+{
+struct SqBoundsSync;
+struct SqRefFinder;
+class ShapeSim;
+
+class SqBoundsManager : public Ps::UserAllocated
+{
+	PX_NOCOPY(SqBoundsManager)
+public:
+									SqBoundsManager();
+
+	void							addShape(ShapeSim& shape);
+	void							removeShape(ShapeSim& shape);
+	void							syncBounds(SqBoundsSync& sync, SqRefFinder& finder, const PxBounds3* bounds, PxU64 contextID, const Cm::BitMap& dirtyShapeSimMap);
+
+private:
+
+	Ps::Array<ShapeSim*>			mShapes;		// 
+	Ps::Array<Sq::PrunerHandle>		mRefs;			// SQ pruner references
+	Ps::Array<PxU32>				mBoundsIndices;	// indices into the Sc bounds array
+	Ps::Array<ShapeSim*>			mRefless;		// shapesims without references
+};
+}
+}
+
+#endif

physx/source/simulationcontroller/src/ScStaticCore.cpp

@@ -0,0 +1,49 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#include "ScStaticCore.h"
+#include "ScStaticSim.h"
+#include "PxRigidStatic.h"
+
+using namespace physx;
+
+Sc::StaticSim* Sc::StaticCore::getSim() const
+{
+	return static_cast<StaticSim*>(Sc::ActorCore::getSim());
+}
+
+void Sc::StaticCore::setActor2World(const PxTransform& actor2World)
+{
+	mCore.body2World = actor2World;
+
+	StaticSim* sim = getSim();
+	if(sim)
+		sim->notifyShapesOfTransformChange();
+}

physx/source/simulationcontroller/src/ScStaticSim.h

@@ -0,0 +1,57 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef PX_PHYSICS_SCP_STATIC_SIM
+#define PX_PHYSICS_SCP_STATIC_SIM
+
+#include "ScRigidSim.h"
+#include "ScStaticCore.h"
+
+namespace physx
+{
+namespace Sc
+{
+	class StaticSim : public RigidSim
+	{
+		//---------------------------------------------------------------------------------
+		// Construction, destruction & initialization
+		//---------------------------------------------------------------------------------
+	public:
+									StaticSim(Scene& scene, StaticCore& core) : RigidSim(scene, core)	{}
+									~StaticSim()														{ getStaticCore().setSim(NULL);	}
+
+		PX_FORCE_INLINE	StaticCore&	getStaticCore()		const											{ return static_cast<StaticCore&>(getRigidCore());	}
+	};
+
+} // namespace Sc
+
+}
+
+#endif

physx/source/simulationcontroller/src/ScTriggerInteraction.cpp

@@ -0,0 +1,135 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#include "ScTriggerInteraction.h"
+#include "ScBodySim.h"
+#include "ScNPhaseCore.h"
+
+using namespace physx;
+using namespace Sc;
+
+TriggerInteraction::TriggerInteraction(	ShapeSim& tShape, ShapeSim& oShape) :
+										ElementSimInteraction(tShape, oShape, InteractionType::eTRIGGER, InteractionFlag::eRB_ELEMENT | InteractionFlag::eFILTERABLE),
+										mFlags(PROCESS_THIS_FRAME),
+										mLastFrameHadContacts(false)
+{
+	// The PxPairFlags eNOTIFY_TOUCH_FOUND and eNOTIFY_TOUCH_LOST get stored and mixed up with internal flags. Make sure any breaking change gets noticed.
+	PX_COMPILE_TIME_ASSERT(PxPairFlag::eNOTIFY_TOUCH_FOUND < PxPairFlag::eNOTIFY_TOUCH_LOST);
+	PX_COMPILE_TIME_ASSERT((PAIR_FLAGS_MASK & PxPairFlag::eNOTIFY_TOUCH_FOUND) == PxPairFlag::eNOTIFY_TOUCH_FOUND);
+	PX_COMPILE_TIME_ASSERT((PAIR_FLAGS_MASK & PxPairFlag::eNOTIFY_TOUCH_LOST) == PxPairFlag::eNOTIFY_TOUCH_LOST);
+	PX_COMPILE_TIME_ASSERT(PxPairFlag::eNOTIFY_TOUCH_FOUND < 0xffff);
+	PX_COMPILE_TIME_ASSERT(PxPairFlag::eNOTIFY_TOUCH_LOST < 0xffff);
+	PX_COMPILE_TIME_ASSERT(LAST < 0xffff);
+
+	bool active = registerInActors();
+	Scene& scene = getScene();
+	scene.registerInteraction(this, active);
+	scene.getNPhaseCore()->registerInteraction(this);
+
+	PX_ASSERT(getTriggerShape().getFlags() & PxShapeFlag::eTRIGGER_SHAPE);
+	mTriggerCache.state = Gu::TRIGGER_DISJOINT;
+}
+
+TriggerInteraction::~TriggerInteraction()
+{
+	Scene& scene = getScene();
+	scene.unregisterInteraction(this);
+	scene.getNPhaseCore()->unregisterInteraction(this);
+	unregisterFromActors();
+}
+
+static bool isOneActorActive(TriggerInteraction* trigger)
+{
+	const BodySim* bodySim0 = trigger->getTriggerShape().getBodySim();
+	if(bodySim0 && bodySim0->isActive())
+	{
+		PX_ASSERT(!bodySim0->isKinematic() || bodySim0->readInternalFlag(BodySim::BF_KINEMATIC_MOVED) || 
+					bodySim0->readInternalFlag(BodySim::InternalFlags(BodySim::BF_KINEMATIC_SETTLING | BodySim::BF_KINEMATIC_SETTLING_2)));
+		return true;
+	}
+
+	const BodySim* bodySim1 = trigger->getOtherShape().getBodySim();
+	if(bodySim1 && bodySim1->isActive())
+	{
+		PX_ASSERT(!bodySim1->isKinematic() || bodySim1->readInternalFlag(BodySim::BF_KINEMATIC_MOVED) || 
+			bodySim1->readInternalFlag(BodySim::InternalFlags(BodySim::BF_KINEMATIC_SETTLING | BodySim::BF_KINEMATIC_SETTLING_2)));
+		return true;
+	}
+
+	return false;
+}
+
+//
+// Some general information about triggers and sleeping
+//
+// The goal is to avoid running overlap tests if both objects are sleeping.
+// This is an optimization for eNOTIFY_TOUCH_LOST events since the overlap state 
+// can not change if both objects are sleeping. eNOTIFY_TOUCH_FOUND should be sent nonetheless.
+// For this to work the following assumptions are made:
+// - On creation or if the pose of an actor is set, the pair will always be checked.
+// - If the scenario above does not apply, then a trigger pair can only be deactivated, if both actors are sleeping.
+// - If an overlapping actor is activated/deactivated, the trigger interaction gets notified
+//
+bool TriggerInteraction::onActivate_(void*)
+{
+	// IMPORTANT: this method can get called concurrently from multiple threads -> make sure shared resources
+	//            are protected (note: there are none at the moment but it might change)
+
+	if(!(readFlag(PROCESS_THIS_FRAME)))
+	{
+		if(isOneActorActive(this))
+		{
+			raiseInteractionFlag(InteractionFlag::eIS_ACTIVE);
+			return true;
+		}
+		else
+			return false;
+	}
+	else
+	{
+		raiseInteractionFlag(InteractionFlag::eIS_ACTIVE);
+		return true;  // newly created trigger pairs should always test for overlap, no matter the sleep state
+	}
+}
+
+bool TriggerInteraction::onDeactivate_()
+{
+	if(!readFlag(PROCESS_THIS_FRAME))
+	{
+		if(!isOneActorActive(this))
+		{
+			clearInteractionFlag(InteractionFlag::eIS_ACTIVE);
+			return true;
+		}
+		else
+			return false;
+	}
+	else
+		return false;
+}

physx/source/simulationcontroller/src/ScTriggerInteraction.h

@@ -0,0 +1,122 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#ifndef PX_COLLISION_TRIGGERINTERACTION
+#define PX_COLLISION_TRIGGERINTERACTION
+
+#include "ScElementSimInteraction.h"
+#include "ScShapeSim.h"
+#include "GuOverlapTests.h"
+
+namespace physx
+{
+namespace Sc
+{
+	class TriggerInteraction : public ElementSimInteraction
+	{
+	public:
+		enum TriggerFlag
+		{
+			PAIR_FLAGS_MASK					= ((PxPairFlag::eNOTIFY_TOUCH_LOST << 1) - 1),	// Bits where the PxPairFlags eNOTIFY_TOUCH_FOUND and eNOTIFY_TOUCH_LOST get stored
+			NEXT_FREE						= ((PAIR_FLAGS_MASK << 1) & ~PAIR_FLAGS_MASK),
+
+			PROCESS_THIS_FRAME				= (NEXT_FREE << 0), // the trigger pair is new or the pose of an actor was set -> initial processing required.
+																// This is important to cover cases where a static or kinematic
+																// (non-moving) trigger is created and overlaps with a sleeping
+																// object. Or for the case where a static/kinematic is teleported to a new
+																// location. TOUCH_FOUND should still get sent in that case.
+			LAST							= (NEXT_FREE << 1)
+		};
+
+											TriggerInteraction(ShapeSim& triggerShape, ShapeSim& otherShape);
+											~TriggerInteraction();
+
+		PX_FORCE_INLINE	Gu::TriggerCache&	getTriggerCache()									{ return mTriggerCache;							}
+		PX_FORCE_INLINE	ShapeSim&			getTriggerShape()							const	{ return static_cast<ShapeSim&>(getElement0());	}
+		PX_FORCE_INLINE	ShapeSim&			getOtherShape()								const	{ return static_cast<ShapeSim&>(getElement1());	}
+
+		PX_FORCE_INLINE bool				lastFrameHadContacts()						const	{ return mLastFrameHadContacts;			}
+		PX_FORCE_INLINE void				updateLastFrameHadContacts(bool hasContact)			{ mLastFrameHadContacts = hasContact;	}
+
+		PX_FORCE_INLINE PxPairFlags			getTriggerFlags()							const	{ return PxPairFlags(PxU32(mFlags) & PAIR_FLAGS_MASK);		}
+		PX_FORCE_INLINE void				setTriggerFlags(PxPairFlags triggerFlags);
+
+		PX_FORCE_INLINE void				raiseFlag(TriggerFlag flag)				{ mFlags |= flag; }
+		PX_FORCE_INLINE void				clearFlag(TriggerFlag flag)				{ mFlags &= ~flag; }
+		PX_FORCE_INLINE	Ps::IntBool			readFlag(TriggerFlag flag)		const	{ return Ps::IntBool(mFlags & flag); }
+
+		PX_FORCE_INLINE void				forceProcessingThisFrame(Sc::Scene& scene);
+
+						bool				onActivate_(void*);
+						bool				onDeactivate_();
+
+	protected:
+						Gu::TriggerCache	mTriggerCache;
+						PxU16				mFlags;
+						bool				mLastFrameHadContacts;
+	};
+
+} // namespace Sc
+
+
+PX_FORCE_INLINE void Sc::TriggerInteraction::setTriggerFlags(PxPairFlags triggerFlags)
+{
+	PX_ASSERT(PxU32(triggerFlags) < (PxPairFlag::eDETECT_CCD_CONTACT << 1));  // to find out if a new PxPairFlag has been added in which case PAIR_FLAGS_MASK needs to get adjusted
+
+#if PX_CHECKED
+	if (triggerFlags & PxPairFlag::eNOTIFY_TOUCH_PERSISTS)
+	{
+		PX_WARN_ONCE("Trigger pairs do not support PxPairFlag::eNOTIFY_TOUCH_PERSISTS events any longer.");
+	}
+#endif
+
+	PxU32 newFlags = mFlags;
+	PxU32 fl = PxU32(triggerFlags) & PxU32(PxPairFlag::eNOTIFY_TOUCH_FOUND|PxPairFlag::eNOTIFY_TOUCH_LOST);
+	newFlags &= (~PAIR_FLAGS_MASK);  // clear old flags
+	newFlags |= fl;
+
+	mFlags = PxU16(newFlags);
+}
+
+
+PX_FORCE_INLINE void Sc::TriggerInteraction::forceProcessingThisFrame(Sc::Scene& scene)
+{
+	raiseFlag(PROCESS_THIS_FRAME);
+
+	if (!readInteractionFlag(InteractionFlag::eIS_ACTIVE))
+	{
+		raiseInteractionFlag(InteractionFlag::eIS_ACTIVE);
+		scene.notifyInteractionActivated(this);
+	}
+}
+
+}
+
+#endif

physx/source/simulationcontroller/src/ScTriggerPairs.h

@@ -0,0 +1,95 @@
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//  * Neither the name of NVIDIA CORPORATION nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
+// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef PX_PHYSICS_SCP_TRIGGER_PAIRS
+#define PX_PHYSICS_SCP_TRIGGER_PAIRS
+
+#include "PsArray.h"
+#include "CmPhysXCommon.h"
+#include "PxFiltering.h"
+#include "PxClient.h"
+#include "PxSimulationEventCallback.h"
+
+namespace physx
+{
+class PxShape;
+
+namespace Sc
+{
+	struct TriggerPairFlag
+	{
+		enum Enum
+		{
+			eTEST_FOR_REMOVED_SHAPES = PxTriggerPairFlag::eNEXT_FREE	// for cases where the pair got deleted because one of the shape volumes got removed from broadphase.
+																		// This covers scenarios like volume re-insertion into broadphase as well since the shape might get removed
+																		// after such an operation. The scenarios to consider are:
+																		//
+																		// - shape gets removed (this includes raising PxActorFlag::eDISABLE_SIMULATION)
+																		// - shape switches to eSCENE_QUERY_SHAPE only
+																		// - shape switches to eSIMULATION_SHAPE
+																		// - resetFiltering()
+																		// - actor gets removed from an aggregate
+		};
+	};
+
+	PX_COMPILE_TIME_ASSERT((1 << (8*sizeof(PxTriggerPairFlags::InternalType))) > TriggerPairFlag::eTEST_FOR_REMOVED_SHAPES);
+
+	struct TriggerPairExtraData
+	{
+		PX_INLINE TriggerPairExtraData() : 
+			shape0ID(0xffffffff),
+			shape1ID(0xffffffff),
+			client0ID(0xff),
+			client1ID(0xff)
+		{
+		}
+
+		PX_INLINE TriggerPairExtraData(PxU32 s0ID, PxU32 s1ID,
+										PxClientID cl0ID, PxClientID cl1ID) : 
+			shape0ID(s0ID),
+			shape1ID(s1ID),
+			client0ID(cl0ID),
+			client1ID(cl1ID)
+		{
+		}
+
+		PxU32						shape0ID;
+		PxU32						shape1ID;
+		PxClientID					client0ID;
+		PxClientID					client1ID;
+	};
+
+	typedef	Ps::Array<TriggerPairExtraData>	TriggerBufferExtraData;
+	typedef	Ps::Array<PxTriggerPair>		TriggerBufferAPI;
+
+} // namespace Sc
+
+}
+
+#endif