XCEngine/engine/third_party/physx/source/lowleveldynamics/src/DyFeatherstoneInverseDynamic.cpp

// 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-2025 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 "CmConeLimitHelper.h"
#include "DySolverConstraint1D.h"
#include "DyFeatherstoneArticulation.h"
#include "PxsRigidBody.h"
#include "PxcConstraintBlockStream.h"
#include "DyArticulationContactPrep.h"
#include "DyDynamics.h"
#include "DyArticulationPImpl.h"
#include "foundation/PxProfiler.h"
#include "DyFeatherstoneArticulationLink.h"
#include "DyFeatherstoneArticulationJointData.h"
#include "DyConstraint.h"
#include "DyConstraintPrep.h"
#include "DySolverContext.h"

namespace physx
{

namespace Dy
{
	void PxcFsFlushVelocity(FeatherstoneArticulation& articulation, Cm::SpatialVectorF* deltaV);

	void FeatherstoneArticulation::computeLinkAccelerationInv(ArticulationData& data, ScratchData& scratchData)
	{
		Cm::SpatialVectorF* motionAccelerations = scratchData.motionAccelerations;
		
		Cm::SpatialVectorF* coriolisVectors = scratchData.coriolisVectors;

		PxReal* jointAccelerations = scratchData.jointAccelerations;

		motionAccelerations[0] = Cm::SpatialVectorF::Zero();

		for (PxU32 linkID = 1; linkID < data.getLinkCount(); ++linkID)
		{
			ArticulationLink& link = data.getLink(linkID);

			Cm::SpatialVectorF pMotionAcceleration = translateSpatialVector(-data.getRw(linkID), motionAccelerations[link.parent]);

			Cm::SpatialVectorF motionAcceleration(PxVec3(0.f), PxVec3(0.f));

			if (jointAccelerations)
			{
				ArticulationJointCoreData& jointDatum = data.getJointData(linkID);
				const PxU32 jointOffset = jointDatum.jointOffset;
				const PxReal* jAcceleration = &jointAccelerations[jointOffset];
				for (PxU32 ind = 0; ind < jointDatum.nbDof; ++ind)
				{
					motionAcceleration.top += data.mWorldMotionMatrix[jointOffset + ind].top * jAcceleration[ind];
					motionAcceleration.bottom += data.mWorldMotionMatrix[jointOffset + ind].bottom * jAcceleration[ind];
				}
			}

			//calculate the acceleration due to external forces, applied external acceleration, and Coriolis force
			//used later to calculate the zero acceleration force
			motionAccelerations[linkID] = pMotionAcceleration + coriolisVectors[linkID] + motionAcceleration;
		}
	}

	//generalized force
	void FeatherstoneArticulation::computeGeneralizedForceInv(ArticulationData& data, ScratchData& scratchData)
	{
		const PxU32 linkCount = data.getLinkCount();

		Cm::SpatialVectorF* spatialZAForces = scratchData.spatialZAVectors;
		PxReal* jointForces = scratchData.jointForces;

		for (PxU32 linkID = (linkCount - 1); linkID > 0; --linkID)
		{
			ArticulationLink& link = data.getLink(linkID);
			
			//joint force

			spatialZAForces[link.parent] += translateSpatialVector(data.getRw(linkID), spatialZAForces[linkID]);

			ArticulationJointCoreData& jointDatum = data.getJointData(linkID);
			//compute generalized force
			const PxU32 jointOffset = jointDatum.jointOffset;
			PxReal* force = &jointForces[jointOffset];

			for (PxU32 ind = 0; ind < jointDatum.nbDof; ++ind)
			{
				force[ind] = data.mWorldMotionMatrix[jointOffset + ind].innerProduct(spatialZAForces[linkID]);
			}
		}
	}

	void FeatherstoneArticulation::computeZAForceInv(ArticulationData& data, ScratchData& scratchData)
	{
		const PxU32 linkCount = data.getLinkCount();

		Cm::SpatialVectorF* motionAccelerations = scratchData.motionAccelerations;
		
		Cm::SpatialVectorF* biasForce = scratchData.spatialZAVectors;

		for (PxU32 linkID = 0; linkID < linkCount; ++linkID)
		{
			ArticulationLink& link = data.getLink(linkID);

			PxsBodyCore& core = *link.bodyCore;

			const PxVec3& ii = core.inverseInertia;

			const PxReal m = core.inverseMass == 0.f ? 0.f : 1.0f / core.inverseMass;
			const PxVec3 inertiaTensor = PxVec3(ii.x == 0.f ? 0.f : (1.f / ii.x), ii.y == 0.f ? 0.f : (1.f / ii.y), ii.z == 0.f ? 0.f : (1.f / ii.z));


			Cm::SpatialVectorF Ia;
			Ia.bottom = core.body2World.rotate(core.body2World.rotateInv(motionAccelerations[linkID].top).multiply(inertiaTensor));
			Ia.top = motionAccelerations[linkID].bottom * m;

			biasForce[linkID] +=Ia;
		}
	}

	void FeatherstoneArticulation::initCompositeSpatialInertia(ArticulationData& data, Dy::SpatialMatrix* compositeSpatialInertia)
	{
		const PxU32 linkCount = data.getLinkCount();

		for (PxU32 linkID = 0; linkID < linkCount; ++linkID)
		{
			SpatialMatrix& spatialInertia = compositeSpatialInertia[linkID];

			ArticulationLink& link = data.getLink(linkID);

			PxsBodyCore& core = *link.bodyCore;

			const PxVec3& ii = core.inverseInertia;

			const PxReal m = core.inverseMass == 0.f ? 0.f : 1.0f / core.inverseMass;

			//construct mass matric
			spatialInertia.topLeft = PxMat33(PxZero);
			spatialInertia.topRight = PxMat33::createDiagonal(PxVec3(m));

			//construct inertia matrix
			PxMat33 rot(data.getLink(linkID).bodyCore->body2World.q);
			PxMat33& I = spatialInertia.bottomLeft;
			const PxVec3 inertiaTensor = PxVec3(ii.x == 0.f ? 0.f : (1.f / ii.x), ii.y == 0.f ? 0.f : (1.f / ii.y), ii.z == 0.f ? 0.f : (1.f / ii.z));
			Cm::transformInertiaTensor(inertiaTensor, rot, I);
		}
	}

	void FeatherstoneArticulation::computeCompositeSpatialInertiaAndZAForceInv(ArticulationData& data, ScratchData& scratchData)
	{
		ArticulationLink* links = data.getLinks();
		const PxU32 linkCount = data.getLinkCount();
		const PxU32 startIndex = PxU32(linkCount - 1);

		Dy::SpatialMatrix* compositeSpatialInertia = scratchData.compositeSpatialInertias;
		Cm::SpatialVectorF* zaForce = scratchData.spatialZAVectors;

		initCompositeSpatialInertia(data, compositeSpatialInertia);

		for (PxU32 linkID = startIndex; linkID > 0; --linkID)
		{
			ArticulationLink& link = links[linkID];

			Dy::SpatialMatrix cSpatialInertia = compositeSpatialInertia[linkID];
			translateInertia(FeatherstoneArticulation::constructSkewSymmetricMatrix(data.getRw(linkID)), cSpatialInertia);
			
			//compute parent's composite spatial inertia
			compositeSpatialInertia[link.parent] += cSpatialInertia;

			//compute zero acceleration force. This is the force that would be required to support the
			//motion of all the bodies in children set if root node acceleration happened to be zero
			zaForce[link.parent] += translateSpatialVector(data.getRw(linkID), zaForce[linkID]);
		}
	}

	void FeatherstoneArticulation::computeRelativeGeneralizedForceInv(ArticulationData& data, ScratchData& scratchData, const bool coriolisFloatingBase)
	{
		Cm::SpatialVectorF* motionAccelerations = scratchData.motionAccelerations;
		Dy::SpatialMatrix* compositeSpatialInertia = scratchData.compositeSpatialInertias;
		Cm::SpatialVectorF* zaForce = scratchData.spatialZAVectors;
		PxReal* jointForces = scratchData.jointForces;

		if (!coriolisFloatingBase)
		{
			Dy::SpatialMatrix invInertia = compositeSpatialInertia[0].invertInertia();
			//obtained by solving the equation: F = I * a + Z for the root where F = 0
			motionAccelerations[0] = -(invInertia * zaForce[0]);
		}

		const PxU32 linkCount = data.getLinkCount();
		ArticulationLink* links = data.getLinks();

		for (PxU32 linkID = 1; linkID < linkCount; ++linkID)
		{
			ArticulationLink& link = links[linkID];

			//transform the acceleration into the child link frame
			motionAccelerations[linkID] = translateSpatialVector(-data.getRw(linkID), motionAccelerations[link.parent]);

			// equation: F = I * a + Z
			zaForce[linkID] = compositeSpatialInertia[linkID] * motionAccelerations[linkID] + zaForce[linkID];
			ArticulationJointCoreData& jointDatum = data.getJointData(linkID);
			//compute generalized force
			const PxU32 jointOffset = jointDatum.jointOffset;
			PxReal* jForce = &jointForces[jointOffset];

			for (PxU32 ind = 0; ind < jointDatum.nbDof; ++ind)
			{
				jForce[ind] = data.mWorldMotionMatrix[jointOffset+ind].innerProduct(zaForce[linkID]);
			}
		}
	}

	void FeatherstoneArticulation::inverseDynamic(ArticulationData& data, const PxVec3& gravity,
		ScratchData& scratchData, bool computeCoriolis)
	{
		//pass 1
		computeLinkVelocities(data, scratchData);

		if(computeCoriolis)
			computeC(data, scratchData);
		else
			PxMemZero(scratchData.coriolisVectors, sizeof(Cm::SpatialVectorF)*data.getLinkCount());

		computeZ(data, gravity, scratchData);

		computeLinkAccelerationInv(data, scratchData);

		computeZAForceInv(data, scratchData);

		//pass 2
		computeGeneralizedForceInv(data, scratchData);
	}

	void FeatherstoneArticulation::inverseDynamicFloatingBase(ArticulationData& data, const PxVec3& gravity,
		ScratchData& scratchData, bool computeCoriolis, const bool rootMotion)
	{
		//pass 1
		computeLinkVelocities(data, scratchData);

		if(computeCoriolis)
			computeC(data, scratchData);
		else
			PxMemZero(scratchData.coriolisVectors, sizeof(Cm::SpatialVectorF)*data.getLinkCount());

		computeZ(data, gravity, scratchData);
		//no gravity, no external accelerations because we have turned those in force in
		//computeZ
		computeLinkAccelerationInv(data, scratchData);

		computeZAForceInv(data, scratchData);

		//pass 2
		computeCompositeSpatialInertiaAndZAForceInv(data, scratchData);

		//pass 3
		const bool coriolisFloatingBase = (computeCoriolis && rootMotion);
		computeRelativeGeneralizedForceInv(data, scratchData, coriolisFloatingBase);
	}


	bool FeatherstoneArticulation::applyCacheToDest(ArticulationData& data, PxArticulationCache& cache,
		PxReal* jVelocities, PxReal* jPositions, PxReal* jointForces, PxReal* jointTargetPositions, PxReal* jointTargetVelocities,
		const PxArticulationCacheFlags flag, bool& shouldWake)
	{
		bool needsScheduling = !mGPUDirtyFlags;

		bool localShouldWake = false;
		if (flag & PxArticulationCacheFlag::eVELOCITY)
		{
			const PxU32 dofCount = data.getDofs();
			for (PxU32 i = 0; i < dofCount; ++i)
			{
				const PxReal jv = cache.jointVelocity[i];
				localShouldWake = localShouldWake || jv != 0.f;
				jVelocities[i] = jv;
			}

			mGPUDirtyFlags |= ArticulationDirtyFlag::eDIRTY_VELOCITIES;
		}

		if (flag & PxArticulationCacheFlag::eROOT_TRANSFORM)
		{
			ArticulationLink& rLink = mArticulationData.getLink(0);
			// PT:: tag: scalar transform*transform
			rLink.bodyCore->body2World = cache.rootLinkData->transform * rLink.bodyCore->getBody2Actor();
			mGPUDirtyFlags |= ArticulationDirtyFlag::eDIRTY_ROOT_TRANSFORM;
		}

		if(flag & PxArticulationCacheFlag::eROOT_VELOCITIES)
		{
			ArticulationLink& rLink = mArticulationData.getLink(0);
			rLink.bodyCore->linearVelocity = cache.rootLinkData->worldLinVel;
			rLink.bodyCore->angularVelocity = cache.rootLinkData->worldAngVel;
			localShouldWake = localShouldWake || (!cache.rootLinkData->worldLinVel.isZero()) || (!cache.rootLinkData->worldAngVel.isZero());
			mGPUDirtyFlags |= ArticulationDirtyFlag::eDIRTY_ROOT_VELOCITIES;
		}
		
		if (flag & PxArticulationCacheFlag::ePOSITION)
		{
			copyJointData(data, jPositions, cache.jointPosition);
			//When we update the joint positions, we also have to update the link state, so need to make links
			//dirty!
			mGPUDirtyFlags |= (ArticulationDirtyFlag::eDIRTY_POSITIONS); 
		}

		if (flag & PxArticulationCacheFlag::eFORCE)
		{
			const PxU32 dofCount = data.getDofs();
			for (PxU32 i = 0; i < dofCount; ++i)
			{
				const PxReal jf = cache.jointForce[i];
				localShouldWake = localShouldWake || jf != 0.f;
				jointForces[i] = jf;
			}
			mGPUDirtyFlags |= ArticulationDirtyFlag::eDIRTY_FORCES;
		}

		if (flag & PxArticulationCacheFlag::eJOINT_TARGET_POSITIONS)
		{
			const PxU32 dofCount = data.getDofs();
			for (PxU32 i = 0; i < dofCount; ++i)
			{
				const PxReal jt = cache.jointTargetPositions[i];
				localShouldWake = localShouldWake || jt != jPositions[i];
				jointTargetPositions[i] = jt;
			}
			mGPUDirtyFlags |= ArticulationDirtyFlag::eDIRTY_JOINT_TARGET_POS;
		}

		if (flag & PxArticulationCacheFlag::eJOINT_TARGET_VELOCITIES)
		{
			const PxU32 dofCount = data.getDofs();
			for (PxU32 i = 0; i < dofCount; ++i)
			{
				const PxReal jv = cache.jointTargetVelocities[i];
				localShouldWake = localShouldWake || jv != jVelocities[i];
				jointTargetVelocities[i] = jv;
			}
			mGPUDirtyFlags |= ArticulationDirtyFlag::eDIRTY_JOINT_TARGET_VEL;
		}

		if (flag & PxArticulationCacheFlag::eLINK_FORCE)
		{
			const PxU32 linkCount = data.getLinkCount();
			for (PxU32 i = 0; i < linkCount; ++i)
			{
				const PxVec3 linkForce = cache.linkForce[i];
				localShouldWake = localShouldWake || linkForce != PxVec3(0.0f);
				ArticulationLink& link = mArticulationData.getLink(i);

				PxsBodyCore& core = *link.bodyCore;
				data.mExternalAcceleration[i].linear = linkForce * core.inverseMass;
			}
			mGPUDirtyFlags |= ArticulationDirtyFlag::eDIRTY_EXT_ACCEL;
		}

		if (flag & PxArticulationCacheFlag::eLINK_TORQUE)
		{
			const PxU32 linkCount = data.getLinkCount();
			for (PxU32 i = 0; i < linkCount; ++i)
			{
				ArticulationLink& link = mArticulationData.getLink(i);
				PxsBodyCore& core = *link.bodyCore;

				const PxQuat& q = core.body2World.q;
				const PxVec3& linkTorque = cache.linkTorque[i];
				const PxVec3 localLinkTorque = q.rotateInv(linkTorque);
				localShouldWake = localShouldWake || localLinkTorque != PxVec3(0.0f);

				// turn localLinkTorque into acceleration in local frame
				const PxVec3 localAccel = core.inverseInertia.multiply(localLinkTorque);
				// turn localAccel into world space
				const PxVec3 worldAccel = q.rotate(localAccel);

				data.mExternalAcceleration[i].angular = worldAccel;
			}
			mGPUDirtyFlags |= ArticulationDirtyFlag::eDIRTY_EXT_ACCEL;
		}

		// the updateKinematic functions rely on updated joint frames.
		if (mJcalcDirty)
		{
			jcalc(data);
		}
		mJcalcDirty = false;

		if (flag & (PxArticulationCacheFlag::ePOSITION | PxArticulationCacheFlag::eROOT_TRANSFORM))
		{
			//update link's position based on the joint position
			teleportLinks(data);
		}

		if (flag & (PxArticulationCacheFlag::eVELOCITY | PxArticulationCacheFlag::ePOSITION | PxArticulationCacheFlag::eROOT_VELOCITIES | PxArticulationCacheFlag::eROOT_TRANSFORM))
		{
			computeLinkVelocities(data);
		}
		shouldWake = localShouldWake;
		return needsScheduling;
	}

	void FeatherstoneArticulation::packJointData(const PxReal* maximum, PxReal* reduced)
	{
		const PxU32 linkCount = mArticulationData.getLinkCount();

		for (PxU32 linkID = 1; linkID < linkCount; linkID++)
		{
	
			ArticulationLink& linkDatum = mArticulationData.getLink(linkID);
			ArticulationJointCore* joint = linkDatum.inboundJoint;
			ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(linkID);

			const PxReal* maxJointData = &maximum[(linkID - 1) * DY_MAX_DOF];
			PxReal* reducedJointData = &reduced[jointDatum.jointOffset];

			PxU32 count = 0;
			for (PxU32 j = 0; j < DY_MAX_DOF; ++j)
			{
				PxArticulationMotions motion = PxArticulationMotions(joint->motion[j]);
				if (motion != PxArticulationMotion::eLOCKED)
				{
					reducedJointData[count] = maxJointData[j];
					count++;
				}
			}

			PX_ASSERT(count == jointDatum.nbDof);
		}

	}

	void FeatherstoneArticulation::unpackJointData(const PxReal* reduced, PxReal* maximum)
	{
		const PxU32 linkCount = mArticulationData.getLinkCount();

		for (PxU32 linkID = 1; linkID < linkCount; linkID++)
		{
			ArticulationLink& linkDatum = mArticulationData.getLink(linkID);
			ArticulationJointCore* joint = linkDatum.inboundJoint;
			ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(linkID);

			PxReal* maxJointData = &maximum[(linkID - 1) * DY_MAX_DOF];
			const PxReal* reducedJointData = &reduced[jointDatum.jointOffset];

			PxU32 count = 0;
			for (PxU32 j = 0; j < DY_MAX_DOF; ++j)
			{
				PxArticulationMotions motion = PxArticulationMotions(joint->motion[j]);
				if (motion != PxArticulationMotion::eLOCKED)
				{
					maxJointData[j] = reducedJointData[count];
					count++;
				}
				else
				{
					maxJointData[j] = 0.f;
				}
			}

			PX_ASSERT(count == jointDatum.nbDof);
		}
	}

	void FeatherstoneArticulation::initializeCommonData()
	{
		if (mJcalcDirty)
		{
			jcalc(mArticulationData);
			mJcalcDirty = false;
		}

		{
			//constants
			const ArticulationLink* links = mArticulationData.getLinks();
			const PxU32 linkCount = mArticulationData.getLinkCount();
			const ArticulationJointCoreData* jointCoreDatas = mArticulationData.getJointData();
			const Cm::UnAlignedSpatialVector* motionMatrices = mArticulationData.getMotionMatrix();

			//outputs
			PxTransform* accumulatedPoses = mArticulationData.getAccumulatedPoses();
			PxVec3* rws = mArticulationData.getRw();
			Cm::UnAlignedSpatialVector* motionMatricesW = mArticulationData.getWorldMotionMatrix();

			computeRelativeTransformC2P(
				links, linkCount, jointCoreDatas, motionMatrices,
				accumulatedPoses, rws, motionMatricesW);
		}

		computeRelativeTransformC2B(mArticulationData);

		computeSpatialInertia(mArticulationData);

		mArticulationData.setDataDirty(false);
	}

	void FeatherstoneArticulation::getGeneralizedGravityForce(const PxVec3& gravity, PxArticulationCache& cache, const bool rootMotion)
	{

		if (mArticulationData.getDataDirty())
		{
			PxGetFoundation().error(PxErrorCode::eINVALID_OPERATION, PX_FL, "Articulation::getGeneralisedGravityForce() commonInit need to be called first to initialize data!");
			return;
		}

#if FEATHERSTONE_DEBUG
		PxReal* jointForce = reinterpret_cast<PxReal*>(PX_ALLOC(sizeof(PxReal) * mArticulationData.getDofs(), "jointForce"));
		{
			
			const PxU32 linkCount = mArticulationData.getLinkCount();

			PxcScratchAllocator* allocator = reinterpret_cast<PxcScratchAllocator*>(cache.scratchAllocator);

			ScratchData scratchData;
			PxU8* tempMemory = allocateScratchSpatialData(allocator, linkCount, scratchData);

			scratchData.jointVelocities = NULL;
			scratchData.jointAccelerations = NULL;
			scratchData.jointForces = jointForce;

			const bool fixBase = mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE;
			if (fixBase)
				inverseDynamic(mArticulationData, gravity, scratchData, false);
			else
				inverseDynamicFloatingBase(mArticulationData, gravity, scratchData, false);

			allocator->free(tempMemory);
		}
#endif

		const PxVec3 tGravity = -gravity;
		PxcScratchAllocator* allocator = reinterpret_cast<PxcScratchAllocator*>(cache.scratchAllocator);
		const PxU32 linkCount = mArticulationData.getLinkCount();
		const bool fixBase = mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE;
		const PxU32 rootDof = (rootMotion && !fixBase) ? 6 : 0; //add the DoF of the root in the floating base case

		//note that with the new API, for both fixed-base and floating-base, we consider no acceleration for all joints and all links
		//this is consistent with the assumption behind the equation of motion
		if (rootMotion || fixBase)
		{
			Cm::SpatialVectorF* spatialZAForces = reinterpret_cast<Cm::SpatialVectorF*>(allocator->alloc(sizeof(Cm::SpatialVectorF) * linkCount));

			for (PxU32 linkID = 0; linkID < linkCount; ++linkID)
			{
				ArticulationLink& link = mArticulationData.getLink(linkID);

				PxsBodyCore& core = *link.bodyCore;

				const PxReal m = 1.0f / core.inverseMass;

				const PxVec3 linkGravity = tGravity;

				spatialZAForces[linkID].top = m * linkGravity;
				spatialZAForces[linkID].bottom = PxVec3(0.0f);
			}

			ScratchData scratchData;
			scratchData.spatialZAVectors = spatialZAForces;

			if (rootMotion)
				scratchData.jointForces = &cache.gravityCompensationForce[rootDof];
			else
				scratchData.jointForces = cache.jointForce;

			computeGeneralizedForceInv(mArticulationData, scratchData);

			//add the gravity compensation for the root, force first
			if (rootDof == 6)
			{
				Cm::SpatialVectorF* zaForce = scratchData.spatialZAVectors;
				cache.gravityCompensationForce[0] = zaForce[0].top.x;
				cache.gravityCompensationForce[1] = zaForce[0].top.y;
				cache.gravityCompensationForce[2] = zaForce[0].top.z;
				cache.gravityCompensationForce[3] = zaForce[0].bottom.x;
				cache.gravityCompensationForce[4] = zaForce[0].bottom.y;
				cache.gravityCompensationForce[5] = zaForce[0].bottom.z;
			}

			//release spatialZA vectors
			allocator->free(spatialZAForces);
		}
		else
		{
			//old API for floating-base articulations, the root link is assumed to be in free fall
			ScratchData scratchData;
			PxU8* tempMemory = allocateScratchSpatialData(allocator, linkCount, scratchData);

			scratchData.jointVelocities = NULL;
			scratchData.jointAccelerations = NULL;
			scratchData.jointForces = cache.jointForce;
			scratchData.externalAccels = NULL;

			inverseDynamicFloatingBase(mArticulationData, tGravity, scratchData, false);

			allocator->free(tempMemory);
		}

#if FEATHERSTONE_DEBUG
		//compare joint force
		const PxU32 totalDofs = mArticulationData.getDofs();
		for (PxU32 i = 0; i < totalDofs; ++i)
		{
			const PxReal dif = jointForce[i] - cache.jointForce[i];
			PX_ASSERT(PxAbs(dif) < 5e-3f);
		}

		PX_FREE(jointForce);
#endif
	}

	//gravity, acceleration and external force(external acceleration) are zero
	void  FeatherstoneArticulation::getCoriolisAndCentrifugalForce(PxArticulationCache& cache, const bool rootMotion)
	{
		if (mArticulationData.getDataDirty())
		{
			PxGetFoundation().error(PxErrorCode::eINVALID_OPERATION, PX_FL, "Articulation::getCoriolisAndCentrifugalForce() commonInit need to be called first to initialize data!");
			return;
		}

		const PxU32 linkCount = mArticulationData.getLinkCount();
		const bool fixBase = mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE;
		const PxU32 rootDof = fixBase ? 0 : 6;

		PxcScratchAllocator* allocator = reinterpret_cast<PxcScratchAllocator*>(cache.scratchAllocator);

		ScratchData scratchData;
		PxU8* tempMemory = allocateScratchSpatialData(allocator, linkCount, scratchData);

		scratchData.jointVelocities = cache.jointVelocity;
		scratchData.jointAccelerations = NULL;
		scratchData.externalAccels = NULL;
		if (rootMotion)
			scratchData.jointForces = &cache.coriolisForce[rootDof];
		else
			scratchData.jointForces = cache.jointForce;
		
		if (fixBase)
			inverseDynamic(mArticulationData, PxVec3(0.0f), scratchData, true);
		else
			inverseDynamicFloatingBase(mArticulationData, PxVec3(0.0f), scratchData, true, rootMotion);

		if(rootMotion && !fixBase)
		{
			//add the Coriolis compensation for the root, force first
			Cm::SpatialVectorF* zaForce = scratchData.spatialZAVectors;
			cache.coriolisForce[0] = zaForce[0].top.x;
			cache.coriolisForce[1] = zaForce[0].top.y;
			cache.coriolisForce[2] = zaForce[0].top.z;
			cache.coriolisForce[3] = zaForce[0].bottom.x;
			cache.coriolisForce[4] = zaForce[0].bottom.y;
			cache.coriolisForce[5] = zaForce[0].bottom.z;
		}

		allocator->free(tempMemory);
	}

	//gravity, joint acceleration and joint velocity are zero
	void  FeatherstoneArticulation::getGeneralizedExternalForce(PxArticulationCache& cache)
	{
		if (mArticulationData.getDataDirty())
		{
			PxGetFoundation().error(PxErrorCode::eINVALID_OPERATION, PX_FL, "Articulation::getCoriolisAndCentrifugalForce() commonInit need to be called first to initialize data!");
			return;
		}

		const PxU32 linkCount = mArticulationData.getLinkCount();

		PxcScratchAllocator* allocator = reinterpret_cast<PxcScratchAllocator*>(cache.scratchAllocator);

		ScratchData scratchData;
		PxU8* tempMemory = allocateScratchSpatialData(allocator, linkCount, scratchData);

		scratchData.jointVelocities = NULL;
		scratchData.jointAccelerations = NULL;
		scratchData.jointForces = cache.jointForce;
		
		Cm::SpatialVector* accels = reinterpret_cast<Cm::SpatialVector*>(allocator->alloc(sizeof(Cm::SpatialVector) * linkCount));

		//turn external forces to external accels
		for (PxU32 i = 0; i < linkCount; ++i)
		{
			ArticulationLink& link = mArticulationData.getLink(i);
			PxsBodyCore& core = *link.bodyCore;
			
			const PxSpatialForce& force = cache.externalForces[i];
			Cm::SpatialVector& accel = accels[i];

			accel.linear = force.force * core.inverseMass;

			PxMat33 inverseInertiaWorldSpace;
			Cm::transformInertiaTensor(core.inverseInertia, PxMat33(core.body2World.q), inverseInertiaWorldSpace);

			accel.angular = inverseInertiaWorldSpace * force.torque;
		}
		
		scratchData.externalAccels = accels;

		const bool fixBase = mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE;
		if (fixBase)
			inverseDynamic(mArticulationData, PxVec3(0.f), scratchData, false);
		else
			inverseDynamicFloatingBase(mArticulationData, PxVec3(0.f), scratchData, false);

		allocator->free(tempMemory);
		allocator->free(accels);
	}

	//provided joint acceleration, calculate joint force
	void FeatherstoneArticulation::getJointForce(PxArticulationCache& cache)
	{
		if (mArticulationData.getDataDirty())
		{
			PxGetFoundation().error(PxErrorCode::eINVALID_OPERATION, PX_FL, "ArticulationHelper::getJointForce() commonInit need to be called first to initialize data!");
			return;
		}

		//const PxU32 size = sizeof(PxReal) * mArticulationData.getDofs();
		PxcScratchAllocator* allocator = reinterpret_cast<PxcScratchAllocator*>(cache.scratchAllocator);
		//PxReal* jointVelocities = reinterpret_cast<PxReal*>(allocator->alloc(size));

		ScratchData scratchData;
		scratchData.jointVelocities = NULL;//jont velocity will be zero
		scratchData.jointAccelerations = cache.jointAcceleration; //input
		scratchData.jointForces = cache.jointForce; //output
		scratchData.externalAccels = NULL;

		PxU8* tempMemory = allocateScratchSpatialData(allocator, mArticulationData.getLinkCount(), scratchData);

		//make sure joint velocity be zero
		//PxMemZero(jointVelocities, sizeof(PxReal) * mArticulationData.getDofs());
		const bool fixBase = mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE;

		if (fixBase)
			inverseDynamic(mArticulationData, PxVec3(0.f), scratchData, false);
		else
			inverseDynamicFloatingBase(mArticulationData, PxVec3(0.f), scratchData, false);

		//allocator->free(jointVelocities);
		allocator->free(tempMemory);
	}

	void FeatherstoneArticulation::jcalcLoopJointSubspace(ArticulationJointCore* joint,
		ArticulationJointCoreData& jointDatum, SpatialSubspaceMatrix& T,
		const Cm::UnAlignedSpatialVector* jointAxis)
	{
		PX_UNUSED(jointDatum);
		const PxVec3 childOffset = -joint->childPose.p;
		const PxVec3 zero(0.f);

		//if the column is free, we put zero for it, this is for computing K(coefficient matrix)
		T.setNumColumns(6);

		//transpose(Tc)*S = 0
		//transpose(Ta)*S = 1
		switch (joint->jointType)
		{
		case PxArticulationJointType::ePRISMATIC:
		{
			PX_ASSERT(jointDatum.nbDof == 1);

			const PxVec3 rx = (joint->childPose.rotate(PxVec3(1.f, 0.f, 0.f))).getNormalized();
			const PxVec3 ry = (joint->childPose.rotate(PxVec3(0.f, 1.f, 0.f))).getNormalized();
			const PxVec3 rz = (joint->childPose.rotate(PxVec3(0.f, 0.f, 1.f))).getNormalized();

			//joint->activeForceSubspace.setNumColumns(1);

			if (jointAxis[0][3] == 1.f)
			{
				//x is the free translation axis
				T.setColumn(0, rx, zero);
				T.setColumn(1, ry, zero);
				T.setColumn(2, rz, zero);
				T.setColumn(3, zero, zero);
				T.setColumn(4, zero, ry);
				T.setColumn(5, zero, rz);

				//joint->activeForceSubspace.setColumn(0, PxVec3(0.f), rx);
			}
			else if (jointAxis[0][4] == 1.f)
			{
				//y is the free translation axis
				T.setColumn(0, rx, zero);
				T.setColumn(1, ry, zero);
				T.setColumn(2, rz, zero);
				T.setColumn(3, zero, rx);
				T.setColumn(4, zero, zero);
				T.setColumn(5, zero, rz);

				//joint->activeForceSubspace.setColumn(0, PxVec3(0.f), ry);
			}
			else if (jointAxis[0][5] == 1.f)
			{
				//z is the free translation axis
				T.setColumn(0, rx, zero);
				T.setColumn(1, ry, zero);
				T.setColumn(2, rx, zero);
				T.setColumn(3, zero, rx);
				T.setColumn(4, zero, ry);
				T.setColumn(5, zero, zero);

				//joint->activeForceSubspace.setColumn(0, PxVec3(0.f), rz);
			}

			break;
		}
		case PxArticulationJointType::eREVOLUTE:
		case PxArticulationJointType::eREVOLUTE_UNWRAPPED:
		{
			//joint->activeForceSubspace.setNumColumns(1);

			const PxVec3 rx = (joint->childPose.rotate(PxVec3(1.f, 0.f, 0.f))).getNormalized();
			const PxVec3 ry = (joint->childPose.rotate(PxVec3(0.f, 1.f, 0.f))).getNormalized();
			const PxVec3 rz = (joint->childPose.rotate(PxVec3(0.f, 0.f, 1.f))).getNormalized();

			const PxVec3 rxXd = rx.cross(childOffset);
			const PxVec3 ryXd = ry.cross(childOffset);
			const PxVec3 rzXd = rz.cross(childOffset);

			if (jointAxis[0][0] == 1.f)
			{
				//x is the free rotation axis

				T.setColumn(0, zero, zero);
				T.setColumn(1, ry, zero);
				T.setColumn(2, rz, zero);

				//joint->activeForceSubspace.setColumn(0, rx, PxVec3(0.f));

			}
			else if (jointAxis[0][1] == 1.f)
			{
				//y is the free rotation axis
				T.setColumn(0, rx, zero);
				T.setColumn(1, zero, zero);
				T.setColumn(2, rz, zero);

				//joint->activeForceSubspace.setColumn(0, ry, PxVec3(0.f));
			}
			else if (jointAxis[0][2] == 1.f)
			{
				//z is the rotation axis
				T.setColumn(0, rx, zero);
				T.setColumn(1, ry, zero);
				T.setColumn(2, zero, zero);

				//joint->activeForceSubspace.setColumn(0, rz, PxVec3(0.f));
			}

			T.setColumn(3, rxXd, rx);
			T.setColumn(4, ryXd, ry);
			T.setColumn(5, rzXd, rz);

			break;
		}
		case PxArticulationJointType::eSPHERICAL:
		{
			//joint->activeForceSubspace.setNumColumns(3);

			const PxVec3 rx = (joint->childPose.rotate(PxVec3(1.f, 0.f, 0.f))).getNormalized();
			const PxVec3 ry = (joint->childPose.rotate(PxVec3(0.f, 1.f, 0.f))).getNormalized();
			const PxVec3 rz = (joint->childPose.rotate(PxVec3(0.f, 0.f, 1.f))).getNormalized();

			const PxVec3 rxXd = rx.cross(childOffset);
			const PxVec3 ryXd = ry.cross(childOffset);
			const PxVec3 rzXd = rz.cross(childOffset);

			T.setColumn(0, zero, zero);
			T.setColumn(1, zero, zero);
			T.setColumn(2, zero, zero);

			T.setColumn(3, rxXd, rx);
			T.setColumn(4, ryXd, ry);
			T.setColumn(5, rzXd, rz);

			//need to implement constraint force subspace matrix and active force subspace matrix

			break;
		}
		case PxArticulationJointType::eFIX:
		{
			//joint->activeForceSubspace.setNumColumns(0);
			//T.setNumColumns(6);

			/*	const PxVec3 rx = (joint->childPose.rotate(PxVec3(1.f, 0.f, 0.f))).getNormalized();
			const PxVec3 ry = (joint->childPose.rotate(PxVec3(0.f, 1.f, 0.f))).getNormalized();
			const PxVec3 rz = (joint->childPose.rotate(PxVec3(0.f, 0.f, 1.f))).getNormalized();

			T.setColumn(0, rx, PxVec3(0.f));
			T.setColumn(1, ry, PxVec3(0.f));
			T.setColumn(2, rz, PxVec3(0.f));
			T.setColumn(3, PxVec3(0.f), rx);
			T.setColumn(4, PxVec3(0.f), ry);
			T.setColumn(5, PxVec3(0.f), rz);
			*/

			T.setColumn(0, PxVec3(1.f, 0.f, 0.f), zero);
			T.setColumn(1, PxVec3(0.f, 1.f, 0.f), zero);
			T.setColumn(2, PxVec3(0.f, 0.f, 1.f), zero);
			T.setColumn(3, zero, PxVec3(1.f, 0.f, 0.f));
			T.setColumn(4, zero, PxVec3(0.f, 1.f, 0.f));
			T.setColumn(5, zero, PxVec3(0.f, 0.f, 1.f));

			PX_ASSERT(jointDatum.nbDof == 0);
			break;
		}
		default:
			break;

		}
	}

	//This method supports just one loopJoint
	void FeatherstoneArticulation::getKMatrix(ArticulationJointCore* loopJoint, const PxU32 parentIndex, const PxU32 childIndex, PxArticulationCache& cache)
	{
		PX_UNUSED(loopJoint);
		PX_UNUSED(parentIndex);
		PX_UNUSED(childIndex);
		PX_UNUSED(cache);

		////initialize all tree links motion subspace matrix
		//jcalc(mArticulationData);

		////linkID is the parent link, ground is the child link so child link is the fix base
		//ArticulationLinkData& pLinkDatum = mArticulationData.getLinkData(parentIndex);

		//ArticulationLink& cLink = mArticulationData.getLink(childIndex);
		//ArticulationLinkData& cLinkDatum = mArticulationData.getLinkData(childIndex);
		//
		//ArticulationJointCoreData loopJointDatum;
		//loopJointDatum.computeJointDof(loopJoint);

		////this is constraintForceSubspace in child body space(T)
		//SpatialSubspaceMatrix T;

		////loop joint constraint subspace matrix(T)
		//jcalcLoopJointSubspace(loopJoint, loopJointDatum, T);

		//const PxU32 linkCount = mArticulationData.getLinkCount();
		////set Jacobian matrix to be zero
		//PxMemZero(cache.jacobian, sizeof(PxKinematicJacobian) * linkCount);

		////transform T to world space
		//PxTransform& body2World = cLink.bodyCore->body2World;

		//for (PxU32 ind = 0; ind < T.getNumColumns(); ++ind)
		//{
		//	Cm::SpatialVectorF& column = T[ind];
		//	T.setColumn(ind, body2World.rotate(column.top), body2World.rotate(column.bottom));
		//}

		//const Cm::SpatialVectorF& pAccel = pLinkDatum.motionAcceleration;
		//const Cm::SpatialVectorF& cAccel = cLinkDatum.motionAcceleration;

		//const Cm::SpatialVectorF& pVel = pLinkDatum.motionVelocity;
		//const Cm::SpatialVectorF& cVel = cLinkDatum.motionVelocity;

		//Cm::SpatialVectorF k = (pAccel - cAccel) + pVel.cross(cVel);
		//k = T.transposeMultiply(k);
		//k = -k;

		//PxU32 i = childIndex;
		//PxU32 j = parentIndex;

		//PxU32* index = NULL;

		//while (i != j)
		//{
		//	if (i > j)
		//		index = &i;
		//	else
		//		index = &j;

		//	const PxU32 linkIndex = *index;

		//	PxKinematicJacobian* K = cache.jacobian + linkIndex;

		//	ArticulationLink& link = mArticulationData.getLink(linkIndex);

		//	ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(linkIndex);

		//	SpatialSubspaceMatrix& S = jointDatum.motionMatrix;

		//	PxTransform& tBody2World = link.bodyCore->body2World;

		//	Cm::SpatialVectorF res;
		//	for (PxU32 ind = 0; ind < S.getNumColumns(); ++ind)
		//	{
		//		Cm::SpatialVectorF& sCol = S[ind];

		//		//transform spatial axis into world space
		//		sCol.top = tBody2World.rotate(sCol.top);
		//		sCol.bottom = tBody2World.rotate(sCol.bottom);

		//		res = T.transposeMultiply(sCol);
		//		res = -res;

		//		PxReal* kSubMatrix = K->j[ind];

		//		kSubMatrix[0] = res.top.x; kSubMatrix[1] = res.top.y; kSubMatrix[2] = res.top.z;
		//		kSubMatrix[3] = res.bottom.x; kSubMatrix[4] = res.bottom.y; kSubMatrix[5] = res.bottom.z;
		//	}

		//	//overwrite either i or j to its parent index
		//	*index = link.parent;
		//}
	}


	void FeatherstoneArticulation::getImpulseResponseSlowInv(Dy::ArticulationLink* links,
		const ArticulationData& data,
		PxU32 linkID0_,
		const Cm::SpatialVector& impulse0,
		Cm::SpatialVector& deltaV0,
		PxU32 linkID1_,
		const Cm::SpatialVector& impulse1,
		Cm::SpatialVector& deltaV1,
		PxReal* jointVelocities,
		Cm::SpatialVectorF* Z)
	{
		PX_UNUSED(jointVelocities);

		const PxU32 numLinks = data.getLinkCount();

		PX_ALLOCA(_stack, PxU32, numLinks);
		PxU32* stack = _stack;

		PxU32 i0, i1, ic;

		PxU32 linkID0 = linkID0_;
		PxU32 linkID1 = linkID1_;


		for (i0 = linkID0, i1 = linkID1; i0 != i1;)	// find common path
		{
			if (i0<i1)
				i1 = links[i1].parent;
			else
				i0 = links[i0].parent;
		}

		PxU32 common = i0;

		Cm::SpatialVectorF Z0(-impulse0.linear, -impulse0.angular);
		Cm::SpatialVectorF Z1(-impulse1.linear, -impulse1.angular);

		Z[linkID0] = Z0;
		Z[linkID1] = Z1;

		//for (i0 = linkID0; i0 != common; i0 = links[i0].parent)
		for (i0 = 0; linkID0 != common; linkID0 = links[linkID0].parent)
		{
			const PxU32 jointOffset = mArticulationData.getJointData(linkID0).jointOffset;
			const PxU8 dofCount = mArticulationData.getJointData(linkID0).nbDof;
			Z0 = propagateImpulseW(
					data.getRw(linkID0), 
					Z0, 
					NULL, &data.getWorldIsInvD(jointOffset), &data.getWorldMotionMatrix(jointOffset), dofCount,
					NULL);
			Z[links[linkID0].parent] = Z0;
			stack[i0++] = linkID0;
		}

		for (i1 = i0; linkID1 != common; linkID1 = links[linkID1].parent)
		{
			const PxU32 jointOffset = mArticulationData.getJointData(linkID1).jointOffset;
			const PxU8 dofCount = mArticulationData.getJointData(linkID1).nbDof;
			Z1 = propagateImpulseW(
					data.getRw(linkID1), 
					Z1, 
					NULL, &data.getWorldIsInvD(jointOffset), &data.getWorldMotionMatrix(jointOffset), dofCount,
					NULL);
			Z[links[linkID1].parent] = Z1;
			stack[i1++] = linkID1;
		}

		//KS - we can replace the following section of code with the impulse response matrix - until next comment!

		Cm::SpatialVectorF ZZ = Z0 + Z1;
		Z[common] = ZZ;
		for (ic = i1; common; common = links[common].parent)
		{
			const PxU32 jointOffset = mArticulationData.getJointData(common).jointOffset;
			const PxU8 dofCount = mArticulationData.getJointData(common).nbDof;
			Z[links[common].parent] = propagateImpulseW(
				data.getRw(common), 
				Z[common], 
				NULL, &data.getWorldIsInvD(jointOffset), &data.getMotionMatrix(jointOffset), dofCount,
				NULL);
			stack[ic++] = common;
		}

		if(data.getArticulationFlags() & PxArticulationFlag::eFIX_BASE)
			Z[0] = Cm::SpatialVectorF(PxVec3(0.f), PxVec3(0.f));

		//SpatialMatrix inverseArticulatedInertia = data.getLinkData(0).spatialArticulatedInertia.getInverse();
		const SpatialMatrix& inverseArticulatedInertia = data.getBaseInvSpatialArticulatedInertiaW();
		Cm::SpatialVectorF v = inverseArticulatedInertia * (-Z[0]);

		for (PxU32 index = ic; (index--) > i1;)
		{
			const PxU32 id = stack[index];
			const PxU32 jointOffset = mArticulationData.getJointData(id).jointOffset;
			const PxU32 dofCount = mArticulationData.getJointData(id).nbDof;
			v = FeatherstoneArticulation::propagateVelocityW(
				data.getRw(id), v, 
				data.mWorldSpatialArticulatedInertia[id], Z[id],
				NULL, data.mInvStIs[id], &data.getWorldMotionMatrix(jointOffset), dofCount,
				jointVelocities);
		}

		//Replace everything to here with the impulse response matrix multiply

		Cm::SpatialVectorF dv1 = v;
		for (PxU32 index = i1; (index--) > i0;)
		{
			const PxU32 id = stack[index];
			const PxU32 jointOffset = mArticulationData.getJointData(id).jointOffset;
			const PxU32 dofCount = mArticulationData.getJointData(id).nbDof;
			dv1 = FeatherstoneArticulation::propagateVelocityW(
				data.getRw(id), v, 
				data.mWorldSpatialArticulatedInertia[id], Z[id], 
				NULL, data.mInvStIs[id], &data.getWorldMotionMatrix(jointOffset), dofCount,
				jointVelocities);
		}

		Cm::SpatialVectorF dv0 = v;
		for (PxU32 index = i0; (index--) > 0;)
		{
			const PxU32 id = stack[index];
			const PxU32 jointOffset = mArticulationData.getJointData(id).jointOffset;
			const PxU32 dofCount = mArticulationData.getJointData(id).nbDof;
			dv0 = FeatherstoneArticulation::propagateVelocityW(
				data.getRw(id), v, 
				data.mWorldSpatialArticulatedInertia[id], Z[id], 
				NULL, data.mInvStIs[id], &data.getWorldMotionMatrix(jointOffset), dofCount,
				jointVelocities);
		}

		deltaV0.linear = dv0.bottom;
		deltaV0.angular = dv0.top;

		deltaV1.linear = dv1.bottom;
		deltaV1.angular = dv1.top;
	}

	void FeatherstoneArticulation::getImpulseSelfResponseInv(const bool fixBase, 
		PxU32 linkID0,
		PxU32 linkID1,
		Cm::SpatialVectorF* Z,
		const Cm::SpatialVector& impulse0,
		const Cm::SpatialVector& impulse1,
		Cm::SpatialVector& deltaV0,
		Cm::SpatialVector& deltaV1,
		PxReal* jointVelocities)
	{
		ArticulationLink* links = mArticulationData.getLinks();

		//transform p(impluse) from work space to the local space of link
		ArticulationLink& link = links[linkID1];
		//ArticulationLinkData& linkDatum = mArticulationData.getLinkData(linkID1);

		if (link.parent == linkID0)
		{
			PX_ASSERT(linkID0 == link.parent);
			PX_ASSERT(linkID0 < linkID1);
			
			//impulse is in world space
			const Cm::SpatialVector& imp1 = impulse1;
			const Cm::SpatialVector& imp0 = impulse0;


			Cm::SpatialVectorF pImpulse(imp0.linear, imp0.angular);

			PX_ASSERT(linkID0 == link.parent);

			const PxU32 jointOffset = mArticulationData.getJointData(linkID1).jointOffset;
			const PxU8 dofCount = mArticulationData.getJointData(linkID1).nbDof;
			
			//initialize child link spatial zero acceleration impulse
			Cm::SpatialVectorF Z1(-imp1.linear, -imp1.angular);
			//this calculate parent link spatial zero acceleration impulse
			Cm::SpatialVectorF Z0 = propagateImpulseW(
				mArticulationData.getRw(linkID1), 
				Z1,
				NULL, &mArticulationData.mISInvStIS[jointOffset], &mArticulationData.mWorldMotionMatrix[jointOffset], dofCount,
				NULL);

			//in parent space
			const Cm::SpatialVectorF impulseDif = pImpulse - Z0;

			Cm::SpatialVectorF delV0(PxVec3(0.f), PxVec3(0.f));
			Cm::SpatialVectorF delV1(PxVec3(0.f), PxVec3(0.f));

			//calculate velocity change start from the parent link to the root
			delV0 = FeatherstoneArticulation::getImpulseResponseWithJ(linkID0, fixBase, mArticulationData, Z, impulseDif, jointVelocities);

			//calculate velocity change for child link
			delV1 = FeatherstoneArticulation::propagateVelocityW(
				mArticulationData.getRw(linkID1), delV0, 
				mArticulationData.mWorldSpatialArticulatedInertia[linkID1], Z1, 
				NULL, mArticulationData.mInvStIs[linkID1], &mArticulationData.mWorldMotionMatrix[jointOffset], dofCount,
				jointVelocities);

			//translate delV0 and delV1 into world space again
			deltaV0.linear = delV0.bottom;
			deltaV0.angular = delV0.top;
			deltaV1.linear = delV1.bottom;
			deltaV1.angular = delV1.top;
		}
		else
		{
			getImpulseResponseSlowInv(links, mArticulationData, linkID0, impulse0, deltaV0, linkID1,impulse1, deltaV1, jointVelocities, Z);
		}
	}

	Cm::SpatialVectorF FeatherstoneArticulation::getImpulseResponseInv(
		const bool fixBase, const PxU32 linkID,
		Cm::SpatialVectorF* Z, 
		const Cm::SpatialVector& impulse,
		PxReal* jointVelocities)
	{
		//impulse lin is contact normal, and ang is raxn. R is body2World, R(t) is world2Body
		//| R(t),	0	|
		//| R(t)*r, R(t)|
		//r is the vector from center of mass to contact point
		//p(impluse) =	|n|
		//				|0|

		ArticulationLink* links = mArticulationData.getLinks();
		//ArticulationLinkData* linkData = mArticulationData.getLinkData();
		ArticulationJointCoreData* jointData = mArticulationData.getJointData();
		const PxU32 linkCount = mArticulationData.getLinkCount();

		//make sure all links' spatial zero acceleration impulse are zero
		PxMemZero(Z, sizeof(Cm::SpatialVectorF) * linkCount);

		Z[linkID] = Cm::SpatialVectorF(-impulse.linear, -impulse.angular);

		for (PxU32 i = linkID; i; i = links[i].parent)
		{
			ArticulationLink& tLink = links[i];
			const PxU32 jointOffset = mArticulationData.getJointData(i).jointOffset;
			const PxU8 dofCount = mArticulationData.getJointData(i).nbDof;
			//ArticulationLinkData& tLinkDatum = linkData[i];
			Z[tLink.parent] = propagateImpulseW(
				mArticulationData.getRw(i),
				Z[i], 
				NULL, &mArticulationData.mISInvStIS[jointOffset], &mArticulationData.mWorldMotionMatrix[jointOffset], dofCount,
				NULL);
		}

		//set velocity change of the root link to be zero
		Cm::SpatialVectorF deltaV = Cm::SpatialVectorF(PxVec3(0.f), PxVec3(0.f));
		if (!fixBase)
			deltaV = mArticulationData.mBaseInvSpatialArticulatedInertiaW * (-Z[0]);

		const PxU32 startIndex = links[linkID].mPathToRootStartIndex;
		const PxU32 numElems = links[linkID].mPathToRootCount;

		const PxU32* pathToRoot = &mArticulationData.mPathToRootElements[startIndex];

		for(PxU32 i = 0; i < numElems; ++i)
		{
			const PxU32 index = pathToRoot[i];
			PX_ASSERT(links[index].parent < index);

			ArticulationJointCoreData& tJointDatum = jointData[index];

			PxReal* jVelocity = &jointVelocities[tJointDatum.jointOffset];
			deltaV = propagateVelocityW(
				mArticulationData.getRw(index), deltaV, 
				mArticulationData.mWorldSpatialArticulatedInertia[index], Z[index], 
				NULL, mArticulationData.mInvStIs[index], &mArticulationData.mWorldMotionMatrix[tJointDatum.jointOffset], tJointDatum.nbDof,
				jVelocity);
		}

		return deltaV;

	}


	void FeatherstoneArticulation::getCoefficientMatrixWithLoopJoints(ArticulationLoopConstraint* lConstraints, const PxU32 nbConstraints, PxArticulationCache& cache)
	{
		if (mArticulationData.getDataDirty())
		{
			PxGetFoundation().error(PxErrorCode::eINVALID_OPERATION, PX_FL, "ArticulationHelper::getCoefficientMatrix() commonInit need to be called first to initialize data!");
			return;
		}

		computeArticulatedSpatialInertia(mArticulationData);

		const PxU32 linkCount = mArticulationData.getLinkCount();
		 
		PxReal* coefficientMatrix = cache.coefficientMatrix;

		const PxU32 elementCount = mArticulationData.getDofs();

		//zero coefficient matrix
		PxMemZero(coefficientMatrix, sizeof(PxReal) * elementCount * nbConstraints);

		const bool fixBase = mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE;

		PxcScratchAllocator* allocator = reinterpret_cast<PxcScratchAllocator*>(cache.scratchAllocator);
		ScratchData scratchData;
		PxU8* tempMemory = allocateScratchSpatialData(allocator, linkCount, scratchData);

		Cm::SpatialVectorF* Z = scratchData.spatialZAVectors;
		const PxU32 totalDofs = mArticulationData.getDofs();

		const PxU32 size = sizeof(PxReal) * totalDofs;

		PxU8* tData = reinterpret_cast<PxU8*>(allocator->alloc(size * 2));

		const PxReal invDt = 1.f / mArticulationData.getDt();
		PxReal* jointVelocities = reinterpret_cast<PxReal*>(tData);
		PxReal* jointAccelerations = reinterpret_cast<PxReal*>(tData + size);

		for (PxU32 a = 0; a < nbConstraints; ++a)
		{
			ArticulationLoopConstraint& lConstraint = lConstraints[a];
			Constraint* aConstraint = lConstraint.constraint;

			Px1DConstraint rows[MAX_CONSTRAINT_ROWS];
			setupConstraintRows(rows, MAX_CONSTRAINT_ROWS);
			
			const PxTransform idt(PxIdentity);
			const PxTransform& body2World0 = aConstraint->body0 ? aConstraint->bodyCore0->body2World : idt;
			const PxTransform& body2World1 = aConstraint->body1 ? aConstraint->bodyCore1->body2World : idt;

			PxVec3p unused_body0WorldOffset(0.0f);
			PxVec3p unused_ra, unused_rb;
			PxConstraintInvMassScale unused_invMassScales;
			//TAG:solverprepcall
			PxU32 constraintCount = (*aConstraint->solverPrep)(rows,
				unused_body0WorldOffset,
				MAX_CONSTRAINT_ROWS,
				unused_invMassScales,
				aConstraint->constantBlock,
				body2World0, body2World1, !!(aConstraint->flags & PxConstraintFlag::eENABLE_EXTENDED_LIMITS), unused_ra, unused_rb);

			const PxU32 linkIndex0 = lConstraint.linkIndex0;
			const PxU32 linkIndex1 = lConstraint.linkIndex1;

			//zero joint Velocites
			PxMemZero(jointVelocities, size);

			for (PxU32 j = 0; j < constraintCount; ++j)
			{
				Px1DConstraint& row = rows[j];

				if (linkIndex0 != 0x80000000 && linkIndex1 != 0x80000000)
				{
					const bool flip = linkIndex0 > linkIndex1;

					Cm::SpatialVector impulse0(row.linear0, row.angular0);
					Cm::SpatialVector impulse1(row.linear1, row.angular1);

					Cm::SpatialVector deltaV0, deltaV1;

					if (flip)
					{
						getImpulseSelfResponseInv(fixBase, linkIndex1, linkIndex0, Z, impulse1, impulse0,
							deltaV1, deltaV0, jointVelocities);
					}
					else
					{
						getImpulseSelfResponseInv(fixBase, linkIndex0, linkIndex1, Z, impulse0, impulse1,
							deltaV0, deltaV1, jointVelocities);
					}
				}
				else
				{
					if (linkIndex0 == 0x80000000)
					{
						Cm::SpatialVector impulse1(row.linear1, row.angular1);
						getImpulseResponseInv(fixBase, linkIndex1, Z, impulse1, jointVelocities);
					}
					else
					{
						Cm::SpatialVector impulse0(row.linear0, row.angular0);
						getImpulseResponseInv(fixBase, linkIndex0, Z, impulse0, jointVelocities);
					}
				}
			}

			//calculate joint acceleration due to velocity change
			for (PxU32 i = 0; i < totalDofs; ++i)
			{
				jointAccelerations[i] = jointVelocities[i] * invDt;
			}

			//reset spatial inertia
			computeSpatialInertia(mArticulationData);

			PxReal* coeCol = &coefficientMatrix[elementCount * a];

			//this means the joint force calculated by the inverse dynamic
			//will be just influenced by joint acceleration change
			scratchData.jointVelocities = NULL;
			scratchData.externalAccels = NULL;

			//Input
			scratchData.jointAccelerations = jointAccelerations;

			//a column of the coefficient matrix is the joint force
			scratchData.jointForces = coeCol;

			if (fixBase)
			{
				inverseDynamic(mArticulationData, PxVec3(0.f), scratchData, false);
			}
			else
			{
				inverseDynamicFloatingBase(mArticulationData, PxVec3(0.f), scratchData, false);
			}
		
			allocator->free(tData);
			allocator->free(tempMemory);
		}
	}

	void FeatherstoneArticulation::constraintPrep(ArticulationLoopConstraint* lConstraints, 
		const PxU32 nbJoints, PxSolverConstraintPrepDesc& prepDesc,
		PxSolverBody& sBody, PxSolverBodyData& sBodyData, PxSolverConstraintDesc* descs,
		PxConstraintAllocator& allocator)
	{
		const PxReal dt = mArticulationData.getDt();
		const PxReal invDt = 1.f / dt;
		//constraint prep
		for (PxU32 a = 0; a < nbJoints; ++a)
		{
			ArticulationLoopConstraint& lConstraint = lConstraints[a];
			Constraint* aConstraint = lConstraint.constraint;

			PxSolverConstraintDesc& desc = descs[a];
			prepDesc.desc = &desc;
			prepDesc.linBreakForce = aConstraint->linBreakForce;
			prepDesc.angBreakForce = aConstraint->angBreakForce;
			prepDesc.writeback = &mContext->getConstraintWriteBackPool()[aConstraint->index];
			setupConstraintFlags(prepDesc, aConstraint->flags);
			prepDesc.minResponseThreshold = aConstraint->minResponseThreshold;

			Px1DConstraint rows[MAX_CONSTRAINT_ROWS];
			setupConstraintRows(rows, MAX_CONSTRAINT_ROWS);

			prepDesc.invMassScales.linear0 = prepDesc.invMassScales.linear1 = prepDesc.invMassScales.angular0 = prepDesc.invMassScales.angular1 = 1.0f;
			prepDesc.body0WorldOffset = PxVec3(0.0f);

			const PxTransform idt(PxIdentity);
			const PxTransform& body2World0 = aConstraint->body0 ? aConstraint->bodyCore0->body2World : idt;
			const PxTransform& body2World1 = aConstraint->body1 ? aConstraint->bodyCore1->body2World : idt;

			PxVec3p unused_ra, unused_rb;
			PxConstraintInvMassScale unused_invMassScales;

			//TAG:solverprepcall
			prepDesc.numRows = (*aConstraint->solverPrep)(rows,
				prepDesc.body0WorldOffset,
				MAX_CONSTRAINT_ROWS,
				unused_invMassScales,
				aConstraint->constantBlock,
				body2World0, body2World1, !!(aConstraint->flags & PxConstraintFlag::eENABLE_EXTENDED_LIMITS),
				unused_ra, unused_rb);
	
			prepDesc.bodyFrame0 = body2World0;
			prepDesc.bodyFrame1 = body2World1;
			prepDesc.rows = rows;

			const PxU32 linkIndex0 = lConstraint.linkIndex0;
			const PxU32 linkIndex1 = lConstraint.linkIndex1;
		
			if (linkIndex0 != 0x80000000 && linkIndex1 != 0x80000000)
			{
				desc.articulationA = this;
				desc.articulationB = this;
				desc.linkIndexA = PxTo8(linkIndex0);
				desc.linkIndexB = PxTo8(linkIndex1);

				desc.bodyA = reinterpret_cast<PxSolverBody*>(this);
				desc.bodyB = reinterpret_cast<PxSolverBody*>(this);

				prepDesc.bodyState0 = PxSolverConstraintPrepDescBase::eARTICULATION;
				prepDesc.bodyState1 = PxSolverConstraintPrepDescBase::eARTICULATION;
			
			}
			else if (linkIndex0 == 0x80000000)
			{
				desc.articulationA = NULL;
				desc.articulationB = this;

				desc.linkIndexA = PxSolverConstraintDesc::RIGID_BODY;
				desc.linkIndexB = PxTo8(linkIndex1);

				desc.bodyA = &sBody;
				desc.bodyB = reinterpret_cast<PxSolverBody*>(this);

				prepDesc.bodyState0 = PxSolverConstraintPrepDescBase::eSTATIC_BODY;
				prepDesc.bodyState1 = PxSolverConstraintPrepDescBase::eARTICULATION;
			}
			else if (linkIndex1 == 0x80000000)
			{
				desc.articulationA = this;
				desc.articulationB = NULL;

				desc.linkIndexA = PxTo8(linkIndex0);
				desc.linkIndexB = PxSolverConstraintDesc::RIGID_BODY;

				desc.bodyA = reinterpret_cast<PxSolverBody*>(this);
				desc.bodyB = &sBody;

				prepDesc.bodyState0 = PxSolverConstraintPrepDescBase::eARTICULATION;
				prepDesc.bodyState1 = PxSolverConstraintPrepDescBase::eSTATIC_BODY;

			}

			prepDesc.body0 = desc.bodyA;
			prepDesc.body1 = desc.bodyB;
			prepDesc.data0 = &sBodyData;
			prepDesc.data1 = &sBodyData;
		
			ConstraintHelper::setupSolverConstraint(prepDesc, allocator, dt, invDt);
		}

	}

	class BlockBasedAllocator
	{
		struct AllocationPage
		{
			static const PxU32 PageSize = 32 * 1024;
			PxU8 mPage[PageSize];

			PxU32 currentIndex;

			AllocationPage() : currentIndex(0) {}

			PxU8* allocate(const PxU32 size)
			{
				PxU32 alignedSize = (size + 15)&(~15);
				if ((currentIndex + alignedSize) < PageSize)
				{
					PxU8* ret = &mPage[currentIndex];
					currentIndex += alignedSize;
					return ret;
				}
				return NULL;
			}
		};

		AllocationPage* currentPage;

		physx::PxArray<AllocationPage*> mAllocatedBlocks;
		PxU32 mCurrentIndex;

	public:
		BlockBasedAllocator() : currentPage(NULL), mCurrentIndex(0)
		{
		}

		virtual PxU8* allocate(const PxU32 byteSize)
		{
			if (currentPage)
			{
				PxU8* data = currentPage->allocate(byteSize);
				if (data)
					return data;
			}

			if (mCurrentIndex < mAllocatedBlocks.size())
			{
				currentPage = mAllocatedBlocks[mCurrentIndex++];
				currentPage->currentIndex = 0;
				return currentPage->allocate(byteSize);
			}
			currentPage = PX_PLACEMENT_NEW(PX_ALLOC(sizeof(AllocationPage), "AllocationPage"), AllocationPage)();
			mAllocatedBlocks.pushBack(currentPage);
			mCurrentIndex = mAllocatedBlocks.size();

			return currentPage->allocate(byteSize);
		}

		void release() { for (PxU32 a = 0; a < mAllocatedBlocks.size(); ++a) PX_FREE(mAllocatedBlocks[a]); mAllocatedBlocks.clear(); currentPage = NULL; mCurrentIndex = 0; }

		void reset() { currentPage = NULL; mCurrentIndex = 0; }

		virtual ~BlockBasedAllocator()
		{
			release();
		}
	};

	class ArticulationBlockAllocator : public PxConstraintAllocator
	{
		BlockBasedAllocator mConstraintAllocator;
		BlockBasedAllocator mFrictionAllocator[2];

		PxU32 currIdx;

	public:

		ArticulationBlockAllocator() : currIdx(0)
		{
		}

		virtual ~ArticulationBlockAllocator() {}

		virtual PxU8* reserveConstraintData(const PxU32 size)
		{
			return reinterpret_cast<PxU8*>(mConstraintAllocator.allocate(size));
		}

		virtual PxU8* reserveFrictionData(const PxU32 byteSize)
		{
			return reinterpret_cast<PxU8*>(mFrictionAllocator[currIdx].allocate(byteSize));
		}

		void release() { currIdx = 1 - currIdx; mConstraintAllocator.release(); mFrictionAllocator[currIdx].release(); }

		PX_NOCOPY(ArticulationBlockAllocator)

	};

	// forward declarations
	void solveExt1D(const PxSolverConstraintDesc& desc, bool isPositionIteration);
	void writeBack1D(const PxSolverConstraintDesc& desc);
	void conclude1D(const PxSolverConstraintDesc& desc);
	void clearExt1D(const PxSolverConstraintDesc& desc);

	bool FeatherstoneArticulation::getLambda(ArticulationLoopConstraint* lConstraints, const PxU32 nbJoints, 
		PxArticulationCache& cache, PxArticulationCache& initialState,
		const PxReal* jointTorque, const PxVec3& gravity, const PxU32 maxIter, const PxReal invLengthScale)
	{
		const PxReal dt = mArticulationData.getDt();
		const PxReal invDt = 1.f / dt;
		const PxU32 totalDofs = mArticulationData.getDofs();
		
		const PxU32 linkCount = mArticulationData.getLinkCount();
	
		ArticulationBlockAllocator bAlloc;

		PxcScratchAllocator* allocator = reinterpret_cast<PxcScratchAllocator*>(cache.scratchAllocator);

		Cm::SpatialVectorF* deltaV = reinterpret_cast<Cm::SpatialVectorF*>(allocator->alloc(sizeof(Cm::SpatialVectorF) * linkCount, true));

		PxReal* previousLambdas = reinterpret_cast<PxReal*>(allocator->alloc(sizeof(PxReal)*nbJoints * 2, true));
		PxReal* lambdas = cache.lambda;

		//this is the joint force changed caused by contact force based on impulse strength is 1
		PxReal* J = cache.coefficientMatrix; 

		PxSolverBody staticSolverBody;
		PxMemZero(&staticSolverBody, sizeof(PxSolverBody));
		PxSolverBodyData staticSolverBodyData;
		PxMemZero(&staticSolverBodyData, sizeof(PxSolverBodyData));
		staticSolverBodyData.maxContactImpulse = PX_MAX_F32;
		staticSolverBodyData.penBiasClamp = -PX_MAX_F32;
		staticSolverBodyData.body2World = PxTransform(PxIdentity);

		PxSolverConstraintDesc* desc = reinterpret_cast<PxSolverConstraintDesc*>(allocator->alloc(sizeof(PxSolverConstraintDesc) * nbJoints, true));
		ArticulationSolverDesc artiDesc;
		
		PxSolverConstraintDesc* constraintDescs = reinterpret_cast<PxSolverConstraintDesc*>(allocator->alloc(sizeof(PxSolverConstraintDesc) * mArticulationData.getLinkCount()-1, true));
		
		//run forward dynamic to calculate the lamba

		artiDesc.articulation = this;
		PxU32 acCount = 0;
		computeUnconstrainedVelocities(artiDesc, dt, acCount, gravity, invLengthScale);

		ScratchData scratchData;
		scratchData.motionVelocities = mArticulationData.getMotionVelocities();
		scratchData.motionAccelerations = mArticulationData.getMotionAccelerations();
		scratchData.coriolisVectors = mArticulationData.getCorioliseVectors();
		scratchData.spatialZAVectors = mArticulationData.getSpatialZAVectors();
		scratchData.jointAccelerations = mArticulationData.getJointAccelerations();
		scratchData.jointVelocities = mArticulationData.getJointVelocities();
		scratchData.jointPositions = mArticulationData.getJointPositions();
		scratchData.jointForces = mArticulationData.getJointForces();
		scratchData.externalAccels = mArticulationData.getExternalAccelerations();

		//prepare constraint data
		PxSolverConstraintPrepDesc prepDesc;
		constraintPrep(lConstraints, nbJoints, prepDesc, staticSolverBody,
			staticSolverBodyData, desc, bAlloc);

		for (PxU32 i = 0; i < nbJoints; ++i)
		{
			previousLambdas[i] = PX_MAX_F32;
		}

		bool found = true;

		for (PxU32 iter = 0; iter < maxIter; ++iter)
		{
			found = true;
			for (PxU32 i = 0; i < nbJoints; ++i)
			{
				clearExt1D(desc[i]);
			}

			//solve
			for (PxU32 itr = 0; itr < 4; itr++)
			{
				for (PxU32 i = 0; i < nbJoints; ++i)
				{
					solveExt1D(desc[i], true); //TODO: Should error accumulation be suppressed?
				}
			}
			for (PxU32 i = 0; i < nbJoints; ++i)
			{
				conclude1D(desc[i]);
			}

			PxcFsFlushVelocity(*this, deltaV);

			for (PxU32 i = 0; i < nbJoints; ++i)
			{
				solveExt1D(desc[i], true); //TODO: Should error accumulation be suppressed?
				writeBack1D(desc[i]);
			}

			PxReal eps = 1e-5f;
			for (PxU32 i = 0; i < nbJoints; ++i)
			{
				Dy::Constraint* constraint = lConstraints->constraint;

				Dy::ConstraintWriteback& solverOutput = mContext->getConstraintWriteBackPool()[constraint->index];
				PxVec3 linearForce = solverOutput.linearImpulse * invDt;

				//linear force is normalize so lambda is the magnitude of linear force
				lambdas[i] = linearForce.magnitude() * dt;

				const PxReal dif = PxAbs(previousLambdas[i] - lambdas[i]);
				if (dif > eps)
					found = false;
		
				previousLambdas[i] = lambdas[i];
			}

			if (found)
				break;

			//joint force
			PxReal* jf3 = cache.jointForce;

			//zero the joint force buffer
			PxMemZero(jf3, sizeof(PxReal)*totalDofs);

			for (PxU32 colInd = 0; colInd < nbJoints; ++colInd)
			{
				PxReal* col = &J[colInd * totalDofs];

				for (PxU32 j = 0; j < totalDofs; ++j)
				{
					jf3[j] += col[j] * lambdas[colInd];
				}
			}

			//jointTorque is M(q)*qddot + C(q,qdot)t - g(q)
			//jointTorque - J*lambda.
			for (PxU32 j = 0; j < totalDofs; ++j)
			{
				jf3[j] = jointTorque[j] - jf3[j];
			}

			bool shouldWake = false;
			//reset all joint velocities/
			applyCache(initialState, PxArticulationCacheFlag::eALL, shouldWake);

			//copy constraint torque to internal data
			applyCache(cache, PxArticulationCacheFlag::eFORCE, shouldWake);

			mArticulationData.init();
			
			computeLinkVelocities(mArticulationData, scratchData);
			computeZ(mArticulationData, gravity, scratchData);
			computeArticulatedSpatialZ(mArticulationData, scratchData);

			{
			//Constant terms.
			const bool doIC = true;
			const PxArticulationFlags articulationFlags = mArticulationData.getArticulationFlags();
			const ArticulationLink* links = mArticulationData.getLinks();
			const ArticulationJointCoreData* jointDatas = mArticulationData.getJointData();
			const Cm::SpatialVectorF* linkSpatialZAExtForces = scratchData.spatialZAVectors;
			const Cm::SpatialVectorF* linkCoriolisForces = scratchData.coriolisVectors;
			const PxVec3* linkRws = mArticulationData.getRw(); 
			const Cm::UnAlignedSpatialVector* jointDofMotionMatrices = mArticulationData.getWorldMotionMatrix();
			const SpatialMatrix& baseInvSpatialArticulatedInertiaW = mArticulationData.getBaseInvSpatialArticulatedInertiaW();

			//Cached constant terms.
			const InvStIs* linkInvStIs = mArticulationData.getInvStIS();
			const Cm::SpatialVectorF* jointDofIsWs = mArticulationData.getIsW();
			const PxReal* jointDofQstZics = mArticulationData.getQstZIc();	

			//Output
			Cm::SpatialVectorF* linkMotionVelocities = scratchData.motionVelocities;
			Cm::SpatialVectorF* linkMotionAccelerations = scratchData.motionAccelerations;
			PxReal* jointAccelerations = scratchData.jointAccelerations;
			PxReal* jointVelocities = scratchData.jointVelocities;
			PxReal* jointNewVelocities = mArticulationData.getJointNewVelocities();

			computeLinkAcceleration(
					doIC, dt, 
					articulationFlags,
					links, linkCount, jointDatas,
					linkSpatialZAExtForces, linkCoriolisForces, linkRws,
					jointDofMotionMatrices, baseInvSpatialArticulatedInertiaW,
					linkInvStIs, jointDofIsWs, jointDofQstZics,
					linkMotionAccelerations, linkMotionVelocities, 
					jointAccelerations, jointVelocities, jointNewVelocities);			
			}

			//zero zero acceleration vector in the articulation data so that we can use this buffer to accumulated
			//impulse for the contacts/constraints in the PGS/TGS solvers
			PxMemZero(mArticulationData.getSpatialZAVectors(), sizeof(Cm::SpatialVectorF) * linkCount);		
		}
		
		allocator->free(constraintDescs);
		allocator->free(previousLambdas);
		allocator->free(deltaV);
		allocator->free(desc);
		bAlloc.release();
		
		bool shouldWake = false;
		//roll back to the current stage
		applyCache(initialState, PxArticulationCacheFlag::eALL, shouldWake);

		return found;

	}

	//i is the current link ID, we need to compute the row/column related to the joint i with all the other joints
	PxU32 computeHi(ArticulationData& data, const PxU32 linkID, PxReal* massMatrix, Cm::SpatialVectorF* f, const PxU32 rootDof = 0)
	{
		ArticulationLink* links = data.getLinks();

		ArticulationJointCoreData& jointDatum = data.getJointData(linkID);

		const PxU32 totalDofs = data.getDofs();
		const PxU32 matSize = totalDofs + rootDof;

		//Hii
		for (PxU32 ind = 0; ind < jointDatum.nbDof; ++ind)
		{
			const PxU32 row = jointDatum.jointOffset + ind + rootDof;
			const Cm::SpatialVectorF& tf = f[ind];
			for (PxU32 ind2 = 0; ind2 < jointDatum.nbDof; ++ind2)
			{
				const PxU32 col = jointDatum.jointOffset + ind2 + rootDof;
				const Cm::UnAlignedSpatialVector& sa = data.getWorldMotionMatrix(jointDatum.jointOffset + ind2);
				massMatrix[row * matSize + col] = sa.innerProduct(tf);
			}
		}

		PxU32 j = linkID;

		ArticulationLink* jLink = &links[j];
		while (jLink->parent != 0)
		{
			for (PxU32 ind = 0; ind < jointDatum.nbDof; ++ind)
			{
				//f[ind] = data.getChildToParent(j) * f[ind];
				f[ind] = FeatherstoneArticulation::translateSpatialVector(data.getRw(j), f[ind]);
			}

			//assign j to the parent link
			j = jLink->parent;
			jLink = &links[j];

			//Hij and Hji
			ArticulationJointCoreData& pJointDatum = data.getJointData(j);

			for (PxU32 ind = 0; ind < pJointDatum.nbDof; ++ind)
			{
				const Cm::UnAlignedSpatialVector& sa = data.getWorldMotionMatrix(pJointDatum.jointOffset + ind);
				const PxU32 col = pJointDatum.jointOffset + ind + rootDof;

				for (PxU32 ind2 = 0; ind2 < jointDatum.nbDof; ++ind2)
				{
					const PxU32 row = jointDatum.jointOffset + ind2 + rootDof;

					Cm::SpatialVectorF& fcol = f[ind2];

					PxReal m = sa.innerProduct(fcol);
					massMatrix[row * matSize + col] = m;
					massMatrix[row + col * matSize] = m;
				}
			}
		}
		return j;
	}

	void FeatherstoneArticulation::calculateHFixBase(PxArticulationCache& cache)
	{
		const PxU32 elementCount = mArticulationData.getDofs();

		PxReal* massMatrix = cache.massMatrix;

		PxMemZero(massMatrix, sizeof(PxReal) * elementCount * elementCount);

		const PxU32 linkCount = mArticulationData.getLinkCount();

		PxcScratchAllocator* allocator = reinterpret_cast<PxcScratchAllocator*>(cache.scratchAllocator);

		ArticulationLink* links = mArticulationData.getLinks();

		const PxU32 startIndex = PxU32(linkCount - 1);

		Dy::SpatialMatrix* compositeSpatialInertia = reinterpret_cast<Dy::SpatialMatrix*>(allocator->alloc(sizeof(Dy::SpatialMatrix) * linkCount));

		//initialize composite spatial inertial
		initCompositeSpatialInertia(mArticulationData, compositeSpatialInertia);

		Cm::SpatialVectorF F[6];
		for (PxU32 i = startIndex; i > 0; --i)
		{
			ArticulationLink& link = links[i];

			Dy::SpatialMatrix cSpatialInertia = compositeSpatialInertia[i];
			//transform current link's spatial inertia to parent's space
			PxVec3 rw = link.bodyCore->body2World.p - links[link.parent].bodyCore->body2World.p;
			FeatherstoneArticulation::translateInertia(FeatherstoneArticulation::constructSkewSymmetricMatrix(rw), cSpatialInertia);

			//compute parent's composite spatial inertia
			compositeSpatialInertia[link.parent] += cSpatialInertia;

			Dy::SpatialMatrix& tSpatialInertia = compositeSpatialInertia[i];

			ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(i);

			for (PxU32 ind = 0; ind < jointDatum.nbDof; ++ind)
			{
				Cm::UnAlignedSpatialVector& sa = mArticulationData.mWorldMotionMatrix[jointDatum.jointOffset + ind];
				Cm::UnAlignedSpatialVector tmp = tSpatialInertia* sa;
				F[ind].top = tmp.top;
				F[ind].bottom = tmp.bottom;
			}

			//Hii, Hij, Hji
			computeHi(mArticulationData, i, massMatrix, F);
		}

		allocator->free(compositeSpatialInertia);
	}

	void FeatherstoneArticulation::calculateHFloatingBase(PxArticulationCache& cache, const bool rootMotion)
	{
		const PxU32 elementCount = mArticulationData.getDofs();
		const PxU32 rootDof = rootMotion ? 6 : 0;

		PxReal* massMatrix = cache.massMatrix;

		PxMemZero(massMatrix, sizeof(PxReal) * (elementCount + rootDof) * (elementCount + rootDof));

		const PxU32 linkCount = mArticulationData.getLinkCount();
		const PxU32 dofCount = mArticulationData.getDofs();
		const PxU32 matSize = dofCount + rootDof; // Add the DoF of the root

		PxcScratchAllocator* allocator = reinterpret_cast<PxcScratchAllocator*>(cache.scratchAllocator);

		ArticulationLink* links = mArticulationData.getLinks();
		// ArticulationLinkData* linkData = mArticulationData.getLinkData();

		const PxU32 startIndex = PxU32(linkCount - 1);

		Dy::SpatialMatrix* compositeSpatialInertia = reinterpret_cast<Dy::SpatialMatrix*>(allocator->alloc(sizeof(Dy::SpatialMatrix) * linkCount));
		Cm::SpatialVectorF* F = reinterpret_cast<Cm::SpatialVectorF*>(allocator->alloc(sizeof(Cm::SpatialVectorF) * elementCount));

		// initialize composite spatial inertial
		initCompositeSpatialInertia(mArticulationData, compositeSpatialInertia);

		for(PxU32 i = startIndex; i > 0; --i)
		{
			ArticulationLink& link = links[i];

			Dy::SpatialMatrix cSpatialInertia = compositeSpatialInertia[i];
			// transform current link's spatial inertia to parent's space
			PxVec3 rw = link.bodyCore->body2World.p - links[link.parent].bodyCore->body2World.p;
			FeatherstoneArticulation::translateInertia(FeatherstoneArticulation::constructSkewSymmetricMatrix(rw), cSpatialInertia);

			// compute parent's composite spatial inertia
			compositeSpatialInertia[link.parent] += cSpatialInertia;

			Dy::SpatialMatrix& tSpatialInertia = compositeSpatialInertia[i];

			ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(i);

			Cm::SpatialVectorF* f = &F[jointDatum.jointOffset];

			for (PxU32 ind = 0; ind < jointDatum.nbDof; ++ind)
			{
				Cm::UnAlignedSpatialVector& sa = mArticulationData.mWorldMotionMatrix[jointDatum.jointOffset + ind];
				Cm::UnAlignedSpatialVector tmp = tSpatialInertia * sa;
				f[ind].top = tmp.top;
				f[ind].bottom = tmp.bottom;
			}

			// Hii, Hij, Hji
			const PxU32 j = computeHi(mArticulationData, i, massMatrix, f, rootDof);

			// transform F to the base link space
			// ArticulationLinkData& fDatum = linkData[j];

			PxVec3 brw = links[j].bodyCore->body2World.p - links[0].bodyCore->body2World.p;
			for (PxU32 ind = 0; ind < jointDatum.nbDof; ++ind)
			{
				f[ind] = translateSpatialVector(brw, f[ind]);
			}

			if(rootMotion)
			{
				// add the resulting force on the root
				for(PxU32 ind = 0; ind < jointDatum.nbDof; ++ind)
				{
					const PxU32 col = jointDatum.jointOffset + ind + rootDof;
					const Cm::SpatialVectorF& tf = f[ind];
					for(PxU32 row = 0; row < 6; ++row)
					{
						massMatrix[col * matSize + row] = tf[row];
						massMatrix[col + row * matSize] = tf[row];
					}
				}
			}
		}

		if(rootMotion)
		{
			// adding the spatial articulated inertia of the root
			// note that the spatial articulated inertia assumes that the root angular acceleration comes first,
			// while the mass matrix assumes that the root linear acceleration comes first
			// we have therefore to invert the angular and linear component of the spatial articulated inertia
			// this also ensures that the mass matrix is symmetric
			const PxReal* rootSpatialInertia = reinterpret_cast<PxReal*>(&compositeSpatialInertia[0]);
			for(PxU32 row = 0; row < 6; ++row)
			{
				const PxU32 rowSpatialInertia = (row < 3) ? row : row - 3; // Convert to the index of a 3 x 3 matrix
				// Only process elements above the diagonal as the matrix is symmetric
				for(PxU32 col = row; col < 6; ++col)
				{
					// This offset is due to how the spatial matrix is indexed and how the linear amd angular components
					// of the acceleration should be inverted, the index is as follows
					//	0	3	6	9	12	15								9	12	15	0	3	6
					//	1	4	7	10	13	16								10	23	16	1	4	7
					//	2	5	8	11	14	17	inversion linear/angular	11	24	17	2	5	8
					//	18	21	24	0	1	2	------------------------>	0	1	2	18	21	24
					//	19	22	25	3	4	5								3	4	5	19	22	25
					//	20	23	26	6	7	8								6	7	8	20	23	26
					const PxU32 offset = (row > 2) ? 18 : (col < 3) * 9;
					const PxU32 colSpatialInertia = (col < 3) ? col : col - 3; // Convert to the index of a 3 x 3 matrix
					const PxU32 index = offset + colSpatialInertia * 3 + rowSpatialInertia;
					massMatrix[row * matSize + col] = rootSpatialInertia[index];
					massMatrix[col * matSize + row] = rootSpatialInertia[index];
				}
			}
		}
		else
		{
			//Ib = base link composite inertia tensor
			//compute transpose(F) * inv(Ib) *F
			Dy::SpatialMatrix invI0 = compositeSpatialInertia[0].invertInertia();

			//H - transpose(F) * inv(Ib) * F;
			for (PxU32 row = 0; row < elementCount; ++row)
			{
				const Cm::SpatialVectorF& f = F[row];
				for (PxU32 col = 0; col < elementCount; ++col)
				{
					const Cm::SpatialVectorF invIf = invI0 * F[col];
					const PxReal v = f.innerProduct(invIf);
					const PxU32 index = row * elementCount + col;
					massMatrix[index] = massMatrix[index] - v;
				}
			}
		}

		allocator->free(compositeSpatialInertia);
		allocator->free(F);
	}

	//calculate a single column of the mass matrix, assuming the presence of a joint acceleration and the absence of:
	//Coriolis effects, external forces, motionVelocities, root acceleration
	//Note that it assumes that the spatial articulated inertia are already calculated
	void FeatherstoneArticulation::calculateMassMatrixColInv(ScratchData& scratchData)
	{
		const PxU32 linkCount = mArticulationData.getLinkCount();

		Cm::SpatialVectorF* motionAccelerations = scratchData.motionAccelerations;
		Cm::SpatialVectorF* spatialZAForces = scratchData.spatialZAVectors;

		// Input
		PxReal* jointAccelerations = scratchData.jointAccelerations;

		// set base link motion acceleration to be zero because H should
		// be just affected by joint position/link position
		motionAccelerations[0] = Cm::SpatialVectorF::Zero();
		spatialZAForces[0] = Cm::SpatialVectorF::Zero();

		// First pass:
		//- Propagates the motion acceleration outward
		//- Calculate the first term of the joint force (I * A)
		for(PxU32 linkID = 1; linkID < linkCount; ++linkID)
		{
			ArticulationLink& link = mArticulationData.getLink(linkID);
			ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(linkID);

			// parent motion accelerations into child space
			Cm::SpatialVectorF accel = translateSpatialVector(-mArticulationData.getRw(linkID), motionAccelerations[link.parent]);
			const PxReal* jAcceleration = &jointAccelerations[jointDatum.jointOffset];

			for (PxU32 ind = 0; ind < jointDatum.nbDof; ++ind)
			{
				accel.top += mArticulationData.mWorldMotionMatrix[jointDatum.jointOffset + ind].top * jAcceleration[ind];
				accel.bottom += mArticulationData.mWorldMotionMatrix[jointDatum.jointOffset + ind].bottom * jAcceleration[ind];
			}

			motionAccelerations[linkID] = accel;

			spatialZAForces[linkID] = mArticulationData.mWorldSpatialArticulatedInertia[linkID] * accel;
		}

		// Second pass:
		//- Propagates forces inward (F_{i-1} = I_{i-1} * a_{i-1} + X * Z_i)
		//- Calculate joint force (tau_i = S_i * F_i)
		computeGeneralizedForceInv(mArticulationData, scratchData);
	}

	void FeatherstoneArticulation::getGeneralizedMassMatrixCRB(PxArticulationCache& cache, const bool rootMotion)
	{
		if (mArticulationData.getDataDirty())
		{
			PxGetFoundation().error(PxErrorCode::eINVALID_OPERATION, PX_FL, "ArticulationHelper::getGeneralizedMassMatrix() commonInit need to be called first to initialize data!");
			return;
		}

		const bool fixBase = mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE;
		if (fixBase)
		{
			calculateHFixBase(cache);
		}
		else
		{
			calculateHFloatingBase(cache, rootMotion);
		}

	}

	void FeatherstoneArticulation::getGeneralizedMassMatrix( PxArticulationCache& cache)
	{
		if (mArticulationData.getDataDirty())
		{
			PxGetFoundation().error(PxErrorCode::eINVALID_OPERATION, PX_FL, "ArticulationHelper::getGeneralizedMassMatrix() commonInit need to be called first to initialize data!");
			return;
		}


		//calculate each column for mass matrix
		PxReal* massMatrix = cache.massMatrix;
		
		const PxU32 linkCount = mArticulationData.getLinkCount();

		const PxU32 elementCount = mArticulationData.getDofs();

		const PxU32 size = sizeof(PxReal) * elementCount;
		PxcScratchAllocator* allocator = reinterpret_cast<PxcScratchAllocator*>(cache.scratchAllocator);

		ScratchData scratchData;
		PxU8* tempMemory = allocateScratchSpatialData(allocator, linkCount, scratchData);

		PxReal* jointAccelerations = reinterpret_cast<PxReal*>(allocator->alloc(size));

		scratchData.jointAccelerations = jointAccelerations;
		scratchData.jointVelocities = NULL;
		scratchData.externalAccels = NULL;

		const bool fixBase = mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE;
		
		//initialize jointAcceleration to be zero
		PxMemZero(jointAccelerations, size);

		for (PxU32 colInd = 0; colInd < elementCount; ++colInd)
		{
			PxReal* col = &massMatrix[colInd * elementCount];

			scratchData.jointForces = col;

			//set joint acceleration 1 in the col + 1 and zero elsewhere
			jointAccelerations[colInd] = 1;

			if (fixBase)
			{
				//jointAcceleration is Q, HQ = ID(model, qdot, Q).
				calculateMassMatrixColInv(scratchData);
			}
			else
			{
				inverseDynamicFloatingBase(mArticulationData, PxVec3(0.f), scratchData, false);
			}

			//reset joint acceleration to be zero
			jointAccelerations[colInd] = 0;
		}

		allocator->free(jointAccelerations);
		allocator->free(tempMemory);
	}

	PxVec3 FeatherstoneArticulation::getArticulationCOM(const bool rootFrame)
	{
		const PxU32 linkCount = mArticulationData.getLinkCount();
		PxVec3 articulationCOM = PxVec3(0.0f);
		PxReal totalMass = 0.0f; // needed because mInvSumMass is not available on GPU
		for(PxU32 linkID = 0; linkID < linkCount; ++linkID)
		{
			const PxVec3 childPose = mArticulationData.mLinks[linkID].bodyCore->body2World.p;
			// This is because mArticulationData.mMasses[linkID] is not always available on GPU
			PxReal mass = 1.0f / mArticulationData.mLinks[linkID].bodyCore->inverseMass;
			totalMass += mass;

			articulationCOM += childPose * mass;
		}
		articulationCOM /= totalMass;

		if(rootFrame)
		{
			articulationCOM = mArticulationData.mLinks[0].bodyCore->body2World.getInverse().transform(articulationCOM);
		}

		return articulationCOM;
	}

	void FeatherstoneArticulation::getCentroidalMomentumMatrix(PxArticulationCache& cache)
	{
		if(mArticulationData.getDataDirty())
		{
			PxGetFoundation().error(PxErrorCode::eINVALID_OPERATION, PX_FL, "ArticulationHelper::getCentroidalMomentumMatrix() commonInit need to be called first to initialize data!");
			return;
		}

		PxReal* centroidalMomentumMatrix = cache.centroidalMomentumMatrix;
		PxReal* centroidalMomentumBias = cache.centroidalMomentumBias;
		const PxU32 dofCount = mArticulationData.getDofs();

		// adding mass matrix terms corresponding to the root DoFs
		for(PxU32 col = 0; col < dofCount + 6; ++col)
		{
			for(PxU32 row = 0; row < 6; ++row)
			{
				centroidalMomentumMatrix[col + row * (dofCount + 6)] = cache.massMatrix[col + row * (dofCount + 6)];
			}
		}

		// calculating the COM momentum from the spatial articulated inertia of the root in the mass matrix
		const PxReal totalMass = cache.massMatrix[2 * (dofCount + 6) + 2];
		const PxVec3 pg =
			PxVec3(cache.massMatrix[2 * (dofCount + 6) + 4], cache.massMatrix[5], cache.massMatrix[(dofCount + 6) + 3]) / totalMass;
		PxMat33 mat = FeatherstoneArticulation::constructSkewSymmetricMatrix(pg);

		// adding remaining contributions
		for(PxU32 col = 0; col < dofCount + 6; ++col)
		{
			const PxVec3 m = PxVec3(cache.massMatrix[0 * (dofCount + 6) + col], cache.massMatrix[1 * (dofCount + 6) + col],
									cache.massMatrix[2 * (dofCount + 6) + col]);
			const PxVec3 tmp = mat * m;

			for(PxU32 row = 3; row < 6; ++row)
			{
				centroidalMomentumMatrix[col + row * (dofCount + 6)] += tmp[row - 3];
			}
		}

		// adding coriolis force terms corresponding to the root DoFs
		for(PxU32 row = 0; row < 6; ++row)
		{
			centroidalMomentumBias[row] = cache.coriolisForce[row];
		}

		// adding remaining contributions
		const PxVec3 m = PxVec3(cache.coriolisForce[0], cache.coriolisForce[1], cache.coriolisForce[2]);
		const PxVec3 tmp = mat * m;
		for(PxU32 row = 3; row < 6; ++row)
		{
			centroidalMomentumBias[row] += tmp[row - 3];
		}
	}
} //namespace Dy

}