//
// 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-2019 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 "PxImmediateMode.h"
#include "../../lowleveldynamics/src/DyBodyCoreIntegrator.h"
#include "../../lowleveldynamics/src/DyContactPrep.h"
#include "../../lowleveldynamics/src/DyCorrelationBuffer.h"
#include "../../lowleveldynamics/src/DyConstraintPrep.h"
#include "../../lowleveldynamics/src/DySolverControl.h"
#include "../../lowleveldynamics/src/DySolverContext.h"
#include "../../lowlevel/common/include/collision/PxcContactMethodImpl.h"
#include "../../lowleveldynamics/src/DyTGSContactPrep.h"
#include "GuPersistentContactManifold.h"
#include "NpConstraint.h"

// PT: just for Dy::DY_ARTICULATION_MAX_SIZE
#include "../../lowleveldynamics/include/DyFeatherstoneArticulation.h"

#include "../../lowlevel/common/include/utils/PxcScratchAllocator.h"

using namespace physx;
using namespace Dy;
using namespace immediate;

void immediate::PxConstructSolverBodies(const PxRigidBodyData* inRigidData, PxSolverBodyData* outSolverBodyData, const PxU32 nbBodies, const PxVec3& gravity, const PxReal dt)
{
	for(PxU32 a=0; a<nbBodies; a++)
	{
		const PxRigidBodyData& rigidData = inRigidData[a];
		PxVec3 lv = rigidData.linearVelocity, av = rigidData.angularVelocity;
		Dy::bodyCoreComputeUnconstrainedVelocity(gravity, dt, rigidData.linearDamping, rigidData.angularDamping, 1.0f, rigidData.maxLinearVelocitySq, rigidData.maxAngularVelocitySq, lv, av, false);
		Dy::copyToSolverBodyData(lv, av, rigidData.invMass, rigidData.invInertia, rigidData.body2World, -rigidData.maxDepenetrationVelocity, rigidData.maxContactImpulse, IG_INVALID_NODE, PX_MAX_F32, outSolverBodyData[a], 0);
	}
}

void immediate::PxConstructStaticSolverBody(const PxTransform& globalPose, PxSolverBodyData& solverBodyData)
{
	const PxVec3 zero(0.0f);
	Dy::copyToSolverBodyData(zero, zero, 0.f, zero, globalPose, -PX_MAX_F32, PX_MAX_F32, IG_INVALID_NODE, PX_MAX_F32, solverBodyData, 0);
}

void immediate::PxIntegrateSolverBodies(PxSolverBodyData* solverBodyData, PxSolverBody* solverBody, const PxVec3* linearMotionVelocity, const PxVec3* angularMotionState, const PxU32 nbBodiesToIntegrate, const PxReal dt)
{
	for (PxU32 i = 0; i < nbBodiesToIntegrate; ++i)
	{
		PxVec3 lmv = linearMotionVelocity[i];
		PxVec3 amv = angularMotionState[i];
		Dy::integrateCore(lmv, amv, solverBody[i], solverBodyData[i], dt);
	}
}

namespace
{
	// PT: this class is makes it possible to call the FeatherstoneArticulation protected functions from here.
	class immArticulation : public FeatherstoneArticulation
	{
		public:
														immArticulation(const PxFeatherstoneArticulationData& data);
														~immArticulation();

		PX_FORCE_INLINE	void							immSolveInternalConstraints(PxReal dt, PxReal invDt, Cm::SpatialVectorF* impulses, Cm::SpatialVectorF* DeltaV, const PxReal elapsedTime, bool velocityIteration, bool isTGS)
														{
															FeatherstoneArticulation::solveInternalConstraints(dt, invDt, impulses, DeltaV, velocityIteration, isTGS, elapsedTime);
														}

		PX_FORCE_INLINE	void							immComputeUnconstrainedVelocitiesTGS(PxReal dt, PxReal totalDt, PxReal invDt, PxReal invTotalDt, const PxVec3& gravity)
														{
															PX_UNUSED(invTotalDt);
															PX_UNUSED(totalDt);
															PX_UNUSED(dt);
															mArticulationData.setDt(totalDt);

															Cm::SpatialVectorF Z[DY_ARTICULATION_MAX_SIZE];
															Cm::SpatialVectorF deltaV[DY_ARTICULATION_MAX_SIZE];

															FeatherstoneArticulation::computeUnconstrainedVelocitiesInternal(gravity, Z, deltaV);

															setupInternalConstraints(mArticulationData.getLinks(), mArticulationData.getLinkCount(), 
																mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE, mArticulationData, Z, dt, totalDt, invDt, 0.7f, true);
														}

		PX_FORCE_INLINE	void							immComputeUnconstrainedVelocities(PxReal dt, const PxVec3& gravity)
		{
														mArticulationData.setDt(dt);

														Cm::SpatialVectorF Z[DY_ARTICULATION_MAX_SIZE];
														Cm::SpatialVectorF deltaV[DY_ARTICULATION_MAX_SIZE];

														FeatherstoneArticulation::computeUnconstrainedVelocitiesInternal(gravity, Z, deltaV);
														const PxReal invDt = 1.f/dt;
														setupInternalConstraints(mArticulationData.getLinks(), mArticulationData.getLinkCount(),
															mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE, mArticulationData, Z, dt, dt, invDt, 1.f, false);
		}

						PxU32							addLink(const PxFeatherstoneArticulationLinkData& data);
			
						void							complete();

						// PT: TODO: why do we have a Ps::Array here if we're limited to DY_ARTICULATION_MAX_SIZE?
						Ps::Array<Dy::ArticulationLink>	mLinks;
						// No dynamic array because we want to store the pointer to these items in ArticulationLink, and we don't want to deal with remapping
						PxsBodyCore						mBodyCores[DY_ARTICULATION_MAX_SIZE];
						Dy::ArticulationJointCore		mArticulationJointCore[DY_ARTICULATION_MAX_SIZE];
						PxArticulationFlags				mFlags;	// PT: PX-1399. Stored in Dy::ArticulationCore for retained mode.

						bool							mImmDirty;
		private:
						void							initJointCore(Dy::ArticulationJointCore& core, const PxFeatherstoneArticulationLinkData& data);
	};

#define MAX_NUM_PARTITIONS 32u

	class RigidBodyClassification
	{
		PX_NOCOPY(RigidBodyClassification)
		PxU8* PX_RESTRICT mBodies;
		const PxU32 mBodySize;
		const PxU32 mBodyStride;
		const PxU32 mBodyCount;

	public:
		RigidBodyClassification(PxU8* PX_RESTRICT bodies, PxU32 bodyCount, PxU32 bodyStride) : mBodies(bodies), mBodySize(bodyCount*bodyStride), mBodyStride(bodyStride), mBodyCount(bodyCount)
		{
		}

		//Returns true if it is a dynamic-dynamic constraint; false if it is a dynamic-static or dynamic-kinematic constraint
		PX_FORCE_INLINE bool classifyConstraint(const PxSolverConstraintDesc& desc, uintptr_t& indexA, uintptr_t& indexB,
			bool& activeA, bool& activeB, PxU32& bodyAProgress, PxU32& bodyBProgress) const
		{
			indexA = uintptr_t(reinterpret_cast<PxU8*>(desc.bodyA) - mBodies) / mBodyStride;
			indexB = uintptr_t(reinterpret_cast<PxU8*>(desc.bodyB) - mBodies) / mBodyStride;
			activeA = indexA < mBodyCount;
			activeB = indexB < mBodyCount;
			bodyAProgress = desc.bodyA->solverProgress;
			bodyBProgress = desc.bodyB->solverProgress;
			return activeA && activeB;
		}

		PX_FORCE_INLINE void reserveSpaceForStaticConstraints(PxU32* numConstraintsPerPartition)
		{
			for (PxU32 a = 0; a < mBodySize; a += mBodyStride)
			{
				PxSolverBody& body = *reinterpret_cast<PxSolverBody*>(mBodies + a);
				body.solverProgress = 0;

				for (PxU32 b = 0; b < body.maxSolverFrictionProgress; ++b)
				{
					PxU32 partId = PxMin(PxU32(body.maxSolverNormalProgress + b), MAX_NUM_PARTITIONS);
					numConstraintsPerPartition[partId]++;
				}
			}
		}

		PX_FORCE_INLINE void storeProgress(const PxSolverConstraintDesc& desc, const PxU32 bodyAProgress, const PxU32 bodyBProgress,
			const PxU16 availablePartition)
		{
			desc.bodyA->solverProgress = bodyAProgress;
			desc.bodyA->maxSolverNormalProgress = PxMax(desc.bodyA->maxSolverNormalProgress, availablePartition);
			desc.bodyB->solverProgress = bodyBProgress;
			desc.bodyB->maxSolverNormalProgress = PxMax(desc.bodyB->maxSolverNormalProgress, availablePartition);
		}


		PX_FORCE_INLINE PxU32 getStaticContactWriteIndex(const PxSolverConstraintDesc& desc,
			bool activeA, bool activeB)
		{
			if (activeA)
				return PxU32(desc.bodyA->maxSolverNormalProgress + desc.bodyA->maxSolverFrictionProgress++);
			else if (activeB)
				return PxU32(desc.bodyB->maxSolverNormalProgress + desc.bodyB->maxSolverFrictionProgress++);

			return 0xffffffff;

		}

		PX_FORCE_INLINE void recordStaticConstraint(const PxSolverConstraintDesc& desc, bool& activeA, bool& activeB) const
		{
			if (activeA)
			{
				desc.bodyA->maxSolverFrictionProgress++;
			}

			if (activeB)
			{
				desc.bodyB->maxSolverFrictionProgress++;
			}
		}

		PX_FORCE_INLINE void zeroBodies()
		{
			for (PxU32 a = 0; a < mBodySize; a += mBodyStride)
			{
				PxSolverBody& body = *reinterpret_cast<PxSolverBody*>(mBodies + a);
				body.solverProgress = 0;
				body.maxSolverFrictionProgress = 0;
				body.maxSolverNormalProgress = 0;
			}
		}

		PX_FORCE_INLINE void afterClassification()
		{
			for (PxU32 a = 0; a < mBodySize; a += mBodyStride)
			{
				PxSolverBody& body = *reinterpret_cast<PxSolverBody*>(mBodies + a);
				body.solverProgress = 0;
				//Keep the dynamic constraint count but bump the static constraint count back to 0.
				//This allows us to place the static constraints in the appropriate place when we see them
				//because we know the maximum index for the dynamic constraints...
				body.maxSolverFrictionProgress = 0;
			}
		}
	};

	class ExtendedRigidBodyClassification
	{
		PX_NOCOPY(ExtendedRigidBodyClassification)

		PxU8* PX_RESTRICT mBodies;
		const PxU32 mBodyCount;
		const PxU32 mBodySize;
		const PxU32 mStride;
		Dy::ArticulationV** PX_RESTRICT mArticulations;
		const PxU32 mNumArticulations;

	public:

		ExtendedRigidBodyClassification(PxU8* PX_RESTRICT bodies, PxU32 numBodies, PxU32 stride, Dy::ArticulationV** articulations, PxU32 numArticulations)
			: mBodies(bodies), mBodyCount(numBodies), mBodySize(numBodies*stride), mStride(stride), mArticulations(articulations), mNumArticulations(numArticulations)
		{
		}

		//Returns true if it is a dynamic-dynamic constriant; false if it is a dynamic-static or dynamic-kinematic constraint
		PX_FORCE_INLINE bool classifyConstraint(const PxSolverConstraintDesc& desc, uintptr_t& indexA, uintptr_t& indexB,
			bool& activeA, bool& activeB, PxU32& bodyAProgress, PxU32& bodyBProgress) const
		{
			bool hasStatic = false;
			if (PxSolverConstraintDesc::NO_LINK == desc.linkIndexA)
			{
				indexA = uintptr_t(reinterpret_cast<PxU8*>(desc.bodyA) - mBodies) / mStride;
				activeA = indexA < mBodyCount;
				hasStatic = !activeA;//desc.bodyADataIndex == 0;
				bodyAProgress = activeA ? desc.bodyA->solverProgress : 0;
			}
			else
			{

				Dy::ArticulationV* articulationA = desc.articulationA;
				indexA = mBodyCount;
				//bodyAProgress = articulationA->getFsDataPtr()->solverProgress;
				bodyAProgress = articulationA->solverProgress;
				activeA = true;
			}
			if (PxSolverConstraintDesc::NO_LINK == desc.linkIndexB)
			{
				indexB = uintptr_t(reinterpret_cast<PxU8*>(desc.bodyB) - mBodies) / mStride;
				activeB = indexB < mBodyCount;
				hasStatic = hasStatic || !activeB;
				bodyBProgress = activeB ? desc.bodyB->solverProgress : 0;
			}
			else
			{
				Dy::ArticulationV* articulationB = desc.articulationB;
				indexB = mBodyCount;
				activeB = true;
				bodyBProgress = articulationB->solverProgress;
			}
			return !hasStatic;
		}

		PX_FORCE_INLINE void recordStaticConstraint(const PxSolverConstraintDesc& desc, bool& activeA, bool& activeB)
		{
			if (activeA)
			{
				if (PxSolverConstraintDesc::NO_LINK == desc.linkIndexA)
					desc.bodyA->maxSolverFrictionProgress++;
				else
				{
					Dy::ArticulationV* articulationA = desc.articulationA;
					articulationA->maxSolverFrictionProgress++;
				}
			}

			if (activeB)
			{
				if (PxSolverConstraintDesc::NO_LINK == desc.linkIndexB)
					desc.bodyB->maxSolverFrictionProgress++;
				else
				{
					Dy::ArticulationV* articulationB = desc.articulationB;
					articulationB->maxSolverFrictionProgress++;
				}
			}
		}

		PX_FORCE_INLINE PxU32 getStaticContactWriteIndex(const PxSolverConstraintDesc& desc,
			bool activeA, bool activeB)
		{
			if (activeA)
			{
				if (PxSolverConstraintDesc::NO_LINK == desc.linkIndexA)
				{
					return PxU32(desc.bodyA->maxSolverNormalProgress + desc.bodyA->maxSolverFrictionProgress++);
				}
				else
				{
					Dy::ArticulationV* articulationA = desc.articulationA;
					return PxU32(articulationA->maxSolverNormalProgress + articulationA->maxSolverFrictionProgress++);
				}
			}
			else if (activeB)
			{
				if (PxSolverConstraintDesc::NO_LINK == desc.linkIndexB)
				{
					return PxU32(desc.bodyB->maxSolverNormalProgress + desc.bodyB->maxSolverFrictionProgress++);
				}
				else
				{
					Dy::ArticulationV* articulationB = desc.articulationB;
					return PxU32(articulationB->maxSolverNormalProgress + articulationB->maxSolverFrictionProgress++);
				}
			}

			return 0xffffffff;
		}

		PX_FORCE_INLINE void storeProgress(const PxSolverConstraintDesc& desc, const PxU32 bodyAProgress, const PxU32 bodyBProgress,
			const PxU16 availablePartition)
		{
			if (PxSolverConstraintDesc::NO_LINK == desc.linkIndexA)
			{
				desc.bodyA->solverProgress = bodyAProgress;
				desc.bodyA->maxSolverNormalProgress = PxMax(desc.bodyA->maxSolverNormalProgress, availablePartition);
			}
			else
			{
				Dy::ArticulationV* articulationA = desc.articulationA;
				articulationA->solverProgress = bodyAProgress;
				articulationA->maxSolverNormalProgress = PxMax(articulationA->maxSolverNormalProgress, availablePartition);
			}

			if (PxSolverConstraintDesc::NO_LINK == desc.linkIndexB)
			{
				desc.bodyB->solverProgress = bodyBProgress;
				desc.bodyB->maxSolverNormalProgress = PxMax(desc.bodyB->maxSolverNormalProgress, availablePartition);
			}
			else
			{
				Dy::ArticulationV* articulationB = desc.articulationB;
				articulationB->solverProgress = bodyBProgress;
				articulationB->maxSolverNormalProgress = PxMax(articulationB->maxSolverNormalProgress, availablePartition);
			}
		}

		PX_FORCE_INLINE void reserveSpaceForStaticConstraints(PxU32* numConstraintsPerPartition)
		{
			for (PxU32 a = 0; a < mBodySize; a += mStride)
			{
				PxSolverBody& body = *reinterpret_cast<PxSolverBody*>(mBodies + a);
				body.solverProgress = 0;

				for (PxU32 b = 0; b < body.maxSolverFrictionProgress; ++b)
				{
					PxU32 partId = PxMin(PxU32(body.maxSolverNormalProgress + b), MAX_NUM_PARTITIONS);
					numConstraintsPerPartition[partId]++;
				}
			}

			for (PxU32 a = 0; a < mNumArticulations; ++a)
			{
				Dy::ArticulationV* articulation = mArticulations[a];
				articulation->solverProgress = 0;

				for (PxU32 b = 0; b < articulation->maxSolverFrictionProgress; ++b)
				{
					PxU32 partId = PxMin(PxU32(articulation->maxSolverNormalProgress + b), MAX_NUM_PARTITIONS);
					numConstraintsPerPartition[partId]++;
				}
			}
		}

		PX_FORCE_INLINE void zeroBodies()
		{
			for (PxU32 a = 0; a < mBodySize; a += mStride)
			{
				PxSolverBody& body = *reinterpret_cast<PxSolverBody*>(mBodies + a);
				body.solverProgress = 0;
				body.maxSolverFrictionProgress = 0;
				body.maxSolverNormalProgress = 0;
			}

			for (PxU32 a = 0; a < mNumArticulations; ++a)
			{
				Dy::ArticulationV* articulation = mArticulations[a];
				articulation->solverProgress = 0;
				articulation->maxSolverFrictionProgress = 0;
				articulation->maxSolverNormalProgress = 0;
			}
		}

		PX_FORCE_INLINE void afterClassification()
		{
			for (PxU32 a = 0; a < mBodySize; a += mStride)
			{
				PxSolverBody& body = *reinterpret_cast<PxSolverBody*>(mBodies + a);
				body.solverProgress = 0;
				//Keep the dynamic constraint count but bump the static constraint count back to 0.
				//This allows us to place the static constraints in the appropriate place when we see them
				//because we know the maximum index for the dynamic constraints...
				body.maxSolverFrictionProgress = 0;
			}

			for (PxU32 a = 0; a < mNumArticulations; ++a)
			{
				Dy::ArticulationV* articulation = mArticulations[a];
				articulation->solverProgress = 0;
				articulation->maxSolverFrictionProgress = 0;
			}
		}
	};

	template <typename Classification>
	void classifyConstraintDesc(const PxSolverConstraintDesc* PX_RESTRICT descs, const PxU32 numConstraints, Classification& classification,
		PxU32* numConstraintsPerPartition)
	{
		const PxSolverConstraintDesc* _desc = descs;
		const PxU32 numConstraintsMin1 = numConstraints - 1;

		PxMemZero(numConstraintsPerPartition, sizeof(PxU32) * (MAX_NUM_PARTITIONS+1));

		for (PxU32 i = 0; i < numConstraints; ++i, _desc++)
		{
			const PxU32 prefetchOffset = PxMin(numConstraintsMin1 - i, 4u);
			Ps::prefetchLine(_desc[prefetchOffset].constraint);
			Ps::prefetchLine(_desc[prefetchOffset].bodyA);
			Ps::prefetchLine(_desc[prefetchOffset].bodyB);
			Ps::prefetchLine(_desc + 8);

			uintptr_t indexA, indexB;
			bool activeA, activeB;

			PxU32 partitionsA, partitionsB;
			const bool notContainsStatic = classification.classifyConstraint(*_desc, indexA, indexB, activeA, activeB,
				partitionsA, partitionsB);

			if (notContainsStatic)
			{
				PxU32 availablePartition;
				{
					const PxU32 combinedMask = (~partitionsA & ~partitionsB);
					availablePartition = combinedMask == 0 ? MAX_NUM_PARTITIONS : Ps::lowestSetBit(combinedMask);
					if (availablePartition == MAX_NUM_PARTITIONS)
					{
						//Write to overflow partition...
						numConstraintsPerPartition[availablePartition]++;
						classification.storeProgress(*_desc, partitionsA, partitionsB, PxU16(MAX_NUM_PARTITIONS));
						continue;
					}

					const PxU32 partitionBit = (1u << availablePartition);
					partitionsA |= partitionBit;
					partitionsB |= partitionBit;
				}

				numConstraintsPerPartition[availablePartition]++;
				availablePartition++;
				classification.storeProgress(*_desc, partitionsA, partitionsB, PxU16(availablePartition));
			}
			else
			{
				//Just count the number of static constraints and store in maxSolverFrictionProgress...
				classification.recordStaticConstraint(*_desc, activeA, activeB);
			}
		}

		classification.reserveSpaceForStaticConstraints(numConstraintsPerPartition);
	}

	template <typename Classification>
	void writeConstraintDesc(const PxSolverConstraintDesc* PX_RESTRICT descs, const PxU32 numConstraints, Classification& classification,
		PxU32* accumulatedConstraintsPerPartition, PxSolverConstraintDesc* PX_RESTRICT eaOrderedConstraintDesc)
	{
		const PxSolverConstraintDesc* _desc = descs;
		const PxU32 numConstraintsMin1 = numConstraints - 1;
		for (PxU32 i = 0; i < numConstraints; ++i, _desc++)
		{
			const PxU32 prefetchOffset = PxMin(numConstraintsMin1 - i, 4u);
			Ps::prefetchLine(_desc[prefetchOffset].constraint);
			Ps::prefetchLine(_desc[prefetchOffset].bodyA);
			Ps::prefetchLine(_desc[prefetchOffset].bodyB);
			Ps::prefetchLine(_desc + 8);

			uintptr_t indexA, indexB;
			bool activeA, activeB;
			PxU32 partitionsA, partitionsB;
			const bool notContainsStatic = classification.classifyConstraint(*_desc, indexA, indexB, activeA, activeB,
				partitionsA, partitionsB);

			if (notContainsStatic)
			{
				PxU32 availablePartition;
				{
					const PxU32 combinedMask = (~partitionsA & ~partitionsB);
					availablePartition = combinedMask == 0 ? MAX_NUM_PARTITIONS : Ps::lowestSetBit(combinedMask);
					if (availablePartition == MAX_NUM_PARTITIONS)
					{
						eaOrderedConstraintDesc[accumulatedConstraintsPerPartition[availablePartition]++] = *_desc;
						continue;
					}

					const PxU32 partitionBit = (1u << availablePartition);

					partitionsA |= partitionBit;
					partitionsB |= partitionBit;
				}

				classification.storeProgress(*_desc, partitionsA, partitionsB, PxU16(availablePartition+1));

				eaOrderedConstraintDesc[accumulatedConstraintsPerPartition[availablePartition]++] = *_desc;
			}
			else
			{
				//Just count the number of static constraints and store in maxSolverFrictionProgress...
				//KS - TODO - handle registration of static contacts with articulations here!
				PxU32 index = classification.getStaticContactWriteIndex(*_desc, activeA, activeB);
				eaOrderedConstraintDesc[accumulatedConstraintsPerPartition[index]++] = *_desc;
			}
		}
	}

	PX_FORCE_INLINE bool isArticulationConstraint(const PxSolverConstraintDesc& desc)
	{
		return desc.linkIndexA != PxSolverConstraintDesc::NO_LINK || desc.linkIndexB != PxSolverConstraintDesc::NO_LINK;
	}

	template <typename Classification>
	PxU32 BatchConstraints(const PxSolverConstraintDesc* solverConstraintDescs, const PxU32 nbConstraints, PxConstraintBatchHeader* outBatchHeaders, 
		PxSolverConstraintDesc* outOrderedConstraintDescs, Classification& classification)
	{
		PxU32 constraintsPerPartition[MAX_NUM_PARTITIONS + 1];

		classification.zeroBodies();

		classifyConstraintDesc(solverConstraintDescs, nbConstraints, classification, constraintsPerPartition);

		PxU32 accumulation = 0;
		for (PxU32 a = 0; a < MAX_NUM_PARTITIONS + 1; ++a)
		{
			PxU32 count = constraintsPerPartition[a];
			constraintsPerPartition[a] = accumulation;
			accumulation += count;
		}

		classification.afterClassification();

		writeConstraintDesc(solverConstraintDescs, nbConstraints, classification, constraintsPerPartition, outOrderedConstraintDescs);

		PxU32 numHeaders = 0;
		PxU32 currentPartition = 0;
		PxU32 maxJ = nbConstraints == 0 ? 0 : constraintsPerPartition[0];

		const PxU32 maxBatchPartition = MAX_NUM_PARTITIONS;
		for (PxU32 a = 0; a < nbConstraints;)
		{
			PxConstraintBatchHeader& header = outBatchHeaders[numHeaders++];
			header.startIndex = a;

			PxU32 loopMax = PxMin(maxJ - a, 4u);
			PxU16 j = 0;
			if (loopMax > 0)
			{
				j = 1;
				PxSolverConstraintDesc& desc = outOrderedConstraintDescs[a];

				if(isArticulationConstraint(desc))
					loopMax = 1;

				if (currentPartition < maxBatchPartition)
				{
					for (; j < loopMax && desc.constraintLengthOver16 == outOrderedConstraintDescs[a + j].constraintLengthOver16
						&& !isArticulationConstraint(outOrderedConstraintDescs[a + j]); ++j);
				}
				header.stride = j;
				header.constraintType = desc.constraintLengthOver16;
			}
			if (maxJ == (a + j) && maxJ != nbConstraints)
			{
				currentPartition++;
				maxJ = constraintsPerPartition[currentPartition];
			}
			a += j;
		}
		return numHeaders;
	}
}

PxU32 immediate::PxBatchConstraints(const PxSolverConstraintDesc* solverConstraintDescs, const PxU32 nbConstraints, PxSolverBody* solverBodies, const PxU32 nbBodies,
									PxConstraintBatchHeader* outBatchHeaders, PxSolverConstraintDesc* outOrderedConstraintDescs,
									Dy::ArticulationV** articulations, const PxU32 nbArticulations)
{
	if(!nbArticulations)
	{
		RigidBodyClassification classification(reinterpret_cast<PxU8*>(solverBodies), nbBodies, sizeof(PxSolverBody));
		return BatchConstraints(solverConstraintDescs, nbConstraints, outBatchHeaders, outOrderedConstraintDescs, classification);
	}
	else
	{
		ExtendedRigidBodyClassification classification(reinterpret_cast<PxU8*>(solverBodies), nbBodies, sizeof(PxSolverBody), articulations, nbArticulations);
		return BatchConstraints(solverConstraintDescs, nbConstraints, outBatchHeaders, outOrderedConstraintDescs, classification);
	}
}

bool immediate::PxCreateContactConstraints(PxConstraintBatchHeader* batchHeaders, const PxU32 nbHeaders, PxSolverContactDesc* contactDescs,
	PxConstraintAllocator& allocator, const PxReal invDt, const PxReal bounceThreshold, const PxReal frictionOffsetThreshold, const PxReal correlationDistance)
{
	PX_ASSERT(invDt > 0.f && PxIsFinite(invDt));
	PX_ASSERT(bounceThreshold < 0.f);
	PX_ASSERT(frictionOffsetThreshold > 0.f);
	PX_ASSERT(correlationDistance > 0.f);

	Dy::SolverConstraintPrepState::Enum state = Dy::SolverConstraintPrepState::eUNBATCHABLE;

	Dy::CorrelationBuffer cb;

	PxU32 currentContactDescIdx = 0;

	for (PxU32 i = 0; i < nbHeaders; ++i)
	{
		PxConstraintBatchHeader& batchHeader = batchHeaders[i];
		if (batchHeader.stride == 4)
		{
			PxU32 totalContacts = contactDescs[currentContactDescIdx].numContacts + contactDescs[currentContactDescIdx + 1].numContacts + 
				contactDescs[currentContactDescIdx + 2].numContacts + contactDescs[currentContactDescIdx + 3].numContacts;

			if (totalContacts <= 64)
			{
				state = Dy::createFinalizeSolverContacts4(cb,
					contactDescs + currentContactDescIdx,
					invDt,
					bounceThreshold,
					frictionOffsetThreshold,
					correlationDistance,
					0.f,
					allocator);
			}
		}

		if (state == Dy::SolverConstraintPrepState::eUNBATCHABLE)
		{
			Cm::SpatialVectorF Z[Dy::DY_ARTICULATION_MAX_SIZE];
			for (PxU32 a = 0; a < batchHeader.stride; ++a)
			{
				Dy::createFinalizeSolverContacts(contactDescs[currentContactDescIdx + a], cb, invDt, bounceThreshold, 
					frictionOffsetThreshold, correlationDistance, 0.f, allocator, Z);
			}
		}
		PxU8 type = *contactDescs[currentContactDescIdx].desc->constraint;

		if (type == DY_SC_TYPE_STATIC_CONTACT)
		{
			//Check if any block of constraints is classified as type static (single) contact constraint.
			//If they are, iterate over all constraints grouped with it and switch to "dynamic" contact constraint
			//type if there's a dynamic contact constraint in the group.
			for (PxU32 c = 1; c < batchHeader.stride; ++c)
			{
				if (*contactDescs[currentContactDescIdx + c].desc->constraint == DY_SC_TYPE_RB_CONTACT)
				{
					type = DY_SC_TYPE_RB_CONTACT;
					break;
				}
			}
		}

		batchHeader.constraintType = type;

		currentContactDescIdx += batchHeader.stride;
	}
	return true;
}

bool immediate::PxCreateJointConstraints(PxConstraintBatchHeader* batchHeaders, const PxU32 nbHeaders, PxSolverConstraintPrepDesc* jointDescs, PxConstraintAllocator& allocator, const PxReal dt, const PxReal invDt)
{
	PX_ASSERT(dt > 0.f);
	PX_ASSERT(invDt > 0.f && PxIsFinite(invDt));

	Dy::SolverConstraintPrepState::Enum state = Dy::SolverConstraintPrepState::eUNBATCHABLE; 
		
	PxU32 currentDescIdx = 0;
	for (PxU32 i = 0; i < nbHeaders; ++i)
	{
		PxConstraintBatchHeader& batchHeader = batchHeaders[i];

		PxU8 type = DY_SC_TYPE_BLOCK_1D;
		if (batchHeader.stride == 4)
		{
			PxU32 totalRows = 0;
			PxU32 maxRows = 0;
			bool batchable = true;
			for (PxU32 a = 0; a < batchHeader.stride; ++a)
			{
				if (jointDescs[currentDescIdx + a].numRows == 0)
				{
					batchable = false;
					break;
				}
				totalRows += jointDescs[currentDescIdx + a].numRows;
				maxRows = PxMax(maxRows, jointDescs[currentDescIdx + a].numRows);
			}

			if (batchable)
			{
				state = Dy::setupSolverConstraint4
					(jointDescs + currentDescIdx,
					dt, invDt, totalRows,
					allocator, maxRows);
			}
		}

		if (state == Dy::SolverConstraintPrepState::eUNBATCHABLE)
		{
			type = DY_SC_TYPE_RB_1D;
			Cm::SpatialVectorF Z[Dy::DY_ARTICULATION_MAX_SIZE];
			for (PxU32 a = 0; a < batchHeader.stride; ++a)
			{
				// PT: TODO: And "isExtended" is already computed in Dy::ConstraintHelper::setupSolverConstraint
				PxSolverConstraintDesc& desc = *jointDescs[currentDescIdx + a].desc;
				const bool isExtended = desc.linkIndexA != PxSolverConstraintDesc::NO_LINK || desc.linkIndexB != PxSolverConstraintDesc::NO_LINK;
				if(isExtended)
					type = DY_SC_TYPE_EXT_1D;

				Dy::ConstraintHelper::setupSolverConstraint(jointDescs[currentDescIdx + a], allocator, dt, invDt, Z);
			}
		}

		batchHeader.constraintType = type;
		currentDescIdx += batchHeader.stride;
	}

	return true;
}

template<class LeafTestT, class ParamsT>
static bool PxCreateJointConstraintsWithShadersT(PxConstraintBatchHeader* batchHeaders, const PxU32 nbHeaders, ParamsT* params, PxSolverConstraintPrepDesc* jointDescs,
	PxConstraintAllocator& allocator, const PxReal dt, const PxReal invDt)
{
	Px1DConstraint allRows[Dy::MAX_CONSTRAINT_ROWS * 4];

	//Runs shaders to fill in rows...

	PxU32 currentDescIdx = 0;

	for(PxU32 i=0; i<nbHeaders; i++)
	{
		PxU32 numRows = 0;
		PxU32 preppedIndex = 0;
		PxU32 maxRows = 0;

		PxConstraintBatchHeader& batchHeader = batchHeaders[i];

		for(PxU32 a=0; a<batchHeader.stride; a++)
		{
			Px1DConstraint* rows = allRows + numRows;
			PxSolverConstraintPrepDesc& desc = jointDescs[currentDescIdx + a];

			PxConstraintSolverPrep prep;
			const void* constantBlock;
			const bool useExtendedLimits = LeafTestT::getData(params, currentDescIdx + a, &prep, &constantBlock);
			PX_ASSERT(prep);

			PxMemZero(rows + preppedIndex, sizeof(Px1DConstraint)*(Dy::MAX_CONSTRAINT_ROWS));
			for (PxU32 b = preppedIndex; b < Dy::MAX_CONSTRAINT_ROWS; ++b)
			{
				Px1DConstraint& c = rows[b];
				//Px1DConstraintInit(c);
				c.minImpulse = -PX_MAX_REAL;
				c.maxImpulse = PX_MAX_REAL;
			}

			desc.invMassScales.linear0 = desc.invMassScales.linear1 = desc.invMassScales.angular0 = desc.invMassScales.angular1 = 1.0f;

			desc.body0WorldOffset = PxVec3(0.0f);

			PxVec3 unused_cA2w, unused_cB2w;
			const PxU32 constraintCount = prep(rows,
				desc.body0WorldOffset,
				Dy::MAX_CONSTRAINT_ROWS,
				desc.invMassScales,
				constantBlock,
				desc.bodyFrame0, desc.bodyFrame1, 
				useExtendedLimits,
				unused_cA2w, unused_cB2w);

			preppedIndex = Dy::MAX_CONSTRAINT_ROWS - constraintCount;

			maxRows = PxMax(constraintCount, maxRows);

			desc.rows = rows;
			desc.numRows = constraintCount;
			numRows += constraintCount;
		}

		PxCreateJointConstraints(&batchHeader, 1, jointDescs + currentDescIdx, allocator, dt, invDt);

		currentDescIdx += batchHeader.stride;
	}
	return true; //KS - TODO - do some error reporting/management...
}

bool immediate::PxCreateJointConstraintsWithShaders(PxConstraintBatchHeader* batchHeaders, const PxU32 nbHeaders, PxConstraint** constraints, PxSolverConstraintPrepDesc* jointDescs,
	PxConstraintAllocator& allocator, const PxReal dt, const PxReal invDt)
{
	class PxConstraintAdapter
	{
	public:
		static PX_FORCE_INLINE bool getData(PxConstraint** constraints, PxU32 i, PxConstraintSolverPrep* prep, const void** constantBlock)
		{
			NpConstraint* npConstraint = static_cast<NpConstraint*>(constraints[i]);

			npConstraint->updateConstants();

			Sc::ConstraintCore& core = npConstraint->getScbConstraint().getScConstraint();

			*prep = core.getPxConnector()->getPrep();
			*constantBlock = core.getPxConnector()->getConstantBlock();
			return core.getFlags() & PxConstraintFlag::eENABLE_EXTENDED_LIMITS;
		}
	};

	return PxCreateJointConstraintsWithShadersT<PxConstraintAdapter>(batchHeaders, nbHeaders, constraints, jointDescs, allocator, dt, invDt);
}

bool immediate::PxCreateJointConstraintsWithImmediateShaders(PxConstraintBatchHeader* batchHeaders, const PxU32 nbHeaders, PxImmediateConstraint* constraints, PxSolverConstraintPrepDesc* jointDescs,
	PxConstraintAllocator& allocator, const PxReal dt, const PxReal invDt)
{
	class immConstraintAdapter
	{
	public:
		static PX_FORCE_INLINE bool getData(PxImmediateConstraint* constraints, PxU32 i, PxConstraintSolverPrep* prep, const void** constantBlock)
		{
			const PxImmediateConstraint& ic = constraints[i];
			*prep = ic.prep;
			*constantBlock = ic.constantBlock;
			return false;
		}
	};

	return PxCreateJointConstraintsWithShadersT<immConstraintAdapter>(batchHeaders, nbHeaders, constraints, jointDescs, allocator, dt, invDt);
}

/*static*/ PX_FORCE_INLINE bool PxIsZero(const PxSolverBody* bodies, PxU32 nbBodies)
{
	for(PxU32 i=0; i<nbBodies; i++)
	{
		if(	!bodies[i].linearVelocity.isZero() ||
			!bodies[i].angularState.isZero())
			return false;
	}
	return true;
}

void immediate::PxSolveConstraints(const PxConstraintBatchHeader* batchHeaders, const PxU32 nbBatchHeaders, const PxSolverConstraintDesc* solverConstraintDescs,
	const PxSolverBody* solverBodies, PxVec3* linearMotionVelocity, PxVec3* angularMotionVelocity, const PxU32 nbSolverBodies, const PxU32 nbPositionIterations, const PxU32 nbVelocityIterations,
	const float dt, const float invDt, const PxU32 nbSolverArticulations, Dy::ArticulationV** solverArticulations)
{
	PX_ASSERT(nbPositionIterations > 0);
	PX_ASSERT(nbVelocityIterations > 0);
	PX_ASSERT(PxIsZero(solverBodies, nbSolverBodies)); //Ensure that solver body velocities have been zeroed before solving

	//Stage 1: solve the position iterations...
	Dy::SolveBlockMethod* solveTable = Dy::getSolveBlockTable();

	Dy::SolveBlockMethod* solveConcludeTable = Dy::getSolverConcludeBlockTable();

	Dy::SolveWriteBackBlockMethod* solveWritebackTable = Dy::getSolveWritebackBlockTable();

	Dy::SolverContext cache;
	cache.mThresholdStreamIndex = 0;
	cache.mThresholdStreamLength = 0xFFFFFFF;
		
	PX_ASSERT(nbPositionIterations > 0);
	PX_ASSERT(nbVelocityIterations > 0);

	Cm::SpatialVectorF Z[DY_ARTICULATION_MAX_SIZE];
	Cm::SpatialVectorF deltaV[DY_ARTICULATION_MAX_SIZE];

	cache.Z = Z;
	cache.deltaV = deltaV;

	struct Articulations
	{
		static PX_FORCE_INLINE void solveInternalConstraints(const float dt, const float invDt, const PxU32 nbSolverArticulations, Dy::ArticulationV** solverArticulations, Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV, bool velIter)
		{
			for(PxU32 a=0;a<nbSolverArticulations;a++)
			{
				immArticulation* immArt = static_cast<immArticulation*>(solverArticulations[a]);
				immArt->immSolveInternalConstraints(dt, invDt, Z, deltaV, 0.f, velIter, false);
			}
		}
	};

	for(PxU32 i=nbPositionIterations; i>1; --i)
	{
		cache.doFriction = i <= 3;
		for(PxU32 a=0; a<nbBatchHeaders; ++a)
		{
			const PxConstraintBatchHeader& batch = batchHeaders[a];
			solveTable[batch.constraintType](solverConstraintDescs + batch.startIndex, batch.stride, cache);
		}

		Articulations::solveInternalConstraints(dt, invDt, nbSolverArticulations, solverArticulations, Z, deltaV, false);
	}

	cache.doFriction = true;
	for(PxU32 a=0; a<nbBatchHeaders; ++a)
	{
		const PxConstraintBatchHeader& batch = batchHeaders[a];
		solveConcludeTable[batch.constraintType](solverConstraintDescs + batch.startIndex, batch.stride, cache);
	}
	Articulations::solveInternalConstraints(dt, invDt, nbSolverArticulations, solverArticulations, Z, deltaV, false);

	//Save motion velocities...

	for(PxU32 a=0; a<nbSolverBodies; a++)
	{
		linearMotionVelocity[a] = solverBodies[a].linearVelocity;
		angularMotionVelocity[a] = solverBodies[a].angularState;
	}

	for(PxU32 a=0;a<nbSolverArticulations;a++)
	{
		// PT: TODO: drop unusedDesc
		ArticulationSolverDesc unusedDesc;
		unusedDesc.articulation = solverArticulations[a];
		FeatherstoneArticulation::saveVelocity(unusedDesc, deltaV);
	}

	for(PxU32 i=nbVelocityIterations; i>1; --i)
	{
		for(PxU32 a=0; a<nbBatchHeaders; ++a)
		{
			const PxConstraintBatchHeader& batch = batchHeaders[a];
			solveTable[batch.constraintType](solverConstraintDescs + batch.startIndex, batch.stride, cache);
		}

		Articulations::solveInternalConstraints(dt, invDt, nbSolverArticulations, solverArticulations, Z, deltaV, true);
	}

	for(PxU32 a=0; a<nbBatchHeaders; ++a)
	{
		const PxConstraintBatchHeader& batch = batchHeaders[a];
		solveWritebackTable[batch.constraintType](solverConstraintDescs + batch.startIndex, batch.stride, cache);
	}
	Articulations::solveInternalConstraints(dt, invDt, nbSolverArticulations, solverArticulations, Z, deltaV, true);
}

static void createCache(Gu::Cache& cache, PxGeometryType::Enum geomType0, PxGeometryType::Enum geomType1, PxCacheAllocator& allocator)
{	
	if (gEnablePCMCaching[geomType0][geomType1])
	{
		if (geomType0 <= PxGeometryType::eCONVEXMESH &&
			geomType1 <= PxGeometryType::eCONVEXMESH)
		{
			if (geomType0 == PxGeometryType::eSPHERE || geomType1 == PxGeometryType::eSPHERE)
			{
				Gu::PersistentContactManifold* manifold = PX_PLACEMENT_NEW(allocator.allocateCacheData(sizeof(Gu::SpherePersistentContactManifold)), Gu::SpherePersistentContactManifold)();
				cache.setManifold(manifold);
			}
			else
			{
				Gu::PersistentContactManifold* manifold = PX_PLACEMENT_NEW(allocator.allocateCacheData(sizeof(Gu::LargePersistentContactManifold)), Gu::LargePersistentContactManifold)();
				cache.setManifold(manifold);

			}
			cache.getManifold().clearManifold();
		}
		else
		{
			//ML: raised 1 to indicate the manifold is multiManifold which is for contact gen in mesh/height field
			//cache.manifold = 1;
			cache.setMultiManifold(NULL);
		}
	}
	else
	{
		//cache.manifold =  0;
		cache.mCachedData = NULL;
		cache.mManifoldFlags = 0;
	}
}

bool immediate::PxGenerateContacts(const PxGeometry* const * geom0, const PxGeometry* const * geom1, const PxTransform* pose0, const PxTransform* pose1, PxCache* contactCache, const PxU32 nbPairs, PxContactRecorder& contactRecorder,
	const PxReal contactDistance, const PxReal meshContactMargin, const PxReal toleranceLength, PxCacheAllocator& allocator)
{
	PX_ASSERT(meshContactMargin > 0.f);
	PX_ASSERT(toleranceLength > 0.f);
	PX_ASSERT(contactDistance > 0.f);
	Gu::ContactBuffer contactBuffer;

	for (PxU32 i = 0; i < nbPairs; ++i)
	{
		contactBuffer.count = 0;
		PxGeometryType::Enum type0 = geom0[i]->getType();
		PxGeometryType::Enum type1 = geom1[i]->getType();

		const PxGeometry* tempGeom0 = geom0[i];
		const PxGeometry* tempGeom1 = geom1[i];

		PX_ALIGN(16, PxTransform) transform0 = pose0[i];
		PX_ALIGN(16, PxTransform) transform1 = pose1[i];

		const bool bSwap = type0 > type1;
		if (bSwap)
		{
			const PxGeometry* temp = tempGeom0;
			tempGeom0 = geom1[i];
			tempGeom1 = temp;
			PxGeometryType::Enum tempType = type0;
			type0 = type1;
			type1 = tempType;
			transform1 = pose0[i];
			transform0 = pose1[i];
		}

		const Gu::GeometryUnion geomUnion0(*tempGeom0);
		const Gu::GeometryUnion geomUnion1(*tempGeom1);

		//Now work out which type of PCM we need...

		Gu::Cache& cache = static_cast<Gu::Cache&>(contactCache[i]);

		bool needsMultiManifold = type1 > PxGeometryType::eCONVEXMESH;

		Gu::NarrowPhaseParams params(contactDistance, meshContactMargin, toleranceLength);

		if (needsMultiManifold)
		{
			Gu::MultiplePersistentContactManifold multiManifold;

			if (cache.isMultiManifold())
			{
				multiManifold.fromBuffer(reinterpret_cast<PxU8*>(&cache.getMultipleManifold()));
			}
			else
			{
				multiManifold.initialize();
			}
			cache.setMultiManifold(&multiManifold);

			//Do collision detection, then write manifold out...
			g_PCMContactMethodTable[type0][type1](geomUnion0, geomUnion1, transform0, transform1, params, cache, contactBuffer, NULL);

			const PxU32 size = (sizeof(Gu::MultiPersistentManifoldHeader) +
				multiManifold.mNumManifolds * sizeof(Gu::SingleManifoldHeader) +
				multiManifold.mNumTotalContacts * sizeof(Gu::CachedMeshPersistentContact));

			PxU8* buffer = allocator.allocateCacheData(size);

			multiManifold.toBuffer(buffer);

			cache.setMultiManifold(buffer);
		}
		else
		{
			//Allocate the type of manifold we need again...
			Gu::PersistentContactManifold* oldManifold = NULL;

			if (cache.isManifold())
				oldManifold = &cache.getManifold();

			//Allocates and creates the PCM...
			createCache(cache, type0, type1, allocator);

			//Copy PCM from old to new manifold...
			if (oldManifold)
			{
				Gu::PersistentContactManifold& manifold = cache.getManifold();
				manifold.mRelativeTransform = oldManifold->mRelativeTransform;
				manifold.mNumContacts = oldManifold->mNumContacts;
				manifold.mNumWarmStartPoints = oldManifold->mNumWarmStartPoints;
				manifold.mAIndice[0] = oldManifold->mAIndice[0]; manifold.mAIndice[1] = oldManifold->mAIndice[1];
				manifold.mAIndice[2] = oldManifold->mAIndice[2]; manifold.mAIndice[3] = oldManifold->mAIndice[3];
				manifold.mBIndice[0] = oldManifold->mBIndice[0]; manifold.mBIndice[1] = oldManifold->mBIndice[1];
				manifold.mBIndice[2] = oldManifold->mBIndice[2]; manifold.mBIndice[3] = oldManifold->mBIndice[3];
				PxMemCopy(manifold.mContactPoints, oldManifold->mContactPoints, sizeof(Gu::PersistentContact)*manifold.mNumContacts);
			}

			g_PCMContactMethodTable[type0][type1](geomUnion0, geomUnion1, transform0, transform1, params, cache, contactBuffer, NULL);
		}

		if (bSwap)
		{
			for (PxU32 a = 0; a < contactBuffer.count; ++a)
			{
				contactBuffer.contacts[a].normal = -contactBuffer.contacts[a].normal;
			}
		}

		if (contactBuffer.count != 0)
		{
			//Record this contact pair...
			contactRecorder.recordContacts(contactBuffer.contacts, contactBuffer.count, i);
		}
	}
	return true;
}

immArticulation::immArticulation(const PxFeatherstoneArticulationData& data) :
	FeatherstoneArticulation(this),
	mFlags					(data.flags),
	mImmDirty				(true)
{
	// From Sc::ArticulationSim ctor, not sure why it's not done in FeatherstoneArticulation ctor
	setDirty(true);

	// PT: TODO: we only need the flags here, maybe drop the solver desc?
	getSolverDesc().initData(NULL, &mFlags);
}

immArticulation::~immArticulation()
{
}

void immArticulation::initJointCore(Dy::ArticulationJointCore& core, const PxFeatherstoneArticulationLinkData& data)
{
	// From Sc::ArticulationJointCore, this should once again just be done in a Dy::ArticulationJointCore ctor
	// =====> hmmm there is a ctor, but it initializes things to different values like PxArticulationJointType::eFIX....
	// TODO: replicate ctor here
	{
		// TODO: in this one I initialized all data but I don't actually know which one is needed

		core.parentPose	= data.inboundJoint.parentPose;
		core.childPose	= data.inboundJoint.childPose;
		PX_ASSERT(core.parentPose.isValid());
		PX_ASSERT(core.childPose.isValid());
		core.dirtyFlag |= Dy::ArticulationJointCoreDirtyFlag::ePOSE;

		// TODO: is this used in the new articulations?
		core.targetPosition = PxQuat(PxIdentity);
		core.targetVelocity = PxVec3(0.f);

		core.driveType = PxArticulationJointDriveType::eTARGET;

		core.spring = 0.0f;
		core.damping = 0.0f;

		core.internalCompliance = 1.0f;
		core.externalCompliance = 1.0f;

		const PxU32* binP = reinterpret_cast<const PxU32*>(data.inboundJoint.targetPos);
		const PxU32* binV = reinterpret_cast<const PxU32*>(data.inboundJoint.targetVel);

		for(PxU32 i=0; i<PxArticulationAxis::eCOUNT; i++)
		{
			core.limits[i].low = data.inboundJoint.limits[i].low;
			core.limits[i].high = data.inboundJoint.limits[i].high;
			core.drives[i].stiffness = data.inboundJoint.drives[i].stiffness;
			core.drives[i].damping = data.inboundJoint.drives[i].damping;
			core.drives[i].maxForce = data.inboundJoint.drives[i].maxForce;
			// TODO: what about isAcceleration? missing in Sc::ArticulationJointCore
			core.drives[i].driveType = data.inboundJoint.drives[i].driveType;

			// See Sc::ArticulationJointCore::setTargetP and Sc::ArticulationJointCore::setTargetV
			core.targetP[i] = 0.f;
			core.targetV[i] = 0.f;
			if(binP[i]!=0xffffffff)
			{
				core.targetP[i] = data.inboundJoint.targetPos[i];
				core.dirtyFlag |= Dy::ArticulationJointCoreDirtyFlag::eTARGETPOSE;
			}
			if(binV[i]!=0xffffffff)
			{
				core.targetV[i] = data.inboundJoint.targetVel[i];
				core.dirtyFlag |= Dy::ArticulationJointCoreDirtyFlag::eTARGETVELOCITY;
			}
		}

		core.twistLimitContactDistance = 0.f;
		core.tanQSwingY = 0.f;
		core.tanQSwingZ = 0.f;
		core.tanQSwingPad = 0.f;

		core.tanQTwistHigh = 0.f;
		core.tanQTwistLow = 0.f;
		core.tanQTwistPad = 0.f;

		core.swingLimited = false;
		core.twistLimited = false;
		core.tangentialStiffness = 0.0f;
		core.tangentialDamping = 0.0f;

		core.frictionCoefficient = data.inboundJoint.frictionCoefficient;
		core.maxJointVelocity = data.inboundJoint.maxJointVelocity;

		core.jointType = PxU8(data.inboundJoint.type);
	}

	// from Sc::ArticulationJointCore::setMotion
	core.dirtyFlag |= Dy::ArticulationJointCoreDirtyFlag::eMOTION;
	for(PxU32 i=0; i<PxArticulationAxis::eCOUNT; i++)
		core.motion[i] = PxU8(data.inboundJoint.motion[i]);
}

PxU32 immArticulation::addLink(const PxFeatherstoneArticulationLinkData& data)
{
	PX_ASSERT(!data.parent || getArticulation(data.parent)==this);
	PX_ASSERT(data.pose.p.isFinite());
	PX_ASSERT(data.pose.q.isFinite());

	mImmDirty = true;
	setDirty(true);

	// Replicate ArticulationSim::addBody
	addBody();

	const PxU32 index = mLinks.size();

	const PxTransform& bodyPose = data.pose;
	//
	PxsBodyCore* bodyCore = &mBodyCores[index];
	{
		// PT: this function inits everything but we only need a fraction of the data there for articulations
		bodyCore->init(	bodyPose, data.inverseInertia, data.inverseMass, 0.0f, 0.0f,
						data.linearDamping, data.angularDamping,
						data.maxLinearVelocitySq, data.maxAngularVelocitySq);

		// PT: TODO: consider exposing all used data to immediate mode API (PX-1398)
//		bodyCore->maxPenBias			= -1e32f;	// <= this one is related to setMaxDepenetrationVelocity
//		bodyCore->linearVelocity		= PxVec3(0.0f);
//		bodyCore->angularVelocity		= PxVec3(0.0f);
//		bodyCore->linearVelocity		= PxVec3(0.0f, 10.0f, 0.0f);
//		bodyCore->angularVelocity		= PxVec3(0.0f, 10.0f, 0.0f);
	}

/*	PX_ASSERT((((index==0) && (joint == 0)) && (parent == 0)) ||
				(((index!=0) && joint) && (parent && (parent->getArticulation() == this))));*/

	// PT: TODO: add ctors everywhere
	ArticulationLink& link = mLinks.insert();

	// void BodySim::postActorFlagChange(PxU32 oldFlags, PxU32 newFlags)
	bodyCore->disableGravity = data.disableGravity;
	link.bodyCore	= bodyCore;
	link.children	= 0;

/*	bool shouldSleep;
	bool currentlyAsleep;
	bool bodyReadyForSleep = body.checkSleepReadinessBesidesWakeCounter();
	PxReal wakeCounter = getCore().getWakeCounter();*/

//	const PxU32 parentIndex = data.mParent;
	const PxU32 parentIndex = getLinkIndex(data.parent);
//	const bool isRoot = parentIndex==0xffffffff;
	const bool isRoot = !data.parent;
//	const bool isRoot = isArticulationRootLink(data.mParent);
	if(!isRoot)
/*	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 = &mArticulationJointCore[index];//&joint->getCore().getCore();
		mLinks[parentIndex].children |= ArticulationBitField(1)<<index;

		initJointCore(*link.inboundJoint, data);
	}
	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);
	PxU32 mMaxDepth = PxMax(depth, mMaxDepth);

	mImpl.setMaxDepth(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);*/
	return index;
}

void immArticulation::complete()
{
	// Based on Sc::ArticulationSim::checkResize()

	if(!mImmDirty)
		return;
	mImmDirty = false;

	setupLinks(mLinks.size(), const_cast<ArticulationLink*>(mLinks.begin()));
}

static bool gRegistration = false;
void immediate::PxRegisterImmediateArticulations()
{
	Dy::PxvRegisterArticulationsReducedCoordinate();
	gRegistration = true;
}

Dy::ArticulationV* immediate::PxCreateFeatherstoneArticulation(const PxFeatherstoneArticulationData& data)
{
	PX_ASSERT(gRegistration && "Please call PxRegisterImmediateArticulations() before creating immediate articulations.");
	void* memory = physx::shdfnd::AlignedAllocator<64>().allocate(sizeof(immArticulation), __FILE__, __LINE__);
	new (memory) immArticulation(data);
	return reinterpret_cast<immArticulation*>(memory);
}

void immediate::PxReleaseArticulation(Dy::ArticulationV* articulation)
{
	if(!articulation)
		return;

	immArticulation* immArt = static_cast<immArticulation*>(articulation);
	immArt->~immArticulation();
	physx::shdfnd::AlignedAllocator<64>().deallocate(articulation);
}

// PT: TODO: add test vs DY_ARTICULATION_MAX_SIZE
Dy::ArticulationLinkHandle immediate::PxAddArticulationLink(Dy::ArticulationV* articulation, const PxFeatherstoneArticulationLinkData& data, bool isLastLink)
{
	if(!articulation)
		return 0;

	immArticulation* immArt = static_cast<immArticulation*>(articulation);
	const PxU32 id = immArt->addLink(data);

	if(isLastLink)
		immArt->complete();

	PX_ASSERT(!(size_t(articulation) & 63));
	return size_t(articulation) | id;
}

PxArticulationCache* immediate::PxCreateArticulationCache(Dy::ArticulationV* articulation) 
{
	immArticulation *immArt = static_cast<immArticulation *>(articulation);
	immArt->complete();

	const PxU32 totalDofs = immArt->getDofs();
	const PxU32 jointCount = immArt->getBodyCount() - 1;

	PxU32 totalSize =
		sizeof(Cm::SpatialVector) * immArt->getBodyCount()				//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(PxArticulationRootLinkData);			

	PxU8* tCache = reinterpret_cast<PxU8*>(PX_ALLOC(totalSize, "Articulation cache"));
	PxMemZero(tCache, totalSize);

	PxArticulationCache* cache = reinterpret_cast<PxArticulationCache*>(tCache);

	PxU32 offset = sizeof(PxArticulationCache);
	cache->externalForces = reinterpret_cast<PxSpatialForce*>(tCache + offset);
	offset += sizeof(PxSpatialForce) * immArt->getBodyCount();
	
	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->rootLinkData = reinterpret_cast<PxArticulationRootLinkData*>(tCache + offset);

	cache->coefficientMatrix = NULL;
	cache->lambda =NULL;

	const PxU32 scratchMemorySize = sizeof(Cm::SpatialVectorF) * immArt->getBodyCount() * 5	//motionVelocity, motionAccelerations, coriolisVectors, spatialZAVectors, externalAccels;
		+ sizeof(Dy::SpatialMatrix) * immArt->getBodyCount()					//compositeSpatialInertias;
		+ sizeof(PxReal) * totalDofs * 5;						//jointVelocity, jointAcceleration, jointForces, jointPositions, jointFrictionForces

	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;	
}

void immediate::PxCopyInternalStateToArticulationCache(Dy::ArticulationV* articulation, PxArticulationCache& cache, PxArticulationCacheFlags flag)
{
	immArticulation *immArt = static_cast<immArticulation *>(articulation);
	immArt->copyInternalStateToCache(cache, flag);	
}

void immediate::PxApplyArticulationCache(Dy::ArticulationV* articulation, PxArticulationCache& cache, PxArticulationCacheFlags flag) {
	immArticulation *immArt = static_cast<immArticulation *>(articulation);
	immArt->applyCache(cache, flag);
}

void immediate::PxReleaseArticulationCache(PxArticulationCache& cache)
{
	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 immediate::PxComputeUnconstrainedVelocities(Dy::ArticulationV* articulation, const PxVec3& gravity, const PxReal dt)
{
	if(!articulation)
		return;

	immArticulation* immArt = static_cast<immArticulation*>(articulation);
	immArt->complete();
	immArt->immComputeUnconstrainedVelocities(dt, gravity);
}

void immediate::PxUpdateArticulationBodies(Dy::ArticulationV* articulation, const PxReal dt)
{
	if(!articulation)
		return;

	FeatherstoneArticulation::updateBodies(static_cast<immArticulation*>(articulation), dt, true);
}

void immediate::PxComputeUnconstrainedVelocitiesTGS(Dy::ArticulationV* articulation, const PxVec3& gravity, const PxReal dt,
	const PxReal totalDt, const PxReal invDt, const PxReal invTotalDt)
{
	if (!articulation)
		return;

	immArticulation* immArt = static_cast<immArticulation*>(articulation);
	immArt->complete();
	immArt->immComputeUnconstrainedVelocitiesTGS(dt, totalDt, invDt, invTotalDt, gravity);
}

void immediate::PxUpdateArticulationBodiesTGS(Dy::ArticulationV* articulation, const PxReal dt)
{
	if (!articulation)
		return;

	FeatherstoneArticulation::updateBodies(static_cast<immArticulation*>(articulation), dt, false);
}

Dy::ArticulationV* immediate::PxGetLinkArticulation(const Dy::ArticulationLinkHandle link)
{
	return Dy::getArticulation(link);
}

PxU32 immediate::PxGetLinkIndex(const Dy::ArticulationLinkHandle link)
{
	return Dy::getLinkIndex(link);
}

static void copyLinkData(PxLinkData& data, const immArticulation* immArt, PxU32 index)
{
	data.pose				= immArt->mBodyCores[index].body2World;
//	data.linearVelocity		= immArt->mBodyCores[index].linearVelocity;
//	data.angularVelocity	= immArt->mBodyCores[index].angularVelocity;
	const Cm::SpatialVectorF& velocity = immArt->getArticulationData().getMotionVelocity(index);
	data.linearVelocity	= velocity.bottom;
	data.angularVelocity	= velocity.top;
}

static PX_FORCE_INLINE const immArticulation* getFromLink(const Dy::ArticulationLinkHandle link, PxU32& index)
{
	const Dy::ArticulationV* articulation = Dy::getArticulation(link);
	if(!articulation)
		return NULL;

	const immArticulation* immArt = static_cast<const immArticulation*>(articulation);
	index = Dy::getLinkIndex(link);

	if(index>=immArt->mLinks.size())
		return NULL;

	return immArt;
}

bool immediate::PxGetLinkData(const Dy::ArticulationLinkHandle link, PxLinkData& data)
{
	PxU32 index;
	const immArticulation* immArt = getFromLink(link, index);
	if(!immArt)
		return false;

	copyLinkData(data, immArt, index);

	return true;
}

PxU32 immediate::PxGetAllLinkData(const Dy::ArticulationV* articulation, PxLinkData* data)
{
	if(!articulation)
		return 0;

	const immArticulation* immArt = static_cast<const immArticulation*>(articulation);

	const PxU32 nb = immArt->mLinks.size();
	if(data)
	{
		for(PxU32 i=0;i<nb;i++)
			copyLinkData(data[i], immArt, i);
	}

	return nb;
}

bool immediate::PxGetMutableLinkData(const Dy::ArticulationLinkHandle link, PxMutableLinkData& data)
{
	PxU32 index;
	const immArticulation* immArt = getFromLink(link, index);
	if(!immArt)
		return false;

	data.inverseInertia			= immArt->mBodyCores[index].inverseInertia;
	data.inverseMass			= immArt->mBodyCores[index].inverseMass;
	data.linearDamping			= immArt->mBodyCores[index].linearDamping;
	data.angularDamping			= immArt->mBodyCores[index].angularDamping;
	data.maxLinearVelocitySq	= immArt->mBodyCores[index].maxLinearVelocitySq;
	data.maxAngularVelocitySq	= immArt->mBodyCores[index].maxAngularVelocitySq;
	data.disableGravity			= immArt->mBodyCores[index].disableGravity!=0;

	return true;
}

bool immediate::PxSetMutableLinkData(Dy::ArticulationLinkHandle link, const PxMutableLinkData& data)
{
	PxU32 index;
	immArticulation* immArt = const_cast<immArticulation*>(getFromLink(link, index));
	if(!immArt)
		return false;

	immArt->mBodyCores[index].inverseInertia		= data.inverseInertia;			// See Sc::BodyCore::setInverseInertia
	immArt->mBodyCores[index].inverseMass			= data.inverseMass;				// See Sc::BodyCore::setInverseMass
	immArt->mBodyCores[index].linearDamping			= data.linearDamping;			// See Sc::BodyCore::setLinearDamping
	immArt->mBodyCores[index].angularDamping		= data.angularDamping;			// See Sc::BodyCore::setAngularDamping
	immArt->mBodyCores[index].maxLinearVelocitySq	= data.maxLinearVelocitySq;		// See Sc::BodyCore::setMaxLinVelSq
	immArt->mBodyCores[index].maxAngularVelocitySq	= data.maxAngularVelocitySq;	// See Sc::BodyCore::setMaxAngVelSq
	immArt->mBodyCores[index].disableGravity		= data.disableGravity;			// See BodySim::postActorFlagChange

	return true;
}

bool immediate::PxGetJointData(const Dy::ArticulationLinkHandle link, PxFeatherstoneArticulationJointData& data)
{
	PxU32 index;
	const immArticulation* immArt = getFromLink(link, index);
	if(!immArt)
		return false;

	const Dy::ArticulationJointCore& core = immArt->mArticulationJointCore[index];

	data.parentPose				= core.parentPose;
	data.childPose				= core.childPose;
	data.type					= PxArticulationJointType::Enum(core.jointType);
	data.frictionCoefficient	= core.frictionCoefficient;
	data.maxJointVelocity		= core.maxJointVelocity;
	for(PxU32 i=0;i<PxArticulationAxis::eCOUNT;i++)
	{
		data.motion[i]					= PxArticulationMotion::Enum(PxU8(core.motion[i]));
		data.limits[i].low				= core.limits[i].low;
		data.limits[i].high				= core.limits[i].high;
		data.drives[i].stiffness		= core.drives[i].stiffness;
		data.drives[i].damping			= core.drives[i].damping;
		data.drives[i].maxForce			= core.drives[i].maxForce;
		data.drives[i].driveType		= core.drives[i].driveType;
		data.targetPos[i]				= core.targetP[i];
		data.targetVel[i]				= core.targetV[i];
	}
	return true;
}

static bool samePoses(const PxTransform& pose0, const PxTransform& pose1)
{
	return (pose0.p == pose1.p) && (pose0.q == pose1.q);
}

// PT: this is not super efficient if you only want to change one parameter. We could consider adding individual, atomic accessors (but that would
// bloat the API) or flags to validate the desired parameters.
bool immediate::PxSetJointData(Dy::ArticulationLinkHandle link, const PxFeatherstoneArticulationJointData& data)
{
	PxU32 index;
	immArticulation* immArt = const_cast<immArticulation*>(getFromLink(link, index));
	if(!immArt)
		return false;

	Dy::ArticulationJointCore& core = immArt->mArticulationJointCore[index];

	bool dirty = false;

	if(!samePoses(core.parentPose, data.parentPose))
	{
		core.parentPose	= data.parentPose;
		core.dirtyFlag |= Dy::ArticulationJointCoreDirtyFlag::ePOSE;
		dirty = true;
	}

	if(!samePoses(core.childPose, data.childPose))
	{
		core.childPose	= data.childPose;
		core.dirtyFlag |= Dy::ArticulationJointCoreDirtyFlag::ePOSE;
		dirty = true;
	}

	if(core.jointType!=PxU8(data.type))
	{
		core.jointType	= PxU8(data.type);
		dirty = true;
	}
	core.frictionCoefficient	= data.frictionCoefficient;
	core.maxJointVelocity		= data.maxJointVelocity;
	for(PxU32 i=0;i<PxArticulationAxis::eCOUNT;i++)
	{
		core.limits[i].low				= data.limits[i].low;
		core.limits[i].high				= data.limits[i].high;
		core.drives[i].stiffness		= data.drives[i].stiffness;
		core.drives[i].damping			= data.drives[i].damping;
		core.drives[i].maxForce			= data.drives[i].maxForce;
		core.drives[i].driveType		= data.drives[i].driveType;

		if(core.motion[i]!=data.motion[i])
		{
			core.motion[i] = PxU8(data.motion[i]);
			core.dirtyFlag |= Dy::ArticulationJointCoreDirtyFlag::eMOTION;
			dirty = true;
		}

		if(core.targetP[i] != data.targetPos[i])
		{
			core.targetP[i] = data.targetPos[i];
			core.dirtyFlag |= Dy::ArticulationJointCoreDirtyFlag::eTARGETPOSE;
			dirty = true;
		}

		if(core.targetV[i] != data.targetVel[i])
		{
			core.targetV[i] = data.targetVel[i];
			core.dirtyFlag |= Dy::ArticulationJointCoreDirtyFlag::eTARGETVELOCITY;
			dirty = true;
		}
	}

	if(dirty)
		immArt->setDirty(true);

	return true;
}

namespace physx
{
namespace Dy
{
	void copyToSolverBodyDataStep(const PxVec3& linearVelocity, const PxVec3& angularVelocity, const PxReal invMass, const PxVec3& invInertia, const PxTransform& globalPose,
		const PxReal maxDepenetrationVelocity, const PxReal maxContactImpulse, const PxU32 nodeIndex, const PxReal reportThreshold,
		const PxReal maxAngVelSq, PxU32 lockFlags, bool isKinematic,
		PxTGSSolverBodyVel& solverVel, PxTGSSolverBodyTxInertia& solverBodyTxInertia, PxTGSSolverBodyData& solverBodyData);

	void integrateCoreStep(PxTGSSolverBodyVel& vel, PxTGSSolverBodyTxInertia& txInertia, const PxF32 dt);
}
}

void immediate::PxConstructSolverBodiesTGS(const PxRigidBodyData* inRigidData, PxTGSSolverBodyVel* outSolverBodyVel, PxTGSSolverBodyTxInertia* outSolverBodyTxInertia, PxTGSSolverBodyData* outSolverBodyData, const PxU32 nbBodies, const PxVec3& gravity, const PxReal dt)
{
	for (PxU32 a = 0; a<nbBodies; a++)
	{
		const PxRigidBodyData& rigidData = inRigidData[a];
		PxVec3 lv = rigidData.linearVelocity, av = rigidData.angularVelocity;
		Dy::bodyCoreComputeUnconstrainedVelocity(gravity, dt, rigidData.linearDamping, rigidData.angularDamping, 1.0f, rigidData.maxLinearVelocitySq, rigidData.maxAngularVelocitySq, lv, av, false);

		Dy::copyToSolverBodyDataStep(lv, av, rigidData.invMass, rigidData.invInertia, rigidData.body2World, -rigidData.maxDepenetrationVelocity, rigidData.maxContactImpulse, IG_INVALID_NODE,
			PX_MAX_F32, rigidData.maxAngularVelocitySq, 0, false, outSolverBodyVel[a], outSolverBodyTxInertia[a], outSolverBodyData[a]);
	}
}

void immediate::PxConstructStaticSolverBodyTGS(const PxTransform& globalPose, PxTGSSolverBodyVel& solverBodyVel, PxTGSSolverBodyTxInertia& solverBodyTxInertia, PxTGSSolverBodyData& solverBodyData)
{
	const PxVec3 zero(0.0f);
	Dy::copyToSolverBodyDataStep(zero, zero, 0.f, zero, globalPose, -PX_MAX_F32, PX_MAX_F32, IG_INVALID_NODE, PX_MAX_F32, PX_MAX_F32, 0, true, solverBodyVel, solverBodyTxInertia, solverBodyData);
}

PxU32 immediate::PxBatchConstraintsTGS(const PxSolverConstraintDesc* solverConstraintDescs, const PxU32 nbConstraints, PxTGSSolverBodyVel* solverBodies, const PxU32 nbBodies,
	PxConstraintBatchHeader* outBatchHeaders, PxSolverConstraintDesc* outOrderedConstraintDescs,
	Dy::ArticulationV** articulations, const PxU32 nbArticulations)
{
	if (!nbArticulations)
	{
		RigidBodyClassification classification(reinterpret_cast<PxU8*>(solverBodies), nbBodies, sizeof(PxTGSSolverBodyVel));
		return BatchConstraints(solverConstraintDescs, nbConstraints, outBatchHeaders, outOrderedConstraintDescs, classification);
	}
	else
	{
		ExtendedRigidBodyClassification classification(reinterpret_cast<PxU8*>(solverBodies), nbBodies, sizeof(PxTGSSolverBodyVel), articulations, nbArticulations);
		return BatchConstraints(solverConstraintDescs, nbConstraints, outBatchHeaders, outOrderedConstraintDescs, classification);
	}
}

bool immediate::PxCreateContactConstraintsTGS(PxConstraintBatchHeader* batchHeaders, const PxU32 nbHeaders, PxTGSSolverContactDesc* contactDescs,
	PxConstraintAllocator& allocator, const PxReal invDt, const PxReal invTotalDt, const PxReal bounceThreshold, const PxReal frictionOffsetThreshold, const PxReal correlationDistance)
{
	PX_ASSERT(invDt > 0.f && PxIsFinite(invDt));
	PX_ASSERT(bounceThreshold < 0.f);
	PX_ASSERT(frictionOffsetThreshold > 0.f);
	PX_ASSERT(correlationDistance > 0.f);

	Dy::SolverConstraintPrepState::Enum state = Dy::SolverConstraintPrepState::eUNBATCHABLE;

	Dy::CorrelationBuffer cb;

	PxU32 currentContactDescIdx = 0;

	for (PxU32 i = 0; i < nbHeaders; ++i)
	{
		PxConstraintBatchHeader& batchHeader = batchHeaders[i];
		if (batchHeader.stride == 4)
		{
			PxU32 totalContacts = contactDescs[currentContactDescIdx].numContacts + contactDescs[currentContactDescIdx + 1].numContacts +
				contactDescs[currentContactDescIdx + 2].numContacts + contactDescs[currentContactDescIdx + 3].numContacts;

			if (totalContacts <= 64)
			{
				state = Dy::createFinalizeSolverContacts4Step(cb,
					contactDescs + currentContactDescIdx,
					invDt,
					invTotalDt,
					bounceThreshold,
					frictionOffsetThreshold,
					correlationDistance,
					0.f,
					allocator);
			}
		}

		if (state == Dy::SolverConstraintPrepState::eUNBATCHABLE)
		{
			for (PxU32 a = 0; a < batchHeader.stride; ++a)
			{
				Dy::createFinalizeSolverContactsStep(contactDescs[currentContactDescIdx + a], cb, invDt, invTotalDt, bounceThreshold,
					frictionOffsetThreshold, correlationDistance, allocator);
			}
		}
		PxU8 type = *contactDescs[currentContactDescIdx].desc->constraint;

		if (type == DY_SC_TYPE_STATIC_CONTACT)
		{
			//Check if any block of constraints is classified as type static (single) contact constraint.
			//If they are, iterate over all constraints grouped with it and switch to "dynamic" contact constraint
			//type if there's a dynamic contact constraint in the group.
			for (PxU32 c = 1; c < batchHeader.stride; ++c)
			{
				if (*contactDescs[currentContactDescIdx + c].desc->constraint == DY_SC_TYPE_RB_CONTACT)
				{
					type = DY_SC_TYPE_RB_CONTACT;
					break;
				}
			}
		}

		batchHeader.constraintType = type;

		currentContactDescIdx += batchHeader.stride;
	}
	return true;
}

bool immediate::PxCreateJointConstraintsTGS(PxConstraintBatchHeader* batchHeaders, const PxU32 nbHeaders, 
	PxTGSSolverConstraintPrepDesc* jointDescs, PxConstraintAllocator& allocator, const PxReal dt, const PxReal totalDt, const PxReal invDt,
	const PxReal invTotalDt, const PxReal lengthScale)
{
	PX_ASSERT(dt > 0.f);
	PX_ASSERT(invDt > 0.f && PxIsFinite(invDt));

	Dy::SolverConstraintPrepState::Enum state = Dy::SolverConstraintPrepState::eUNBATCHABLE;

	PxU32 currentDescIdx = 0;
	for (PxU32 i = 0; i < nbHeaders; ++i)
	{
		PxConstraintBatchHeader& batchHeader = batchHeaders[i];

		PxU8 type = DY_SC_TYPE_BLOCK_1D;
		if (batchHeader.stride == 4)
		{
			PxU32 totalRows = 0;
			PxU32 maxRows = 0;
			bool batchable = true;
			for (PxU32 a = 0; a < batchHeader.stride; ++a)
			{
				if (jointDescs[currentDescIdx + a].numRows == 0)
				{
					batchable = false;
					break;
				}
				totalRows += jointDescs[currentDescIdx + a].numRows;
				maxRows = PxMax(maxRows, jointDescs[currentDescIdx + a].numRows);
			}

			if (batchable)
			{
				state = Dy::setupSolverConstraintStep4
				(jointDescs + currentDescIdx,
					dt, totalDt, invDt, invTotalDt, totalRows,
					allocator, maxRows, lengthScale);
			}
		}

		if (state == Dy::SolverConstraintPrepState::eUNBATCHABLE)
		{
			type = DY_SC_TYPE_RB_1D;
			for (PxU32 a = 0; a < batchHeader.stride; ++a)
			{
				// PT: TODO: And "isExtended" is already computed in Dy::ConstraintHelper::setupSolverConstraint
				PxSolverConstraintDesc& desc = *jointDescs[currentDescIdx + a].desc;
				const bool isExtended = desc.linkIndexA != PxSolverConstraintDesc::NO_LINK || desc.linkIndexB != PxSolverConstraintDesc::NO_LINK;
				if (isExtended)
					type = DY_SC_TYPE_EXT_1D;


				Dy::setupSolverConstraintStep(jointDescs[currentDescIdx + a], allocator, dt, totalDt, invDt, invTotalDt, lengthScale);
			}
		}

		batchHeader.constraintType = type;
		currentDescIdx += batchHeader.stride;
	}

	return true;
}


template<class LeafTestT, class ParamsT>
static bool PxCreateJointConstraintsWithShadersTGS_T(PxConstraintBatchHeader* batchHeaders, const PxU32 nbHeaders, ParamsT* params, PxTGSSolverConstraintPrepDesc* jointDescs,
	PxConstraintAllocator& allocator, const PxReal dt, const PxReal totalDt, const PxReal invDt, const PxReal invTotalDt, const PxReal lengthScale)
{
	Px1DConstraint allRows[Dy::MAX_CONSTRAINT_ROWS * 4];

	//Runs shaders to fill in rows...

	PxU32 currentDescIdx = 0;

	for (PxU32 i = 0; i<nbHeaders; i++)
	{
		PxU32 numRows = 0;
		PxU32 preppedIndex = 0;
		PxU32 maxRows = 0;

		PxConstraintBatchHeader& batchHeader = batchHeaders[i];

		for (PxU32 a = 0; a<batchHeader.stride; a++)
		{
			Px1DConstraint* rows = allRows + numRows;
			PxTGSSolverConstraintPrepDesc& desc = jointDescs[currentDescIdx + a];

			PxConstraintSolverPrep prep;
			const void* constantBlock;
			const bool useExtendedLimits = LeafTestT::getData(params, currentDescIdx + a, &prep, &constantBlock);
			PX_ASSERT(prep);

			PxMemZero(rows + preppedIndex, sizeof(Px1DConstraint)*(Dy::MAX_CONSTRAINT_ROWS));
			for (PxU32 b = preppedIndex; b < Dy::MAX_CONSTRAINT_ROWS; ++b)
			{
				Px1DConstraint& c = rows[b];
				//Px1DConstraintInit(c);
				c.minImpulse = -PX_MAX_REAL;
				c.maxImpulse = PX_MAX_REAL;
			}

			desc.invMassScales.linear0 = desc.invMassScales.linear1 = desc.invMassScales.angular0 = desc.invMassScales.angular1 = 1.0f;

			desc.body0WorldOffset = PxVec3(0.0f);

			const PxU32 constraintCount = prep(rows,
				desc.body0WorldOffset,
				Dy::MAX_CONSTRAINT_ROWS,
				desc.invMassScales,
				constantBlock,
				desc.bodyFrame0, desc.bodyFrame1,
				useExtendedLimits,
				desc.cA2w, desc.cB2w);

			preppedIndex = Dy::MAX_CONSTRAINT_ROWS - constraintCount;

			maxRows = PxMax(constraintCount, maxRows);

			desc.rows = rows;
			desc.numRows = constraintCount;
			numRows += constraintCount;
		}

		PxCreateJointConstraintsTGS(&batchHeader, 1, jointDescs + currentDescIdx, allocator, dt, totalDt, invDt,
			invTotalDt, lengthScale);

		currentDescIdx += batchHeader.stride;
	}
	return true; //KS - TODO - do some error reporting/management...
}


bool immediate::PxCreateJointConstraintsWithShadersTGS(PxConstraintBatchHeader* batchHeaders, const PxU32 nbBatchHeaders, PxConstraint** constraints, PxTGSSolverConstraintPrepDesc* jointDescs, PxConstraintAllocator& allocator, const PxReal dt,
	const PxReal totalDt, const PxReal invDt, const PxReal invTotalDt, const PxReal lengthScale)
{
	class PxConstraintAdapter
	{
	public:
		static PX_FORCE_INLINE bool getData(PxConstraint** constraints, PxU32 i, PxConstraintSolverPrep* prep, const void** constantBlock)
		{
			NpConstraint* npConstraint = static_cast<NpConstraint*>(constraints[i]);

			npConstraint->updateConstants();

			Sc::ConstraintCore& core = npConstraint->getScbConstraint().getScConstraint();

			*prep = core.getPxConnector()->getPrep();
			*constantBlock = core.getPxConnector()->getConstantBlock();
			return core.getFlags() & PxConstraintFlag::eENABLE_EXTENDED_LIMITS;
		}
	};

	return PxCreateJointConstraintsWithShadersTGS_T<PxConstraintAdapter>(batchHeaders, nbBatchHeaders, constraints, jointDescs, allocator, dt, totalDt, invDt, invTotalDt, lengthScale);
}

bool immediate::PxCreateJointConstraintsWithImmediateShadersTGS(PxConstraintBatchHeader* batchHeaders, const PxU32 nbHeaders, PxImmediateConstraint* constraints, PxTGSSolverConstraintPrepDesc* jointDescs,
	PxConstraintAllocator& allocator, const PxReal dt, const PxReal totalDt, const PxReal invDt, const PxReal invTotalDt, const PxReal lengthScale)
{
	class immConstraintAdapter
	{
	public:
		static PX_FORCE_INLINE bool getData(PxImmediateConstraint* constraints, PxU32 i, PxConstraintSolverPrep* prep, const void** constantBlock)
		{
			const PxImmediateConstraint& ic = constraints[i];
			*prep = ic.prep;
			*constantBlock = ic.constantBlock;
			return false;
		}
	};

	return PxCreateJointConstraintsWithShadersTGS_T<immConstraintAdapter>(batchHeaders, nbHeaders, constraints, jointDescs, allocator, dt, totalDt, invDt, invTotalDt, lengthScale);
}


void immediate::PxSolveConstraintsTGS(const PxConstraintBatchHeader* batchHeaders, const PxU32 nbBatchHeaders, const PxSolverConstraintDesc* solverConstraintDescs,
	PxTGSSolverBodyVel* solverBodies, PxTGSSolverBodyTxInertia* txInertias, const PxU32 nbSolverBodies, const PxU32 nbPositionIterations, const PxU32 nbVelocityIterations,
	const float dt, const float invDt, const PxU32 nbSolverArticulations, Dy::ArticulationV** solverArticulations)
{
	PX_UNUSED(solverBodies);
	PX_UNUSED(nbSolverBodies);
	PX_ASSERT(nbPositionIterations > 0);
	PX_ASSERT(nbVelocityIterations > 0);

													   //Stage 1: solve the position iterations...
	Dy::TGSSolveBlockMethod* solveTable = Dy::g_SolveTGSMethods;

	Dy::TGSSolveConcludeMethod* solveConcludeTable = Dy::g_SolveConcludeTGSMethods;

	Dy::TGSWriteBackMethod* writebackTable = Dy::g_WritebackTGSMethods;

	Dy::SolverContext cache;
	cache.mThresholdStreamIndex = 0;
	cache.mThresholdStreamLength = 0xFFFFFFF;

	PX_ASSERT(nbPositionIterations > 0);
	PX_ASSERT(nbVelocityIterations > 0);

	Cm::SpatialVectorF Z[DY_ARTICULATION_MAX_SIZE];
	Cm::SpatialVectorF deltaV[DY_ARTICULATION_MAX_SIZE];

	cache.Z = Z;
	cache.deltaV = deltaV;

	struct Articulations
	{
		static PX_FORCE_INLINE void solveInternalConstraints(const float dt, const float invDt, const PxU32 nbSolverArticulations, Dy::ArticulationV** solverArticulations, Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV, 
			const PxReal elapsedTime, bool velIter)
		{
			for (PxU32 a = 0; a<nbSolverArticulations; a++)
			{
				immArticulation* immArt = static_cast<immArticulation*>(solverArticulations[a]);
				immArt->immSolveInternalConstraints(dt, invDt, Z, deltaV, elapsedTime, velIter, true);
			}
		}

		static PX_FORCE_INLINE void stepArticulations(const float dt, const PxU32 nbSolverArticulations, Dy::ArticulationV** solverArticulations,
			Cm::SpatialVectorF* deltaV)
		{
			for (PxU32 a = 0; a<nbSolverArticulations; a++)
			{
				immArticulation* immArt = static_cast<immArticulation*>(solverArticulations[a]);
				immArt->recordDeltaMotion(immArt->getSolverDesc(), dt, deltaV);
			}
		}
	};

	PxReal elapsedTime = 0.f;
	for (PxU32 i = nbPositionIterations; i>1; --i)
	{
		Articulations::solveInternalConstraints(dt, invDt, nbSolverArticulations, solverArticulations, Z, deltaV, elapsedTime, false);

		cache.doFriction = true;
		for (PxU32 a = 0; a<nbBatchHeaders; ++a)
		{
			const PxConstraintBatchHeader& batch = batchHeaders[a];
			solveTable[batch.constraintType](batch, solverConstraintDescs, txInertias, -PX_MAX_F32, elapsedTime, cache);
		}

		for(PxU32 j = 0; j < nbSolverBodies; ++j)
			Dy::integrateCoreStep(solverBodies[j], txInertias[j], dt);

		Articulations::stepArticulations(dt, nbSolverArticulations, solverArticulations, deltaV);
		elapsedTime += dt;
	}

	cache.doFriction = true;

	Articulations::solveInternalConstraints(dt, invDt, nbSolverArticulations, solverArticulations, Z, deltaV, elapsedTime, false);

	for (PxU32 a = 0; a<nbBatchHeaders; ++a)
	{
		const PxConstraintBatchHeader& batch = batchHeaders[a];
		solveConcludeTable[batch.constraintType](batch, solverConstraintDescs, txInertias, elapsedTime, cache);
	}
	

	for (PxU32 j = 0; j < nbSolverBodies; ++j)
		Dy::integrateCoreStep(solverBodies[j], txInertias[j], dt);

	Articulations::stepArticulations(dt, nbSolverArticulations, solverArticulations, deltaV);
	elapsedTime += dt;

	for (PxU32 i = nbVelocityIterations; i>1; --i)
	{
		Articulations::solveInternalConstraints(dt, invDt, nbSolverArticulations, solverArticulations, Z, deltaV, elapsedTime, true);
		for (PxU32 a = 0; a<nbBatchHeaders; ++a)
		{
			const PxConstraintBatchHeader& batch = batchHeaders[a];
			solveTable[batch.constraintType](batch, solverConstraintDescs, txInertias, 0.f, elapsedTime, cache);
		}
	}

	Articulations::solveInternalConstraints(dt, invDt, nbSolverArticulations, solverArticulations, Z, deltaV, elapsedTime, true);

	for (PxU32 a = 0; a<nbBatchHeaders; ++a)
	{
		const PxConstraintBatchHeader& batch = batchHeaders[a];
		solveTable[batch.constraintType](batch, solverConstraintDescs, txInertias, 0.f, elapsedTime, cache);
		writebackTable[batch.constraintType](batch, solverConstraintDescs, &cache);
	}
	
}

void immediate::PxIntegrateSolverBodiesTGS(PxTGSSolverBodyVel* solverBody, PxTGSSolverBodyTxInertia* txInertia, PxTransform* poses, const PxU32 nbBodiesToIntegrate, const PxReal dt)
{
	PX_UNUSED(dt);
	for (PxU32 i = 0; i < nbBodiesToIntegrate; ++i)
	{
		solverBody[i].angularVelocity = txInertia[i].sqrtInvInertia * solverBody[i].angularVelocity;
		poses[i].p = txInertia[i].deltaBody2World.p;
		poses[i].q = (txInertia[i].deltaBody2World.q * poses[i].q).getNormalized();
	}
}