Skip to content

A fast 2D and 3D game engine — TypeScript SDK, C++/WebAssembly core, visual editor. Ship one project to web, desktop, WeChat MiniGames, playable ads, and native Android / iOS.

194 KB
Raw Blame
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// SPDX-FileCopyrightText: Copyright (c) 2008-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
// SPDX-License-Identifier: Apache-2.0

#include "DyFeatherstoneArticulation.h"
#include "DyDynamics.h"
#include "DyConstraintPrep.h"
#include "PxsContactManager.h"
#include "DyContactPrep.h"
#include "DySolverContext.h"
#include "DyTGSContactPrep.h"

#ifndef FEATURESTONE_DEBUG
#define FEATURESTONE_DEBUG 0
#endif

namespace physx
{
namespace Dy
{
	// PT: using C-style arrays instead of PxArrays saves 320 bytes per articulation

	ArticulationData::~ArticulationData()
	{
		PX_FREE(mISInvStIS);
		PX_FREE(mWorldMotionMatrix);
		PX_FREE(mMotionMatrix);
		PX_FREE(qstZIntIc);
		PX_FREE(qstZIc);
		PX_FREE(mIsW);
		PX_FREE(mDeferredQstZ);
		PX_FREE(mPosIterJointVelocities);
		PX_FREE(mJointTargetVelocities);
		PX_FREE(mJointTargetPositions);
		PX_FREE(mJointForce);
		PX_FREE(mJointPosition);
		PX_FREE(mJointNewVelocity);
		PX_FREE(mJointVelocity);
		PX_FREE(mJointAcceleration);

		PX_FREE(mRelativeQuat);
		PX_FREE(mRw);
		PX_FREE(mJointTransmittedForce);
		PX_FREE(mPosIterMotionVelocities);
		PX_FREE(mDeltaQ);
		PX_FREE(mAccumulatedPoses);
		PX_FREE(mInvStIs);
		PX_FREE(mMasses);
		PX_FREE(mWorldIsolatedSpatialArticulatedInertia);
		PX_FREE(mWorldSpatialArticulatedInertia);
		PX_FREE(mResponseMatrixW);
		PX_FREE(mPreTransform);
		PX_FREE(mDeltaMotionVector);
		PX_FREE(mStaticContactConstraintStartIndex);
		PX_FREE(mNbStaticContactConstraints);
		PX_FREE(mStatic1DConstraintStartIndex);
		PX_FREE(mNbStatic1DConstraints);
		PX_FREE(mZAInternalForces);
		PX_FREE(mZAForces);
		PX_FREE(mCoriolisVectors);
		PX_FREE(mMotionAccelerationsInternal);
		PX_FREE(mLinkIncomingJointForces);
		PX_FREE(mMotionAccelerations);
		PX_FREE(mSolverLinkSpatialImpulses);
		PX_FREE(mSolverLinkSpatialDeltaVels);
		PX_FREE(mMotionVelocities);

		PX_FREE(mJointData);
		PX_FREE(mPathToRootElements);
	}

	void ArticulationData::resizeLinkData(const PxU32 linkCount)
	{
		const PxU32 oldSize = mLinkCapacity;
		mLinkCapacity = linkCount;
		// PT: these could be allocated in a single linear array to further reduce the size of the class.
		mMotionVelocities = PX_ALLOCATE(Cm::SpatialVectorF, linkCount, "mMotionVelocities");
		mSolverLinkSpatialDeltaVels = PX_ALLOCATE(Cm::SpatialVectorF, linkCount, "mSolverLinkSpatialDeltaVels");
		mSolverLinkSpatialImpulses = PX_ALLOCATE(Cm::SpatialVectorF, linkCount, "mSolverLinkSpatialImpulses");
		mMotionAccelerations = PX_ALLOCATE(Cm::SpatialVectorF, linkCount, "mMotionAccelerations");
		mLinkIncomingJointForces = PX_ALLOCATE(Cm::SpatialVectorF, linkCount, "mLinkIncomingJointForces");
		mMotionAccelerationsInternal = PX_ALLOCATE(Cm::SpatialVectorF, linkCount, "mMotionAccelerationsInternal");
		mCoriolisVectors = PX_ALLOCATE(Cm::SpatialVectorF, linkCount, "mCoriolisVectors");
		mZAForces = PX_ALLOCATE(Cm::SpatialVectorF, linkCount, "mZAForces");
		mZAInternalForces = PX_ALLOCATE(Cm::SpatialVectorF, linkCount, "mZAInternalForces");
		mNbStatic1DConstraints = PX_ALLOCATE(PxU32, linkCount, "mNbStatic1DConstraints");
		mStatic1DConstraintStartIndex = PX_ALLOCATE(PxU32, linkCount, "mStatic1DConstraintStartIndex");
		mNbStaticContactConstraints = PX_ALLOCATE(PxU32, linkCount, "mNbStaticContactConstraints");
		mStaticContactConstraintStartIndex = PX_ALLOCATE(PxU32, linkCount, "mStaticContactConstraintStartIndex");
		mDeltaMotionVector = PX_ALLOCATE(Cm::SpatialVectorF, linkCount, "mDeltaMotionVector");
		mPreTransform = PX_ALLOCATE(PxTransform, linkCount, "mPreTransform");
		mResponseMatrixW = PX_ALLOCATE(TestImpulseResponse, linkCount, "mResponseMatrixW");
		mWorldSpatialArticulatedInertia = PX_ALLOCATE(SpatialMatrix, linkCount, "mWorldSpatialArticulatedInertia");
		mWorldIsolatedSpatialArticulatedInertia = PX_ALLOCATE(PxMat33, linkCount, "mWorldIsolatedSpatialArticulatedInertia");
		mMasses = PX_ALLOCATE(PxReal, linkCount, "mMasses");
		mInvStIs = PX_ALLOCATE(InvStIs, linkCount, "mInvStIs");
		mAccumulatedPoses = PX_ALLOCATE(PxTransform, linkCount, "mAccumulatedPoses");
		mDeltaQ = PX_ALLOCATE(PxQuat, linkCount, "mDeltaQ");
		mPosIterMotionVelocities = PX_ALLOCATE(Cm::SpatialVectorF, linkCount, "mPosIterMotionVelocities");
		mJointTransmittedForce = PX_ALLOCATE(Cm::SpatialVectorF, linkCount, "mJointTransmittedForce");
		mRw = PX_ALLOCATE(PxVec3, linkCount, "mRw");
		mRelativeQuat = PX_ALLOCATE(PxQuat, linkCount, "mRelativeQuat");

		if (oldSize < linkCount)
		{
			ArticulationJointCoreData* oldJoints = mJointData;

			mJointData = PX_ALLOCATE(ArticulationJointCoreData, linkCount, "ArticulationJointCoreData");

			PxMemCopy(mJointData, oldJoints, sizeof(ArticulationJointCoreData)*oldSize);

			PX_FREE(oldJoints);

			const PxU32 newElems = (linkCount - oldSize);

			PxMemZero(mJointData + oldSize, sizeof(ArticulationJointCoreData) * newElems);

			for (PxU32 linkID = oldSize; linkID < linkCount; ++linkID)
			{
				PX_PLACEMENT_NEW(mJointData + linkID, ArticulationJointCoreData)();
			}
		}
	}

	void ArticulationData::resizeJointData(const PxU32 dofs)
	{
		mDofCapacity = dofs;
		// PT: these could be allocated in a single linear array to further reduce the size of the class.
		mJointAcceleration = PX_ALLOCATE(PxReal, dofs, "mJointAcceleration");
		mJointVelocity = PX_ALLOCATE(PxReal, dofs, "mJointVelocity");
		mJointNewVelocity = PX_ALLOCATE(PxReal, (dofs + 3), "mJointVelocity");	// PT: TODO: why +3 ?
		mJointPosition = PX_ALLOCATE(PxReal, dofs, "mJointPosition");
		mJointForce = PX_ALLOCATE(PxReal, dofs, "mJointForce");
		mJointTargetPositions = PX_ALLOCATE(PxReal, dofs, "mJointTargetPositions");
		mJointTargetVelocities = PX_ALLOCATE(PxReal, dofs, "mJointTargetVelocities");
		mPosIterJointVelocities = PX_ALLOCATE(PxReal, dofs, "mPosIterJointVelocities");
		mDeferredQstZ = PX_ALLOCATE(PxReal, dofs, "mDeferredQstZ");
		mIsW = PX_ALLOCATE(Cm::SpatialVectorF, dofs, "mIsW");
		qstZIc = PX_ALLOCATE(PxReal, dofs, "qstZIc");
		qstZIntIc = PX_ALLOCATE(PxReal, dofs, "qstZIntIc");
		mMotionMatrix = PX_ALLOCATE(Cm::UnAlignedSpatialVector, dofs, "mMotionMatrix");
		mWorldMotionMatrix = PX_ALLOCATE(Cm::UnAlignedSpatialVector, dofs, "mWorldMotionMatrix");
		mISInvStIS = PX_ALLOCATE(Cm::SpatialVectorF, dofs, "mISInvStIS");

		const PxU32 size = sizeof(PxReal) * dofs;
		PxMemZero(mJointAcceleration, size);
		PxMemZero(mJointVelocity, size);
		PxMemZero(mJointPosition, size);
		PxMemZero(mJointForce, size);
		PxMemZero(mJointTargetPositions, size);
		PxMemZero(mJointTargetVelocities, size);
	}

	PxU32 ArticulationData::countDofs() const
	{
		const PxU32 linkCount = mLinkCount;
		PxU32 totalDofs = 0;

		for (PxU32 linkID = 1; linkID < linkCount; ++linkID)
		{
			const ArticulationLink& link = mLinks[linkID];
			const ArticulationJointCoreData& jointDatum = mJointData[linkID];
			const PxU8 dof = jointDatum.countJointDofs(link.inboundJoint);
			totalDofs += dof;
		}
		
		return totalDofs;
	}

	void ArticulationData::configureDofs()
	{
		const PxU32 linkCount = mLinkCount;
		PxU32 totalDof = 0;

		for(PxU32 linkID = 1; linkID < linkCount; ++linkID)
		{
			ArticulationJointCoreData& jointDatum = mJointData[linkID];
			const PxU8 dof = jointDatum.configureJointDofs(mLinks[linkID].inboundJoint);
			jointDatum.jointOffset = totalDof;
			totalDof += dof;
		}
	}

	void ArticulationData::setupDofs()
	{
		const PxU32 totalDofs = countDofs();

		const PxU32 existedTotalDofs = mDofs;

		if(totalDofs != existedTotalDofs)
		{
			resizeJointData(totalDofs + 1);
			mDofs = totalDofs;
		}
		configureDofs();
	}

	/////

	FeatherstoneArticulation::FeatherstoneArticulation() : mJcalcDirty(true)
	{
		mGPUDirtyFlags = 0;
	}

	FeatherstoneArticulation::~FeatherstoneArticulation()
	{
	}

	void FeatherstoneArticulation::setupLinks(PxU32 nbLinks, Dy::ArticulationLink* links)
	{
		if (nbLinks != mArticulationData.mLinkCount)
			mArticulationData.resizeLinkData(nbLinks);

		mArticulationData.mLinks				= links;
		mArticulationData.mLinkCount			= nbLinks;
		mArticulationData.mFlags				= mSolverDescCore ? &mSolverDescCore->flags : mSolverDescFlags;	// PT: PX-1399
		mArticulationData.mExternalAcceleration	= mAcceleration.begin();
		mArticulationData.mArticulation			= this;

		//allocate memory for articulation data
		mArticulationData.setupDofs();
	}

	void FeatherstoneArticulation::initPathToRoot()
	{
		Dy::ArticulationLink* links = mArticulationData.getLinks();
		
		const PxU32 linkCount = mArticulationData.getLinkCount();

		links[0].mPathToRootCount = 0;
		links[0].mPathToRootStartIndex = 0;

		PxU32 totalPathToRootCount = 1; //add on root

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

			PxU32 parent = link.parent;
			PxU32 pathToRootCount = 1; // add myself to the path
			while (parent != 0) // don't add the root
			{
				parent = links[parent].parent;
				pathToRootCount++;
			}

			link.mPathToRootStartIndex = totalPathToRootCount;
			link.mPathToRootCount = PxU16(pathToRootCount);
			totalPathToRootCount += pathToRootCount;
		}

		if (mArticulationData.mNumPathToRootElements < totalPathToRootCount)
		{
			PX_FREE(mArticulationData.mPathToRootElements);
			mArticulationData.mPathToRootElements = PX_ALLOCATE(PxU32, totalPathToRootCount, "PxU32");
			mArticulationData.mNumPathToRootElements = totalPathToRootCount;
		}
		PxU32* pathToRootElements = mArticulationData.mPathToRootElements;

		pathToRootElements[0] = 0; //add on root index

		for (PxU32 linkID = 1; linkID < linkCount; ++linkID)
		{
			const Dy::ArticulationLink& link = links[linkID];
			PxU32* pathToRoot = &pathToRootElements[link.mPathToRootStartIndex];
			PxU32 numElements = link.mPathToRootCount;

			pathToRoot[--numElements] = linkID;
			PxU32 parent = links[linkID].parent;
			while (parent != 0)
			{
				pathToRoot[--numElements] = parent;
				parent = links[parent].parent;
			}
		}
	}

	void FeatherstoneArticulation::assignTendons(const PxU32 nbTendons, Dy::ArticulationSpatialTendon** tendons)
	{
		mArticulationData.mSpatialTendons = tendons;
		mArticulationData.mNumSpatialTendons = nbTendons;
	}

	void FeatherstoneArticulation::assignTendons(const PxU32 nbTendons, Dy::ArticulationFixedTendon** tendons)
	{
		mArticulationData.mFixedTendons = tendons;
		mArticulationData.mNumFixedTendons = nbTendons;
	}

	void FeatherstoneArticulation::assignMimicJoints(const PxU32 nbMimicJoints, Dy::ArticulationMimicJointCore** mimicJoints)
	{
		mArticulationData.mMimicJoints = mimicJoints;
		mArticulationData.mNbMimicJoints = nbMimicJoints;
	}

	PxU32 FeatherstoneArticulation::getDofs() const
	{
		return mArticulationData.getDofs();
	}

	PxU32 FeatherstoneArticulation::getDof(const PxU32 linkID)
	{
		const ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(linkID);
		return jointDatum.nbDof;
	}

	bool FeatherstoneArticulation::applyCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag, bool& shouldWake)
	{
		return applyCacheToDest(mArticulationData, cache, mArticulationData.getJointVelocities(), mArticulationData.getJointPositions(),
			mArticulationData.getJointForces(), mArticulationData.getJointTargetPositions(), mArticulationData.getJointTargetVelocities(), flag, shouldWake);
	}

	void FeatherstoneArticulation::copyInternalStateToCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag, const bool isGpuSimEnabled)
	{
		if (flag & PxArticulationCacheFlag::eVELOCITY)
			copyJointData(mArticulationData, cache.jointVelocity, mArticulationData.getJointVelocities());

		if (flag & PxArticulationCacheFlag::eACCELERATION)
			copyJointData(mArticulationData, cache.jointAcceleration, mArticulationData.getJointAccelerations());

		if (flag & PxArticulationCacheFlag::ePOSITION)
			copyJointData(mArticulationData, cache.jointPosition, mArticulationData.getJointPositions());

		if (flag & PxArticulationCacheFlag::eFORCE)
			copyJointData(mArticulationData, cache.jointForce, mArticulationData.getJointForces());

		if (flag & PxArticulationCacheFlag::eJOINT_TARGET_POSITIONS)
			copyJointData(mArticulationData, cache.jointTargetPositions, mArticulationData.getJointTargetPositions());

		if (flag & PxArticulationCacheFlag::eJOINT_TARGET_VELOCITIES)
			copyJointData(mArticulationData, cache.jointTargetVelocities, mArticulationData.getJointTargetVelocities());

		if (flag & PxArticulationCacheFlag::eLINK_VELOCITY)
		{
			const Cm::SpatialVectorF* vels = mArticulationData.getMotionVelocities();
			const PxU32 numLinks = mArticulationData.getLinkCount();
			for (PxU32 i = 0; i < numLinks; ++i)
			{
				const Cm::SpatialVectorF& vel = vels[i];
				cache.linkVelocity[i].linear = vel.bottom;
				cache.linkVelocity[i].angular = vel.top;
			}
		}

		const float dt = mArticulationData.getDt();

		if (flag & PxArticulationCacheFlag::eLINK_ACCELERATION)
		{
			const PxU32 linkCount = mArticulationData.getLinkCount();

			if(dt == 0.0f)
			{
				PxMemZero(cache.linkAcceleration, sizeof(PxSpatialVelocity)*linkCount);
			}
			else if(isGpuSimEnabled)
			{
				const Cm::SpatialVectorF* PX_RESTRICT linkMotionAccelerationsW = mArticulationData.getMotionAccelerations();

				//Iterate over all links and compute the acceleration for each link.
				for (PxU32 i = 0; i < linkCount; ++i)
				{
					cache.linkAcceleration[i].linear = linkMotionAccelerationsW[i].bottom;
					cache.linkAcceleration[i].angular = linkMotionAccelerationsW[i].top;
				}
			}
			else
			{
				const PxReal invDt = 1.0f / dt;

				const Cm::SpatialVectorF* PX_RESTRICT linkMotionAccelerationsW = mArticulationData.getMotionAccelerations();
				const Cm::SpatialVectorF* PX_RESTRICT linkSpatialDeltaVelsW = mArticulationData.getSolverLinkSpatialDeltaVels();

				//Iterate over all links and compute the acceleration for each link.
				for (PxU32 i = 0; i < linkCount; ++i)
				{
					cache.linkAcceleration[i].linear = linkMotionAccelerationsW[i].bottom + linkSpatialDeltaVelsW[i].bottom*invDt;
					cache.linkAcceleration[i].angular = linkMotionAccelerationsW[i].top + linkSpatialDeltaVelsW[i].top*invDt;
				}
			}
		}

		if(flag & PxArticulationCacheFlag::eLINK_INCOMING_JOINT_FORCE)
		{
			const PxU32 linkCount = mArticulationData.getLinkCount();
			if (dt == 0.0f)
			{
				PxMemZero(cache.linkIncomingJointForce, sizeof(PxSpatialForce)*linkCount);
			}
			else if(isGpuSimEnabled)
			{
				//Calculation already completed on gpu.
				const Cm::SpatialVectorF* incomingJointForces = mArticulationData.getLinkIncomingJointForces();

				//Root links have no incoming joint.
				cache.linkIncomingJointForce[0].force = PxVec3(PxZero);
				cache.linkIncomingJointForce[0].torque = PxVec3(PxZero);
			
				//Iterate over all links and get the incoming joint force for each link.
				for (PxU32 i = 1; i < linkCount; ++i)
				{
					cache.linkIncomingJointForce[i].force = incomingJointForces[i].top;
					cache.linkIncomingJointForce[i].torque = incomingJointForces[i].bottom;
				}
			}
			else
			{
				const PxReal invDt = 1.0f / dt;

				//Get everything we need.
				const Cm::SpatialVectorF* PX_RESTRICT linkZAForcesExtW = mArticulationData.getSpatialZAVectors();
				const Cm::SpatialVectorF* PX_RESTRICT linkZAForcesIntW = mArticulationData.getSpatialZAInternalVectors();
				const Cm::SpatialVectorF* PX_RESTRICT linkMotionAccelerationsW = mArticulationData.getMotionAccelerations();
				const SpatialMatrix* PX_RESTRICT linkSpatialInertiasW = mArticulationData.getWorldSpatialArticulatedInertia();
				const Cm::SpatialVectorF* PX_RESTRICT linkSpatialDeltaVelsW = mArticulationData.getSolverLinkSpatialDeltaVels();
				const Cm::SpatialVectorF* PX_RESTRICT linkSpatialImpulsesW = mArticulationData.getSolverLinkSpatialImpulses();

				//Root links have no incoming joint.
				cache.linkIncomingJointForce[0].force = PxVec3(PxZero);
				cache.linkIncomingJointForce[0].torque = PxVec3(PxZero);

				//Iterate over all links and compute the incoming joint force for each link.
				for (PxU32 i = 1; i < linkCount; ++i)
				{
					const ArticulationLink& link = mArticulationData.getLink(i);
					const ArticulationJointCore* joint = link.inboundJoint;
					const PxTransform Gc = link.bodyCore->body2World;
					const PxTransform Lc = joint->childPose;
					const PxTransform GcLc = Gc*Lc;
					const PxVec3 dW = Gc.rotate(Lc.p);

					//Compute the force measured at the link.
					const Cm::SpatialVectorF incomingJointForceAtLinkW =
						linkSpatialInertiasW[i]*(linkMotionAccelerationsW[i] + linkSpatialDeltaVelsW[i]*invDt) +
							(linkZAForcesExtW[i] + linkZAForcesIntW[i] + linkSpatialImpulsesW[i]*invDt);

					//Compute the equivalent force measured at the joint.
					const Cm::SpatialVectorF incomingJointForceAtJointW =
						 FeatherstoneArticulation::translateSpatialVector(-dW, incomingJointForceAtLinkW);

					//Transform the force to the child joint frame.
					cache.linkIncomingJointForce[i].force = GcLc.rotateInv(incomingJointForceAtJointW.top);
					cache.linkIncomingJointForce[i].torque = GcLc.rotateInv(incomingJointForceAtJointW.bottom);
				}
			}
		}

		if (flag & PxArticulationCacheFlag::eROOT_TRANSFORM)
		{
			const ArticulationLink& rLink = mArticulationData.getLink(0);
			const PxsBodyCore& rBodyCore = *rLink.bodyCore;
			const PxTransform& body2World = rBodyCore.body2World;
			// PT:: tag: scalar transform*transform
			cache.rootLinkData->transform = body2World * rBodyCore.getBody2Actor().getInverse();
		}

		if (flag & PxArticulationCacheFlag::eROOT_VELOCITIES)
		{
			const Cm::SpatialVectorF& vel = mArticulationData.getMotionVelocity(0);
			cache.rootLinkData->worldLinVel = vel.bottom;
			cache.rootLinkData->worldAngVel = vel.top;

			const Cm::SpatialVectorF& accel = mArticulationData.getMotionAcceleration(0);
			cache.rootLinkData->worldLinAccel = accel.bottom;
			cache.rootLinkData->worldAngAccel = accel.top;
		}
	}

	PxU32 FeatherstoneArticulation::getCacheDataSize(PxU32 totalDofs, PxU32 linkCount)
	{
		const PxU32 totalSize =
			sizeof(PxSpatialForce) * linkCount						// external force
			+ sizeof(PxReal) * (6 + totalDofs) * (linkCount * 6)	// Free floating base dofs = 6 + totalDofs, and each link (incl. base) velocity has 6 elements
																	// == offset to end of dense jacobian (assuming free floating base)
			+ sizeof(PxReal) * (totalDofs + 6) * (totalDofs + 6)	// mass matrix
			+ sizeof(PxReal) * (totalDofs + 6) 						// Coriolis and centrifugal compensation force
			+ sizeof(PxReal) * (totalDofs + 6)						// gravity compensation force
			+ sizeof(PxReal) * 6 * (totalDofs + 6)					// centroidal momentum matrix
			+ sizeof(PxReal) * 6									// centroidal momentum bias
			+ sizeof(PxReal) * totalDofs * 6						// jointVelocity (PxArticulationCacheFlag::eVELOCITY)
																	// jointAcceleration (PxArticulationCacheFlag::eACCELERATION)
																	// jointPosition (PxArticulationCacheFlag::ePOSITION)
																	// joint force (PxArticulationCacheFlag::eFORCE)
																	// joint target positions (PxArticulationCacheFlag::eJOINT_TARGET_POSITIONS)
																	// joint target velocities (PxArticulationCacheFlag::eJOINT_TARGET_VELOCITIES)
			+ sizeof(PxSpatialVelocity) * linkCount * 2				// link velocity, (PxArticulationCacheFlag::eLINK_VELOCITY)
																	// link acceleration (PxArticulationCacheFlag::eLINK_ACCELERATION)
			+ sizeof(PxSpatialForce) * linkCount					// link incoming joint forces (PxArticulationCacheFlag::eLINK_INCOMING_JOINT_FORCE)
			+ sizeof(PxVec3) * linkCount * 2						// link force (PxArticulationCacheFlag::eLINK_FORCE)
																	// link torque (PxArticulationCacheFlag::eLINK_TORQUE)
			+ sizeof(PxArticulationRootLinkData);					// root link data (PxArticulationCacheFlag::eROOT_TRANSFORM, PxArticulationCacheFlag::eROOT_VELOCITIES)
	
		return totalSize;
	}

	// AD: attention - some of the types here have 16B alignment requirements.
	// But the size of PxArticulationCache is not necessarily a multiple of 16B.
	PX_COMPILE_TIME_ASSERT(sizeof(Cm::SpatialVector)==sizeof(PxSpatialForce));
	PxArticulationCache* FeatherstoneArticulation::createCache(PxU32 totalDofs, PxU32 linkCount)
	{
		const PxU32 pxArticulationCacheSize16BAligned = (sizeof(PxArticulationCache) + 15) & ~15;

		const PxU32 totalSize = getCacheDataSize(totalDofs, linkCount) + pxArticulationCacheSize16BAligned;

		PxU8* tCache = reinterpret_cast<PxU8*>(PX_ALLOC(totalSize, "Articulation cache"));
		PX_ASSERT(((size_t)tCache & 15) == 0);

		PxMemZero(tCache, totalSize);

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

		PxU32 offset = pxArticulationCacheSize16BAligned;

		// the following code assumes that the size of PxSpatialForce and PxSpatialVelocity are multiples of 16B
		PX_COMPILE_TIME_ASSERT((sizeof(PxSpatialForce) & 15) == 0);
		PX_COMPILE_TIME_ASSERT((sizeof(PxSpatialVelocity) & 15) == 0);

		// PT: filled in FeatherstoneArticulation::getGeneralizedExternalForce, size = mArticulationData.getLinkCount()
		// 16B aligned
		PX_ASSERT((offset & 15) == 0);
		cache->externalForces = reinterpret_cast<PxSpatialForce*>(tCache + offset);
		offset += sizeof(PxSpatialForce) * linkCount;

		// PT: PxArticulationCacheFlag::eLINK_VELOCITY
		// 16B aligned
		PX_ASSERT((offset & 15) == 0);
		cache->linkVelocity = reinterpret_cast<PxSpatialVelocity*>(tCache + offset);
		offset += sizeof(PxSpatialVelocity) * linkCount;

		// PT: PxArticulationCacheFlag::eLINK_ACCELERATION
		// 16B aligned
		PX_ASSERT((offset & 15) == 0);
		cache->linkAcceleration = reinterpret_cast<PxSpatialVelocity*>(tCache + offset);
		offset += sizeof(PxSpatialVelocity) * linkCount;

		// PT: PxArticulationCacheFlag::eLINK_INCOMING_JOINT_FORCE
		// 16B aligned
		PX_ASSERT((offset & 15) == 0);
		cache->linkIncomingJointForce = reinterpret_cast<PxSpatialForce*>(tCache + offset);
		offset += sizeof(PxSpatialForce) * linkCount;

		// PxArticulationCacheFlag::eLINK_FORCE
		PX_ASSERT((offset & 15) == 0);
		cache->linkForce = reinterpret_cast<PxVec3*>(tCache + offset);
        offset += sizeof(PxVec3) * linkCount;

		// PxArticulationCacheFlag::eLINK_TORQUE
		cache->linkTorque = reinterpret_cast<PxVec3*>(tCache + offset);
		offset += sizeof(PxVec3) * linkCount;

		cache->denseJacobian = reinterpret_cast<PxReal*>(tCache + offset);
		offset += sizeof(PxReal) * (6 + totalDofs) * (linkCount * 6);				//size of dense jacobian assuming free floating base link.

		cache->massMatrix = reinterpret_cast<PxReal*>(tCache + offset);
		offset += sizeof(PxReal) * (totalDofs + 6) * (totalDofs + 6);

		cache->coriolisForce = reinterpret_cast<PxReal*>(tCache + offset);
		offset += sizeof(PxReal) * (totalDofs + 6);

		cache->gravityCompensationForce = reinterpret_cast<PxReal*>(tCache + offset);
		offset += sizeof(PxReal) * (totalDofs + 6);

		cache->centroidalMomentumMatrix = reinterpret_cast<PxReal*>(tCache + offset);
		offset += sizeof(PxReal) * 6 * (totalDofs + 6);

		cache->centroidalMomentumBias = reinterpret_cast<PxReal*>(tCache + offset);
		offset += sizeof(PxReal) * 6;

		// PT: PxArticulationCacheFlag::eVELOCITY
		cache->jointVelocity = reinterpret_cast<PxReal*>(tCache + offset);
		offset += sizeof(PxReal) * totalDofs;

		// PT: PxArticulationCacheFlag::eACCELERATION
		cache->jointAcceleration = reinterpret_cast<PxReal*>(tCache + offset);
		offset += sizeof(PxReal) * totalDofs;

		// PT: PxArticulationCacheFlag::ePOSITION
		cache->jointPosition = reinterpret_cast<PxReal*>(tCache + offset);
		offset += sizeof(PxReal) * totalDofs;

		// PT: PxArticulationCacheFlag::eFORCE
		cache->jointForce = reinterpret_cast<PxReal*>(tCache + offset);
		offset += sizeof(PxReal) * totalDofs;

		// PxArticulationCacheFlag::eJOINT_TARGET_POSITIONS
		cache->jointTargetPositions = reinterpret_cast<PxReal*>(tCache + offset);
		offset += sizeof(PxReal) * totalDofs;

		// PxArticulationCacheFlag::eJOINT_TARGET_VELOCITIES
		cache->jointTargetVelocities = reinterpret_cast<PxReal*>(tCache + offset);
		offset += sizeof(PxReal) * totalDofs;

		// PT: PxArticulationCacheFlag::eROOT_TRANSFORM, PxArticulationCacheFlag::eROOT_VELOCITIES
		cache->rootLinkData = reinterpret_cast<PxArticulationRootLinkData*>(tCache + offset);
		PX_ASSERT((offset + sizeof(PxArticulationRootLinkData)) == totalSize);

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

		PxU32 scratchMemorySize =
			sizeof(Cm::SpatialVectorF) * linkCount * 5	//motionVelocity, motionAccelerations, coriolisVectors, spatialZAVectors, externalAccels;
			+ sizeof(Dy::SpatialMatrix) * linkCount		//compositeSpatialInertias;
			+ sizeof(PxVec3) * linkCount * 2			//linkForce, linkTorque
			+ sizeof(PxReal) * totalDofs * 7;			//jointVelocity, jointAcceleration, jointForces, jointPositions, jointFrictionForces, jointTargetPositions, jointTargetVelocities

		scratchMemorySize = (scratchMemorySize+15)&~15;

		void* scratchMemory = PX_ALLOC(scratchMemorySize, "Cache scratch memory");
		cache->scratchMemory = scratchMemory;

		PxcScratchAllocator* sa = PX_NEW(PxcScratchAllocator);
		sa->setBlock(scratchMemory, scratchMemorySize);
		cache->scratchAllocator = sa;

		return cache;
	}

	static PX_FORCE_INLINE Mat33V loadPxMat33(const PxMat33& m)
	{
		return Mat33V(Vec3V_From_Vec4V(V4LoadU(&m.column0.x)), 
			Vec3V_From_Vec4V(V4LoadU(&m.column1.x)), V3LoadU(&m.column2.x));
	}

	static PX_FORCE_INLINE void storePxMat33(const Mat33V& src, PxMat33& dst)
	{
		V3StoreU(src.col0, dst.column0);
		V3StoreU(src.col1, dst.column1);
		V3StoreU(src.col2, dst.column2);
	}

	void FeatherstoneArticulation::translateInertia(const PxMat33& sTod, SpatialMatrix& inertia)
	{
#if 1
		Mat33V sTodV = loadPxMat33(sTod);
		Mat33V dTos = M33Trnsps(sTodV);

		const Mat33V tL = loadPxMat33(inertia.topLeft);
		const Mat33V tR = loadPxMat33(inertia.topRight);
		const Mat33V bL = loadPxMat33(inertia.bottomLeft);

		const Mat33V bl = M33Add(M33MulM33(sTodV, tL), bL);
		const Mat33V br = M33Add(M33MulM33(sTodV, tR), M33Trnsps(tL));
		const Mat33V bottomLeft = M33Add(bl, M33MulM33(br, dTos));

		storePxMat33(M33Add(tL, M33MulM33(tR, dTos)), inertia.topLeft);
		storePxMat33(M33Scale(M33Add(bottomLeft, M33Trnsps(bottomLeft)), FHalf()), inertia.bottomLeft);
#else
		const PxMat33 dTos = sTod.getTranspose();

		PxMat33 bl = sTod * inertia.topLeft + inertia.bottomLeft;
		PxMat33 br = sTod * inertia.topRight + inertia.getBottomRight();

		inertia.topLeft = inertia.topLeft + inertia.topRight * dTos;
		inertia.bottomLeft = bl + br * dTos;

		//aligned inertia - make it symmetrical! OPTIONAL!!!!
		inertia.bottomLeft = (inertia.bottomLeft + inertia.bottomLeft.getTranspose()) * 0.5f;
#endif
	}

	void FeatherstoneArticulation::getImpulseResponse(
		PxU32 linkID,
		const Cm::SpatialVector& impulse,
		Cm::SpatialVector& deltaVV) const
	{
		//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(impulse) =	|n|
		//				|0|

		Cm::SpatialVectorF deltaV = mArticulationData.getImpulseResponseMatrixWorld()[linkID].getLinkDeltaVImpulseResponse(reinterpret_cast<const Cm::SpatialVectorF&>(impulse));

		deltaVV.linear = deltaV.bottom;
		deltaVV.angular = deltaV.top;		
	}

	void FeatherstoneArticulation::getImpulseResponse(
		PxU32 linkID,
		const Cm::SpatialVectorV& impulse,
		Cm::SpatialVectorV& deltaVV) const
	{
#if 0
		const PxTransform& body2World = mArticulationData.getPreTransform(linkID);

		QuatV rot = QuatVLoadU(&body2World.q.x);

		Cm::SpatialVectorV impl(QuatRotateInv(rot, impulse.linear), QuatRotateInv(rot, impulse.angular));

		//transform p(impulse) from world space to the local space of linkId

		//Cm::SpatialVectorF impl(impulse.linear, impulse.angular);
		Cm::SpatialVectorV deltaV = mArticulationData.getImpulseResponseMatrix()[linkID].getResponse(impl);
		deltaVV.linear = QuatRotate(rot, deltaV.angular);
		deltaVV.angular = QuatRotate(rot, deltaV.linear);
#else
		const Cm::SpatialVectorV deltaV = mArticulationData.getImpulseResponseMatrixWorld()[linkID].getLinkDeltaVImpulseResponse(impulse);
		deltaVV.linear = deltaV.angular;
		deltaVV.angular = deltaV.linear;
#endif
	}

	Cm::SpatialVector FeatherstoneArticulation::getMotionAcceleration(const PxU32 linkID, const bool isGpuSimEnabled) const
	{
		Cm::SpatialVector a = Cm::SpatialVector::zero();
		const float dt = mArticulationData.getDt();
		if(dt > 0.0f)
		{
			if(isGpuSimEnabled)
			{
				const Cm::SpatialVectorF& linkAccel = mArticulationData.mMotionAccelerations[linkID];
				a = Cm::SpatialVector(linkAccel.bottom, linkAccel.top);
			}
			else
			{
				const PxReal invDt = 1.0f / dt;
				const Cm::SpatialVectorF linkAccel = 
					mArticulationData.mMotionAccelerations[linkID] + mArticulationData.mSolverLinkSpatialDeltaVels[linkID] * invDt;
				a = Cm::SpatialVector(linkAccel.bottom, linkAccel.top);
			}
		}
		return a;
	}

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

		ArticulationData& data = articulation.mArticulationData;
		const bool fixBase = data.getArticulationFlags() & PxArticulationFlag::eFIX_BASE;
		Cm::SpatialVectorF* PX_RESTRICT motionVelocities = data.getMotionVelocities();
		//Cm::SpatialVectorF* deferredZ = data.getSpatialZAVectors();
		const ArticulationLink* PX_RESTRICT links = data.getLinks();
		ArticulationJointCoreData* PX_RESTRICT jointData = data.getJointData();

		//PxTransform* poses = data.getAccumulatedPoses();
		//const PxTransform* poses = data.getPreTransform();

		//This will be zero at the beginning of the frame
		PxReal* PX_RESTRICT jointNewVelocities = data.getJointNewVelocities();

		if (fixBase)
		{
			deltaV[0] = Cm::SpatialVectorF(PxVec3(0.f), PxVec3(0.f));
		}
		else
		{
			//ArticulationLink& link = links[0];

			deltaV[0] = data.getBaseInvSpatialArticulatedInertiaW() * -data.getRootDeferredZ();

			motionVelocities[0] += deltaV[0];
			data.incrementSolverSpatialDeltaVel(0, deltaV[0]);

			PX_ASSERT(motionVelocities[0].isFinite());
		}

		const PxVec3* PX_RESTRICT rw = data.getRw();

		const PxU32 linkCount = data.getLinkCount();

		for (PxU32 i = 1; i < linkCount; i++)
		{
			const ArticulationLink& tLink = links[i];
			const ArticulationJointCoreData& tJointDatum = jointData[i];

			const Cm::SpatialVectorF dV = propagateAccelerationW(
				rw[i], deltaV[tLink.parent], 
				data.getInvStIs(i), &data.getWorldMotionMatrix(tJointDatum.jointOffset), 
				&data.getIsW(tJointDatum.jointOffset), &data.getDeferredQstZ()[tJointDatum.jointOffset], tJointDatum.nbDof,
				&jointNewVelocities[tJointDatum.jointOffset]);

			deltaV[i] = dV;
			motionVelocities[i] += dV;
			data.incrementSolverSpatialDeltaVel(i, dV);

			PX_ASSERT(motionVelocities[i].isFinite());
		}

		//PxMemZero(deferredZ, sizeof(Cm::SpatialVectorF)*linkCount);
		PxMemZero(data.getDeferredQstZ(), sizeof(PxReal) * data.getDofs());

		data.getRootDeferredZ() = Cm::SpatialVectorF::Zero();
	}

	// Apply substep external forces and joint actuation, propagate from leaves to root
	// Gravity and external force/acceleration fullstep application happens in FeatherstoneArticulation::computeLinkStates
	// Joint force fullstep application happens in FeatherstoneArticulation::computePropagateSpatialInertia_ZA_ZIc
	void FeatherstoneArticulation::applyTgsSubstepForces(FeatherstoneArticulation* articulation, PxReal stepDt, Cm::SpatialVectorF* scratchExtForcesArticulatedYW)
	{
		ArticulationData& data = articulation->mArticulationData;

		// inputs
		const ArticulationLink* PX_RESTRICT links = data.getLinks();
		const PxU32 linkCount = data.getLinkCount();
		const Cm::UnAlignedSpatialVector* PX_RESTRICT jointDofMotionMatrixW = data.getWorldMotionMatrix();
		const Cm::SpatialVectorF* PX_RESTRICT jointDofISInvStISW  = data.getISInvStIS();
		const PxVec3* PX_RESTRICT childToParentRW = data.getRw();
		const ArticulationJointCoreData* PX_RESTRICT jointData = data.getJointData();
		const bool fixBase = data.getArticulationFlags() & PxArticulationFlag::eFIX_BASE;
		const PxReal* PX_RESTRICT jointForces = data.getJointForces();
		const Cm::SpatialVector* PX_RESTRICT linkExternalAcceleration = data.getExternalAccelerations(); // we have converted to spatial forces here already, see FeatherstoneArticulation::computeLinkStates

		// outputs
		PxReal* jointDofQStY = data.getDeferredQstZ();
		Cm::SpatialVectorF& rootDeferredZ = data.getRootDeferredZ();
		data.mJointDirty = true;

		// Scratch only temporarily needed until this function returns, passed in from outside to avoid allocating here.
		// We set it to zero because it might be in a dirty state.
		PX_ASSERT(scratchExtForcesArticulatedYW);
		Cm::SpatialVectorF* extForcesArticulatedYW = scratchExtForcesArticulatedYW;
		for(PxU32 linkID = 0; linkID < linkCount; linkID++)
		{
			extForcesArticulatedYW[linkID] = Cm::SpatialVectorF::Zero();
		}

		// Iterate from leaves to root, accumulating the substep forces in articulatedY up the tree
		// This is like FeatherstoneArticulation::propagateImpulseW but with impulses on all links
		const PxI32 startIndex = linkCount - 1;
		for (PxI32 linkID = startIndex; linkID > 0; --linkID)
		{
			const ArticulationLink& link = links[linkID];
			const Cm::SpatialVector& extForceStoredAsMotionVec = linkExternalAcceleration[linkID];
			const Cm::SpatialVectorF isolatedYW(extForceStoredAsMotionVec.linear, extForceStoredAsMotionVec.angular); // order defined in FeatherstoneArticulation::computeLinkStates
			const Cm::SpatialVectorF articulatedYW = isolatedYW * stepDt + extForcesArticulatedYW[linkID]; // could avoid adding this for leaf links
			const ArticulationJointCoreData& jointDatum = jointData[linkID];
			const PxU32 jointOffset = jointDatum.jointOffset;
			const PxU8 dofCount = jointDatum.nbDof;

			Cm::SpatialVectorF parentYW = articulatedYW;
			for(PxU8 jointDof = 0; jointDof < dofCount; jointDof++)
			{
				const PxU32 jointIdx = jointOffset + jointDof;
				const Cm::UnAlignedSpatialVector& sa = jointDofMotionMatrixW[jointIdx];
				const Cm::SpatialVectorF& ISInvStISW = jointDofISInvStISW[jointIdx];
				const PxReal jointForce = jointForces[jointIdx];

				const PxReal stZY = jointForce * stepDt - sa.innerProduct(articulatedYW);
				PX_ASSERT(PxIsFinite(stZY));

				parentYW += ISInvStISW * stZY;
				jointDofQStY[jointIdx] += stZY; // store as deferred impulse in joint space
			}

			const PxU32 parentLinkID = link.parent;
			if(parentLinkID == 0 && fixBase)
				continue; // no need to propagate to a fixed base

			parentYW = FeatherstoneArticulation::translateSpatialVector(childToParentRW[linkID], parentYW);
			extForcesArticulatedYW[parentLinkID] += parentYW; // add rather than assign because other children may have already contributed
		}

		// process the root if it's not fixed
		if(!fixBase)
		{
			const Cm::SpatialVector& extForceStoredAsMotionVec = linkExternalAcceleration[0];
			const Cm::SpatialVectorF isolatedYW(extForceStoredAsMotionVec.linear, extForceStoredAsMotionVec.angular); // order defined in FeatherstoneArticulation::computeLinkStates
			const Cm::SpatialVectorF articulatedYW = isolatedYW * stepDt + extForcesArticulatedYW[0];
			rootDeferredZ += articulatedYW;
		}
	}

	void FeatherstoneArticulation::recordDeltaMotionTGS(FeatherstoneArticulation* articulation, const PxReal dt, Cm::SpatialVectorF* deltaV)
	{
		PX_ASSERT(deltaV);

		ArticulationData& data = articulation->mArticulationData;
		const PxU32 linkCount = data.getLinkCount();

		const PxU32 flags = data.getArticulationFlags();

		if (data.mJointDirty)
		{
			PxcFsFlushVelocity(*articulation, deltaV);
		}

		Cm::SpatialVectorF* PX_RESTRICT deltaMotion = data.getDeltaMotionVector();
		Cm::SpatialVectorF* PX_RESTRICT posMotionVelocities = data.getPosIterMotionVelocities();
		Cm::SpatialVectorF* PX_RESTRICT motionVelocities = data.getMotionVelocities();

		PxReal* PX_RESTRICT jointPosition = data.getJointPositions();
		PxReal* PX_RESTRICT jointNewVelocities = data.getJointNewVelocities();
		
		const PxTransform* PX_RESTRICT preTransform = data.getPreTransform();
		PxTransform* PX_RESTRICT accumulatedPoses = data.getAccumulatedPoses();
		PxQuat* PX_RESTRICT deltaQ = data.getDeltaQ();

		//data.mAccumulatedDt += dt;
		data.setDt(dt);

		const bool fixBase = flags & PxArticulationFlag::eFIX_BASE;

		if (!fixBase)
		{
			const Cm::SpatialVectorF& motionVelocity = motionVelocities[0];
			PX_ASSERT(motionVelocity.top.isFinite());
			PX_ASSERT(motionVelocity.bottom.isFinite());

			const PxTransform preTrans = accumulatedPoses[0];

			const PxVec3 lin = motionVelocity.bottom;
			const PxVec3 ang = motionVelocity.top;

			const PxVec3 newP = preTrans.p + lin * dt;

			const PxTransform newPose = PxTransform(newP, PxExp(ang*dt) * preTrans.q);

			//PxVec3 lin, ang;
			/*calculateNewVelocity(newPose, preTransform[0],
				1.f, lin, ang);		*/	

			accumulatedPoses[0] = newPose;

			PxQuat dq = newPose.q * preTransform[0].q.getConjugate();

			if (dq.w < 0.f)
				dq = -dq;

			deltaQ[0] = dq;

			Cm::SpatialVectorF delta = motionVelocity * dt;

			deltaMotion[0] += delta;
			posMotionVelocities[0] += delta;
		}

		for (PxU32 linkID = 1; linkID < linkCount; linkID++)
		{
			ArticulationJointCoreData& jointDatum = data.getJointData(linkID);
			
			const PxTransform newPose = articulation->propagateTransform(linkID, data.getLinks(), jointDatum, data.getMotionVelocities(),
				dt, accumulatedPoses[data.getLink(linkID).parent], accumulatedPoses[linkID], 
				jointNewVelocities, jointPosition, &data.getMotionMatrix(jointDatum.jointOffset));

			//data.mDeltaQ[linkID] = preTransform[linkID].q.getConjugate() * newPose.q;
			PxQuat dq = newPose.q * preTransform[linkID].q.getConjugate();

			if(dq.w < 0.f)
				dq = -dq;

			deltaQ[linkID] = dq;

			/*PxVec3 lin, ang;
			calculateNewVelocity(newPose, preTransform[linkID],
				1.f, lin, ang);*/

			PxVec3 lin = (newPose.p - preTransform[linkID].p);

			Cm::SpatialVectorF delta = motionVelocities[linkID] * dt;
			
			//deltaMotion[linkID].top = ang;// motionVeloties[linkID].top * dt;
			deltaMotion[linkID].top += delta.top;
			deltaMotion[linkID].bottom = lin;// motionVeloties[linkID].top * dt;
			posMotionVelocities[linkID] += delta;

			//Record the new current pose
			accumulatedPoses[linkID] = newPose;
		}
	}

	Cm::SpatialVectorV FeatherstoneArticulation::pxcFsGetVelocity(const PxU32 linkID, PxReal* jointDofSpeeds) const
	{
		const bool fixBase = mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE;
	
		const ArticulationLink* PX_RESTRICT links = mArticulationData.getLinks();

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

		if (!fixBase)
		{
			//deltaV = mArticulationData.mBaseInvSpatialArticulatedInertia * (-deferredZ[0]);
			//DeferredZ now in world space!
			deltaV = mArticulationData.mBaseInvSpatialArticulatedInertiaW * -mArticulationData.getRootDeferredZ();
		}

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

		//Take care of the case where elementCount is 0. 
		const PxU32 elementCountMinusOne = (0 == elementCount) ? 0 : elementCount -1;

		const PxU32* pathToRootElements = &mArticulationData.mPathToRootElements[startIndex];
	
		const PxReal* PX_RESTRICT deferredQstZ = mArticulationData.getDeferredQstZ();
		const InvStIs* PX_RESTRICT invStIS = mArticulationData.getInvStIS();
		const Cm::SpatialVectorF* PX_RESTRICT isW = mArticulationData.getIsW();
		const Cm::UnAlignedSpatialVector* PX_RESTRICT worldMotionMatrix = mArticulationData.getWorldMotionMatrix();
		const PxVec3* PX_RESTRICT rw = mArticulationData.getRw();

		//We don't need to compute the deltaJointDofSpeed for these links and joints.
		for (PxU32 i = 0; i < elementCountMinusOne; ++i)
		{
			const PxU32 index = pathToRootElements[i];
			PX_ASSERT(links[index].parent < index);

			const PxU32 jointOffset = mArticulationData.getJointData(index).jointOffset;
			const PxU32 dofCount = mArticulationData.getJointData(index).nbDof;

			deltaV = propagateAccelerationW(
				rw[index], deltaV,
				invStIS[index], &worldMotionMatrix[jointOffset], &isW[jointOffset],
				&deferredQstZ[jointOffset], dofCount, 
				NULL);
		}
	
		//The last link might need to compute the deltaJointDofSpeed.
		PxReal deltaJointDofSpeeds[3] = {0, 0, 0};
		PxReal* optionalDeltaJointDofSpeeds = jointDofSpeeds ? deltaJointDofSpeeds : NULL;
		for (PxU32 i = elementCountMinusOne; i < elementCount; ++i)
		{
			const PxU32 index = pathToRootElements[i];
			PX_ASSERT(links[index].parent < index);

			const PxU32 jointOffset = mArticulationData.getJointData(index).jointOffset;
			const PxU32 dofCount = mArticulationData.getJointData(index).nbDof;

			deltaV = propagateAccelerationW(
				rw[index], deltaV,
				invStIS[index], &worldMotionMatrix[jointOffset], &isW[jointOffset],
				&deferredQstZ[jointOffset], dofCount, 
				optionalDeltaJointDofSpeeds);
		}

		if(jointDofSpeeds)
		{
			const PxU32 jointOffset = mArticulationData.getJointData(linkID).jointOffset;
			const PxU32 dofCount = mArticulationData.getJointData(linkID).nbDof;
			const PxReal* jointNewVelocities = mArticulationData.getJointNewVelocities();

			for(PxU32 i = 0; i < dofCount; i++)
			{
				jointDofSpeeds[i] = jointNewVelocities[jointOffset + i] + deltaJointDofSpeeds[i];	
			}
		}
		const Cm::SpatialVectorF vel = mArticulationData.getMotionVelocity(linkID) + deltaV;			
		return Cm::SpatialVectorV(Cm::SpatialVector(vel.bottom, vel.top));
	}

	void FeatherstoneArticulation::pxcFsGetVelocities(PxU32 linkID, PxU32 linkID1, Cm::SpatialVectorV& v0, Cm::SpatialVectorV& v1) const
	{
		{
			const bool fixBase = mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE;

			const ArticulationLink* PX_RESTRICT links = mArticulationData.getLinks();

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

			if (!fixBase)
			{
				//deltaV = mArticulationData.mBaseInvSpatialArticulatedInertia * (-deferredZ[0]);
				deltaV = mArticulationData.mBaseInvSpatialArticulatedInertiaW * (-mArticulationData.mRootDeferredZ);
			}

			const PxU32* PX_RESTRICT pathToRootElements = mArticulationData.mPathToRootElements;
			const PxReal* PX_RESTRICT deferredQstZ = mArticulationData.getDeferredQstZ();
			const InvStIs* PX_RESTRICT invStIS = mArticulationData.getInvStIS();
			const Cm::SpatialVectorF* PX_RESTRICT isW = mArticulationData.getIsW();
			const Cm::UnAlignedSpatialVector* PX_RESTRICT worldMotionMatrix = mArticulationData.getWorldMotionMatrix();
			const PxVec3* PX_RESTRICT rw = mArticulationData.getRw();
			const ArticulationJointCoreData* PX_RESTRICT jointData = mArticulationData.getJointData();

			const Dy::ArticulationLink& link0 = links[linkID];
			const Dy::ArticulationLink& link1 = links[linkID1];

			const PxU32* pathToRoot0 = &pathToRootElements[link0.mPathToRootStartIndex];
			const PxU32* pathToRoot1 = &pathToRootElements[link1.mPathToRootStartIndex];

			const PxU32 numElems0 = link0.mPathToRootCount;
			const PxU32 numElems1 = link1.mPathToRootCount;

			PxU32 offset = 0;
			while (pathToRoot0[offset] == pathToRoot1[offset])
			{
				const PxU32 index = pathToRoot0[offset++];
				PX_ASSERT(links[index].parent < index);
				if (offset >= numElems0 || offset >= numElems1)
					break;

				const PxU32 jointOffset = jointData[index].jointOffset;
				const PxU32 dofCount = jointData[index].nbDof;

				deltaV = propagateAccelerationW(
					rw[index], deltaV, 
					invStIS[index], &worldMotionMatrix[jointOffset], 
					&isW[jointOffset], &deferredQstZ[jointOffset], dofCount,
					NULL);
			}

			Cm::SpatialVectorF deltaV1 = deltaV;

			for (PxU32 idx = offset; idx < numElems0; ++idx)
			{
				const PxU32 index = pathToRoot0[idx];
				PX_ASSERT(links[index].parent < index);

				const PxU32 jointOffset = jointData[index].jointOffset;
				const PxU32 dofCount = jointData[index].nbDof;

				deltaV = propagateAccelerationW(
					rw[index], deltaV,
					invStIS[index],
					&worldMotionMatrix[jointOffset], &isW[jointOffset],
					&deferredQstZ[jointOffset], dofCount, 
					NULL);
			}

			for (PxU32 idx = offset; idx < numElems1; ++idx)
			{
				const PxU32 index = pathToRoot1[idx];
				PX_ASSERT(links[index].parent < index);

				const PxU32 jointOffset = jointData[index].jointOffset;
				const PxU32 dofCount = jointData[index].nbDof;

				deltaV1 = propagateAccelerationW(
					rw[index], deltaV1, 
					invStIS[index], &worldMotionMatrix[jointOffset], 
					&isW[jointOffset], &deferredQstZ[jointOffset], dofCount, 
					NULL);
			}

			const Cm::SpatialVectorF vel = mArticulationData.getMotionVelocity(linkID) + deltaV;

			v0 = Cm::SpatialVectorV(Cm::SpatialVector(vel.bottom, vel.top));

			const Cm::SpatialVectorF vel1 = mArticulationData.getMotionVelocity(linkID1) + deltaV1;

			v1 = Cm::SpatialVectorV(Cm::SpatialVector(vel1.bottom, vel1.top));
		}
	}

	/*Cm::SpatialVectorV FeatherstoneArticulation::pxcFsGetVelocity(PxU32 linkID)
	{

	Cm::SpatialVectorF& vel = mArticulationData.getMotionVelocity(linkID);

	return Cm::SpatialVector(vel.bottom, vel.top);
	}*/

	//This is used in the solveExt1D, solveExtContact
	void FeatherstoneArticulation::pxcFsApplyImpulse(PxU32 linkID, aos::Vec3V linkImpulseLinear, aos::Vec3V linkImpulseAngular, const PxReal* jointImpulse)
	{
		const ArticulationLink* links = mArticulationData.mLinks;
		ArticulationData& data = mArticulationData;

		data.mJointDirty = true;

		//impulse is in world space
		Cm::SpatialVector impulse;
		V4StoreA(Vec4V_From_Vec3V(linkImpulseAngular), &impulse.angular.x);
		V4StoreA(Vec4V_From_Vec3V(linkImpulseLinear), &impulse.linear.x);
		Cm::SpatialVectorF Z0(-impulse.linear, -impulse.angular);

		PxReal* PX_RESTRICT deferredQstZ = data.getDeferredQstZ();
		const Cm::UnAlignedSpatialVector* PX_RESTRICT worldMotionMatrix = data.getWorldMotionMatrix();
		const Cm::SpatialVectorF* PX_RESTRICT iSInvStIS = data.getISInvStIS();
		Cm::SpatialVectorF* PX_RESTRICT solverLinkSpatialImpulses = data.getSolverLinkSpatialImpulses();
		const PxVec3* PX_RESTRICT rw = data.getRw();

		for (PxU32 i = linkID; i; i = links[i].parent)
		{
			const PxU32 jointOffset = data.getJointData(i).jointOffset;
			const PxU8 dofCount = data.getJointData(i).nbDof;

			solverLinkSpatialImpulses[i] += Z0;

			//Only apply the joint impulse to the inbound joint of linkID.
			const PxReal* jointImpulseToApply = (linkID == i) ? jointImpulse : NULL;
			
			Z0 = propagateImpulseW(
					rw[i],
					Z0, 
					jointImpulseToApply, &iSInvStIS[jointOffset], &worldMotionMatrix[jointOffset], dofCount, 
					&deferredQstZ[jointOffset]);
		}

		data.mRootDeferredZ += Z0;
	}

	void FeatherstoneArticulation::pxcFsApplyImpulses(
		PxU32 linkID1, const aos::Vec3V& linear1, const aos::Vec3V& angular1, const PxReal* jointImpulse1,
		PxU32 linkID2, const aos::Vec3V& linear2, const aos::Vec3V& angular2, const PxReal* jointImpulse2)
	{
		if (0)
		{
			pxcFsApplyImpulse(linkID1, linear1, angular1, jointImpulse1);
			pxcFsApplyImpulse(linkID2, linear2, angular2, jointImpulse2);
		}
		else
		{
			ArticulationData& data = mArticulationData;
			data.mJointDirty = true;
			ArticulationLink* links = mArticulationData.mLinks;
			//impulse is in world space
			Cm::SpatialVector impulse0;
			V3StoreU(angular1, impulse0.angular);
			V3StoreU(linear1, impulse0.linear);

			Cm::SpatialVector impulse1;
			V3StoreU(angular2, impulse1.angular);
			V3StoreU(linear2, impulse1.linear);

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

			ArticulationLink& link1 = links[linkID1];
			ArticulationLink& link2 = links[linkID2];
			
			const PxU32* pathToRoot1 = &data.mPathToRootElements[link1.mPathToRootStartIndex];
			const PxU32* pathToRoot2 = &data.mPathToRootElements[link2.mPathToRootStartIndex];

			const PxU32 numElems1 = link1.mPathToRootCount;
			const PxU32 numElems2 = link2.mPathToRootCount;

			//find the common link, work from one to that common, then the other to that common, then go from there upwards...
			PxU32 offset = 0;
			PxU32 commonLink = 0;
			while (pathToRoot1[offset] == pathToRoot2[offset])
			{
				commonLink = pathToRoot1[offset++];
				PX_ASSERT(links[commonLink].parent < commonLink);
				if (offset >= numElems1 || offset >= numElems2)
					break;
			}

			PxReal* PX_RESTRICT deferredQstZ = data.getDeferredQstZ();
			const Cm::UnAlignedSpatialVector* PX_RESTRICT worldMotionMatrix = data.getWorldMotionMatrix();
			const Cm::SpatialVectorF* PX_RESTRICT iSInvStIS = data.getISInvStIS();
			Cm::SpatialVectorF* PX_RESTRICT solverLinkSpatialImpulses = data.getSolverLinkSpatialImpulses();
			const PxVec3* PX_RESTRICT rw = data.getRw();

			//The common link will either be linkID2, or its ancestors.
			//The common link cannot be an index before either linkID2 or linkID
			for (PxU32 i = linkID2; i != commonLink; i = links[i].parent)
			{
				const PxU32 jointOffset = data.getJointData(i).jointOffset;
				const PxU8 dofCount = data.getJointData(i).nbDof;

				//Note: linkID2 might be the common link. If this is the case, we will only apply 
				//jointImpulse2 when we propagate from the common link to the root.
				//Watch out for that when we propagate from the common link.
				const PxReal* jointImpulseToApply = (linkID2 == i) ? jointImpulse2 : NULL;

				solverLinkSpatialImpulses[i] += Z2;

				Z2 = propagateImpulseW(
						rw[i],
						Z2,
						jointImpulseToApply, &iSInvStIS[jointOffset], &worldMotionMatrix[jointOffset], dofCount, 
						&deferredQstZ[jointOffset]);
			}

			for (PxU32 i = linkID1; i != commonLink; i = links[i].parent)
			{
				const PxU32 jointOffset = data.getJointData(i).jointOffset;
				const PxU8 dofCount = data.getJointData(i).nbDof;

				//Note: linkID1 might be the common link. If this is the case, we will only apply 
				//jointImpulse1 when we propagate from the common link to the root.
				//Watch out for that when we propagate from the common link.
				const PxReal* jointImpulseToApply = (linkID1 == i) ? jointImpulse1 : NULL;

				solverLinkSpatialImpulses[i] += Z1;

				Z1 = propagateImpulseW(
						rw[i],
						Z1, 
						jointImpulseToApply, &iSInvStIS[jointOffset], &worldMotionMatrix[jointOffset],dofCount,
						&deferredQstZ[jointOffset]);
			}

			//If linkID1 (or linkID2) is the common link then we will not yet have applied 
			//jointImpulse1 (or jointImpulse2) to the inbound joint of the link.  
			//Work out how much joint impulse to apply to the inbound joint of the common link.
			PxReal jointImpulseToApplyAtCommonLink[3] = {0, 0, 0};
			if(((linkID1 == commonLink) && jointImpulse1) || ((linkID2 == commonLink) && jointImpulse2))
			{
				const PxU32 linkIndices[2] = {linkID1, linkID2};
				const PxReal* jointImpulses[2]= {jointImpulse1, jointImpulse2};
				const PxU32 dofCountAtCommonLink = data.getJointData(commonLink).nbDof;
				for(PxU32 k = 0; k < 2; k++)
				{
					const PxU32 linkId = linkIndices[k];
					const PxReal* jointImpulse = jointImpulses[k];
					if((linkId == commonLink) && jointImpulse)
					{
						for(PxU32 i = 0; i < dofCountAtCommonLink; i++)
						{
							jointImpulseToApplyAtCommonLink[i] += jointImpulse[i];
						}
					}
				}
			}

			Cm::SpatialVectorF ZCommon = Z1 + Z2;

			for (PxU32 i = commonLink; i; i = links[i].parent)
			{
				const PxU32 jointOffset = data.getJointData(i).jointOffset;
				const PxU8 dofCount = data.getJointData(i).nbDof;

				//Only apply a joint impulse to the inbound joint of commonLink.
				//The joint impulse to apply to the inbound joint will only be non-zero if the common link 
				//is either linkID1 or linkID2.
				const PxReal* jointImpulseToApply = (commonLink == i) ? jointImpulseToApplyAtCommonLink : NULL;

				solverLinkSpatialImpulses[i] += ZCommon;

				ZCommon = propagateImpulseW(
						rw[i],
						ZCommon, 
						jointImpulseToApply, &iSInvStIS[jointOffset], &worldMotionMatrix[jointOffset], dofCount,
						&deferredQstZ[jointOffset]);
			}

			data.mRootDeferredZ += ZCommon;
		}
	}

	//This version uses in updateBodies
	PxQuat computeSphericalJointPositions(const PxQuat& relativeQuat,
		const PxQuat& newRot, const PxQuat& pBody2WorldRot,
		PxReal* jPositions, const Cm::UnAlignedSpatialVector* motionMatrix,
		const PxU32 dofs);

	PxQuat computeSphericalJointPositions(const PxQuat& relativeQuat,
		const PxQuat& newRot, const PxQuat& pBody2WorldRot);

	PxTransform FeatherstoneArticulation::propagateTransform(const PxU32 linkID, const ArticulationLink* links,
		const ArticulationJointCoreData& jointDatum, const Cm::SpatialVectorF* motionVelocities, const PxReal dt, const PxTransform& pBody2World, 
		const PxTransform& currentTransform, PxReal* jointVelocities, PxReal* jointPositions,
		const Cm::UnAlignedSpatialVector* motionMatrix)
	{
		const ArticulationLink& link = links[linkID];

		const PxQuat* PX_RESTRICT relativeQuats = mArticulationData.getRelativeQuat();

		const PxQuat& relativeQuat = relativeQuats[linkID];
		
		ArticulationJointCore* joint = link.inboundJoint;
		
		PxReal* jVelocity = &jointVelocities[jointDatum.jointOffset];
		PxReal* jPosition = &jointPositions[jointDatum.jointOffset];
		
		PxQuat newParentToChild;
		PxQuat newWorldQ;
		PxVec3 r;
		
		const PxVec3 childOffset = -joint->childPose.p;
		const PxVec3 parentOffset = joint->parentPose.p;
		
		switch (joint->jointType)
		{
		case PxArticulationJointType::ePRISMATIC:
		{
			const PxReal tJointPosition = jPosition[0] + (jVelocity[0]) * dt;

			jPosition[0] = tJointPosition;

			newParentToChild = relativeQuat;
			const PxVec3 e = newParentToChild.rotate(parentOffset);
			const PxVec3 d = childOffset;
		
			r = e + d + motionMatrix[0].bottom * tJointPosition;
			break;
		}
		case PxArticulationJointType::eREVOLUTE:
		case PxArticulationJointType::eREVOLUTE_UNWRAPPED:
		{
			const PxReal tJointPosition = jPosition[0] + (jVelocity[0]) * dt;

			jPosition[0] = tJointPosition;
		
			const PxVec3& u = motionMatrix[0].top;
		
			PxQuat jointRotation = PxQuat(-tJointPosition, u);
			if (jointRotation.w < 0)	//shortest angle.
				jointRotation = -jointRotation;
		
			newParentToChild = (jointRotation * relativeQuat).getNormalized();
		
			const PxVec3 e = newParentToChild.rotate(parentOffset);
			const PxVec3 d = childOffset;
			r = e + d;
		
			PX_ASSERT(r.isFinite());
		
			break;
		}
		case PxArticulationJointType::eSPHERICAL:
		{
			Cm::SpatialVectorF worldVel = motionVelocities[linkID];

			const PxTransform oldTransform = currentTransform;

			PxVec3 worldAngVel = worldVel.top;
			//PxVec3 worldAngVel = motionVelocities[linkID].top;

			PxReal dist = worldAngVel.normalize() * dt;

			if (dist > 1e-6f)
				newWorldQ = PxQuat(dist, worldAngVel) * oldTransform.q;
			else
				newWorldQ = oldTransform.q;

			//newWorldQ = Ps::exp(worldAngVel*dt) * oldTransform.q;

			//PxVec3 axis;

			newParentToChild = computeSphericalJointPositions(relativeQuats[linkID], newWorldQ,
				pBody2World.q);

			PxQuat jointRotation = newParentToChild * relativeQuat.getConjugate();

			if(jointRotation.w < 0.0f)
				jointRotation = -jointRotation;

			/*PxVec3 axis = jointRotation.getImaginaryPart();
			for (PxU32 i = 0; i < jointDatum.dof; ++i)
			{
				PxVec3 sa = mArticulationData.getMotionMatrix(jointDatum.jointOffset + i).top;
				PxReal angle = -compAng(sa.dot(axis), jointRotation.w);
				jPosition[i] = angle;
			}*/

			PxVec3 axis; PxReal angle;
			jointRotation.toRadiansAndUnitAxis(angle, axis);
			axis *= angle;
			for (PxU32 i = 0; i < jointDatum.nbDof; ++i)
			{
				PxVec3 sa = mArticulationData.getMotionMatrix(jointDatum.jointOffset + i).top;
				PxReal ang = -sa.dot(axis);
				jPosition[i] = ang;
			}
			const PxVec3 e = newParentToChild.rotate(parentOffset);
			const PxVec3 d = childOffset;
			r = e + d;

			PX_ASSERT(r.isFinite());
		
			break;
		}
		case PxArticulationJointType::eFIX:
		{
			//this is fix joint so joint don't have velocity
			newParentToChild = relativeQuat;
		
			const PxVec3 e = newParentToChild.rotate(parentOffset);
			const PxVec3 d = childOffset;
		
			r = e + d;
			break;
		}
		default:
			break;
		}
		
		PxTransform cBody2World;
		cBody2World.q = (pBody2World.q * newParentToChild.getConjugate()).getNormalized();
		cBody2World.p = pBody2World.p + cBody2World.q.rotate(r);
		
		PX_ASSERT(cBody2World.isSane());
		
		return cBody2World;
	}

	Cm::SpatialVectorF FeatherstoneArticulation::propagateVelocityW(
		const PxVec3& parentToChild, const Cm::SpatialVectorF& parentLinkDeltaV,
		const Dy::SpatialMatrix& spatialInertia, const Cm::SpatialVectorF& Z,
		const PxReal* jointDofImpulses, const InvStIs& invStIs, const Cm::UnAlignedSpatialVector* motionMatrixW, const PxU32 dofCount, 
		PxReal* jointVelocity)
	{
		const Cm::SpatialVectorF pDeltaV = translateSpatialVector(-parentToChild, parentLinkDeltaV); //parent velocity change

		// -s_i^T * [I_i^A * translated(vParent) + Z]
		const Cm::SpatialVectorF temp = spatialInertia * pDeltaV + Z;
		PxReal tJointDelta[6];
		for (PxU32 ind = 0; ind < dofCount; ++ind)
		{
			const Cm::UnAlignedSpatialVector& sa = motionMatrixW[ind];
		    const PxReal jointDofImpulse = jointDofImpulses ? jointDofImpulses[ind] : 0.0f;
			tJointDelta[ind] = jointDofImpulse - sa.innerProduct(temp);
		}

		//qdot = [1/ s_i^T * i_i^A * s_i] * { -[s_i^T * I_i^A * translated(vParent) + Z]}
		//deltaV = qdot * s^T_i
		Cm::SpatialVectorF jointSpatialDeltaV(PxVec3(0.f), PxVec3(0.f));
		for (PxU32 ind = 0; ind < dofCount; ++ind)
		{
			PxReal jDelta = 0.f;
			for (PxU32 ind2 = 0; ind2 < dofCount; ++ind2)
			{
				jDelta += invStIs.invStIs[ind2][ind] * tJointDelta[ind2];
			}

			if(jointVelocity)
				jointVelocity[ind] += jDelta;
			jointSpatialDeltaV.top += motionMatrixW[ind].top * jDelta;
			jointSpatialDeltaV.bottom += motionMatrixW[ind].bottom * jDelta;
		}

		//deltaV = translated(parentLinkDeltaV) + qdot * s^T_i
		return pDeltaV + jointSpatialDeltaV;
	}

	Cm::SpatialVectorF FeatherstoneArticulation::getDeltaVWithDeltaJV(const bool fixBase, const PxU32 linkID, 
		const ArticulationData& data, Cm::SpatialVectorF* Z,
		PxReal* jointVelocities)
	{
		Cm::SpatialVectorF deltaV = Cm::SpatialVectorF::Zero();
		if (!fixBase)
		{
			//velocity change
			//SpatialMatrix inverseArticulatedInertia = hLinkDatum.spatialArticulatedInertia.getInverse();
			const SpatialMatrix& inverseArticulatedInertia = data.mBaseInvSpatialArticulatedInertiaW;
			deltaV = inverseArticulatedInertia * (-Z[0]);
		}

		const ArticulationLink* PX_RESTRICT links = data.getLinks();
		const ArticulationLink& link = links[linkID];
		const PxU32* PX_RESTRICT pathToRoot = &data.mPathToRootElements[link.mPathToRootStartIndex];
		const PxU32 numElems = link.mPathToRootCount;

		const SpatialMatrix* PX_RESTRICT worldSpatialArticulatedInertia = data.getWorldSpatialArticulatedInertia();
		const InvStIs* PX_RESTRICT invStIS = data.getInvStIS();
		const Cm::UnAlignedSpatialVector* PX_RESTRICT worldMotionMatrix = data.getWorldMotionMatrix();
		const PxVec3* PX_RESTRICT rw = data.getRw();

		for (PxU32 i = 0; i < numElems; ++i)
		{
			const PxU32 index = pathToRoot[i];
			PX_ASSERT(links[index].parent < index);
			ArticulationJointCoreData& tJointDatum = data.getJointData(index);
			PxReal* jVelocity = &jointVelocities[tJointDatum.jointOffset];
			deltaV = FeatherstoneArticulation::propagateVelocityW(
				rw[index], deltaV,
				worldSpatialArticulatedInertia[index], Z[index], 
				NULL, invStIS[index], &worldMotionMatrix[tJointDatum.jointOffset], tJointDatum.nbDof,
				jVelocity);
		}

		return deltaV;
	}

	void FeatherstoneArticulation::getZ(const PxU32 linkID,
		const ArticulationData& data, Cm::SpatialVectorF* Z, 
		const Cm::SpatialVectorF& impulse)
	{
		const ArticulationLink* PX_RESTRICT links = data.getLinks();
		const Cm::UnAlignedSpatialVector* PX_RESTRICT worldMotionMatrix = data.getWorldMotionMatrix();
		const Cm::SpatialVectorF* PX_RESTRICT iSInvStIS = data.getISInvStIS();
		const PxVec3* PX_RESTRICT rw = data.getRw();

		//impulse need to be in linkID space!!!
		Z[linkID] = -impulse;

		for (PxU32 i = linkID; i; i = links[i].parent)
		{
			const ArticulationLink& tLink = links[i];
			const PxU32 jointOffset = data.getJointData(i).jointOffset;
			const PxU8 dofCount = data.getJointData(i).nbDof;
			Z[tLink.parent] = propagateImpulseW(
				rw[i],
				Z[i], 
				NULL, &iSInvStIS[jointOffset], &worldMotionMatrix[jointOffset], dofCount,
				NULL);
		}
	}

	Cm::SpatialVectorF FeatherstoneArticulation::getImpulseResponseW(
		const PxU32 linkID,
		const ArticulationData& data,
		const Cm::SpatialVectorF& impulse)
	{
		return data.getImpulseResponseMatrixWorld()[linkID].getLinkDeltaVImpulseResponse(impulse);
	}

	//This method use in impulse self response. The input impulse is in the link space
	Cm::SpatialVectorF FeatherstoneArticulation::getImpulseResponseWithJ(
		const PxU32 linkID,
		const bool fixBase,
		const ArticulationData& data,
		Cm::SpatialVectorF* Z,
		const Cm::SpatialVectorF& impulse,
		PxReal* jointVelocites)
	{
		getZ(linkID, data, Z, impulse);

		return getDeltaVWithDeltaJV(fixBase, linkID, data, Z, jointVelocites);
	}

	void FeatherstoneArticulation::saveVelocity(FeatherstoneArticulation* articulation, Cm::SpatialVectorF* deltaV)
	{
		ArticulationData& data = articulation->mArticulationData;

		//update all links' motion velocity, joint delta velocity if there are contacts/constraints
		if (data.mJointDirty)
		{
			PxcFsFlushVelocity(*articulation, deltaV);
		}

		const PxU32 linkCount = data.getLinkCount();
		//copy motion velocites
		const Cm::SpatialVectorF* vels = data.getMotionVelocities();
		Cm::SpatialVectorF* posVels = data.getPosIterMotionVelocities();
		PxMemCopy(posVels, vels, sizeof(Cm::SpatialVectorF) * linkCount);
	
		//copy joint velocities
		const PxU32 dofs = data.getDofs();

		PxReal* jPosVels = data.getPosIterJointVelocities();

		const PxReal* jNewVels = data.getJointNewVelocities();

		PxMemCopy(jPosVels, jNewVels, sizeof(PxReal) * dofs);

		articulation->concludeInternalConstraints(false);
	}

	void FeatherstoneArticulation::saveVelocityTGS(FeatherstoneArticulation* articulation, PxReal invDtF32)
	{
		ArticulationData& data = articulation->mArticulationData;

		//KS - we should not need to flush velocity because we will have already stepped the articulation with TGS

		const PxU32 linkCount = data.getLinkCount();
		Cm::SpatialVectorF* posVels = data.getPosIterMotionVelocities();
		for (PxU32 i = 0; i < linkCount; ++i)
		{
			posVels[i] = posVels[i] * invDtF32;
		}

		// Under eENABLE_EXTERNAL_FORCES_EVERY_ITERATION_TGS, computeLinkStates split the
		// per-link isolated external Z (gravity + user accelerations) out of mZAForces
		// into mExternalAcceleration so it could be re-applied each substep. Substeps are
		// done now, so fold the cumulative external Z chain back into mZAForces - that
		// restores the same "full chain" semantics non-TGS-ext has at end of step, so the
		// on-demand joint-force readback in copyInternalStateToCache is uniform across
		// the flag setting. mExternalAcceleration is wiped right after this by
		// Sc::ArticulationSim::clearAcceleration, but mZAForces persists until the next
		// sim step's prep overwrites it.
		//
		// The leaves->root sweep uses propagateImpulseW (no joint impulse) so that
		// each joint's free DOFs are projected out of the propagated wrench - same
		// pattern as applyTgsSubstepForces and the prep-time backward sweep. Without
		// this projection, free axes (e.g. prismatic slide axis aligned with gravity)
		// would over-report a transmitted force where the descendants are actually
		// free-falling along that DOF.
		if (data.mIsExternalForcesEveryTgsIterationEnabled && linkCount > 1)
		{
			Cm::SpatialVectorF* PX_RESTRICT linkZAForcesExtW = data.getSpatialZAVectors();
			const Cm::SpatialVector* PX_RESTRICT linkExternalAccelsW = data.getExternalAccelerations();
			const PxVec3* PX_RESTRICT linkRsW = data.getRw();
			const ArticulationLink* PX_RESTRICT links = data.getLinks();
			const Cm::SpatialVectorF* PX_RESTRICT iSInvStIS = data.getISInvStIS();
			const Cm::UnAlignedSpatialVector* PX_RESTRICT worldMotionMatrix = data.getWorldMotionMatrix();

			// linear<=>top, angular<=>bottom - order set in computeLinkStates
			PX_ALLOCA(cumulativeExternalZ, Cm::SpatialVectorF, linkCount);
			for (PxU32 j = 0; j < linkCount; ++j)
				cumulativeExternalZ[j] = Cm::SpatialVectorF(linkExternalAccelsW[j].linear, linkExternalAccelsW[j].angular);

			// Children have higher indices, so cumulativeExternalZ[j] is final by the
			// time we process j. Root (index 0) is skipped: the readback in
			// copyInternalStateToCache loops i>=1.
			for (PxU32 j = linkCount - 1; j > 0; --j)
			{
				const PxU32 parent = links[j].parent;
				const ArticulationJointCoreData& jd = data.getJointData(j);
				cumulativeExternalZ[parent] += propagateImpulseW(
					linkRsW[j], cumulativeExternalZ[j],
					NULL, &iSInvStIS[jd.jointOffset], &worldMotionMatrix[jd.jointOffset], jd.nbDof,
					NULL);
				linkZAForcesExtW[j] += cumulativeExternalZ[j];
			}
		}
	}

	void FeatherstoneArticulation::getImpulseResponseSlow(const Dy::ArticulationLink* links, ArticulationData& data,
		PxU32 linkID0_, const Cm::SpatialVector& impulse0, Cm::SpatialVector& deltaV0,
		PxU32 linkID1_, const Cm::SpatialVector& impulse1, Cm::SpatialVector& deltaV1)
	{
		const PxU32 linkCount = data.getLinkCount();

		PX_ALLOCA(_stack, PxU32, linkCount);
		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);

		PxReal qstZ[192];

		PxMemZero(qstZ, data.getDofs() * sizeof(PxReal));

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

		const PxVec3* PX_RESTRICT rw = data.getRw();

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

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

		Cm::SpatialVectorF ZZ = Z0 + Z1;
		
		Cm::SpatialVectorF v = data.getImpulseResponseMatrixWorld()[common].getLinkDeltaVImpulseResponse(-ZZ); 
	
		Cm::SpatialVectorF dv1 = v;

		const InvStIs* PX_RESTRICT invStIS = data.getInvStIS();
		const Cm::SpatialVectorF* PX_RESTRICT isW = data.getIsW();
		const Cm::UnAlignedSpatialVector* PX_RESTRICT worldMotionMatrix = data.getWorldMotionMatrix();

		for (PxU32 index = i1; (index--) > i0;)
		{
			const PxU32 id = stack[index];
			const PxU32 jointOffset = data.getJointData(id).jointOffset;
			const PxU32 dofCount = data.getJointData(id).nbDof;
			dv1 = propagateAccelerationW(
				rw[id], dv1, 
				invStIS[id], &worldMotionMatrix[jointOffset], &isW[jointOffset], &qstZ[jointOffset], dofCount, 
				NULL);
		}

		Cm::SpatialVectorF dv0 = v;
		for (PxU32 index = i0; (index--) > 0;)
		{
			const PxU32 id = stack[index];
			const PxU32 jointOffset = data.getJointData(id).jointOffset;
			const PxU32 dofCount = data.getJointData(id).nbDof;
			dv0 = propagateAccelerationW(
				rw[id], dv0, invStIS[id],
				&worldMotionMatrix[jointOffset], &isW[jointOffset], &qstZ[jointOffset], dofCount, 
				NULL);
		}

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

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

	void FeatherstoneArticulation::getImpulseSelfResponse(const ArticulationLink* links, ArticulationData& data,
		PxU32 linkID0, const Cm::SpatialVector& impulse0, Cm::SpatialVector& deltaV0,
		PxU32 linkID1, const Cm::SpatialVector& impulse1, Cm::SpatialVector& deltaV1)
	{
		const ArticulationLink& link = links[linkID1];

		if (link.parent == linkID0)
		{
			PX_ASSERT(linkID0 == link.parent);
			PX_ASSERT(linkID0 < linkID1);

			//impulse is in world space
			const Cm::SpatialVectorF imp1(impulse1.linear, impulse1.angular);
			const Cm::SpatialVectorF imp0(impulse0.linear, impulse0.angular);

			Cm::SpatialVectorF Z1W(-imp1.top, -imp1.bottom);
			
			const PxU32 jointOffset1 = data.getJointData(linkID1).jointOffset;
			const PxU8 dofCount1 = data.getJointData(linkID1).nbDof;

			const Cm::UnAlignedSpatialVector* PX_RESTRICT worldMotionMatrix = data.getWorldMotionMatrix();
			const Cm::SpatialVectorF* PX_RESTRICT iSInvStIS = data.getISInvStIS();
			const PxVec3* PX_RESTRICT rw = data.getRw();

			PxReal qstZ[3] = { 0.f, 0.f, 0.f };
			const Cm::SpatialVectorF Z0W = propagateImpulseW(
				rw[linkID1],
				Z1W, 
				NULL, &iSInvStIS[jointOffset1], &worldMotionMatrix[jointOffset1], dofCount1, 
				qstZ);
			
			//in parent space
			const Cm::SpatialVectorF impulseDifW = imp0 - Z0W;
			
			//calculate velocity change start from the parent link to the root
			const Cm::SpatialVectorF delV0W = FeatherstoneArticulation::getImpulseResponseW(linkID0, data, impulseDifW);
			
			const InvStIs* PX_RESTRICT invStIS = data.getInvStIS();
			const Cm::SpatialVectorF* PX_RESTRICT isW = data.getIsW();

			const Cm::SpatialVectorF delV1W = propagateAccelerationW(
				rw[linkID1], delV0W, 
				invStIS[linkID1], &worldMotionMatrix[jointOffset1], &isW[jointOffset1], qstZ, dofCount1, 
				NULL);
			
			deltaV0.linear = delV0W.bottom;
			deltaV0.angular = delV0W.top;
			deltaV1.linear = delV1W.bottom;
			deltaV1.angular = delV1W.top;
		}
		else
		{
			getImpulseResponseSlow(	links, data,
									linkID0, impulse0, deltaV0,
									linkID1, impulse1, deltaV1);
		}
	}

	struct ArticulationStaticConstraintSortPredicate
	{
		bool operator()(const PxSolverConstraintDesc& left, const PxSolverConstraintDesc& right) const
		{
			PxU32 linkIndexA = left.linkIndexA != PxSolverConstraintDesc::RIGID_BODY ? left.linkIndexA : left.linkIndexB;
			PxU32 linkIndexB = right.linkIndexA != PxSolverConstraintDesc::RIGID_BODY ? right.linkIndexA : right.linkIndexB;

			return linkIndexA < linkIndexB;
		}
	};

	bool createFinalizeSolverContactsStep(PxTGSSolverContactDesc& contactDesc,
		PxsContactManagerOutput& output,
		ThreadContext& threadContext,
		const PxReal invDtF32,
		const PxReal invTotalDt,
		const PxReal totalDt,
		const PxReal stepDt,
		const PxReal bounceThresholdF32,
		const PxReal frictionOffsetThreshold,
		const PxReal correlationDistance,
		const PxReal biasCoefficient,
		PxConstraintAllocator& constraintAllocator);

	void FeatherstoneArticulation::prepareStaticConstraintsTGS(const PxReal stepDt, const PxReal totalDt, const PxReal invStepDt, const PxReal invTotalDt, 
		PxsContactManagerOutputIterator& outputs, Dy::ThreadContext& threadContext, PxReal correlationDist, PxReal bounceThreshold, PxReal frictionOffsetThreshold,
		PxTGSSolverBodyData* solverBodyData, PxTGSSolverBodyTxInertia* txInertia, PxsConstraintBlockManager& blockManager,
		Dy::ConstraintWriteback* constraintWritebackPool, const PxReal rigidContactBiasCoefficient, const PxReal jointBiasCoefficient, const PxReal lengthScale)
	{
		BlockAllocator blockAllocator(blockManager, threadContext.mConstraintBlockStream, threadContext.mFrictionPatchStreamPair, threadContext.mConstraintSize);

		const PxTransform id(PxIdentity);

		PxSort<PxSolverConstraintDesc, ArticulationStaticConstraintSortPredicate>(mStatic1DConstraints.begin(), mStatic1DConstraints.size(), ArticulationStaticConstraintSortPredicate());
		PxSort<PxSolverConstraintDesc, ArticulationStaticConstraintSortPredicate>(mStaticContactConstraints.begin(), mStaticContactConstraints.size(), ArticulationStaticConstraintSortPredicate());

		for (PxU32 i = 0; i < mStatic1DConstraints.size(); ++i)
		{
			PxSolverConstraintDesc& desc = mStatic1DConstraints[i];

			PxU32 linkIndex = desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY ? desc.linkIndexA : desc.linkIndexB;
			
			PX_ASSERT(desc.constraintType == DY_SC_TYPE_RB_1D);
			const Constraint* constraint = reinterpret_cast<const Constraint*>(desc.constraint);

			SolverConstraintShaderPrepDesc shaderPrepDesc;
			PxTGSSolverConstraintPrepDesc prepDesc;

			const PxConstraintSolverPrep solverPrep = constraint->solverPrep;
			const void* constantBlock = constraint->constantBlock;
			const PxU32 constantBlockByteSize = constraint->constantBlockSize;
			const PxTransform& pose0 = (constraint->body0 ? constraint->body0->getPose() : id);
			const PxTransform& pose1 = (constraint->body1 ? constraint->body1->getPose() : id);
			const PxTGSSolverBodyVel* sbody0 = desc.tgsBodyA;
			const PxTGSSolverBodyVel* sbody1 = desc.tgsBodyB;
			PxTGSSolverBodyData* sbodyData0 = &solverBodyData[desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY ? 0 : desc.bodyADataIndex];
			PxTGSSolverBodyData* sbodyData1 = &solverBodyData[desc.linkIndexB != PxSolverConstraintDesc::RIGID_BODY ? 0 : desc.bodyBDataIndex];
			PxTGSSolverBodyTxInertia& txI0 = txInertia[desc.bodyADataIndex];
			PxTGSSolverBodyTxInertia& txI1 = txInertia[desc.bodyBDataIndex];

			shaderPrepDesc.constantBlock = constantBlock;
			shaderPrepDesc.constantBlockByteSize = constantBlockByteSize;
			shaderPrepDesc.constraint = constraint;
			shaderPrepDesc.solverPrep = solverPrep;

			prepDesc.desc = &desc;
			prepDesc.bodyFrame0 = pose0;
			prepDesc.bodyFrame1 = pose1;
			prepDesc.body0 = sbody0;
			prepDesc.body1 = sbody1;
			prepDesc.body0TxI = &txI0;
			prepDesc.body1TxI = &txI1;
			prepDesc.bodyData0 = sbodyData0;
			prepDesc.bodyData1 = sbodyData1;
			prepDesc.linBreakForce = constraint->linBreakForce;
			prepDesc.angBreakForce = constraint->angBreakForce;
			prepDesc.writeback = &constraintWritebackPool[constraint->index];
			setupConstraintFlags(prepDesc, constraint->flags);
			prepDesc.minResponseThreshold = constraint->minResponseThreshold;

			prepDesc.bodyState0 = desc.linkIndexA == PxSolverConstraintDesc::RIGID_BODY ? PxSolverContactDesc::eDYNAMIC_BODY : PxSolverContactDesc::eARTICULATION;
			prepDesc.bodyState1 = desc.linkIndexB == PxSolverConstraintDesc::RIGID_BODY ? PxSolverContactDesc::eDYNAMIC_BODY : PxSolverContactDesc::eARTICULATION;

			SetupSolverConstraintStep(shaderPrepDesc, prepDesc, blockAllocator, stepDt, totalDt, invStepDt, invTotalDt, lengthScale, jointBiasCoefficient);

			if (desc.constraint)
			{
				if (mArticulationData.mNbStatic1DConstraints[linkIndex] == 0)
					mArticulationData.mStatic1DConstraintStartIndex[linkIndex] = i;
				mArticulationData.mNbStatic1DConstraints[linkIndex]++;
			}
			else
			{
				//Shuffle down 
				mStatic1DConstraints.remove(i);
				i--;
			}
		}

		for (PxU32 i = 0; i < mStaticContactConstraints.size(); ++i)
		{
			PxSolverConstraintDesc& desc = mStaticContactConstraints[i];

			PxU32 linkIndex = desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY ? desc.linkIndexA : desc.linkIndexB;

			PX_ASSERT(desc.constraintType == DY_SC_TYPE_RB_CONTACT);
			
			PxTGSSolverContactDesc blockDesc;
			PxsContactManager* cm = reinterpret_cast<PxsContactManager*>(desc.constraint);
			PxcNpWorkUnit& unit = cm->getWorkUnit();
			PxsContactManagerOutput* cmOutput = &outputs.getContactManagerOutput(unit.mNpIndex);

			PxTGSSolverBodyVel& b0 = *desc.tgsBodyA;
			PxTGSSolverBodyVel& b1 = *desc.tgsBodyB;

			PxTGSSolverBodyData& data0 = desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY ? solverBodyData[0] : solverBodyData[desc.bodyADataIndex];
			PxTGSSolverBodyData& data1 = desc.linkIndexB != PxSolverConstraintDesc::RIGID_BODY ? solverBodyData[0] : solverBodyData[desc.bodyBDataIndex];

			PxTGSSolverBodyTxInertia& txI0 = txInertia[desc.bodyADataIndex];
			PxTGSSolverBodyTxInertia& txI1 = txInertia[desc.bodyBDataIndex];

			blockDesc.bodyFrame0 = unit.mRigidCore0->body2World;
			blockDesc.bodyFrame1 = unit.mRigidCore1->body2World;
			blockDesc.shapeInteraction = cm->getShapeInteraction();
			blockDesc.contactForces = cmOutput->contactForces;
			blockDesc.desc = &desc;
			blockDesc.body0 = &b0;
			blockDesc.body1 = &b1;
			blockDesc.body0TxI = &txI0;
			blockDesc.body1TxI = &txI1;
			blockDesc.bodyData0 = &data0;
			blockDesc.bodyData1 = &data1;
			blockDesc.hasForceThresholds = !!(unit.mFlags & PxcNpWorkUnitFlag::eFORCE_THRESHOLD);
			blockDesc.disableStrongFriction = !!(unit.mFlags & PxcNpWorkUnitFlag::eDISABLE_STRONG_FRICTION);
			blockDesc.bodyState0 = (unit.mFlags & PxcNpWorkUnitFlag::eARTICULATION_BODY0) ? PxSolverContactDesc::eARTICULATION : PxSolverContactDesc::eDYNAMIC_BODY;
			blockDesc.bodyState1 = (unit.mFlags & PxcNpWorkUnitFlag::eARTICULATION_BODY1) ? PxSolverContactDesc::eARTICULATION : (unit.mFlags & PxcNpWorkUnitFlag::eHAS_KINEMATIC_ACTOR) ? PxSolverContactDesc::eKINEMATIC_BODY :
				((unit.mFlags & PxcNpWorkUnitFlag::eDYNAMIC_BODY1) ? PxSolverContactDesc::eDYNAMIC_BODY : PxSolverContactDesc::eSTATIC_BODY);
			//blockDesc.flags = unit.flags;

			const PxReal maxImpulse0 = (unit.mFlags & PxcNpWorkUnitFlag::eARTICULATION_BODY0) ? static_cast<const PxsBodyCore*>(unit.mRigidCore0)->maxContactImpulse : data0.maxContactImpulse;
			const PxReal maxImpulse1 = (unit.mFlags & PxcNpWorkUnitFlag::eARTICULATION_BODY1) ? static_cast<const PxsBodyCore*>(unit.mRigidCore1)->maxContactImpulse : data1.maxContactImpulse;

			unit.setInvMassScaleFromDominance(blockDesc.invMassScales);

			blockDesc.restDistance = unit.mRestDistance;
			blockDesc.frictionPtr = unit.mFrictionDataPtr;
			blockDesc.frictionCount = unit.mFrictionPatchCount;
			blockDesc.maxCCDSeparation = PX_MAX_F32;
			blockDesc.maxImpulse = PxMin(maxImpulse0, maxImpulse1);
			blockDesc.torsionalPatchRadius = unit.mTorsionalPatchRadius;
			blockDesc.minTorsionalPatchRadius = unit.mMinTorsionalPatchRadius;
			blockDesc.offsetSlop = unit.mOffsetSlop;

			createFinalizeSolverContactsStep(blockDesc, *cmOutput, threadContext,
				invStepDt, invTotalDt, totalDt, stepDt, bounceThreshold, frictionOffsetThreshold, 
				correlationDist, rigidContactBiasCoefficient, blockAllocator);

			getContactManagerConstraintDesc(*cmOutput, *cm, desc);

			updateFrictionAnchorCountAndPosition(desc, *cmOutput, blockDesc);

			unit.mFrictionDataPtr = blockDesc.frictionPtr;
			unit.mFrictionPatchCount = blockDesc.frictionCount;
			//KS - Don't track this for now!
			//axisConstraintCount += blockDesc.axisConstraintCount;

			if (desc.constraint)
			{
				if (mArticulationData.mNbStaticContactConstraints[linkIndex] == 0)
					mArticulationData.mStaticContactConstraintStartIndex[linkIndex] = i;
				mArticulationData.mNbStaticContactConstraints[linkIndex]++;
			}
			else
			{
				//Shuffle down 
				mStaticContactConstraints.remove(i);
				i--;
			}
		}
	}

	void FeatherstoneArticulation::prepareStaticConstraints(const PxReal dt, const PxReal invDt, PxsContactManagerOutputIterator& outputs,
		Dy::ThreadContext& threadContext, PxReal correlationDist, PxReal bounceThreshold, PxReal frictionOffsetThreshold,
		PxReal ccdMaxSeparation, const PxReal rigidContactBiasCoefficient, const PxReal jointBiasCoefficient,
		PxSolverBodyData* solverBodyData, PxsConstraintBlockManager& blockManager,
		Dy::ConstraintWriteback* constraintWritebackPool)
	{
		BlockAllocator blockAllocator(blockManager, threadContext.mConstraintBlockStream, threadContext.mFrictionPatchStreamPair, threadContext.mConstraintSize);

		const PxTransform id(PxIdentity);

		Cm::SpatialVectorF* Z = threadContext.mZVector.begin();

		PxSort<PxSolverConstraintDesc, ArticulationStaticConstraintSortPredicate>(mStatic1DConstraints.begin(), mStatic1DConstraints.size(), ArticulationStaticConstraintSortPredicate());
		PxSort<PxSolverConstraintDesc, ArticulationStaticConstraintSortPredicate>(mStaticContactConstraints.begin(), mStaticContactConstraints.size(), ArticulationStaticConstraintSortPredicate());

		for (PxU32 i = 0; i < mStatic1DConstraints.size(); ++i)
		{
			PxSolverConstraintDesc& desc = mStatic1DConstraints[i];

			PxU32 linkIndex = desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY ? desc.linkIndexA : desc.linkIndexB;

			PX_ASSERT(desc.constraintType == DY_SC_TYPE_RB_1D);
			const Constraint* constraint = reinterpret_cast<const Constraint*>(desc.constraint);

			SolverConstraintShaderPrepDesc shaderPrepDesc;
			PxSolverConstraintPrepDesc prepDesc;

			const PxConstraintSolverPrep solverPrep = constraint->solverPrep;
			const void* constantBlock = constraint->constantBlock;
			const PxU32 constantBlockByteSize = constraint->constantBlockSize;
			const PxTransform& pose0 = (constraint->body0 ? constraint->body0->getPose() : id);
			const PxTransform& pose1 = (constraint->body1 ? constraint->body1->getPose() : id);
			const PxSolverBody* sbody0 = desc.bodyA;
			const PxSolverBody* sbody1 = desc.bodyB;
			PxSolverBodyData* sbodyData0 = &solverBodyData[desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY ? 0 : desc.bodyADataIndex];
			PxSolverBodyData* sbodyData1 = &solverBodyData[desc.linkIndexB != PxSolverConstraintDesc::RIGID_BODY ? 0 : desc.bodyBDataIndex];

			shaderPrepDesc.constantBlock = constantBlock;
			shaderPrepDesc.constantBlockByteSize = constantBlockByteSize;
			shaderPrepDesc.constraint = constraint;
			shaderPrepDesc.solverPrep = solverPrep;

			prepDesc.desc = &desc;
			prepDesc.bodyFrame0 = pose0;
			prepDesc.bodyFrame1 = pose1;
			prepDesc.data0 = sbodyData0;
			prepDesc.data1 = sbodyData1;
			prepDesc.body0 = sbody0;
			prepDesc.body1 = sbody1;
			prepDesc.linBreakForce = constraint->linBreakForce;
			prepDesc.angBreakForce = constraint->angBreakForce;
			prepDesc.writeback = &constraintWritebackPool[constraint->index];
			setupConstraintFlags(prepDesc, constraint->flags);
			prepDesc.minResponseThreshold = constraint->minResponseThreshold;

			SetupSolverConstraint(shaderPrepDesc, prepDesc, blockAllocator, dt, invDt, jointBiasCoefficient);

			if (desc.constraint)
			{
				if (mArticulationData.mNbStatic1DConstraints[linkIndex] == 0)
					mArticulationData.mStatic1DConstraintStartIndex[linkIndex] = i;
				mArticulationData.mNbStatic1DConstraints[linkIndex]++;
			}
			else
			{
				//Shuffle down 
				mStatic1DConstraints.remove(i);
				i--;
			}
		}

		for (PxU32 i = 0; i < mStaticContactConstraints.size(); ++i)
		{
			PxSolverConstraintDesc& desc = mStaticContactConstraints[i];

			PxU32 linkIndex = desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY ? desc.linkIndexA : desc.linkIndexB;

			PX_ASSERT(desc.constraintType == DY_SC_TYPE_RB_CONTACT);
			
			PxSolverContactDesc blockDesc;
			PxsContactManager* cm = reinterpret_cast<PxsContactManager*>(desc.constraint);
			PxcNpWorkUnit& unit = cm->getWorkUnit();
			PxsContactManagerOutput* cmOutput = &outputs.getContactManagerOutput(unit.mNpIndex);

			PxSolverBodyData& data0 = desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY ? solverBodyData[0] : solverBodyData[desc.bodyADataIndex];
			PxSolverBodyData& data1 = desc.linkIndexB != PxSolverConstraintDesc::RIGID_BODY ? solverBodyData[0] : solverBodyData[desc.bodyBDataIndex];

			blockDesc.data0 = &data0;
			blockDesc.data1 = &data1;

			PxU8 flags = unit.mRigidCore0->mFlags;
			if (unit.mRigidCore1)
				flags |= PxU8(unit.mRigidCore1->mFlags);

			blockDesc.bodyFrame0 = unit.mRigidCore0->body2World;
			blockDesc.bodyFrame1 = unit.mRigidCore1 ? unit.mRigidCore1->body2World : id;
			blockDesc.shapeInteraction = cm->getShapeInteraction();
			blockDesc.contactForces = cmOutput->contactForces;
			blockDesc.desc = &desc;
			blockDesc.body0 = desc.bodyA;
			blockDesc.body1 = desc.bodyB;
			blockDesc.hasForceThresholds = !!(unit.mFlags & PxcNpWorkUnitFlag::eFORCE_THRESHOLD);
			blockDesc.disableStrongFriction = !!(unit.mFlags & PxcNpWorkUnitFlag::eDISABLE_STRONG_FRICTION);
			blockDesc.bodyState0 = (unit.mFlags & PxcNpWorkUnitFlag::eARTICULATION_BODY0) ? PxSolverContactDesc::eARTICULATION : PxSolverContactDesc::eDYNAMIC_BODY;
			blockDesc.bodyState1 = (unit.mFlags & PxcNpWorkUnitFlag::eARTICULATION_BODY1) ? PxSolverContactDesc::eARTICULATION : (unit.mFlags & PxcNpWorkUnitFlag::eHAS_KINEMATIC_ACTOR) ? PxSolverContactDesc::eKINEMATIC_BODY :
				((unit.mFlags & PxcNpWorkUnitFlag::eDYNAMIC_BODY1) ? PxSolverContactDesc::eDYNAMIC_BODY : PxSolverContactDesc::eSTATIC_BODY);
			//blockDesc.flags = unit.flags;

			unit.setInvMassScaleFromDominance(blockDesc.invMassScales);

			blockDesc.restDistance = unit.mRestDistance;
			blockDesc.frictionPtr = unit.mFrictionDataPtr;
			blockDesc.frictionCount = unit.mFrictionPatchCount;
			blockDesc.maxCCDSeparation = (flags & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD) ? ccdMaxSeparation : PX_MAX_F32;
			blockDesc.offsetSlop = unit.mOffsetSlop;

			createFinalizeSolverContacts(blockDesc, *cmOutput, threadContext, invDt, dt, bounceThreshold,
				frictionOffsetThreshold, correlationDist, rigidContactBiasCoefficient, blockAllocator, Z);

			getContactManagerConstraintDesc(*cmOutput, *cm, desc);

			updateFrictionAnchorCountAndPosition(desc, *cmOutput, blockDesc);

			unit.mFrictionDataPtr = blockDesc.frictionPtr;
			unit.mFrictionPatchCount = blockDesc.frictionCount;
			//KS - Don't track this for now!
			//axisConstraintCount += blockDesc.axisConstraintCount;

			if (desc.constraint)
			{
				if (mArticulationData.mNbStaticContactConstraints[linkIndex] == 0)
					mArticulationData.mStaticContactConstraintStartIndex[linkIndex] = i;
				mArticulationData.mNbStaticContactConstraints[linkIndex]++;
			}
			else
			{
				mStaticContactConstraints.remove(i);
				i--;
			}
		}
	}

	static PX_FORCE_INLINE void setupDeltaV(ArticulationInternalConstraintBase& c, const Cm::SpatialVector& deltaVA, const Cm::SpatialVector& deltaVB)
	{
		c.deltaVA.top = deltaVA.angular;
		c.deltaVA.bottom = deltaVA.linear;
		c.deltaVB.top = deltaVB.angular;
		c.deltaVB.bottom = deltaVB.linear;
	}

	static void setupComplexLimit(const ArticulationLink* links, ArticulationData& data, PxU32 linkID, PxReal angle, PxReal lowLimit, PxReal highLimit,
		const PxVec3& axis, PxReal cfm, ArticulationInternalConstraint& complexConstraint, ArticulationInternalLimit& limit)
	{
		Cm::SpatialVector deltaVA, deltaVB;
		FeatherstoneArticulation::getImpulseSelfResponse(links, data,
			links[linkID].parent, Cm::SpatialVector(PxVec3(0), axis), deltaVA,
			linkID, Cm::SpatialVector(PxVec3(0), -axis), deltaVB);

		const PxReal r0 = deltaVA.angular.dot(axis);
		const PxReal r1 = deltaVB.angular.dot(axis);

		const PxReal unitResponse = r0 - r1;

		const PxReal recipResponse = unitResponse > DY_ARTICULATION_MIN_RESPONSE ? 1.0f / (cfm + unitResponse) : 0.0f;

		complexConstraint.row0 = Cm::UnAlignedSpatialVector(PxVec3(0), axis);
		complexConstraint.row1 = Cm::UnAlignedSpatialVector(PxVec3(0), axis);
		setupDeltaV(complexConstraint, deltaVA, deltaVB);
		complexConstraint.recipResponse = recipResponse;
		complexConstraint.response = unitResponse;
		complexConstraint.isLinearConstraint = true;
		limit.errorLow = angle - lowLimit;
		limit.errorHigh = highLimit - angle;
		limit.lowImpulse = 0.f;
		limit.highImpulse = 0.f;
	}

namespace
{
	struct LazyTransforms	// PT: helper to lazy-evaluate cA2w and cB2w instead of always computing this at each step of the recursion
	{
		LazyTransforms() : mAvailable(false)	{}
		PxTransform mCA2w;
		PxTransform mCB2w;
		bool		mAvailable;

		void getTransforms(const ArticulationLink& pLink, const ArticulationLink& link, const ArticulationJointCore& j)
		{
			if(!mAvailable)
			{
				mAvailable = true;
				// PT:: tag: scalar transform*transform
				mCA2w = pLink.bodyCore->body2World.transform(j.parentPose);
				mCB2w = link.bodyCore->body2World.transform(j.childPose);
			}
		}
	};
}

static PX_FORCE_INLINE void fillArticConstraint(ArticulationInternalConstraint* constraints,
	const Cm::SpatialVectorF& row0,
	const Cm::SpatialVectorF& row1,
	const Cm::SpatialVector& deltaVA,
	const Cm::SpatialVector& deltaVB,
	const PxJointFrictionParams& friction,
	const PxArticulationDrive& drive,
	PxReal maxForceScale,
	PxReal externalJointForce,
	PxReal unitResponse,
	PxReal recipResponse,
	PxReal transmissionForce,
	PxReal maxJointVelocity,
	bool isLinearConstraint
)
{
	constraints->row0 = row0;
	constraints->row1 = row1;
	constraints->recipResponse = recipResponse;
	constraints->response = unitResponse;
	setupDeltaV(*constraints, deltaVA, deltaVB);

	constraints->envelope = drive.envelope;
	constraints->externalJointForce = externalJointForce;
	constraints->driveImpulse = 0.0f;
	constraints->driveMaxImpulse = drive.maxForce * maxForceScale;
	constraints->dynamicFrictionEffort = friction.dynamicFrictionEffort;
	constraints->staticFrictionEffort = friction.staticFrictionEffort;
	constraints->viscousFrictionCoefficient = friction.viscousFrictionCoefficient;
	constraints->deprecatedFrictionMaxForce = transmissionForce;
	constraints->accumulatedFrictionImpulse = 0.0f;
	constraints->maxJointVelocity = maxJointVelocity;
	constraints->isLinearConstraint = isLinearConstraint;
}

	void FeatherstoneArticulation::setupInternalConstraintsRecursive(
		const ArticulationLink* links,
		ArticulationData& data,
		const PxReal stepDt,
		const PxReal dt,
		const bool isTGSSolver, 
		const PxU32 linkID,
		const PxReal maxForceScale
		)
	{
		const ArticulationLink& link = links[linkID];

		const ArticulationJointCoreData& jointDatum = data.getJointData(linkID);

		const PxU32 parent = link.parent;
		const ArticulationLink& pLink = links[parent];

		const ArticulationJointCore& j = *link.inboundJoint;

		PxReal transmissionForce = 0.0f;
		if(j.frictionCoefficient > 0.0f)	// PT: avoid computing the deprecated transmissionForce if we don't have to
		{
			const PxReal deprecatedFCoefficient = j.frictionCoefficient * stepDt;
			transmissionForce = data.getTransmittedForce(linkID).magnitude() * deprecatedFCoefficient;
		}

		LazyTransforms transforms;

		const PxReal cfm = PxMax(link.cfm, pLink.cfm);

		//Linear, then angular...
		const PxReal* PX_RESTRICT jointPositions = data.getJointPositions();
		const PxReal* PX_RESTRICT jointTargetPositions = data.getJointTargetPositions();
		const Cm::UnAlignedSpatialVector* PX_RESTRICT worldMotionMatrix = data.getWorldMotionMatrix();

		PxVec3 driveError(0.f);
		PxVec3 angles(0.f);
		PxVec3 row[3];
		if (j.jointType == PxArticulationJointType::eSPHERICAL && jointDatum.nbDof > 1)
		{
			//It's a spherical joint. We can't directly work on joint positions with spherical joints, so we instead need to compute the quaternion
			//and from that compute the joint error projected onto the DOFs. This will yield a rotation that is singularity-free, where the joint
			//angles match the target joint angles provided the angles are within +/- Pi around each axis. Spherical joints do not support
			//quaternion double cover cases/wide angles.

			PxVec3 driveAxis(0.f);

			bool hasAngularDrives = false;

			PxU32 tmpDofId = 0;

			for (PxU32 i = 0; i < PxArticulationAxis::eX; ++i)
			{
				if (j.motion[i] != PxArticulationMotion::eLOCKED)
				{
					const bool hasDrive = (j.motion[i] != PxArticulationMotion::eLOCKED && j.drives[i].driveType != PxArticulationDriveType::eNONE);

					if (hasDrive)
					{
						// The code further below relies on x,y,z of a PxVec3 to map to twist, swing1, swing2
						PX_COMPILE_TIME_ASSERT(PxArticulationAxis::eTWIST == 0);
						PX_COMPILE_TIME_ASSERT(PxArticulationAxis::eSWING1 == 1);
						PX_COMPILE_TIME_ASSERT(PxArticulationAxis::eSWING2 == 2);
						PX_ASSERT(i < 3);

						driveAxis[i] = jointTargetPositions[jointDatum.jointOffset + tmpDofId];

						hasAngularDrives = true;
					}

					tmpDofId++;
				}
			}
			
			{
				{
					//Spherical joint drive calculation using 3x child-space Euler angles
					if (hasAngularDrives)
					{
						transforms.getTransforms(pLink, link, j);
						const PxTransform& cA2w = transforms.mCA2w;
						const PxTransform& cB2w = transforms.mCB2w;
						
						const PxQuat qB2qA = cA2w.q.getConjugate() * cB2w.q;

						PxReal angle = driveAxis.normalize();

						if (angle < 1e-12f)
						{
							driveAxis = PxVec3(1.f, 0.f, 0.f);
							angle = 0.f;
						}

						PxQuat targetQ = PxQuat(angle, driveAxis);
						// note: the targets are defined relative to the parent link joint frame, so this transform is targetQ2qA.

						if (targetQ.dot(qB2qA) < 0.f)
							targetQ = -targetQ;

						driveError = -2.f * (targetQ.getConjugate() * qB2qA).getImaginaryPart();
					}

					for (PxU32 i = 0, tmpDof = 0; i < PxArticulationAxis::eX; ++i)
					{
						if (j.motion[i] != PxArticulationMotion::eLOCKED)
						{
							angles[i] = jointPositions[jointDatum.jointOffset + tmpDof];
							row[i] = worldMotionMatrix[jointDatum.jointOffset + tmpDof].top;
							tmpDof++;
						}
					}
				}
			}
		}
		else
		{
			for (PxU32 i = 0; i < PxArticulationAxis::eX; ++i)
			{
				if (j.motion[i] != PxArticulationMotion::eLOCKED)
				{
					const float jPos = jointPositions[jointDatum.jointOffset];
					driveError[i] = jointTargetPositions[jointDatum.jointOffset] - jPos;
					angles[i] = jPos;
					row[i] = worldMotionMatrix[jointDatum.jointOffset].top;

					break;
				}
			}
		}

		PxU32 dofId = 0;

		for (PxU32 i = 0; i < PxArticulationAxis::eX; ++i)
		{
			if (j.motion[i] != PxArticulationMotion::eLOCKED)
			{
				//Impulse response vector and axes are common for all constraints on this axis besides locked axis!!!
				const PxVec3& axis = row[i];

				Cm::SpatialVector deltaVA, deltaVB;
				FeatherstoneArticulation::getImpulseSelfResponse(links, data,
					parent, Cm::SpatialVector(PxVec3(0), axis), deltaVA,
					linkID, Cm::SpatialVector(PxVec3(0), -axis), deltaVB);

				const PxReal r0 = deltaVA.angular.dot(axis);
				const PxReal r1 = deltaVB.angular.dot(axis);

				const PxReal unitResponse = r0 - r1;

				const PxReal recipResponse = unitResponse <= 0.f ? 0.f : 1.0f / (unitResponse + cfm);

				const PxU32 count = data.mInternalConstraints.size();
				data.mInternalConstraints.forceSize_Unsafe(count + 1);
				ArticulationInternalConstraint* constraints = &data.mInternalConstraints[count];

				const Cm::SpatialVectorF R(PxVec3(0), axis);
				fillArticConstraint(constraints,
					R,
					R,
					deltaVA,
					deltaVB,
					j.frictionParams[i],
					j.drives[i],
					maxForceScale,
					data.getJointForces()[jointDatum.jointOffset + dofId],
					unitResponse,
					recipResponse,
					transmissionForce,
					j.maxJointVelocity[i],
					false
				);

				const bool hasDrive = (j.motion[i] != PxArticulationMotion::eLOCKED && j.drives[i].driveType != PxArticulationDriveType::eNONE);
				if(hasDrive)
				{
					constraints->setImplicitDriveDesc(
						computeImplicitDriveParams(
							j.drives[i].driveType, j.drives[i].stiffness, j.drives[i].damping,
							isTGSSolver ? stepDt : dt, dt,
							unitResponse, recipResponse, driveError[i], data.getJointTargetVelocities()[jointDatum.jointOffset + dofId],
							isTGSSolver));
				}
				else
				{
					constraints->setImplicitDriveDesc(ArticulationImplicitDriveDesc(PxZero));
				}

				if (j.motion[i] == PxArticulationMotion::eLIMITED)
				{
					const PxU32 limitCount = data.mInternalLimits.size();
					data.mInternalLimits.forceSize_Unsafe(limitCount + 1);
					ArticulationInternalLimit* limits = &data.mInternalLimits[limitCount];

					const PxReal jPos = angles[i];
					limits->errorHigh = j.limits[i].high - jPos;
					limits->errorLow = jPos - j.limits[i].low;
					limits->lowImpulse = 0.f;
					limits->highImpulse = 0.f;
				}
				dofId++;
			}
		}

		for (PxU32 i = PxArticulationAxis::eX; i < PxArticulationAxis::eCOUNT; ++i)
		{
			if (j.motion[i] != PxArticulationMotion::eLOCKED)
			{
				transforms.getTransforms(pLink, link, j);
				const PxTransform& cA2w = transforms.mCA2w;
				const PxTransform& cB2w = transforms.mCB2w;

				//Impulse response vector and axes are common for all constraints on this axis besides locked axis!!!
				const PxVec3 axis = worldMotionMatrix[jointDatum.jointOffset + dofId].bottom;
				const PxVec3 ang0 = (cA2w.p - pLink.bodyCore->body2World.p).cross(axis);
				const PxVec3 ang1 = (cB2w.p - link.bodyCore->body2World.p).cross(axis);

				Cm::SpatialVector deltaVA, deltaVB;
				FeatherstoneArticulation::getImpulseSelfResponse(links, data,
					links[linkID].parent, Cm::SpatialVector(axis, ang0), deltaVA,
					linkID, Cm::SpatialVector(-axis, -ang1), deltaVB);

				const PxReal r0 = deltaVA.linear.dot(axis) + deltaVA.angular.dot(ang0);
				const PxReal r1 = deltaVB.linear.dot(axis) + deltaVB.angular.dot(ang1);

				const PxReal unitResponse = r0 - r1;

				//const PxReal recipResponse = unitResponse > DY_ARTICULATION_MIN_RESPONSE ? 1.0f / (unitResponse + cfm) : 0.0f;
				const PxReal recipResponse = 1.0f / (unitResponse + cfm);

				const PxU32 count = data.mInternalConstraints.size();
				data.mInternalConstraints.forceSize_Unsafe(count + 1);
				ArticulationInternalConstraint* constraints = &data.mInternalConstraints[count];

				fillArticConstraint(constraints,
					Cm::SpatialVectorF(axis, ang0),
					Cm::SpatialVectorF(axis, ang1),
					deltaVA,
					deltaVB,
					j.frictionParams[i],
					j.drives[i],
					maxForceScale,
					data.getJointForces()[jointDatum.jointOffset + dofId],
					unitResponse,
					recipResponse,
					transmissionForce,
					j.maxJointVelocity[i],
					true
				);
									  
				const bool hasDrive = (j.motion[i] != PxArticulationMotion::eLOCKED && (j.drives[i].envelope.maxEffort > 0.0f || j.drives[i].maxForce > 0.0f) && (j.drives[i].stiffness > 0.f || j.drives[i].damping > 0.f));
				if(hasDrive)
				{
					const PxU32 jOffset = jointDatum.jointOffset + dofId;

					constraints->setImplicitDriveDesc(
						computeImplicitDriveParams(
							j.drives[i].driveType, j.drives[i].stiffness, j.drives[i].damping,
							isTGSSolver ? stepDt : dt, dt,
							unitResponse, recipResponse,
							jointTargetPositions[jOffset] - jointPositions[jOffset],
							data.getJointTargetVelocities()[jOffset],
							isTGSSolver));
				}
				else
				{
					constraints->setImplicitDriveDesc(ArticulationImplicitDriveDesc(PxZero));
				}

				if (j.motion[i] == PxArticulationMotion::eLIMITED)
				{
					const PxU32 limitCount = data.mInternalLimits.size();
					data.mInternalLimits.forceSize_Unsafe(limitCount + 1);
					ArticulationInternalLimit* limits = &data.mInternalLimits[limitCount];
					const PxReal jPos = jointPositions[jointDatum.jointOffset + dofId];
					limits->errorHigh = j.limits[i].high - jPos;
					limits->errorLow = jPos - j.limits[i].low;
					limits->lowImpulse = 0.f;
					limits->highImpulse = 0.f;
				}
				dofId++;
			}
		}

		if (jointDatum.dofLimitMask)
		{
			for (PxU32 dof = 0; dof < jointDatum.nbDof; ++dof)
			{
				PxU32 i = j.dofIds[dof];
				if (j.motion[i] == PxArticulationMotion::eLOCKED)
				{
					const PxU32 count = data.mInternalConstraints.size();
					data.mInternalConstraints.forceSize_Unsafe(count + 1);
					ArticulationInternalConstraint* constraints = &data.mInternalConstraints[count];

					const PxU32 limitCount = data.mInternalLimits.size();
					data.mInternalLimits.forceSize_Unsafe(limitCount + 1);
					ArticulationInternalLimit* limits = &data.mInternalLimits[limitCount];

					const PxVec3 axis = row[i];

					PxReal angle = angles[i];

					//A locked axis is equivalent to a limit of 0
					PxReal low = 0.f;
					PxReal high = 0.f;

					setupComplexLimit(links, data, linkID, angle,
						low, high, axis, cfm, *constraints, *limits++);
				}
			}
		}

		const PxU32 numChildren = link.mNumChildren;
		const PxU32 offset = link.mChildrenStartIndex;
		for (PxU32 i = 0; i < numChildren; ++i)
		{
			const PxU32 child = offset + i;
			setupInternalConstraintsRecursive(links, data, stepDt, dt, isTGSSolver, child, maxForceScale);
		}
	}

	void FeatherstoneArticulation::setupInternalSpatialTendonConstraintsRecursive(
		const ArticulationLink* links,
		ArticulationAttachment* attachments,
		const PxU32 attachmentCount,
		const PxVec3& pAttachPoint,
		const bool fixBase,
		ArticulationData& data,
		const PxReal stepDt,
		const bool isTGSSolver,
		const PxU32 attachmentID,
		const PxReal stiffness,
		const PxReal damping,
		const PxReal limitStiffness,
		const PxReal accumLength,
		const PxU32 startLink,
		const PxVec3& startAxis,
		const PxVec3& startRaXn)
	{
		ArticulationAttachment& attachment = attachments[attachmentID];
		
		const ArticulationLink& cLink = links[attachment.linkInd];

		const PxTransform cBody2World = cLink.bodyCore->body2World;

		const PxVec3 rb = cBody2World.q.rotate(attachment.relativeOffset);

		const PxVec3 cAttachPoint = cBody2World.p + rb;

		const PxVec3 dif = pAttachPoint - cAttachPoint;
		const PxReal distanceSq = dif.magnitudeSquared();
		const PxReal distance = PxSqrt(distanceSq);

		const PxReal u = distance * attachment.coefficient + accumLength;		
	
		const PxU32 childCount = attachment.childCount;
		if (childCount)
		{
			for (ArticulationBitField children = attachment.children; children != 0; children &= (children - 1))
			{
				//index of child of link h on path to link linkID
				const PxU32 child = PxLowestSetBit(children);

				setupInternalSpatialTendonConstraintsRecursive(links, attachments, attachmentCount, cAttachPoint, fixBase, data, stepDt,
					isTGSSolver, child, stiffness, damping, limitStiffness, u, startLink,
					startAxis, startRaXn);
			}
		}
		else
		{
			const PxVec3 axis = distance > 0.001f ? dif / distance : PxVec3(0.f);

			const PxVec3 rbXn = rb.cross(axis);

			Cm::SpatialVector deltaVA, deltaVB;
			FeatherstoneArticulation::getImpulseSelfResponse(links, data,
				startLink, Cm::SpatialVector(startAxis, startRaXn), deltaVA,
				attachment.linkInd, Cm::SpatialVector(-axis, -rbXn), deltaVB);

			const PxReal r0 = deltaVA.linear.dot(startAxis) + deltaVA.angular.dot(startRaXn);
			const PxReal r1 = deltaVB.linear.dot(axis) + deltaVB.angular.dot(rbXn);

			const PxReal unitResponse = (r0 - r1);

			//const PxReal recipResponse = unitResponse > DY_ARTICULATION_MIN_RESPONSE ? 1.0f / (unitResponse + cfm) : 0.0f;
			//const PxReal recipResponse = unitResponse > DY_ARTICULATION_MIN_RESPONSE ? 1.0f / (unitResponse) : 0.0f;
			const PxReal recipResponse = 1.0f / (unitResponse + cLink.cfm);

			const PxU32 count = data.mInternalSpatialTendonConstraints.size();
			data.mInternalSpatialTendonConstraints.forceSize_Unsafe(count + 1);

			ArticulationInternalTendonConstraint* constraint = &data.mInternalSpatialTendonConstraints[count];
			attachment.mConstraintInd = PxU16(count);

			constraint->row0 = Cm::SpatialVectorF(startAxis, startRaXn);
			constraint->row1 = Cm::SpatialVectorF(axis, rbXn);
			constraint->linkID0 = startLink;
			constraint->linkID1 = attachment.linkInd;
			constraint->recipResponse = recipResponse;
			constraint->appliedForce = 0.f;
			constraint->limitAppliedForce = 0.f;

			constraint->setTendonImplicitSpringParams(
				computeTendonSpringParams
					(stepDt, isTGSSolver,
					 unitResponse,
					stiffness, damping,
					limitStiffness));

			constraint->accumulatedLength = u;// + u*0.2f;
			constraint->restDistance = attachment.restLength;
			constraint->lowLimit = attachment.lowLimit;
			constraint->highLimit = attachment.highLimit;
		}
	}

	void FeatherstoneArticulation::updateSpatialTendonConstraintsRecursive(
		const ArticulationAttachment* attachments, const PxU32 attachmentID, 
		const PxReal accumLength, const PxVec3& pAttachPoint, 
		ArticulationData& data)
	{
		const ArticulationAttachment& attachment = attachments[attachmentID];

		//const PxReal restDist = attachment.restDistance;

		const PxTransform& cBody2World = data.getAccumulatedPoses()[attachment.linkInd];

		const PxVec3 rb = cBody2World.q.rotate(attachment.relativeOffset);

		const PxVec3 cAttachPoint = cBody2World.p + rb;

		const PxVec3 dif = pAttachPoint - cAttachPoint;
		const PxReal distanceSq = dif.magnitudeSquared();
		const PxReal distance = PxSqrt(distanceSq);

		const PxReal u = distance * attachment.coefficient + accumLength;

		const PxU32 childCount = attachment.childCount;
		if (childCount)
		{
			for (ArticulationBitField children = attachment.children; children != 0; children &= (children - 1))
			{
				//index of child of link h on path to link linkID
				const PxU32 child = PxLowestSetBit(children);

				updateSpatialTendonConstraintsRecursive(attachments, child, u, cAttachPoint, data);
			}
		}
		else
		{
			PxU32 index = attachment.mConstraintInd;
			ArticulationInternalTendonConstraint& constraint = data.mInternalSpatialTendonConstraints[index];
			
			constraint.accumulatedLength = u;
		}
	}

	void FeatherstoneArticulation::setupInternalFixedTendonConstraintsRecursive(
		const ArticulationLink* links,
		ArticulationTendonJoint* tendonJoints,
		const bool fixBase,
		ArticulationData& data,
		const PxReal stepDt,
		const bool isTGSSolver,
		const PxU32 tendonJointID,
		const PxReal stiffness,
		const PxReal damping,
		const PxReal limitStiffness,
		const PxU32 startLink,
		const PxVec3& startAxis,
		const PxVec3& startRaXn)
	{
		ArticulationTendonJoint& tendonJoint = tendonJoints[tendonJointID];

		const ArticulationLink& cLink = links[tendonJoint.linkInd];

		const PxTransform& cBody2World = cLink.bodyCore->body2World;

		const PxReal cfm = PxMax(cLink.cfm, links[startLink].cfm);

		ArticulationJointCoreData& jointDatum = data.getJointData(tendonJoint.linkInd);
		ArticulationJointCore& joint = *cLink.inboundJoint;

		PxU16 tendonJointAxis = tendonJoint.axis;

		PX_ASSERT(joint.motion[tendonJointAxis] != PxArticulationMotion::eLOCKED);

		//Cm::SpatialVector cImpulse;
		PxU32 dofIndex = joint.invDofIds[tendonJointAxis];

		tendonJoint.startJointOffset = PxTo16(jointDatum.jointOffset + dofIndex);

		///*PxReal jointPose = jointPositions[jointDatum.jointOffset + dofIndex] * tendonJoint.coefficient;

		//jointPose += accumulatedJointPose;*/

		{
			const Cm::UnAlignedSpatialVector* PX_RESTRICT worldMotionMatrix = data.getWorldMotionMatrix();

			PxVec3 axis, rbXn;
			if (tendonJointAxis < PxArticulationAxis::eX)
			{
				const PxVec3 tAxis = worldMotionMatrix[jointDatum.jointOffset + dofIndex].top;
				axis = PxVec3(0.f);
				rbXn = tAxis;
			}
			else
			{
				// PT:: tag: scalar transform*transform
				const PxTransform cB2w = cBody2World.transform(joint.childPose);
				const PxVec3 tAxis = worldMotionMatrix[jointDatum.jointOffset + dofIndex].bottom;
				axis = tAxis;
				rbXn = (cB2w.p - cBody2World.p).cross(axis);
			}
		
			Cm::SpatialVector deltaVA, deltaVB;
			FeatherstoneArticulation::getImpulseSelfResponse(links, data,
				startLink, Cm::SpatialVector(startAxis, startRaXn), deltaVA,
				tendonJoint.linkInd, Cm::SpatialVector(-axis, -rbXn), deltaVB);

			/*const PxU32 pLinkInd = cLink.parent;
			printf("(%i, %i) deltaV1(%f, %f, %f, %f, %f, %f)\n",
				pLinkInd, tendonJoint.linkInd, deltaV1.linear.x, deltaV1.linear.y, deltaV1.linear.z, deltaV1.angular.x,
				deltaV1.angular.y, deltaV1.angular.z);*/

			const PxReal r0 = deltaVA.linear.dot(startAxis) + deltaVA.angular.dot(startRaXn);
			const PxReal r1 = deltaVB.linear.dot(axis) + deltaVB.angular.dot(rbXn);

			const PxReal unitResponse = r0 - r1;

			const PxReal recipResponse = 1.0f / (unitResponse + cfm);

			const PxU32 count = data.mInternalFixedTendonConstraints.size();
			data.mInternalFixedTendonConstraints.forceSize_Unsafe(count + 1);

			ArticulationInternalTendonConstraint* constraint = &data.mInternalFixedTendonConstraints[count];
			tendonJoint.mConstraintInd = PxU16(count);

			constraint->row0 = Cm::UnAlignedSpatialVector(startAxis, startRaXn);
			constraint->row1 = Cm::UnAlignedSpatialVector(axis, rbXn);
			constraint->deltaVA = r0; //We only need to record the change in velocity projected onto the dof for this!
			constraint->deltaVB = Cm::UnAlignedSpatialVector(deltaVB.angular, deltaVB.linear);
			constraint->linkID0 = startLink;
			constraint->linkID1 = tendonJoint.linkInd;
			constraint->recipResponse = recipResponse;
			constraint->appliedForce = 0.0f;
			constraint->limitAppliedForce = 0.0f;

			constraint->setTendonImplicitSpringParams(
				computeTendonSpringParams
					(stepDt, isTGSSolver, unitResponse, 
					 stiffness, damping, 
					 limitStiffness));
		}

		const PxU32 childCount = tendonJoint.childCount;

		if (childCount)
		{
			for (ArticulationBitField children = tendonJoint.children; children != 0; children &= (children - 1))
			{
				//index of child of link h on path to link linkID
				const PxU32 child = PxLowestSetBit(children);

				setupInternalFixedTendonConstraintsRecursive(links, tendonJoints, fixBase, data, stepDt,
					isTGSSolver, child, stiffness, damping, limitStiffness, startLink, startAxis, startRaXn);
			}
		}
	}

	void FeatherstoneArticulation::setupInternalConstraints(ArticulationData& data, PxReal stepDt, PxReal dt, PxReal /*invDt*/, bool isTGSSolver)
	{
		const ArticulationLink* PX_RESTRICT links = data.getLinks();
		const bool fixBase = data.getArticulationFlags() & PxArticulationFlag::eFIX_BASE;

		data.mInternalConstraints.forceSize_Unsafe(0);
		data.mInternalConstraints.reserve(data.getDofs());

		data.mInternalLimits.forceSize_Unsafe(0);
		data.mInternalLimits.reserve(data.getDofs());

		const PxReal maxForceScale = data.getArticulationFlags() & PxArticulationFlag::eDRIVE_LIMITS_ARE_FORCES ? dt : 1.f;

		const PxU32 numChildren = links[0].mNumChildren;
		const PxU32 offset = links[0].mChildrenStartIndex;
		for (PxU32 i = 0; i < numChildren; ++i)
		{
			const PxU32 child = offset + i;

			setupInternalConstraintsRecursive(links, data, stepDt, dt, isTGSSolver, child, maxForceScale);
		}

		PxU32 totalNumAttachments = 0;
		for (PxU32 i = 0; i < data.mNumSpatialTendons; ++i)
		{
			Dy::ArticulationSpatialTendon* tendon = data.mSpatialTendons[i];
			totalNumAttachments += tendon->getNumAttachments();
		}

		data.mInternalSpatialTendonConstraints.forceSize_Unsafe(0);
		data.mInternalSpatialTendonConstraints.reserve(totalNumAttachments);

		for (PxU32 i = 0; i < data.mNumSpatialTendons; ++i)
		{
			Dy::ArticulationSpatialTendon* tendon = data.mSpatialTendons[i];

			Dy::ArticulationAttachment* attachments = tendon->getAttachments();

			ArticulationAttachment& pAttachment = attachments[0];

			//const PxU32 childCount = pAttachment.childCount;
			
			//PxReal scale = 1.f/PxReal(childCount);

			const PxReal coefficient = pAttachment.coefficient;

			const PxU32 startLink = pAttachment.linkInd;
			const ArticulationLink& pLink = links[startLink];
			const PxTransform& pBody2World = pLink.bodyCore->body2World;
			const PxVec3 ra = pBody2World.q.rotate(pAttachment.relativeOffset);
			const PxVec3 pAttachPoint = pBody2World.p + ra;
			
			for (ArticulationAttachmentBitField children = pAttachment.children; children != 0; children &= (children - 1))
			{
				//index of child of link h on path to link linkID
				const PxU32 child = PxLowestSetBit(children);

				ArticulationAttachment& attachment = attachments[child];
				const ArticulationLink& cLink = links[attachment.linkInd];
				const PxTransform cBody2World = cLink.bodyCore->body2World;
				const PxVec3 rb = cBody2World.q.rotate(attachment.relativeOffset);
				const PxVec3 cAttachPoint = cBody2World.p + rb;

				const PxVec3 axis = (pAttachPoint - cAttachPoint).getNormalized();
				const PxVec3 raXn = ra.cross(axis);

				setupInternalSpatialTendonConstraintsRecursive(links, attachments, tendon->getNumAttachments(), pAttachPoint, fixBase, data, stepDt, isTGSSolver,
					child, tendon->mStiffness, tendon->mDamping, tendon->mLimitStiffness, tendon->mOffset*coefficient, startLink,
					axis, raXn);
			}
		}

		PxU32 totalNumTendonJoints = 0;
		for (PxU32 i = 0; i < data.mNumFixedTendons; ++i)
		{
			Dy::ArticulationFixedTendon* tendon = data.mFixedTendons[i];
			totalNumTendonJoints += tendon->getNumJoints();
		}

		data.mInternalFixedTendonConstraints.forceSize_Unsafe(0);
		data.mInternalFixedTendonConstraints.reserve(totalNumTendonJoints);

		for (PxU32 i = 0; i < data.mNumFixedTendons; ++i)
		{
			ArticulationFixedTendon* tendon = data.mFixedTendons[i];

			ArticulationTendonJoint* tendonJoints = tendon->getTendonJoints();

			ArticulationTendonJoint& pTendonJoint = tendonJoints[0];

			const PxU32 startLinkInd = pTendonJoint.linkInd;
			const ArticulationLink& pLink = links[startLinkInd];
			const PxTransform& pBody2World = pLink.bodyCore->body2World;
			
			for (ArticulationAttachmentBitField children = pTendonJoint.children; children != 0; children &= (children - 1))
			{
				//index of child of link h on path to link linkID
				const PxU32 child = PxLowestSetBit(children);

				ArticulationTendonJoint& cTendonJoint = tendonJoints[child];

				const ArticulationLink& cLink = links[cTendonJoint.linkInd];

				ArticulationJointCore* joint = cLink.inboundJoint;

				PX_ASSERT(joint != NULL);

				ArticulationJointCoreData* jointDatum = &data.getJointData(cTendonJoint.linkInd);

				PxU16 tendonJointAxis = cTendonJoint.axis;

				PX_ASSERT(joint->motion[tendonJointAxis] != PxArticulationMotion::eLOCKED);

				PxVec3 startAxis, raXn;
				PxU32 dofIndex = joint->invDofIds[tendonJointAxis];

				const Cm::UnAlignedSpatialVector* PX_RESTRICT worldMotionMatrix = data.getWorldMotionMatrix();

				if (tendonJointAxis < PxArticulationAxis::eX)
				{
					const PxVec3 axis = worldMotionMatrix[jointDatum->jointOffset + dofIndex].top;
					startAxis = PxVec3(0.f);
					raXn = axis;
				}
				else
				{
					// PT:: tag: scalar transform*transform
					const PxTransform cA2w = pBody2World.transform(joint->parentPose);
					const PxVec3 axis = worldMotionMatrix[jointDatum->jointOffset + dofIndex].bottom;
					const PxVec3 ang0 = (cA2w.p - pBody2World.p).cross(axis);
					startAxis = axis;
					raXn = ang0;
				}

				setupInternalFixedTendonConstraintsRecursive(links, tendonJoints, fixBase, data, stepDt, isTGSSolver,
					child, tendon->mStiffness, tendon->mDamping, tendon->mLimitStiffness, startLinkInd, startAxis, raXn);
			}
		}

		//Now set up the internal mimic joint constraints.
		setupInternalMimicJointConstraints(data);
	}

	void FeatherstoneArticulation::setupSolverConstraints(ArticulationData& data)
	{
		setupInternalConstraints(data, data.getDt(), data.getDt(), 1.f / data.getDt(), false);
	}

	void FeatherstoneArticulation::setupSolverConstraintsTGS(FeatherstoneArticulation* articulation, PxReal dt, PxReal invDt, PxReal totalDt)
	{
		articulation->setupInternalConstraints(articulation->mArticulationData, dt, totalDt, invDt, true);
	}

	void FeatherstoneArticulation::teleportLinks(ArticulationData& data)
	{
		const ArticulationLink* PX_RESTRICT links = data.getLinks();
	
		ArticulationJointCoreData* PX_RESTRICT jointData = data.getJointData();

		const PxReal* PX_RESTRICT jointPositions = data.getJointPositions();
		const Cm::UnAlignedSpatialVector* PX_RESTRICT motionMatrix = data.getMotionMatrix();

		const PxU32 linkCount = data.getLinkCount();
		const PxQuat* PX_RESTRICT relativeQuats = data.getRelativeQuat();

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

			const ArticulationLink& pLink = links[link.parent];
			const PxTransform pBody2World = pLink.bodyCore->body2World;

			const ArticulationJointCore* joint = link.inboundJoint;

			const PxReal* jPosition = &jointPositions[jointDatum.jointOffset];

			PxQuat newParentToChild;
			PxQuat newWorldQ;
			PxVec3 r;

			const PxVec3 childOffset = -joint->childPose.p;
			const PxVec3 parentOffset = joint->parentPose.p;

			const PxQuat relativeQuat = relativeQuats[linkID];

			switch (joint->jointType)
			{
			case PxArticulationJointType::ePRISMATIC:
			{
				newParentToChild = relativeQuat;
				const PxVec3 e = newParentToChild.rotate(parentOffset);
				const PxVec3 d = childOffset;

				const PxVec3& u = motionMatrix[jointDatum.jointOffset].bottom;

				r = e + d + u * jPosition[0];
				break;
			}
			case PxArticulationJointType::eREVOLUTE:
			case PxArticulationJointType::eREVOLUTE_UNWRAPPED:
			{
				const PxVec3& u = motionMatrix[jointDatum.jointOffset].top;

				PxQuat jointRotation = PxQuat(-jPosition[0], u);
				if (jointRotation.w < 0)	//shortest angle.
					jointRotation = -jointRotation;

				newParentToChild = (jointRotation * relativeQuat).getNormalized();

				const PxVec3 e = newParentToChild.rotate(parentOffset);
				const PxVec3 d = childOffset;
				r = e + d;

				PX_ASSERT(r.isFinite());

				break;
			}
			case PxArticulationJointType::eSPHERICAL:
			{
				PxQuat jointRotation(PxIdentity);

				{
					PxVec3 axis(0.f);
					for (PxU32 d = 0; d < jointDatum.nbDof; ++d)
					{
						axis += motionMatrix[jointDatum.jointOffset + d].top * -jPosition[d];
					}
					PxReal angle = axis.normalize();
					jointRotation = angle < 1e-10f ? PxQuat(PxIdentity) : PxQuat(angle, axis);
					if(jointRotation.w < 0.f)
						jointRotation = -jointRotation;
				}

				newParentToChild = (jointRotation * relativeQuat).getNormalized();

				const PxVec3 e = newParentToChild.rotate(parentOffset);
				const PxVec3 d = childOffset;
				r = e + d;
				break;
			}
			case PxArticulationJointType::eFIX:
			{
				//this is fix joint so joint don't have velocity
				newParentToChild = relativeQuat;

				const PxVec3 e = newParentToChild.rotate(parentOffset);
				const PxVec3 d = childOffset;

				r = e + d;
				break;
			}
			default:
				break;
			}

			PxTransform& body2World = link.bodyCore->body2World;
			body2World.q = (pBody2World.q * newParentToChild.getConjugate()).getNormalized();
			body2World.p = pBody2World.p + body2World.q.rotate(r);

			PX_ASSERT(body2World.isSane());
		}
	}

	void FeatherstoneArticulation::computeLinkVelocities(ArticulationData& data)
	{
		const ArticulationLink* PX_RESTRICT links = data.getLinks();
		const PxU32 linkCount = data.getLinkCount();

		Cm::SpatialVectorF* PX_RESTRICT motionVelocities = data.getMotionVelocities();
		const PxReal* PX_RESTRICT jointVelocities = data.getJointVelocities();

		// sync root motion vel:
		const PxsBodyCore& rootBodyCore = *links[0].bodyCore;
		motionVelocities[0].top = rootBodyCore.angularVelocity;
		motionVelocities[0].bottom = rootBodyCore.linearVelocity;

		const Cm::UnAlignedSpatialVector* PX_RESTRICT motionMatrix = data.getMotionMatrix();

		for (PxU32 linkID = 1; linkID < linkCount; ++linkID)
		{
			const ArticulationLink& link = links[linkID];
			
			PxsBodyCore& bodyCore = *link.bodyCore;

			const PxTransform& body2World = bodyCore.body2World;

			const ArticulationLink& plink = links[link.parent];
			const PxsBodyCore& pbodyCore = *plink.bodyCore;

			const Cm::SpatialVectorF parentVel(pbodyCore.angularVelocity, pbodyCore.linearVelocity);

			const PxTransform& pBody2World = pbodyCore.body2World;

			const PxVec3 rw = body2World.p - pBody2World.p;

			Cm::SpatialVectorF vel = FeatherstoneArticulation::translateSpatialVector(-rw, parentVel);

			if (jointVelocities)
			{
				const ArticulationJointCoreData& jointDatum = data.getJointData(linkID);
				const PxReal* jVelocity = &jointVelocities[jointDatum.jointOffset];

				Cm::UnAlignedSpatialVector deltaV = Cm::UnAlignedSpatialVector::Zero();
				for (PxU32 ind = 0; ind < jointDatum.nbDof; ++ind)
				{
					deltaV += motionMatrix[jointDatum.jointOffset + ind] * jVelocity[ind];
				}

				vel.top += body2World.q.rotate(deltaV.top);
				vel.bottom += body2World.q.rotate(deltaV.bottom);
			}

			bodyCore.linearVelocity = vel.bottom;
			bodyCore.angularVelocity = vel.top;
			motionVelocities[linkID] = vel;
		}
	}

	// AD: needed because we define the templated function in a CPP.
	template void FeatherstoneArticulation::jcalc<false>(ArticulationData& data);
	template void FeatherstoneArticulation::jcalc<true>(ArticulationData& data);

	template<bool immediateMode>
	void FeatherstoneArticulation::jcalc(ArticulationData& data)
	{
		const ArticulationLink* PX_RESTRICT links = data.getLinks();
		ArticulationJointCoreData* PX_RESTRICT jointData = data.getJointData();
		PxQuat* PX_RESTRICT relativeQuats = data.getRelativeQuat();
		Cm::UnAlignedSpatialVector* PX_RESTRICT motionMatrix = data.getMotionMatrix();

		const PxU32 linkCount = data.getLinkCount();

		PxU32 totalDof = 0;

		for (PxU32 linkID = 1; linkID < linkCount; ++linkID)
		{
			const ArticulationLink& link = links[linkID];
			ArticulationJointCore* joint = link.inboundJoint;
			ArticulationJointCoreData& jointDatum = jointData[linkID];

			PX_CHECK_AND_RETURN(joint->jointType != PxArticulationJointType::eUNDEFINED, "FeatherstoneArticulation::jcalc application need to define valid joint type and motion");

			// AD: only used in immediate mode because we don't have the llArticulation there to write directly.
			if (immediateMode)
			{
				// compute joint dof
				const PxU32 dof = jointDatum.configureJointDofs(joint);
				PxReal* PX_RESTRICT jointTargetPositions = data.getJointTargetPositions();
				PxReal* PX_RESTRICT jointTargetVelocities = data.getJointTargetVelocities();

				for(PxU32 dofId = 0; dofId < dof; ++dofId)
				{
					PxU32 id = totalDof + dofId;
					PxU32 llDofId = joint->dofIds[dofId];
					jointTargetPositions[id] = joint->targetP[llDofId];
					jointTargetVelocities[id] = joint->targetV[llDofId];
				}

				jointDatum.jointOffset = totalDof;
				totalDof += dof;
			}

			// PT: we now compute the joint axes on-the-fly (as we do in the GPU version) instead of storing them
			Cm::UnAlignedSpatialVector jointAxis[DY_MAX_DOF];
			{
				PxU32 nbDof = 0;
				for (PxU8 i = 0; i < DY_MAX_DOF; ++i)
				{
					// PT: make sure the data is initialized to a valid vector
					jointAxis[i] = Cm::UnAlignedSpatialVector::Zero();
					jointAxis[i].top.x = 1.0f;

					if (joint->motion[i] != PxArticulationMotion::eLOCKED)
					{
						Cm::UnAlignedSpatialVector axis = Cm::UnAlignedSpatialVector::Zero();
						//axis is in the local space of joint
						axis[i] = 1.f;

						jointAxis[nbDof++] = axis;
					}
				}
			}

			joint->setJointFrame(&motionMatrix[jointDatum.jointOffset], jointAxis,
								relativeQuats[linkID],
								jointDatum.nbDof);
		}

		if(immediateMode)
		{
			if(totalDof != data.getDofs())
			{
				data.resizeJointData(totalDof);
			}
			data.setDofs(totalDof);
		} 
	}

	//compute link's spatial inertia tensor
	void FeatherstoneArticulation::computeSpatialInertia(ArticulationData& data)
	{
		SpatialMatrix* PX_RESTRICT worldSpatialArticulatedInertia = data.getWorldSpatialArticulatedInertia();
		PxMat33* PX_RESTRICT worldIsolatedSpatialArticulatedInertia = data.getWorldIsolatedSpatialArticulatedInertia();
		PxReal* PX_RESTRICT masses = data.getMasses();
		const PxU32 nbLinks = data.getLinkCount();
		for (PxU32 linkID = 0; linkID < nbLinks; ++linkID)
		{
			const ArticulationLink& link = data.getLink(linkID);

			const PxsBodyCore& core = *link.bodyCore;

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

			//construct inertia matrix
			const PxVec3p inertiaTensor = Cm::safeRecip<PxVec3p>(core.inverseInertia);
			SpatialMatrix& worldArticulatedInertia = worldSpatialArticulatedInertia[linkID];

			worldArticulatedInertia.topLeft = PxMat33(PxZero);
			worldArticulatedInertia.topRight = PxMat33::createDiagonal(PxVec3(m));
			Cm::transformInertiaTensor(inertiaTensor, core.body2World.q, worldArticulatedInertia.bottomLeft);

			worldIsolatedSpatialArticulatedInertia[linkID] = worldArticulatedInertia.bottomLeft;
			masses[linkID] = m;
		}
	}

	void FeatherstoneArticulation::computeZ(const ArticulationData& data, 
		const PxVec3& gravity, ScratchData& scratchData)
	{
		const Cm::SpatialVectorF* PX_RESTRICT motionVelocities = scratchData.motionVelocities;
		Cm::SpatialVectorF* PX_RESTRICT spatialZAForces = scratchData.spatialZAVectors;
		const Cm::SpatialVector* PX_RESTRICT externalAccels = scratchData.externalAccels;	

		const SpatialMatrix* PX_RESTRICT worldSpatialArticulatedInertia = data.getWorldSpatialArticulatedInertia();

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

			const PxsBodyCore& core = *link.bodyCore;
			//const PxTransform& body2World = core.body2World;

			const PxMat33& I = worldSpatialArticulatedInertia[linkID].bottomLeft;

			//construct spatial zero acceleration
			Cm::SpatialVectorF& z = spatialZAForces[linkID];

			//Cm::SpatialVectorF v;
			//v.top = motionVelocities[linkID].top;
			//v.bottom = motionVelocities[linkID].bottom;

			//KS - limit the magnitude of the angular velocity that contributes to the geometric term. This is a
			//v^2 term and can become unstable if it is too large!

			Cm::SpatialVectorF v = motionVelocities[linkID];

			PxVec3 vA = v.top;

			PxVec3 gravLinAccel(0.f);
			if(!core.disableGravity)
				gravLinAccel = -gravity;

			PX_ASSERT(core.inverseMass != 0.f);

			const PxReal m = 1.0f / core.inverseMass;

			Cm::SpatialVectorF zTmp;

			zTmp.top = (gravLinAccel * m);
			zTmp.bottom = vA.cross(I * vA);

			PX_ASSERT(zTmp.top.isFinite());
			PX_ASSERT(zTmp.bottom.isFinite());

			if (externalAccels)
			{
				const Cm::SpatialVector& externalAccel = externalAccels[linkID];

				const PxVec3 exLinAccel = -externalAccel.linear;
				const PxVec3 exAngAccel = -externalAccel.angular;

				zTmp.top += (exLinAccel * m);
				zTmp.bottom += I * exAngAccel;
			}

			z = zTmp;
		}
	}

	//compute coriolis and centrifugal term
	void FeatherstoneArticulation::computeC(const ArticulationData& data, ScratchData& scratchData)
	{
		const PxReal* jointVelocities = scratchData.jointVelocities;
		Cm::SpatialVectorF* coriolisVectors = scratchData.coriolisVectors;

		const PxU32 linkCount = data.getLinkCount();

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

		const PxVec3* PX_RESTRICT rw = data.getRw();

		for (PxU32 linkID = 1; linkID < linkCount; ++linkID)
		{
			const ArticulationLink& link = data.getLink(linkID);
			const ArticulationJointCoreData& jointDatum = data.getJointData(linkID);

			const PxReal* jVelocity = &jointVelocities[jointDatum.jointOffset];
			Cm::SpatialVectorF& coriolis = coriolisVectors[linkID];
			
			//const PxTransform& body2World = link.bodyCore->body2World;
			//transform parent link's angular velocity into current link's body space
			const PxVec3 pAngular = scratchData.motionVelocities[link.parent].top;
			
			PxVec3 torque = pAngular.cross(pAngular.cross(rw[linkID]));

			//PX_ASSERT(parentAngular.magnitude() < 100.f);

			PxVec3 force(0.f);
			if (jointDatum.nbDof > 0)
			{
				const Cm::UnAlignedSpatialVector* PX_RESTRICT worldMotionMatrix = data.getWorldMotionMatrix();

				Cm::SpatialVectorF relVel(PxVec3(0.f), PxVec3(0.f));
				for (PxU32 ind = 0; ind < jointDatum.nbDof; ++ind)
				{
					//Clamp joint velocity used in coriolis terms to reduce chances of unstable feed-back loops
					const PxReal jV = jVelocity[ind];
					relVel.top += worldMotionMatrix[jointDatum.jointOffset + ind].top * jV;
					relVel.bottom += worldMotionMatrix[jointDatum.jointOffset + ind].bottom * jV;
				}
				const PxVec3 aVec = relVel.top;
				force = pAngular.cross(aVec);

				//compute linear part
				const PxVec3 lVel = relVel.bottom;

				const PxVec3 temp1 = 2.f * pAngular.cross(lVel);
				const PxVec3 temp2 = aVec.cross(lVel);
				torque += temp1 + temp2;
			}

			PX_ASSERT(force.isFinite());
			PX_ASSERT(torque.isFinite());
	
			coriolis = Cm::SpatialVectorF(force, torque);//.rotateInv(body2World);
		}
	}

	void FeatherstoneArticulation::computeRelativeTransformC2P(
		const ArticulationLink* links, const PxU32 linkCount, const ArticulationJointCoreData* jointCoreDatas,
		const Cm::UnAlignedSpatialVector* jointDofMotionMatrices,
		PxTransform* linkAccumulatedPoses, PxVec3* linkRws, Cm::UnAlignedSpatialVector* jointDofMotionMatricesW)
	{
		linkAccumulatedPoses[0] = links[0].bodyCore->body2World;

		for (PxU32 linkID = 1; linkID < linkCount; ++linkID)
		{
			const ArticulationLink& link = links[linkID];
			const PxsBodyCore& bodyCore = *link.bodyCore;
			const PxU32 jointOffset = jointCoreDatas[linkID].jointOffset;
			const PxU32 dofCount = jointCoreDatas[linkID].nbDof;

			const PxTransform& body2World = bodyCore.body2World;

			const ArticulationLink& pLink = links[link.parent];
			const PxsBodyCore& pBodyCore = *pLink.bodyCore;
			const PxTransform& pBody2World = pBodyCore.body2World;

			//const PxTransform tC2P = pBody2World.transformInv(body2World).getNormalized();
			
			linkRws[linkID] = body2World.p - pBody2World.p;
			
			const Cm::UnAlignedSpatialVector* motionMatrix = &jointDofMotionMatrices[jointOffset];
			Cm::UnAlignedSpatialVector* worldMotionMatrix = &jointDofMotionMatricesW[jointOffset];

			for (PxU32 i = 0; i < dofCount; ++i)
			{
				const Cm::UnAlignedSpatialVector worldRow = motionMatrix[i].rotate(body2World);
				worldMotionMatrix[i] = worldRow;
			}

			linkAccumulatedPoses[linkID] = body2World;

#if FEATURESTONE_DEBUG
			{
				//debug
				PxMat33 pToC = c2p.getTranspose();
				//parentToChild -rR
				PxMat33 T2 = skewMatrixPR * pToC;

				PX_ASSERT(SpatialMatrix::isTranspose(linkDatum.childToParent.T, T2));
			}
#endif
		}
	}

	void FeatherstoneArticulation::getDenseJacobian(PxArticulationCache& cache, PxU32 & nRows, PxU32 & nCols)
	{
		//make sure motionMatrix has been set
		//jcalc(mArticulationData);
		initializeCommonData();

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

#if 0	//Enable this if you want to compare results of this function with results of computeLinkVelocities().

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

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

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

		scratchData.jointVelocities = mArticulationData.getJointVelocities();


		computeLinkVelocities(mArticulationData, scratchData);
		const ArticulationLink& baseLink = links[0];
		PxsBodyCore& core0 = *baseLink.bodyCore;
#endif

		const PxU32 totalDofs = getDofs();
		const bool fixBase = mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE;
		//matrix dims is 
		nCols = (fixBase ? 0 : 6) + totalDofs;
		nRows = (fixBase ? 0 : 6) + (linkCount - 1) * 6;

//		auto jacobian = [&](PxU32 row, PxU32 col) -> PxReal &  { return cache.denseJacobian[nCols * row + col]; } ;
		#define jacobian(row, col)	cache.denseJacobian[nCols * (row) + (col)]
		
		PxU32 destRow = 0;
		PxU32 destCol = 0;

		if (!fixBase)
		{
			jacobian(0, 0) = 1.0f;
			jacobian(0, 1) = 0.0f;
			jacobian(0, 2) = 0.0f;
			jacobian(0, 3) = 0.0f;
			jacobian(0, 4) = 0.0f;
			jacobian(0, 5) = 0.0f;

			jacobian(1, 0) = 0.0f;
			jacobian(1, 1) = 1.0f;
			jacobian(1, 2) = 0.0f;
			jacobian(1, 3) = 0.0f;
			jacobian(1, 4) = 0.0f;
			jacobian(1, 5) = 0.0f;

			jacobian(2, 0) = 0.0f;
			jacobian(2, 1) = 0.0f;
			jacobian(2, 2) = 1.0f;
			jacobian(2, 3) = 0.0f;
			jacobian(2, 4) = 0.0f;
			jacobian(2, 5) = 0.0f;

			jacobian(3, 0) = 0.0f;
			jacobian(3, 1) = 0.0f;
			jacobian(3, 2) = 0.0f;
			jacobian(3, 3) = 1.0f;
			jacobian(3, 4) = 0.0f;
			jacobian(3, 5) = 0.0f;

			jacobian(4, 0) = 0.0f;
			jacobian(4, 1) = 0.0f;
			jacobian(4, 2) = 0.0f;
			jacobian(4, 3) = 0.0f;
			jacobian(4, 4) = 1.0f;
			jacobian(4, 5) = 0.0f;

			jacobian(5, 0) = 0.0f;
			jacobian(5, 1) = 0.0f;
			jacobian(5, 2) = 0.0f;
			jacobian(5, 3) = 0.0f;
			jacobian(5, 4) = 0.0f;
			jacobian(5, 5) = 1.0f;

			destRow += 6;
			destCol += 6;
		}

		const PxVec3* PX_RESTRICT rw = mArticulationData.getRw();

		for (PxU32 linkID = 1; linkID < linkCount; ++linkID)//each iteration of this writes 6 rows in the matrix
		{
			const ArticulationLink& link = links[linkID];
			const PxsBodyCore& bodyCore = *link.bodyCore;

			const PxTransform& body2World = bodyCore.body2World;

			const ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(linkID);
			const PxU32 parentLinkID = link.parent;

			if (parentLinkID || !fixBase)
			{
				// VR: mArticulationData.getJointData(0) isn't initialized. parentLinkID can be 0 if fixBase is false.
				//const ArticulationJointCoreData& parentJointDatum = mArticulationData.getJointData(parentLinkID);
				//const PxU32 parentsFirstDestCol = parentJointDatum.jointOffset + (fixBase ? 0 : 6);
				//const PxU32 parentsLastDestCol = parentsFirstDestCol + parentJointDatum.dof;
				const PxU32 parentsLastDestCol = parentLinkID ?
													mArticulationData.getJointData(parentLinkID).jointOffset + (fixBase ? 0 : 6) + mArticulationData.getJointData(parentLinkID).nbDof :
													6;

				// VR: With parentLinkID == 0 this expression has two unsigned integer overflows, but the result is still correct.
				const PxU32 parentsDestRow = (fixBase ? 0 : 6) + (parentLinkID - 1) * 6;

				for (PxU32 col = 0; col < parentsLastDestCol; col++)
				{
					//copy downward the 6 cols from parent
					const PxVec3 parentAng(
						jacobian(parentsDestRow + 3, col),
						jacobian(parentsDestRow + 4, col),
						jacobian(parentsDestRow + 5, col)
						);

					const PxVec3 parentAngxRw = parentAng.cross(rw[linkID]);

					jacobian(destRow + 0, col) = jacobian(parentsDestRow + 0, col) + parentAngxRw.x;
					jacobian(destRow + 1, col) = jacobian(parentsDestRow + 1, col) + parentAngxRw.y;
					jacobian(destRow + 2, col) = jacobian(parentsDestRow + 2, col) + parentAngxRw.z;

					jacobian(destRow + 3, col) = parentAng.x;
					jacobian(destRow + 4, col) = parentAng.y;
					jacobian(destRow + 5, col) = parentAng.z;
				}

				for (PxU32 col = parentsLastDestCol; col < destCol; col++)
				{
					//fill with zeros.
					jacobian(destRow + 0, col) = 0.0f;
					jacobian(destRow + 1, col) = 0.0f;
					jacobian(destRow + 2, col) = 0.0f;

					jacobian(destRow + 3, col) = 0.0f;
					jacobian(destRow + 4, col) = 0.0f;
					jacobian(destRow + 5, col) = 0.0f;
				}
			}

			//diagonal block:
			const Cm::UnAlignedSpatialVector* PX_RESTRICT motionMatrix = mArticulationData.getMotionMatrix();
			for (PxU32 ind = 0; ind < jointDatum.nbDof; ++ind)
			{
				const Cm::UnAlignedSpatialVector& v = motionMatrix[jointDatum.jointOffset + ind];

				const PxVec3 ang = body2World.rotate(v.top);
				const PxVec3 lin = body2World.rotate(v.bottom);

				jacobian(destRow + 0, destCol) = lin.x;
				jacobian(destRow + 1, destCol) = lin.y;
				jacobian(destRow + 2, destCol) = lin.z;

				jacobian(destRow + 3, destCol) = ang.x;
				jacobian(destRow + 4, destCol) = ang.y;
				jacobian(destRow + 5, destCol) = ang.z;

				destCol++;
			}

			//above diagonal block: always zero
			for (PxU32 col = destCol; col < nCols; col++)
			{
				jacobian(destRow + 0, col) = 0.0f;
				jacobian(destRow + 1, col) = 0.0f;
				jacobian(destRow + 2, col) = 0.0f;

				jacobian(destRow + 3, col) = 0.0f;
				jacobian(destRow + 4, col) = 0.0f;
				jacobian(destRow + 5, col) = 0.0f;
			}

			destRow += 6;
		}
		#undef jacobian

#if 0  //Enable this if you want to compare results of this function with results of computeLinkVelocities().

		PxReal * jointVels = mArticulationData.getJointVelocities();//size is totalDofs

		PxReal * jointSpaceVelsVector = new PxReal[nCols];
		PxReal * worldSpaceVelsVector = new PxReal[nRows];

		PxU32 offset = 0;

		//stack input:

		if (!fixBase)
		{
			jointSpaceVelsVector[0] = core0.linearVelocity[0];
			jointSpaceVelsVector[1] = core0.linearVelocity[1];
			jointSpaceVelsVector[2] = core0.linearVelocity[2];

			jointSpaceVelsVector[3] = core0.angularVelocity[0];
			jointSpaceVelsVector[4] = core0.angularVelocity[1];
			jointSpaceVelsVector[5] = core0.angularVelocity[2];

			offset = 6;
		}

		for (PxU32 i = 0; i < totalDofs; i++)
			jointSpaceVelsVector[i + offset] = jointVels[i];

		//multiply:

		for (PxU32 row = 0; row < nRows; row++)
		{
			worldSpaceVelsVector[row] = 0.0f;
			for (PxU32 col = 0; col < nCols; col++)
			{
				worldSpaceVelsVector[row] += jacobian(row, col)*jointSpaceVelsVector[col];
			}
		}

		//worldSpaceVelsVector should now contain the same result as scratchData.motionVelocities (except for swapped linear/angular vec3 order).

		delete[] jointSpaceVelsVector;
		delete[] worldSpaceVelsVector;

		allocator->free(tempMemory);
#endif
	}

	void FeatherstoneArticulation::computeLinkStates(
		const PxF32 dt, const PxReal invLengthScale, const PxVec3& gravity,
		const bool fixBase,
		const PxU32 linkCount,
		const PxTransform* linkAccumulatedPosesW, Cm::SpatialVector* linkExternalAccelsW, const PxVec3* linkRsW, const Cm::UnAlignedSpatialVector* jointDofMotionMatricesW,
		const Dy::ArticulationJointCoreData* jointCoreData, bool externalForcesEveryTgsIterationEnabled,
		Dy::ArticulationLink* links,
		Cm::SpatialVectorF* linkMotionAccelerationsW, Cm::SpatialVectorF* linkMotionVelocitiesW,
		Cm::SpatialVectorF* linkZAExtForcesW, Cm::SpatialVectorF* linkZAIntForcesW, Cm::SpatialVectorF* linkCoriolisVectorsW,
		PxMat33* linkIsolatedSpatialArticulatedInertiasW, PxF32* linkMasses, Dy::SpatialMatrix* linkSpatialArticulatedInertiasW,
		PxReal* jointDofVelocities, PxVec3& comW, PxF32& invSumMass)
	{
		const PxReal invDt = dt == 0.0f ? 0.0f : 1.0f / dt;

		//Initialise motion velocity, motion acceleration and coriolis vector of root link.
		Cm::SpatialVectorF rootLinkVel;
		{
			const Dy::ArticulationLink& baseLink = links[0];
			const PxsBodyCore& core0 = *baseLink.bodyCore;
			rootLinkVel = fixBase ? Cm::SpatialVectorF::Zero() : Cm::SpatialVectorF(core0.angularVelocity, core0.linearVelocity);
			linkMotionVelocitiesW[0] = rootLinkVel;
			linkMotionAccelerationsW[0] = fixBase ? Cm::SpatialVectorF::Zero() : linkMotionAccelerationsW[0];
			linkCoriolisVectorsW[0] = Cm::SpatialVectorF::Zero();
		}

		//Is it really necessary? It is already resolved as an internal constraint.
		PxReal ratio = 1.f;
		if (jointDofVelocities)
		{
			for (PxU32 linkID = 1; linkID < linkCount; ++linkID)
			{
				const ArticulationLink& link = links[linkID];
				const ArticulationJointCoreData& jointDatum = jointCoreData[linkID];
				const PxReal* jVelocity = &jointDofVelocities[jointDatum.jointOffset];
				for (PxU32 ind = 0; ind < jointDatum.nbDof; ++ind)
				{
					const PxReal maxJVelocity = link.inboundJoint->maxJointVelocity[ind];
					const PxReal jVel = jVelocity[ind];
					ratio = (jVel != 0.0f) ? PxMin(ratio, maxJVelocity / PxAbs(jVel)) : ratio;
				}
			}
		}

		PxReal sumMass = 0.f;
		PxVec3 COM(0.f);
		for (PxU32 linkID = 0; linkID < linkCount; ++linkID)
		{
			ArticulationLink& link = links[linkID];
			const PxsBodyCore& bodyCore = *link.bodyCore;

			//Set the cfm value from bodyCore
			link.cfm = (fixBase && linkID == 0) ? 0.f : bodyCore.cfmScale * invLengthScale;

			//Read the inertia and mass from bodyCore
			const PxVec3p inertiaTensor = Cm::safeRecip<PxVec3p>(bodyCore.inverseInertia);

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

			//Compute the inertia matrix Iw = R * I * Rtranspose
			//Compute the articulated inertia.
			PxMat33 Iw; //R * I * Rtranspose
			SpatialMatrix& worldArticulatedInertia = linkSpatialArticulatedInertiasW[linkID];
			{
				Cm::transformInertiaTensor(inertiaTensor, linkAccumulatedPosesW[linkID].q, Iw);

				worldArticulatedInertia.topLeft = PxMat33(PxZero);
				worldArticulatedInertia.topRight = PxMat33::createDiagonal(PxVec3(m));
				worldArticulatedInertia.bottomLeft = Iw;
			}

			//Set the articulated inertia, inertia and mass of the link.
			linkIsolatedSpatialArticulatedInertiasW[linkID] = Iw;
			linkMasses[linkID] = m;

			//Accumulate the centre of mass.
			sumMass += m;
			COM += linkAccumulatedPosesW[linkID].p * m;

			Cm::SpatialVectorF vel;
			if (linkID != 0)
			{
				//Propagate spatial velocity of link parent to link's spatial velocity.
				const Cm::SpatialVectorF pVel = linkMotionVelocitiesW[link.parent];
				vel = FeatherstoneArticulation::translateSpatialVector(-linkRsW[linkID], pVel);

				//Propagate joint dof velocities to the link's spatial velocity vector.
				//Accumulate spatial forces that the joint applies to the link.
				if (jointDofVelocities)
				{
					//The coriolis vector depends on the type of joint and the joint motion matrix.
					//However, some terms in the coriolis vector are common to all joint types. 
					//Write down the term that is independent of the joint.
					Cm::SpatialVectorF coriolisVector(PxVec3(PxZero), pVel.top.cross(pVel.top.cross(linkRsW[linkID])));
					const ArticulationJointCoreData& jointDatum = jointCoreData[linkID];
					if (jointDatum.nbDof)
					{
						//Compute the effect of the joint velocities on the link.
						PxReal* jVelocity = &jointDofVelocities[jointDatum.jointOffset];
						Cm::UnAlignedSpatialVector deltaV = Cm::UnAlignedSpatialVector::Zero();
						for (PxU32 ind = 0; ind < jointDatum.nbDof; ++ind)
						{
							const PxReal jVel = jVelocity[ind] * ratio;
							deltaV += jointDofMotionMatricesW[jointDatum.jointOffset + ind] * jVel;
							jVelocity[ind] = jVel;
						}

						//Add the effect of the joint velocities to the link.
						vel.top += deltaV.top;
						vel.bottom += deltaV.bottom;

						//Compute the coriolis vector.
						//mu(i) is the velocity arising from the joint motion: mu(i) = motionMatrix*qdot
						//where qdot is the joint velocity.
						//mu(i) is already computed and cached in deltaV.
						//For the revolute joint we have: 
						//deltaV = {mu(i), mu(i) X d(i)}
						//coriolis vector += {omega(i-1) X mu(i), 2*omega(i-1) X (mu(i) X d(i)) + mu(i) X (mu(i) X d(i))}
						//For the prismatic joint we have:
						//deltaV = {0, mu(i)}
						//coriolis vector += {0, 2*omega(i-1) X mu(i)}
						coriolisVector += Cm::SpatialVectorF(pVel.top.cross(deltaV.top), 2.0f*pVel.top.cross(deltaV.bottom) + deltaV.top.cross(deltaV.bottom));
					}
					//TODOGY - if jointVelocities is null we do not appear to set coriolisVectors[linkId] but we do set coriolisVectors[0]
					linkCoriolisVectorsW[linkID] = coriolisVector;
				}

				//PX_ASSERT(vel.top.isFinite() && PxAbs(vel.top.x) < 10000.f && PxAbs(vel.top.y) < 10000.f && PxAbs(vel.top.z) < 10000.f);
				//PX_ASSERT(vel.bottom.isFinite() && PxAbs(vel.bottom.x) < 10000.f && PxAbs(vel.bottom.y) < 10000.f && PxAbs(vel.bottom.z) < 10000.f);
				linkMotionVelocitiesW[linkID] = vel;
			}
			else
			{
				vel = rootLinkVel;
				//Note: we have already set motionVelocities[0] and coriolisVectors[0] so no need to set them again.
			}

			//Account for force arising from external accelerations
			//Account for damping force arising from the velocity and from the velocity that will accumulate from the acceleration terms.
			//Account for scaling force that will bring velocity back to the maximum allowed velocity if velocity exceed the maximum allowed.
			//Example for the linear term:
			//acceleration force = m*(g + extLinAccel)
			//linVelNew = linVel + (g + extLinAccel)*dt
			//damping force = -m*linVelNew*linDamp = -m*linVel*linDamp - m*(g + extLinAccel)*linDamp*dt
			//scaling force = -m*linVel*linScale/dt with linScale = (1.0f - maxLinVel/linVel)
			//Put it all together
			// m*(g + extLinAccel)*(1 - linDamp*dt) - m*linVel*(linDamp + linScale/dt)
			//Bit more reordering:
			//m*[g + extLinAccel)*(1 - linDamp*dt) - linVel*(linDamp + linScale/dt)]
			//Zero acceleration means we need to work against change:
			//-m*[g + extLinAccel)*(1 - linDamp*dt) - linVel*(linDamp + linScale/dt)]
			Cm::SpatialVectorF zExtForces;
			Cm::SpatialVectorF zDamping;
			{
				const PxVec3 g = bodyCore.disableGravity ? PxVec3(PxZero) : gravity;
				const PxVec3 extLinAccel = linkExternalAccelsW ? linkExternalAccelsW[linkID].linear : PxVec3(PxZero);
				const PxF32 lindamp = bodyCore.linearDamping > 0.f ?  PxMin(bodyCore.linearDamping, invDt) : 0.0f;
				const float m2 = vel.bottom.magnitudeSquared();
				const PxF32 linscale = (m2 > bodyCore.maxLinearVelocitySq) ? (1.0f - (PxSqrt(bodyCore.maxLinearVelocitySq)/PxSqrt(m2))) : 0.0f;
				zExtForces.top = (m * (lindamp * dt - 1.0f)) * (g + extLinAccel);
				zDamping.top = (m * (lindamp + linscale * invDt)) * vel.bottom;
			}
			{
				const PxVec3 extAngAccel = linkExternalAccelsW ? linkExternalAccelsW[linkID].angular : PxVec3(PxZero);
				const PxF32 angdamp = bodyCore.angularDamping > 0.f ? PxMin(bodyCore.angularDamping, invDt) : 0.0f;
				const float m2 = vel.top.magnitudeSquared();
				const PxF32 angscale = (m2 > bodyCore.maxAngularVelocitySq) ? (1.0f - (PxSqrt(bodyCore.maxAngularVelocitySq)/PxSqrt(m2))) : 0.0f;
				zExtForces.bottom = Iw * (extAngAccel * (angdamp * dt - 1.0f));
				zDamping.bottom = Iw * (vel.top * (angdamp + angscale * invDt));
			}

			if(externalForcesEveryTgsIterationEnabled)
			{
				// We precompute the isolated z vector for gravity and ext forces here so we don't need to load inertias in the per-substep application.
				// The ugly thing is that we start storing spatial forces into the spatial motion vector linkExternalAccelsW.
				// Reusing linkExternalAccelsW is ok because at the end of the sim step they are anyways cleared or refreshed from velMod in Sc::ArticulationSim::clearAcceleration
				// and inverse dynamics does not use it.

				linkZAExtForcesW[linkID] = zDamping; // only apply damping once for the full step
				linkExternalAccelsW[linkID].linear = zExtForces.top; // must use the same order (linear<=>top) again when picking this up in the substep application
				linkExternalAccelsW[linkID].angular = zExtForces.bottom;
			}
			else
			{
				linkZAExtForcesW[linkID] = zExtForces + zDamping;
			}

			//Account for forces arising from internal accelerations.
			//Note: Mirtich thesis introduces a single spatial zero acceleration force that contains an external [mass*gravity] term and the internal [omega X  (Iw *omega)] term. 
			//In Mirtich it looks like this:
			//	[-m_i * g					]
			//	[omega_i * (I_i * omega_i)	]
			//We split the spatial zero acceleration force into external (above) and internal (below).
			//The sum of the two (external and internal) corresponds to Z_i in the Mirtich formulation.
			// In this function, we're only computing the isolated zero acceleration Z_i but already store
			// it in the Z_i^A (articulated) fields as they will be accumulated later.
			const Cm::SpatialVectorF zInternal(PxVec3(0.f), vel.top.cross(Iw * vel.top));
			linkZAIntForcesW[linkID] = zInternal;
		}

		const PxReal invMass = 1.f / sumMass;
		comW = COM * invMass;
		invSumMass = invMass;
	}

	//compute all links velocities
	void FeatherstoneArticulation::computeLinkVelocities(ArticulationData& data, ScratchData& scratchData)
	{
		Cm::SpatialVectorF* PX_RESTRICT coriolisVectors = scratchData.coriolisVectors;
		const ArticulationLink* PX_RESTRICT links = data.getLinks();
		const PxU32 linkCount = data.getLinkCount();
		const bool fixBase = data.getArticulationFlags() & PxArticulationFlag::eFIX_BASE;

		//motion velocities has to be in world space to avoid numerical errors caused by space 
		Cm::SpatialVectorF* PX_RESTRICT motionVelocities = scratchData.motionVelocities;
		Cm::SpatialVectorF* PX_RESTRICT motionAccelerations = scratchData.motionAccelerations;

		PxReal* PX_RESTRICT jointVelocities = scratchData.jointVelocities;

		const ArticulationLink& baseLink = links[0];

		PxsBodyCore& core0 = *baseLink.bodyCore;

		if (fixBase)
		{
			motionVelocities[0] = Cm::SpatialVectorF(PxVec3(0.f), PxVec3(0.f));
			motionAccelerations[0] = Cm::SpatialVectorF(PxVec3(0.f), PxVec3(0.f));
		}
		else
		{
			motionVelocities[0] = Cm::SpatialVectorF(core0.angularVelocity, core0.linearVelocity);

			//PX_ASSERT(core0.angularVelocity.isFinite() && PxAbs(core0.angularVelocity.x) < 10000.f && PxAbs(core0.angularVelocity.y) < 10000.f && PxAbs(core0.angularVelocity.z) < 10000.f);
			//PX_ASSERT(core0.linearVelocity.isFinite() && PxAbs(core0.linearVelocity.x) < 10000.f && PxAbs(core0.linearVelocity.y) < 10000.f && PxAbs(core0.linearVelocity.z) < 10000.f);
		}

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

		//Is it really necessary? It is already resolved as an internal constraint.
		//const PxU32 dofCount = data.mDofs;
		PxReal ratio = 1.f;
		if (jointVelocities)
		{
			for (PxU32 linkID = 1; linkID < linkCount; ++linkID)
			{
				const ArticulationLink& link = links[linkID];
				ArticulationJointCoreData& jointDatum = data.getJointData(linkID);
				PxReal* jVelocity = &jointVelocities[jointDatum.jointOffset];
				for (PxU32 ind = 0; ind < jointDatum.nbDof; ++ind)
				{
					const PxReal maxJVelocity = link.inboundJoint->maxJointVelocity[ind];
					PxReal absJvel = PxAbs(jVelocity[ind]);
					ratio = ratio * absJvel > maxJVelocity ? (maxJVelocity / absJvel) : ratio;
				}
			}
		}

		const PxVec3* PX_RESTRICT rw = data.getRw();

		for (PxU32 linkID = 1; linkID < linkCount; ++linkID)
		{
			const ArticulationLink& link = links[linkID];
			const PxsBodyCore& bodyCore = *link.bodyCore;

			const Cm::SpatialVectorF pVel = motionVelocities[link.parent];
			Cm::SpatialVectorF vel = FeatherstoneArticulation::translateSpatialVector(-rw[linkID], pVel);
			const PxTransform& body2World = bodyCore.body2World;

			if (jointVelocities)
			{
				PxVec3 torque = pVel.top.cross(pVel.top.cross(rw[linkID]));
				ArticulationJointCoreData& jointDatum = data.getJointData(linkID);
				PxReal* jVelocity = &jointVelocities[jointDatum.jointOffset];
				
				PxVec3 force(0.f);
				if (jointDatum.nbDof)
				{
					const Cm::UnAlignedSpatialVector* PX_RESTRICT motionMatrix = data.getMotionMatrix();

					Cm::UnAlignedSpatialVector deltaV = Cm::UnAlignedSpatialVector::Zero();
					for (PxU32 ind = 0; ind < jointDatum.nbDof; ++ind)
					{
						const PxReal jVel = jVelocity[ind] * ratio;
						//deltaV += data.mWorldMotionMatrix[jointDatum.jointOffset + ind] * jVel;
						deltaV += motionMatrix[jointDatum.jointOffset+ind].rotate(body2World) * jVel;
						jVelocity[ind] = jVel;
					}

					vel.top += deltaV.top;
					vel.bottom += deltaV.bottom;

					const PxVec3 aVec = deltaV.top;
					force = pVel.top.cross(aVec);

					//compute linear part
					const PxVec3 lVel = deltaV.bottom;

					const PxVec3 temp1 = 2.f * pVel.top.cross(lVel);
					const PxVec3 temp2 = aVec.cross(lVel);
					torque += temp1 + temp2;

				}
				coriolisVectors[linkID] = Cm::SpatialVectorF(force, torque);
			}

			//PX_ASSERT(vel.top.isFinite() && PxAbs(vel.top.x) < 10000.f && PxAbs(vel.top.y) < 10000.f && PxAbs(vel.top.z) < 10000.f);
			//PX_ASSERT(vel.bottom.isFinite() && PxAbs(vel.bottom.x) < 10000.f && PxAbs(vel.bottom.y) < 10000.f && PxAbs(vel.bottom.z) < 10000.f);

			motionVelocities[linkID] = vel;
		}
	}

	void solveExtContact(const PxSolverConstraintDesc& desc, Vec3V& linVel0, Vec3V& linVel1, Vec3V& angVel0, Vec3V& angVel1,
		Vec3V& linImpulse0, Vec3V& linImpulse1, Vec3V& angImpulse0, Vec3V& angImpulse1, bool doFriction);

	void solveExt1D(const PxSolverConstraintDesc& desc, Vec3V& linVel0, Vec3V& linVel1, Vec3V& angVel0, Vec3V& angVel1,
		Vec3V& li0, Vec3V& li1, Vec3V& ai0, Vec3V& ai1);

	void solveExt1D(const PxSolverConstraintDesc& desc, Vec3V& linVel0, Vec3V& linVel1, Vec3V& angVel0, Vec3V& angVel1,
		const Vec3V& linMotion0, const Vec3V& linMotion1, const Vec3V& angMotion0, const Vec3V& angMotion1,
		const QuatV& rotA, const QuatV& rotB, const PxReal elapsedTimeF32, Vec3V& linImpulse0, Vec3V& linImpulse1, Vec3V& angImpulse0,
		Vec3V& angImpulse1);

	void solveExtContactStep(const PxSolverConstraintDesc& desc, Vec3V& linVel0, Vec3V& linVel1, Vec3V& angVel0, Vec3V& angVel1,
		Vec3V& linDelta0, Vec3V& linDelta1, Vec3V& angDelta0, Vec3V& angDelta1, Vec3V& linImpulse0, Vec3V& linImpulse1, Vec3V& angImpulse0, Vec3V& angImpulse1,
		bool doFriction, const PxReal minPenetration, const PxReal elapsedTimeF32);

	void solveStaticConstraint(const PxSolverConstraintDesc& desc, Cm::SpatialVectorF& linkV,
		Cm::SpatialVectorF& impulse, Cm::SpatialVectorF& deltaV, const Cm::SpatialVectorF& motion,
		const PxQuat& rot, bool isTGS, PxReal elapsedTime, const PxReal minPenetration)
	{
		Vec3V linVel = V3LoadA(linkV.bottom);
		Vec3V angVel = V3LoadA(linkV.top);

		Vec3V linVel0, linVel1, angVel0, angVel1;
		Vec3V li0 = V3Zero(), li1 = V3Zero(), ai0 = V3Zero(), ai1 = V3Zero();

		if (isTGS)
		{
			PxQuat idt(PxIdentity);
			Vec3V linMotion0, angMotion0, linMotion1, angMotion1;
			QuatV rotA, rotB;
			if (desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY)
			{
				linVel0 = linVel;
				angVel0 = angVel;
				linMotion0 = V3LoadA(motion.bottom);
				angMotion0 = V3LoadA(motion.top);
				rotA = QuatVLoadU(&rot.x);
				rotB = QuatVLoadU(&idt.x);
				linVel1 = angVel1 = linMotion1 = angMotion1 = V3Zero();
			}
			else
			{
				linVel1 = linVel;
				angVel1 = angVel;
				linMotion1 = V3LoadA(motion.bottom);
				angMotion1 = V3LoadA(motion.top);
				rotB = QuatVLoadU(&rot.x);
				rotA = QuatVLoadU(&idt.x);
				linVel0 = angVel0 = linMotion0 = angMotion0 = V3Zero();
			}

			if (*desc.constraint == DY_SC_TYPE_EXT_CONTACT)
			{
				Dy::solveExtContactStep(desc, 
					linVel0, linVel1, angVel0, angVel1, 
					linMotion0, linMotion1, angMotion0, angMotion1,
					li0, li1, ai0, ai1, 
					true, minPenetration, elapsedTime);
			}
			else
			{
				Dy::solveExt1D(desc, 
					linVel0, linVel1, angVel0, angVel1, 
					linMotion0, linMotion1, angMotion0, angMotion1, 
					rotA, rotB, elapsedTime,
					li0, li1, ai0, ai1);
			}
		}
		else
		{
			if (desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY)
			{
				linVel0 = linVel;
				angVel0 = angVel;
				linVel1 = angVel1 = V3Zero();
			}
			else
			{
				linVel1 = linVel;
				angVel1 = angVel;
				linVel0 = angVel0 = V3Zero();
			}

			if (*desc.constraint == DY_SC_TYPE_EXT_CONTACT)
			{
				Dy::solveExtContact(desc, linVel0, linVel1, angVel0, angVel1, li0, li1, ai0, ai1, true);
			}
			else
			{
				Dy::solveExt1D(desc, linVel0, linVel1, angVel0, angVel1, li0, li1, ai0, ai1);
			}
		}

		Cm::SpatialVectorF newVel, newImp;

		if (desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY)
		{
			V4StoreA(Vec4V_From_Vec3V(linVel0), &newVel.bottom.x);
			V4StoreA(Vec4V_From_Vec3V(angVel0), &newVel.top.x);

			V4StoreA(Vec4V_From_Vec3V(li0), &newImp.top.x);
			V4StoreA(Vec4V_From_Vec3V(ai0), &newImp.bottom.x);
		}
		else
		{
			V4StoreA(Vec4V_From_Vec3V(linVel1), &newVel.bottom.x);
			V4StoreA(Vec4V_From_Vec3V(angVel1), &newVel.top.x);

			V4StoreA(Vec4V_From_Vec3V(li1), &newImp.top.x);
			V4StoreA(Vec4V_From_Vec3V(ai1), &newImp.bottom.x);
		}

		deltaV.top += (newVel.top - linkV.top);
		deltaV.bottom += (newVel.bottom - linkV.bottom);
		linkV.top = newVel.top;
		linkV.bottom = newVel.bottom;
		impulse -= newImp;
	}

	// Forward declarations
	void writeBackContact(const PxSolverConstraintDesc& desc, SolverContext& cache,
			PxSolverBodyData& bd0, PxSolverBodyData& bd1);
	void writeBackContact(const PxSolverConstraintDesc& desc, SolverContext* cache);
	void writeBack1D(const PxSolverConstraintDesc& desc);
	void writeBack1DStep(const PxSolverConstraintDesc& desc);

	void FeatherstoneArticulation::writebackInternalConstraints(bool isTGS)
	{
		SolverContext context;
		PxSolverBodyData data;

		for (PxU32 i = 0; i < mStatic1DConstraints.size(); ++i)
		{
			PxSolverConstraintDesc& desc = mStatic1DConstraints[i];

			PX_ASSERT(*desc.constraint == DY_SC_TYPE_EXT_1D);

			if (isTGS)
			{
				writeBack1DStep(desc);
			}
			else
			{
				writeBack1D(desc);
			}
		}

		for (PxU32 i = 0; i < mStaticContactConstraints.size(); ++i)
		{
			PxSolverConstraintDesc& desc = mStaticContactConstraints[i];

			PX_ASSERT(*desc.constraint == DY_SC_TYPE_EXT_CONTACT);
			if (isTGS)
			{
				writeBackContact(static_cast<PxSolverConstraintDesc&>(desc), NULL);
			}
			else
			{
				writeBackContact(desc, context, data, data);
			}
		}
	}

	// Forward declarations
	void concludeContact(const PxSolverConstraintDesc& desc);
	void concludeContactStep(const PxSolverConstraintDesc& desc);
	void conclude1D(const PxSolverConstraintDesc& desc);
	void conclude1DStep(const PxSolverConstraintDesc& desc);

	void FeatherstoneArticulation::concludeInternalConstraints(bool isTGS)
	{
		for (PxU32 i = 0; i < mStatic1DConstraints.size(); ++i)
		{
			PxSolverConstraintDesc& desc = mStatic1DConstraints[i];

			PX_ASSERT(*desc.constraint == DY_SC_TYPE_EXT_1D);
			if (isTGS)
			{
				conclude1DStep(desc);
			}
			else
			{
				conclude1D(desc);
			}
		}

		for (PxU32 i = 0; i < mStaticContactConstraints.size(); ++i)
		{
			PxSolverConstraintDesc& desc = mStaticContactConstraints[i];
			PX_ASSERT(*desc.constraint == DY_SC_TYPE_EXT_CONTACT);

			if (isTGS)
			{
				concludeContactStep(desc);
			}
			else
			{
				concludeContact(desc);
			}
		}
	}

	struct InternalConstraintSolverData
	{
		const PxReal dt;
		const PxReal stepDt;
		const PxReal invStepDt;
		const PxReal elapsedTime;
		const PxReal erp;
		const bool isTGS;
		const bool isVelIter;
		const bool isExternalForceEveryStep;
		const bool doStaticContactAnd1dConstraint;
		const bool doFrictionDrivePosLimit;
		const ArticulationConstraintProcessingConfig::VelLimit::Enum doVelLimit;
 
		InternalConstraintSolverData(const PxReal dt_, const PxReal stepDt_, const PxReal invStepDt_, const PxReal elapsedTime_, const PxReal erp_,
									const bool isTGS_, const bool isVelIter_,
									const bool isExternalForcesEveryTgsIterationEnabled_,
									const bool doStaticContactAnd1dConstraint_,
									const bool doFrictionDrivePosLimit_,
									const ArticulationConstraintProcessingConfig::VelLimit::Enum doVelLimit_
									)
			: dt(dt_)
			, stepDt(stepDt_)
			, invStepDt(invStepDt_)
			, elapsedTime(elapsedTime_)
			, erp(erp_)
			, isTGS(isTGS_)
			, isVelIter(isVelIter_)
			, isExternalForceEveryStep(isExternalForcesEveryTgsIterationEnabled_)
			, doStaticContactAnd1dConstraint(doStaticContactAnd1dConstraint_)
			, doFrictionDrivePosLimit(doFrictionDrivePosLimit_)
			, doVelLimit(doVelLimit_)
		{
		}

		PX_NOCOPY(InternalConstraintSolverData)
	};

	static void accumulateLinkImpulsesAndLinkVelocities(
		const PxReal deltaF, const ArticulationInternalConstraint& constraint, 
		Cm::UnAlignedSpatialVector& i0, Cm::SpatialVectorF& i1, 
		Cm::SpatialVectorF& parentV, Cm::SpatialVectorF& childV, 
		Cm::SpatialVectorF& dv1)
	{
		i0 += constraint.row0 * deltaF;
		i1.top -= constraint.row1.top * deltaF;
		i1.bottom -= constraint.row1.bottom * deltaF;

		const Cm::UnAlignedSpatialVector deltaVP = constraint.deltaVA * (-deltaF);
		const Cm::UnAlignedSpatialVector deltaVC = constraint.deltaVB * (-deltaF);

		parentV += Cm::SpatialVectorF(deltaVP.top, deltaVP.bottom);
		childV += Cm::SpatialVectorF(deltaVC.top, deltaVC.bottom);

		dv1.top += deltaVC.top;
		dv1.bottom += deltaVC.bottom;
	}

	Cm::SpatialVectorF FeatherstoneArticulation::solveInternalJointConstraintRecursive
	(const InternalConstraintSolverData& PX_RESTRICT data, PxU32 linkID,
	 const Cm::SpatialVectorF& PX_RESTRICT parentDeltaV, PxU32& dofId, PxU32& limitId)
	{
		//PxU32 linkID = stack[stackSize];
		const ArticulationLink* links = mArticulationData.mLinks;
		const ArticulationLink& link = links[linkID];
	
		const PxU32 startDofId = dofId;

		const ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(linkID);

		PxReal* deferredQstZ = mArticulationData.getDeferredQstZ();

		const Cm::SpatialVectorF* PX_RESTRICT motionVelocities = mArticulationData.getMotionVelocities();

		//We know the absolute parentDeltaV from the call to this function so no need to modify it. 
		Cm::SpatialVectorF parentV = parentDeltaV + motionVelocities[link.parent];

		const InvStIs* PX_RESTRICT invStIS = mArticulationData.getInvStIS();
		const Cm::SpatialVectorF* PX_RESTRICT isW = mArticulationData.getIsW();
		const Cm::UnAlignedSpatialVector* PX_RESTRICT worldMotionMatrix = mArticulationData.getWorldMotionMatrix();
		const PxVec3* PX_RESTRICT rw = mArticulationData.getRw();

		Cm::SpatialVectorF parentVelContrib = propagateAccelerationW(
			rw[linkID], parentDeltaV, 
			invStIS[linkID], &worldMotionMatrix[jointDatum.jointOffset], 
			&isW[jointDatum.jointOffset], &deferredQstZ[jointDatum.jointOffset], jointDatum.nbDof,
			NULL);

		Cm::SpatialVectorF childV = motionVelocities[linkID] + parentVelContrib;

		Cm::UnAlignedSpatialVector i0(PxVec3(0.f), PxVec3(0.f));
		Cm::SpatialVectorF i1(PxVec3(0.f), PxVec3(0.f));

		Cm::SpatialVectorF dv1 = parentVelContrib;

		//Process internal constraints (parent/child limits/locks/drives)
		if(data.doFrictionDrivePosLimit || (ArticulationConstraintProcessingConfig::VelLimit::eBEFORE_STATIC_CONSTRAINTS == data.doVelLimit))
		{
			for (PxU32 dof = 0; dof < jointDatum.nbDof; ++dof)
			{
				if (mArticulationData.mInternalConstraints.size() <= (startDofId + dof))
					continue;

				ArticulationInternalConstraint& constraint = mArticulationData.mInternalConstraints[startDofId + dof];

				// This jointV is just used to compute velocity-dependent forces (friction, limits, ...). It is not stored to the articulation joint velocities.
				PxReal jointV = constraint.row1.innerProduct(childV) - constraint.row0.innerProduct(parentV);

				PxReal frictionDeltaF = 0.0f;
				PxReal driveDeltaF = 0.0f;
				PxReal posLimitDeltaF = 0.0f;
				if(data.doFrictionDrivePosLimit)
				{
					const PxReal jointPDelta = constraint.row1.innerProduct(mArticulationData.mDeltaMotionVector[linkID]) - constraint.row0.innerProduct(mArticulationData.mDeltaMotionVector[link.parent]);

					const bool newFrictionModel = constraint.staticFrictionEffort !=  0.0f || constraint.viscousFrictionCoefficient !=  0.0f;
					const bool isPerStep = (data.isTGS && data.isExternalForceEveryStep && !(data.isVelIter));
					const PxReal effectiveTimestep = isPerStep ? data.stepDt : data.dt;

					// deprecated friction model
					if (!newFrictionModel)
					{
						// Friction force is accumulated through all position iterations only for PGS
						const PxReal appliedFriction = data.isTGS ? 0.0f : constraint.accumulatedFrictionImpulse;
						const PxReal frictionForce = PxClamp(-jointV * constraint.recipResponse + appliedFriction,
															-constraint.deprecatedFrictionMaxForce, constraint.deprecatedFrictionMaxForce);
						constraint.accumulatedFrictionImpulse = frictionForce; // This is not used for TGS

						frictionDeltaF = frictionForce - appliedFriction;

						jointV += frictionDeltaF * constraint.response;
					}

					if (constraint.envelope.maxEffort > 0.0f)
					{
						const PxReal appliedDriveImpulse = isPerStep ? 0.0f : constraint.driveImpulse;
						const PxReal maxImpulse = constraint.envelope.maxEffort * effectiveTimestep;
						const PxReal speedImpulseGradient = constraint.envelope.speedEffortGradient / effectiveTimestep;
						const PxReal velocityDependentResistance = constraint.envelope.velocityDependentResistance * effectiveTimestep;
						const PxReal externalJointImpulse = constraint.externalJointForce * effectiveTimestep;

						const PxReal unclampedImpulse = (data.isTGS && data.isVelIter)
							? appliedDriveImpulse
							: computeDriveImpulse(appliedDriveImpulse, jointV, jointPDelta, data.elapsedTime, constraint.getImplicitDriveDesc());

						const PxReal clampedImpulse = clampDriveImpulse(
							jointV,
							appliedDriveImpulse + externalJointImpulse,
							unclampedImpulse + externalJointImpulse,
							constraint.response,
							constraint.envelope.maxActuatorVelocity,
							maxImpulse,
							speedImpulseGradient,
							velocityDependentResistance
						) - externalJointImpulse;

						driveDeltaF = clampedImpulse - appliedDriveImpulse;
						constraint.driveImpulse += driveDeltaF;
					}
					else
					{
						const PxReal unclampedImpulse = (data.isTGS && data.isVelIter) ? constraint.driveImpulse : 
							computeDriveImpulse(constraint.driveImpulse, jointV, jointPDelta, data.elapsedTime, constraint.getImplicitDriveDesc());
						const PxReal clampedImpulse = PxClamp(unclampedImpulse, -constraint.driveMaxImpulse, constraint.driveMaxImpulse);
						driveDeltaF = (clampedImpulse - constraint.driveImpulse);
						constraint.driveImpulse = clampedImpulse;
					}
					jointV += driveDeltaF * constraint.response;
		
					if (newFrictionModel)
					{
						const PxReal staticFrictionImpulse = constraint.staticFrictionEffort * effectiveTimestep;
						const PxReal dynamicFrictionImpulse = constraint.dynamicFrictionEffort * effectiveTimestep;
						const PxReal viscousFrictionCoefficient = constraint.viscousFrictionCoefficient * effectiveTimestep;
						const PxReal accumulatedFrictionImpulse = isPerStep ? 0.0f : constraint.accumulatedFrictionImpulse;

						const PxReal totalFrictionImpulse = computeFrictionImpulse(
							accumulatedFrictionImpulse - jointV * constraint.recipResponse,
							staticFrictionImpulse,
							dynamicFrictionImpulse,
							viscousFrictionCoefficient,
							jointV);

						frictionDeltaF = totalFrictionImpulse - accumulatedFrictionImpulse;
						constraint.accumulatedFrictionImpulse += frictionDeltaF; // to keep track of accumulated impulse for velIter in TGS with isExternalForceEveryStep
						jointV += frictionDeltaF * constraint.response;
					}

					//Where we will be next frame - we use this to compute error bias terms to correct limits and drives...

					//printf("LinkID %i driveDeltaV = %f, jointV = %f\n", linkID, driveDeltaF, jointV);

					if (jointDatum.dofLimitMask & (1 << dof))
					{
						ArticulationInternalLimit& limit = mArticulationData.mInternalLimits[limitId++];
						posLimitDeltaF = computeLimitImpulse(
							data.stepDt, data.invStepDt, data.isVelIter,
							constraint.response, constraint.recipResponse, data.erp,
							limit.errorLow, limit.errorHigh,
							jointPDelta, 
							limit.lowImpulse, limit.highImpulse, jointV);
					}
				} //doFrictionDrivePosLimit

				PxReal velLimitDeltaF = 0.0f;
				const PxReal maxJointVel = constraint.maxJointVelocity;
				if ((ArticulationConstraintProcessingConfig::VelLimit::eBEFORE_STATIC_CONSTRAINTS == data.doVelLimit) && (PxAbs(jointV) > maxJointVel))
				{
					PxReal newJointV = PxClamp(jointV, -maxJointVel, maxJointVel);
					velLimitDeltaF = (newJointV - jointV) * constraint.recipResponse;
					jointV = newJointV;
				}

				const PxReal deltaF = frictionDeltaF + driveDeltaF + posLimitDeltaF + velLimitDeltaF;

				if (deltaF != 0.f)
				{
					accumulateLinkImpulsesAndLinkVelocities(deltaF, constraint, i0, i1, parentV, childV, dv1);
				}
			}
		}

		//Cache the impulse arising from internal constraints.
		//We'll subtract this from the total impulse applied later in this function.
		Cm::SpatialVectorF i1Internal = i1;

		//Resolve static constraints and record the change in i1.
		Cm::SpatialVectorF i1FromStaticContactAnd1dConstraints(PxVec3(0.0f, 0.0f, 0.0f), PxVec3(0.0f, 0.0f, 0.0f));
		const PxU32 nbStatic1DConstraints = mArticulationData.mNbStatic1DConstraints[linkID];
		const PxU32 nbStaticContactConstraints = mArticulationData.mNbStaticContactConstraints[linkID];
		const bool processStaticContactAnd1dConstraints = data.doStaticContactAnd1dConstraint && ((nbStatic1DConstraints > 0) || (nbStaticContactConstraints > 0));
		if(processStaticContactAnd1dConstraints)
		{
			const Cm::SpatialVectorF i1BeforeStaticContactAnd1dConstraints = i1;

			const Cm::SpatialVectorF& deltaMotion = mArticulationData.getDeltaMotionVector(linkID);
			const PxQuat& deltaQ = getDeltaQ(linkID);

			PxU32 start1DIdx = mArticulationData.mStatic1DConstraintStartIndex[linkID];
			for (PxU32 i = 0; i < nbStatic1DConstraints; ++i)
			{
				PxSolverConstraintDesc& desc = mStatic1DConstraints[start1DIdx++];
				solveStaticConstraint(
					desc, childV, i1, dv1, deltaMotion, deltaQ, data.isTGS, data.elapsedTime, data.isVelIter ? 0.f : -PX_MAX_F32);
			}

			PxU32 startContactIdx = mArticulationData.mStaticContactConstraintStartIndex[linkID];
			for (PxU32 i = 0; i < nbStaticContactConstraints; ++i)
			{
				PxSolverConstraintDesc& desc = mStaticContactConstraints[startContactIdx++];
				solveStaticConstraint(
					desc, childV, i1, dv1, deltaMotion, deltaQ, data.isTGS, data.elapsedTime, data.isVelIter ? 0.f : -PX_MAX_F32);
			}

			i1FromStaticContactAnd1dConstraints = i1 - i1BeforeStaticContactAnd1dConstraints;
		}

		//Process vel limit of each dof on their own.
		if(ArticulationConstraintProcessingConfig::VelLimit::eAFTER_STATIC_CONSTRAINTS == data.doVelLimit)
		{
			const Cm::SpatialVectorF i1BeforeVelLimit = i1;

			//Work out deltaVParent that is a consequence of applying static contact/constraint impulses to the child link.
			Cm::SpatialVectorF deltaVParent(PxVec3(0.0f, 0.0f, 0.0f), PxVec3(0.0f, 0.0f, 0.0f));
			if(processStaticContactAnd1dConstraints)
			{
				//We know the impulse applied by static contact/constraint to the child link.
				//We also know the current velocity of the child link after static contact/constraint.
				//jointV is no longer up to date because we have not accounted for static contact/constraint impulses
				//by propagating them to the parent link.
				//We need to know the velocity of the parent link so that we may recompute the dof speeds and apply 
				//the dof speed limits.
				//To compute the parent link velocity we need to propagate the static contact/constraint impulse applied to the 
				//child inwards to the parent and then compute the deltaVel arising at the parent. We can add that deltaV to the 
				//last known parent link velocity computed immediately before static contact/constraint.
				const PxVec3& r = rw[linkID];
				const Cm::SpatialVectorF* PX_RESTRICT iSInvStIS = mArticulationData.getISInvStIS();
				const Cm::SpatialVectorF* jointDofISInvStISW = &iSInvStIS[jointDatum.jointOffset];
				const Cm::UnAlignedSpatialVector* jointDofMotionMatrixW = &worldMotionMatrix[jointDatum.jointOffset];
				const PxU8 nbDofs = jointDatum.nbDof;
				const Cm::SpatialVectorF propagatedImpulseAtParentW = propagateImpulseW(
					r,
					i1FromStaticContactAnd1dConstraints, NULL, 
					jointDofISInvStISW, jointDofMotionMatrixW, nbDofs, NULL);
				deltaVParent = -getImpulseResponseW(link.parent, mArticulationData, propagatedImpulseAtParentW);
			}

			for (PxU32 dof = 0; dof < jointDatum.nbDof; ++dof)
			{
				if (mArticulationData.mInternalConstraints.size() <= (startDofId + dof))
					continue;

				const ArticulationInternalConstraint& constraint = mArticulationData.mInternalConstraints[startDofId + dof];

				//Note that we need to account for the deltaVParent arising from the static contact/constraint impulse applied
				//to the child.
				PxReal jointV = constraint.row1.innerProduct(childV) - constraint.row0.innerProduct(parentV + deltaVParent);

				PxReal velLimitDeltaF = 0.0f;
				const PxReal maxJointVel =constraint.maxJointVelocity;
				if (PxAbs(jointV) > maxJointVel)
				{
					PxReal newJointV = PxClamp(jointV, -maxJointVel, maxJointVel);
					velLimitDeltaF = (newJointV - jointV) * constraint.recipResponse;
					jointV = newJointV;
				}

				if (velLimitDeltaF != 0.f)
				{
					accumulateLinkImpulsesAndLinkVelocities(velLimitDeltaF, constraint, i0, i1, parentV, childV, dv1);
				}
			}

			i1Internal += (i1 - i1BeforeVelLimit);
		}

		dofId = startDofId + jointDatum.nbDof;

		PxU32 numChildren = link.mNumChildren;
		PxU32 offset = link.mChildrenStartIndex;

		const TestImpulseResponse* responseMatrix = mArticulationData.getImpulseResponseMatrixWorld();

		for(PxU32 i = 0; i < numChildren; ++i)
		{
			const PxU32 child = offset+i;

			Cm::SpatialVectorF childImp = solveInternalJointConstraintRecursive(data, child, dv1, dofId, limitId);
			i1 += childImp;

			if ((numChildren-i) > 1)
			{
				//Propagate the childImp to my dv1 so that the next constraint gets to see an updated velocity state based
				//on the propagation of the child velocities
				Cm::SpatialVectorF deltaV = responseMatrix[linkID].getLinkDeltaVImpulseResponse(-childImp);
				dv1 += deltaV;
				childV += deltaV;	// PT: is that necessary?
			}
		} 
			
		Cm::SpatialVectorF propagatedImpulseAtParentW;
		{
			//const inputs
			const PxVec3& r = rw[linkID];
			const Cm::SpatialVectorF* PX_RESTRICT iSInvStIS = mArticulationData.getISInvStIS();
			const Cm::SpatialVectorF* jointDofISInvStISW = &iSInvStIS[jointDatum.jointOffset];
			const Cm::UnAlignedSpatialVector* jointDofMotionMatrixW = &worldMotionMatrix[jointDatum.jointOffset];
			const PxU8 nbDofs = jointDatum.nbDof;

			//output
			PxReal* jointDofDeferredQstZ = &deferredQstZ[jointDatum.jointOffset];
			propagatedImpulseAtParentW = propagateImpulseW(
				r,
				i1, 
				NULL, jointDofISInvStISW, jointDofMotionMatrixW, nbDofs,
				jointDofDeferredQstZ);
		}

		Cm::SpatialVectorF* PX_RESTRICT solverLinkSpatialImpulses = mArticulationData.getSolverLinkSpatialImpulses();

		//Accumulate the propagated impulse at the link.
		//Don't forget to subtract the impulse arising from internal constraints.
		//This can be used to compute the link's incoming joint force.
		solverLinkSpatialImpulses[linkID] += (i1 - i1Internal);

		return Cm::SpatialVectorF(i0.top, i0.bottom) + propagatedImpulseAtParentW;
	}

	void FeatherstoneArticulation::solveInternalJointConstraints(const PxReal dt, const PxReal stepDt, const PxReal invStepDt, 
																 bool isVelIter, bool isTGS,
																 bool doFrictionDrivePosLimit, 
																 ArticulationConstraintProcessingConfig::VelLimit::Enum doVelLimit, 
																 bool doStaticContactAnd1dConstraint,
																 const PxReal elapsedTime, const PxReal biasCoefficient,
																 bool isExternalForcesEveryTgsIterationEnabled)
	{
		//const PxU32 count = mArticulationData.getLinkCount();

		const PxReal erp = biasCoefficient;

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

		PxU32* static1DConstraintCounts = mArticulationData.mNbStatic1DConstraints;
		PxU32* static1DConstraintStarts = mArticulationData.mStatic1DConstraintStartIndex;

		PxU32* staticContactConstraintCounts = mArticulationData.mNbStaticContactConstraints;
		PxU32* staticContactConstraintStarts = mArticulationData.mStaticContactConstraintStartIndex;

		ArticulationLink* links = mArticulationData.getLinks();
		const Cm::SpatialVectorF* baseVelocities = mArticulationData.getMotionVelocities();
		//PxTransform* transforms = mArticulationData.mPreTransform.begin();		

		//Cm::SpatialVectorF* deferredZ = mArticulationData.getSpatialZAVectors();

		const PxReal minPenetration = isVelIter ? 0.f : -PX_MAX_F32;

		Cm::SpatialVectorF rootLinkV;
		{
			Cm::SpatialVectorF rootLinkDeltaV(PxVec3(0.f), PxVec3(0.f));

			if (!fixBase)
			{
				//Cm::SpatialVectorF temp =  mArticulationData.getBaseInvSpatialArticulatedInertia() * (-deferredZ[0]);
				//const PxTransform& body2World0 = transforms[0];
				//rootLinkDeltaV = temp.rotate(body2World0);
				//temp is now in world space!
				rootLinkDeltaV = mArticulationData.getBaseInvSpatialArticulatedInertiaW() * -mArticulationData.mRootDeferredZ;
			}

			rootLinkV = rootLinkDeltaV + baseVelocities[0];

			Cm::SpatialVectorF im0 = Cm::SpatialVectorF::Zero();
			{
				const PxU32 nbStatic1DConstraints = static1DConstraintCounts[0];

				if (doStaticContactAnd1dConstraint && nbStatic1DConstraints)
				{
					const Cm::SpatialVectorF& deltaMotion = mArticulationData.getDeltaMotionVector(0);
					const PxQuat& deltaQ = getDeltaQ(0);

					PxU32 startIdx = static1DConstraintStarts[0];
					for (PxU32 i = 0; i < nbStatic1DConstraints; ++i)
					{
						PxSolverConstraintDesc& desc = mStatic1DConstraints[startIdx++];

						solveStaticConstraint(desc, rootLinkV, im0, rootLinkDeltaV, deltaMotion, deltaQ, isTGS, elapsedTime, minPenetration);
					}

					//Impulses and deferredZ are now in world space, not link space!
					/*im0.top = transforms[0].rotateInv(im0.top);
					im0.bottom = transforms[0].rotateInv(im0.bottom);*/
				}

				const PxU32 nbStaticContactConstraints = staticContactConstraintCounts[0];

				if (doStaticContactAnd1dConstraint && nbStaticContactConstraints)
				{
					const Cm::SpatialVectorF& deltaMotion = mArticulationData.getDeltaMotionVector(0);
					const PxQuat& deltaQ = getDeltaQ(0);

					PxU32 startIdx = staticContactConstraintStarts[0];
					for (PxU32 i = 0; i < nbStaticContactConstraints; ++i)
					{
						PxSolverConstraintDesc& desc = mStaticContactConstraints[startIdx++];

						solveStaticConstraint(desc, rootLinkV, im0, rootLinkDeltaV, deltaMotion, deltaQ, isTGS, elapsedTime, minPenetration);
					}

					//Impulses and deferredZ are now in world space, not link space!
					/*im0.top = transforms[0].rotateInv(im0.top);
					im0.bottom = transforms[0].rotateInv(im0.bottom);*/
				}
			}
			
			//Store the constant that will be used by every dof and limit encountered.
			const InternalConstraintSolverData data(dt, stepDt, invStepDt, elapsedTime, erp, isTGS, isVelIter, isExternalForcesEveryTgsIterationEnabled,
													doStaticContactAnd1dConstraint, doFrictionDrivePosLimit, doVelLimit);

			//Increment dofId and limitId as each is encountered to make sure we stay in sync with the ordering of 
			//ArticulationData::mInternalConstraints and ArticulationData::mInternalLimits.
			PxU32 dofId = 0;
			PxU32 limitId = 0;

			const PxU32 numChildren = links[0].mNumChildren;
			const PxU32 offset = links[0].mChildrenStartIndex;

			for (PxU32 i = 0; i < numChildren; ++i)
			{
				const PxU32 child = offset + i;

				Cm::SpatialVectorF imp = solveInternalJointConstraintRecursive(data, child, rootLinkDeltaV, dofId, limitId);

				im0 += imp;

				//There's an impulse, we have to work out how it impacts our velocity (only if required (we have more children to traverse))!
				if (!fixBase && (numChildren - 1) != 0)
				{
					//Impulses and deltaVs are all now in world space
					rootLinkDeltaV += mArticulationData.getBaseInvSpatialArticulatedInertiaW() * (-imp);
				}
			}

			mArticulationData.mRootDeferredZ += im0;
			mArticulationData.mJointDirty = true;
		}
	}

	void FeatherstoneArticulation::solveInternalSpatialTendonConstraints(bool isTGS)
	{
		if (mArticulationData.mInternalSpatialTendonConstraints.size() == 0)
			return;

		if (isTGS)
		{
			//Update the error terms in the tendons recursively...
			const PxU32 nbTendons = mArticulationData.mNumSpatialTendons;

			for (PxU32 i = 0; i < nbTendons; ++i)
			{
				Dy::ArticulationSpatialTendon* tendon = mArticulationData.mSpatialTendons[i];

				Dy::ArticulationAttachment& attachment = tendon->getAttachment(0);

				//const PxU32 childCount = attachment.childCount;

				//PxReal scale = 1.f / PxReal(childCount);

				PxReal coefficient = attachment.coefficient;

				const PxU32 startLink = tendon->getAttachment(0).linkInd;
				const PxTransform pBody2World = mArticulationData.getAccumulatedPoses()[startLink];
				const PxVec3 pAttachPoint = pBody2World.transform(tendon->getAttachment(0).relativeOffset);

				Dy::ArticulationAttachment* attachments = tendon->getAttachments();
				for (ArticulationAttachmentBitField children = attachments[0].children; children != 0; children &= (children - 1))
				{
					//index of child of link h on path to link linkID
					const PxU32 child = PxLowestSetBit(children);

					updateSpatialTendonConstraintsRecursive(attachments, child, tendon->mOffset*coefficient, pAttachPoint, mArticulationData);
				}
			}
		}

		for (PxU32 i = 0; i < mArticulationData.mInternalSpatialTendonConstraints.size(); ++i)
		{
			ArticulationInternalTendonConstraint& constraint = mArticulationData.mInternalSpatialTendonConstraints[i];

			const PxU32 parentID = constraint.linkID0;
			const PxU32 linkID = constraint.linkID1;

			//Cm::SpatialVectorF childDeltaP = mArticulationData.mDeltaMotionVector[linkID];
			//Cm::SpatialVectorF parentDeltaP = mArticulationData.mDeltaMotionVector[parentID];

			//PxReal deltaP = constraint.row1.innerProduct(childDeltaP) - constraint.row0.innerProduct(parentDeltaP);// + deltaErr;

			Cm::SpatialVectorV childVel = pxcFsGetVelocity(linkID);
			Cm::SpatialVectorV parentVel = pxcFsGetVelocity(parentID);

			Cm::UnAlignedSpatialVector childV;
			V3StoreU(childVel.angular, childV.top);
			V3StoreU(childVel.linear, childV.bottom);

			Cm::UnAlignedSpatialVector parentV;
			V3StoreU(parentVel.angular, parentV.top);
			V3StoreU(parentVel.linear, parentV.bottom);

			PxReal error = constraint.restDistance - constraint.accumulatedLength;// + deltaP;

			PxReal limitError = 0.f;
			if (constraint.accumulatedLength > constraint.highLimit)
				limitError = constraint.highLimit - constraint.accumulatedLength;
			if (constraint.accumulatedLength < constraint.lowLimit)
				limitError = constraint.lowLimit - constraint.accumulatedLength;

			PxReal jointV = constraint.row1.innerProduct(childV) - constraint.row0.innerProduct(parentV);

			PX_ASSERT(PxIsFinite(jointV));

			PxReal unclampedForce = 0.0f;
			PxReal unclampedLimitForce = 0.0f;
			PxReal deltaF = 0.0f;
			computeTendonImpulse(
				1.0f, 
				constraint.getTendonImplicitSpringParams(),
				error, jointV, limitError, 
				constraint.appliedForce, constraint.limitAppliedForce,
				unclampedForce, unclampedLimitForce, deltaF);
				
			constraint.appliedForce = unclampedForce;
			constraint.limitAppliedForce = unclampedLimitForce;

			//Accumulate error even if it is zero because the increment of the counter affects the RMS value  
			//Ignore tendons for now
			//(isVelIter ? mInternalErrorAccumulatorVelIter : mInternalErrorAccumulatorPosIter).accumulateErrorLocal(deltaF, constraint.recipResponse);

			if (deltaF != 0.f)
			{
				Cm::UnAlignedSpatialVector i0 = constraint.row0 * -deltaF;
				Cm::UnAlignedSpatialVector i1 = constraint.row1 * deltaF;
				pxcFsApplyImpulses(
					parentID, V3LoadU(i0.top), V3LoadU(i0.bottom), NULL,
					linkID, V3LoadU(i1.top), V3LoadU(i1.bottom), NULL);
			}
		}
	}

	PxVec3 FeatherstoneArticulation::calculateFixedTendonVelocityAndPositionRecursive(
		const ArticulationTendonJoint* tendonJoints, const PxU32 tendonJointID,
		const Cm::SpatialVectorF& parentV, const Cm::SpatialVectorF& parentDeltaV)
	{
		const ArticulationTendonJoint& tendonJoint = tendonJoints[tendonJointID];

		const ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(tendonJoint.linkInd);

		const PxReal* PX_RESTRICT deferredQstZ = mArticulationData.getDeferredQstZ();
		const InvStIs* PX_RESTRICT invStIS = mArticulationData.getInvStIS();
		const Cm::SpatialVectorF* PX_RESTRICT isW = mArticulationData.getIsW();
		const Cm::UnAlignedSpatialVector* PX_RESTRICT worldMotionMatrix = mArticulationData.getWorldMotionMatrix();
		const PxVec3* PX_RESTRICT rw = mArticulationData.getRw();

		Cm::SpatialVectorF deltaV = propagateAccelerationW(
			rw[tendonJoint.linkInd], parentDeltaV, 
			invStIS[tendonJoint.linkInd],
			&worldMotionMatrix[jointDatum.jointOffset], 
			&isW[jointDatum.jointOffset], &deferredQstZ[jointDatum.jointOffset], jointDatum.nbDof,
			NULL);

		const Cm::SpatialVectorF* PX_RESTRICT motionVelocities = mArticulationData.getMotionVelocities();
		Cm::SpatialVectorF childV = motionVelocities[tendonJoint.linkInd] + deltaV;

		const PxU32 index = tendonJoint.mConstraintInd;
		ArticulationInternalTendonConstraint& constraint = mArticulationData.mInternalFixedTendonConstraints[index];

		const PxU32 childCount = tendonJoint.childCount;

		PxVec3 jointVError;

		const PxReal jointV = constraint.row1.innerProduct(childV) - constraint.row0.innerProduct(parentV);
		const PxReal jointP = mArticulationData.getJointPositions()[tendonJoint.startJointOffset];

		jointVError.x = jointV * tendonJoint.coefficient;

		jointVError.y = jointP * tendonJoint.coefficient;

		//printf("%i: jointPose = %f, jointV = %f, coefficient = %f\n", tendonJoint.linkInd, jointP, jointV, tendonJoint.coefficient);

		jointVError.z = 1.f;

		if (childCount)
		{
			for (ArticulationBitField children = tendonJoint.children; children != 0; children &= (children - 1))
			{
				//index of child of link h on path to link linkID
				const PxU32 child = PxLowestSetBit(children);
				jointVError += calculateFixedTendonVelocityAndPositionRecursive(tendonJoints, child, childV, deltaV);
			}
		}

		return jointVError;
	}

	Cm::SpatialVectorF FeatherstoneArticulation::solveFixedTendonConstraintsRecursive
	(const ArticulationTendonJoint* tendonJoints,const PxU32 tendonJointID,
	 const PxReal lengthError, const PxReal limitError, const PxReal jointV,
	 PxReal& rootImp)
	{
		const ArticulationTendonJoint& tendonJoint = tendonJoints[tendonJointID];

		const ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(tendonJoint.linkInd);
			   
		PxU32 index = tendonJoint.mConstraintInd;
		ArticulationInternalTendonConstraint& constraint = mArticulationData.mInternalFixedTendonConstraints[index];

		const PxU32 childCount = tendonJoint.childCount;

		// the constraint bias coefficients need to flip signs together with the tendon joint's coefficient
		// in order for the constraint force to point into the correct direction:
		PxReal unclampedForce = 0.0f;
		PxReal unclampedForceLimit = 0.0f;
		PxReal deltaF = 0.0f;
		computeTendonImpulse(
			tendonJoint.recipCoefficient,
			constraint.getTendonImplicitSpringParams(),
			lengthError, jointV, limitError,
			constraint.appliedForce, constraint.limitAppliedForce,
			unclampedForce, unclampedForceLimit, deltaF);

		//Ignore tendons for now
		//(isVelIter ? mInternalErrorAccumulatorVelIter : mInternalErrorAccumulatorPosIter).accumulateErrorLocal(deltaF, constraint.recipResponse);

		constraint.appliedForce = unclampedForce;
		constraint.limitAppliedForce = unclampedForceLimit;

		rootImp += deltaF;
	
		Cm::SpatialVectorF impulse(constraint.row1.top * -deltaF, constraint.row1.bottom * -deltaF);
		const Cm::SpatialVectorF YInt = impulse;
		if (childCount)
		{
			for (ArticulationBitField children = tendonJoint.children; children != 0; children &= (children - 1))
			{
				//index of child of link h on path to link linkID
				const PxU32 child = PxLowestSetBit(children);

				Cm::SpatialVectorF propagatedImpulse = solveFixedTendonConstraintsRecursive(tendonJoints, child, lengthError, limitError, jointV, rootImp);

				impulse.top += propagatedImpulse.top;
				impulse.bottom += propagatedImpulse.bottom;
			}
		}

		Cm::SpatialVectorF* PX_RESTRICT solverLinkSpatialImpulses = mArticulationData.getSolverLinkSpatialImpulses();
		solverLinkSpatialImpulses[tendonJoint.linkInd] += impulse - YInt;

		PxReal* deferredQstZ = mArticulationData.getDeferredQstZ();
		const Cm::UnAlignedSpatialVector* worldMotionMatrix = mArticulationData.getWorldMotionMatrix();
		const Cm::SpatialVectorF* iSInvStIS = mArticulationData.getISInvStIS();
		const PxVec3* rw = mArticulationData.getRw();

		return propagateImpulseW(
			rw[tendonJoint.linkInd], 
			impulse, 
			NULL, &iSInvStIS[jointDatum.jointOffset], &worldMotionMatrix[jointDatum.jointOffset], jointDatum.nbDof, 
			&deferredQstZ[jointDatum.jointOffset]);
	}

	void FeatherstoneArticulation::solveInternalFixedTendonConstraints(bool isTGS)
	{
		PX_UNUSED(isTGS);
		if (mArticulationData.mInternalFixedTendonConstraints.size() == 0)
			return;

		{
			//Update the error terms in the tendons recursively...
			const PxU32 nbTendons = mArticulationData.mNumFixedTendons;

			ArticulationLink* links = mArticulationData.getLinks();
			const Cm::SpatialVectorF* motionVelocities = mArticulationData.getMotionVelocities();

			for (PxU32 i = 0; i < nbTendons; ++i)
			{
				Dy::ArticulationFixedTendon* tendon = mArticulationData.mFixedTendons[i];

				ArticulationTendonJoint* tendonJoints = tendon->getTendonJoints();

				Dy::ArticulationTendonJoint& pTendonJoint = tendonJoints[0];

				//const PxU32 childCount = pTendonJoint.childCount;

				const PxU32 startLink = pTendonJoint.linkInd;

				Cm::SpatialVectorV parentVel = pxcFsGetVelocity(startLink);
				Cm::SpatialVectorF parentV;
				V3StoreU(parentVel.angular, parentV.top);
				V3StoreU(parentVel.linear, parentV.bottom);

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

				Cm::SpatialVectorF parentDeltaV = parentV - motionVelocities[startLink];

				PxReal lengthError = 0.0f;
				PxReal limitError = 0.0f;
				PxReal rootVel = 0.0f;
				{
					//sum.x  = sum_i{c_i * jointSpeed_i}
					//sum.y  = sum_i{c_i * jointPos_i}
					//sum.x = nbArticulationJointsInTendon
					PxVec3 sum(0.f);						   
					for (ArticulationAttachmentBitField children = pTendonJoint.children; children != 0; children &= (children - 1))
					{
						//index of child of link h on path to link linkID
						const PxU32 child = PxLowestSetBit(children);
						sum += calculateFixedTendonVelocityAndPositionRecursive(tendonJoints, child, parentV, parentDeltaV);
					}

					const PxReal recipScale = sum.z == 0.f ? 0.f : 1.f / sum.z;
					const PxReal length = sum.y + tendon->mOffset;
					lengthError = (length - tendon->mRestLength)*recipScale;		
					rootVel = sum.x*recipScale;
					limitError = (length < tendon->mLowLimit) ? ((length - tendon->mLowLimit)*recipScale) : ((length > tendon->mHighLimit) ? (length - tendon->mHighLimit)*recipScale : 0.0f);
				}
				

				for (ArticulationAttachmentBitField children = pTendonJoint.children; children != 0; children &= (children - 1))
				{
					//index of child of link h on path to link linkID
					const PxU32 child = PxLowestSetBit(children);
					const ArticulationTendonJoint& tendonJoint = tendonJoints[child];

					const ArticulationInternalTendonConstraint& constraint = mArticulationData.mInternalFixedTendonConstraints[tendonJoint.mConstraintInd];

					//KS - TODO - hook up offsets
					PxReal rootImp = 0.0f;
					Cm::SpatialVectorF propagatedImpulse = solveFixedTendonConstraintsRecursive(tendonJoints, child, lengthError, limitError, rootVel, rootImp);

					propagatedImpulse.top += constraint.row0.top * rootImp;
					propagatedImpulse.bottom += constraint.row0.bottom * rootImp;

					Z += propagatedImpulse;
				}
				
				PxReal* deferredQstZ = mArticulationData.getDeferredQstZ();
				const Cm::UnAlignedSpatialVector* worldMotionMatrix = mArticulationData.getWorldMotionMatrix();
				const Cm::SpatialVectorF* iSInvStIS = mArticulationData.getISInvStIS();
				const PxVec3* rw = mArticulationData.getRw();

				for (PxU32 linkID = pTendonJoint.linkInd; linkID; linkID = links[linkID].parent)
				{
					const PxU32 jointOffset = mArticulationData.getJointData(linkID).jointOffset;
					const PxU8 dofCount = mArticulationData.getJointData(linkID).nbDof;

					Z = propagateImpulseW(
							rw[linkID],
							Z, 
							NULL, &iSInvStIS[jointOffset], &worldMotionMatrix[jointOffset], dofCount, 
							&deferredQstZ[jointOffset]);
				}

				mArticulationData.mRootDeferredZ += Z;
				mArticulationData.mJointDirty = true;
			}
		}
	}

	void FeatherstoneArticulation::solveInternalConstraints
	(const PxReal dt, const PxReal stepDt, const PxReal invStepDt, 
	 bool velocityIteration, bool isTGS,
	 const ArticulationConstraintProcessingConfigCPU& articulationConstraintProcessingConfig,
	 const PxReal elapsedTime, const PxReal biasCoefficient,
	 bool isExternalForcesEveryTgsIterationEnabled)
	{
		if(articulationConstraintProcessingConfig.mDoSpatialTendonsFixedTendonsMimicJoints)
		{
			solveInternalSpatialTendonConstraints(isTGS);
			solveInternalFixedTendonConstraints(isTGS);
			solveInternalMimicJointConstraints(stepDt, invStepDt, velocityIteration, isTGS, biasCoefficient);
		}
		solveInternalJointConstraints(
				dt, stepDt, invStepDt, 
				velocityIteration, isTGS, 
				articulationConstraintProcessingConfig.mDoFrictionDrivePosLimit, 
				articulationConstraintProcessingConfig.mDoVelLimit, 
				articulationConstraintProcessingConfig.mDoStaticContactsAnd1dConstraints,
				elapsedTime, 
				biasCoefficient, 
				isExternalForcesEveryTgsIterationEnabled);
	}

	bool FeatherstoneArticulation::storeStaticConstraint(const PxSolverConstraintDesc& desc)
	{
		if (DY_STATIC_CONTACTS_IN_INTERNAL_SOLVER)
		{
			if (desc.constraintType == DY_SC_TYPE_RB_CONTACT)
				mStaticContactConstraints.pushBack(desc);
			else
				mStatic1DConstraints.pushBack(desc);
		}
		return DY_STATIC_CONTACTS_IN_INTERNAL_SOLVER;
	}

	void FeatherstoneArticulation::setRootLinearVelocity(const PxVec3& velocity)
	{
		const ArticulationLink& rLink = mArticulationData.getLink(0);
		rLink.bodyCore->linearVelocity = velocity;
		mGPUDirtyFlags |= ArticulationDirtyFlag::eDIRTY_ROOT_VELOCITIES;
		computeLinkVelocities(mArticulationData);
	}

	void FeatherstoneArticulation::setRootAngularVelocity(const PxVec3& velocity)
	{
		const ArticulationLink& rLink = mArticulationData.getLink(0);
		rLink.bodyCore->angularVelocity = velocity;
		mGPUDirtyFlags |= ArticulationDirtyFlag::eDIRTY_ROOT_VELOCITIES;
		computeLinkVelocities(mArticulationData);
	}

	//This method is for user update the root link transform so we need to
	//fix up other link's position. In this case, we should assume all joint
	//velocity/pose is to be zero
	void FeatherstoneArticulation::teleportRootLink()
	{
		//make sure motionMatrix has been set
		//jcalc(mArticulationData);

		const PxU32 linkCount = mArticulationData.getLinkCount();
		ArticulationLink* PX_RESTRICT links = mArticulationData.getLinks();
		PxReal* PX_RESTRICT jointPositions = mArticulationData.getJointPositions();
		Cm::SpatialVectorF* PX_RESTRICT motionVelocities = mArticulationData.getMotionVelocities();
		const Cm::UnAlignedSpatialVector* PX_RESTRICT motionMatrix = mArticulationData.getMotionMatrix();
		const PxQuat* PX_RESTRICT relativeQuats = mArticulationData.getRelativeQuat();

		for (PxU32 linkID = 1; linkID < linkCount; ++linkID)
		{
			const ArticulationLink& link = links[linkID];
			const PxTransform oldTransform = link.bodyCore->body2World;

			const ArticulationLink& pLink = links[link.parent];
			const PxTransform pBody2World = pLink.bodyCore->body2World;

			ArticulationJointCore* joint = link.inboundJoint;
			ArticulationJointCoreData& jointDatum = mArticulationData.getJointData(linkID);

			PxReal* jPosition = &jointPositions[jointDatum.jointOffset];

			PxQuat newParentToChild;
			PxQuat newWorldQ;
			PxVec3 r;

			const PxVec3 childOffset = -joint->childPose.p;
			const PxVec3 parentOffset = joint->parentPose.p;

			const PxQuat relativeQuat = relativeQuats[linkID];

			switch (joint->jointType)
			{
			case PxArticulationJointType::ePRISMATIC:
			{
				newParentToChild = relativeQuat;
				const PxVec3 e = newParentToChild.rotate(parentOffset);
				const PxVec3 d = childOffset;

				const PxVec3& u = motionMatrix[jointDatum.jointOffset].bottom;

				r = e + d + u * jPosition[0];
				break;
			}
			case PxArticulationJointType::eREVOLUTE:
			case PxArticulationJointType::eREVOLUTE_UNWRAPPED:
			{
				const PxVec3& u = motionMatrix[jointDatum.jointOffset].top;

				PxQuat jointRotation = PxQuat(-jPosition[0], u);
				if (jointRotation.w < 0)	//shortest angle.
					jointRotation = -jointRotation;

				newParentToChild = (jointRotation * relativeQuat).getNormalized();

				const PxVec3 e = newParentToChild.rotate(parentOffset);
				const PxVec3 d = childOffset;
				r = e + d;

				/*PxVec3 worldAngVel = oldTransform.rotate(link.motionVelocity.top);

				newWorldQ = PxExp(worldAngVel*dt) * oldTransform.q;

				PxQuat newParentToChild2 = (newWorldQ.getConjugate() * joint->relativeQuat * pBody2World.q).getNormalized();

				const PxVec3 e2 = newParentToChild2.rotate(parentOffset);
				const PxVec3 d2 = childOffset;
				r = e2 + d2;*/

				PX_ASSERT(r.isFinite());

				break;
			}
			case PxArticulationJointType::eSPHERICAL:
			{
				//PxVec3 angVel(joint->jointVelocity[0], joint->jointVelocity[1], joint->jointVelocity[2]);
				//PxVec3 worldAngVel = pLink.bodyCore->angularVelocity + oldTransform.rotate(angVel);

				PxVec3 worldAngVel = motionVelocities[linkID].top;

				/*const PxReal eps = 0.001f;
				const PxVec3 dif = worldAngVel - worldAngVel2;
				PX_ASSERT(PxAbs(dif.x) < eps && PxAbs(dif.y) < eps && PxAbs(dif.z) < eps);*/

				newWorldQ = PxExp(worldAngVel) * oldTransform.q;

				newParentToChild = (newWorldQ.getConjugate() * relativeQuat * pBody2World.q).getNormalized();

				const PxVec3 e = newParentToChild.rotate(parentOffset);
				const PxVec3 d = childOffset;
				r = e + d;

				PX_ASSERT(r.isFinite());
				break;
			}
			case PxArticulationJointType::eFIX:
			{
				//this is fix joint so joint don't have velocity
				newParentToChild = relativeQuat;

				const PxVec3 e = newParentToChild.rotate(parentOffset);
				const PxVec3 d = childOffset;

				r = e + d;
				break;
			}
			default:
				break;
			}

			PxTransform& body2World = link.bodyCore->body2World;
			body2World.q = (pBody2World.q * newParentToChild.getConjugate()).getNormalized();
			body2World.p = pBody2World.p + body2World.q.rotate(r);

			PX_ASSERT(body2World.isSane());
		}
	}

	PxU8* FeatherstoneArticulation::allocateScratchSpatialData(PxcScratchAllocator* allocator,
		const PxU32 linkCount, ScratchData& scratchData, bool fallBackToHeap)
	{
		const PxU32 size = sizeof(Cm::SpatialVectorF) * linkCount;
		const PxU32 totalSize = size * 4 + sizeof(Dy::SpatialMatrix) * linkCount;

		PxU8* tempMemory = reinterpret_cast<PxU8*>(allocator->alloc(totalSize, fallBackToHeap));

		scratchData.motionVelocities = reinterpret_cast<Cm::SpatialVectorF*>(tempMemory);
		PxU32 offset = size;
		scratchData.motionAccelerations = reinterpret_cast<Cm::SpatialVectorF*>(tempMemory + offset);
		offset += size;
		scratchData.coriolisVectors = reinterpret_cast<Cm::SpatialVectorF*>(tempMemory + offset);
		offset += size;
		scratchData.spatialZAVectors = reinterpret_cast<Cm::SpatialVectorF*>(tempMemory + offset);
		offset += size;
		scratchData.compositeSpatialInertias = reinterpret_cast<Dy::SpatialMatrix*>(tempMemory + offset);

		return tempMemory;
	}

/*	void FeatherstoneArticulation::allocateScratchSpatialData(DyScratchAllocator& allocator,
		const PxU32 linkCount, ScratchData& scratchData)
	{
		const PxU32 size = sizeof(Cm::SpatialVectorF) * linkCount;
		const PxU32 totalSize = size * 5 + sizeof(Dy::SpatialMatrix) * linkCount;

		PxU8* tempMemory = allocator.alloc<PxU8>(totalSize);

		scratchData.motionVelocities = reinterpret_cast<Cm::SpatialVectorF*>(tempMemory);
		PxU32 offset = size;
		scratchData.motionAccelerations = reinterpret_cast<Cm::SpatialVectorF*>(tempMemory + offset);
		offset += size;
		scratchData.coriolisVectors = reinterpret_cast<Cm::SpatialVectorF*>(tempMemory + offset);
		offset += size;
		scratchData.spatialZAVectors = reinterpret_cast<Cm::SpatialVectorF*>(tempMemory + offset);
		offset += size;
		scratchData.externalAccels = reinterpret_cast<Cm::SpatialVector*>(tempMemory + offset);
		offset += size;
		scratchData.compositeSpatialInertias = reinterpret_cast<Dy::SpatialMatrix*>(tempMemory + offset);
		
	}*/

}//namespace Dy
}