Initial commit:

PhysX 3.4.0 Update @ 21294896
APEX 1.4.0 Update @ 21275617

[CL 21300167]

1 files changed, 575 insertions, 0 deletions

diff --git a/PhysX_3.4/Source/LowLevelDynamics/src/DyArticulationScalar.cpp b/PhysX_3.4/Source/LowLevelDynamics/src/DyArticulationScalar.cpp
new file mode 100644
index 00000000..af00a367
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevelDynamics/src/DyArticulationScalar.cpp
@@ -0,0 +1,575 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#include "DyArticulationUtils.h"
+#include "DyArticulationScalar.h"
+#include "DyArticulationReference.h"
+#include "DyArticulationFnsDebug.h"
+
+namespace physx
+{
+namespace Dy
+{
+namespace ArticulationRef
+{
+	Cm::SpatialVector propagateImpulse(const FsRow& row, 
+									   const FsJointVectors& j,
+									   PxVec3& SZ,
+									   const Cm::SpatialVector& Z,
+									   const FsRowAux& aux)
+	{
+		typedef ArticulationFnsScalar Fns;
+	
+		SZ = Fns::axisDot(reinterpret_cast<const Cm::SpatialVector*>(aux.S), Z);
+		return Fns::translateForce(getParentOffset(j), Z - Fns::axisMultiply(getDSI(row), SZ));
+	}
+
+	Cm::SpatialVector propagateVelocity(const FsRow& row,
+									    const FsJointVectors& j,
+										const PxVec3& SZ,
+										const Cm::SpatialVector& v,
+										const FsRowAux& aux)
+	{
+		typedef ArticulationFnsScalar Fns;
+
+		Cm::SpatialVector w = Fns::translateMotion(-getParentOffset(j), v);
+		PxVec3 DSZ = Fns::multiply(row.D, SZ);
+
+		return w - Fns::axisMultiply(reinterpret_cast<const Cm::SpatialVector*>(aux.S), DSZ + Fns::axisDot(getDSI(row), w));
+	}
+
+	void applyImpulse(const FsData& matrix, 
+					  Cm::SpatialVector* velocity,
+					  PxU32 linkID, 
+					  const Cm::SpatialVector& impulse)
+	{
+		typedef ArticulationFnsScalar Fns;
+
+		PX_ASSERT(matrix.linkCount<=DY_ARTICULATION_MAX_SIZE);
+
+		const FsRow* rows = getFsRows(matrix);
+		const FsRowAux* aux = getAux(matrix);
+		const FsJointVectors* jointVectors = getJointVectors(matrix);
+
+		Cm::SpatialVector dV[DY_ARTICULATION_MAX_SIZE];
+		PxVec3 SZ[DY_ARTICULATION_MAX_SIZE];
+
+		for(PxU32 i=0;i<matrix.linkCount;i++)
+			SZ[i] = PxVec3(0);
+
+		Cm::SpatialVector Z = -impulse;
+
+		for(;linkID!=0; linkID = matrix.parent[linkID])
+			Z = ArticulationRef::propagateImpulse(rows[linkID], jointVectors[linkID], SZ[linkID], Z, aux[linkID]);
+
+		dV[0] = Fns::getRootDeltaV(matrix,-Z);
+
+		for(PxU32 i=1;i<matrix.linkCount; i++)
+			dV[i] = ArticulationRef::propagateVelocity(rows[i], jointVectors[i], SZ[i], dV[matrix.parent[i]], aux[i]);
+
+		for(PxU32 i=0;i<matrix.linkCount;i++)
+			velocity[i] += dV[i];
+	}
+
+	void ltbFactor(FsData& m)
+	{
+		typedef ArticulationFnsScalar Fns;
+		LtbRow* rows = getLtbRows(m);
+
+		SpInertia inertia[DY_ARTICULATION_MAX_SIZE];
+		for(PxU32 i=0;i<m.linkCount;i++)
+			inertia[i] = ArticulationFnsDebug::unsimdify(rows[i].inertia);
+
+		Cm::SpatialVector j[3];
+		for(PxU32 i=m.linkCount; --i>0;)
+		{
+			LtbRow& b = rows[i];
+			inertia[i] = Fns::invertInertia(inertia[i]);
+			PxU32 p = m.parent[i];
+
+			Cm::SpatialVector* j0 = &reinterpret_cast<Cm::SpatialVector&>(*b.j0),
+							 * j1 = &reinterpret_cast<Cm::SpatialVector&>(*b.j1);
+
+			Fns::multiply(j, inertia[i], j1);
+			PxMat33 jResponse = Fns::invertSym33(-Fns::multiplySym(j, j1));
+			j1[0] = j[0]; j1[1] = j[1]; j1[2] = j[2];
+
+			b.jResponse = Mat33V_From_PxMat33(jResponse);
+			Fns::multiply(j, j0, jResponse);
+			inertia[p] = Fns::multiplySubtract(inertia[p], j, j0);
+			j0[0] = j[0]; j0[1] = j[1]; j0[2] = j[2];
+		}
+
+		rows[0].inertia = Fns::invertInertia(inertia[0]);
+		for(PxU32 i=1;i<m.linkCount;i++)
+			rows[i].inertia = inertia[i];
+	}
+
+
+}
+
+#if 0
+
+
+void ltbSolve(const FsData& m, 
+			  Vec3V* c,					// rhs error to solve for
+			  Cm::SpatialVector* y)		// velocity delta output
+{
+	typedef ArticulationFnsScalar Fns;
+
+	PxVec4* b = reinterpret_cast<PxVec4*>(c);
+	const LtbRow* rows = getLtbRows(m);
+	PxMemZero(y, m.linkCount*sizeof(Cm::SpatialVector));
+
+	for(PxU32 i=m.linkCount;i-->1;)
+	{
+		PxU32 p = m.parent[i];
+		const LtbRow& r = rows[i];
+		b[i] -= PxVec4(Fns::axisDot(&static_cast<const Cm::SpatialVector&>(*r.j1), y[i]),0);
+		y[p] -= Fns::axisMultiply(&static_cast<const Cm::SpatialVector&>(*r.j0), b[i].getXYZ());
+	}
+
+	y[0] = Fns::multiply(rows[0].inertia,y[0]);
+
+	for(PxU32 i=1; i<m.linkCount; i++)
+	{
+		PxU32 p = m.parent[i];
+		const LtbRow& r = rows[i];
+		PxVec3 t = Fns::multiply(r.jResponse, b[i].getXYZ()) - Fns::axisDot(&static_cast<const Cm::SpatialVector&>(*r.j0), y[p]);
+		y[i] = Fns::multiply(r.inertia, y[i]) - Fns::axisMultiply(&static_cast<const Cm::SpatialVector&>(*r.j1), t);
+	}
+}
+
+void PxcFsPropagateDrivenInertiaScalar(FsData& matrix,
+								 const FsInertia* baseInertia,
+								 const PxReal* isf,
+								 const Mat33V* load)
+{
+	typedef ArticulationFnsScalar Fns;
+
+	Cm::SpatialVector IS[3], DSI[3];
+	PxMat33 D;
+
+	FsRow* rows = getFsRows(matrix);
+	const FsRowAux* aux = getAux(matrix);
+	const FsJointVectors* jointVectors = getJointVectors(matrix);
+
+	SpInertia inertia[DY_ARTICULATION_MAX_SIZE];
+	for(PxU32 i=0;i<matrix.linkCount;i++)
+		inertia[i] = ArticulationFnsDebug::unsimdify(baseInertia[i]);
+
+	for(PxU32 i=matrix.linkCount; --i>0;)
+	{
+		FsRow& r = rows[i];
+		const FsRowAux& a = aux[i];
+		const FsJointVectors& jv = jointVectors[i];
+
+		Fns::multiply(IS, inertia[i], &static_cast<const Cm::SpatialVector&>(*a.S));
+
+		PX_ALIGN(16, PxMat33) L;
+		PxMat33_From_Mat33V(load[i], L);
+		D = Fns::invertSym33(Fns::multiplySym(&static_cast<const Cm::SpatialVector&>(*a.S), IS) + L*isf[i]);
+
+		Fns::multiply(DSI, IS, D);
+
+		r.D = Mat33V_From_PxMat33(D);
+		static_cast<Cm::SpatialVector&>(r.DSI[0]) = DSI[0];
+		static_cast<Cm::SpatialVector&>(r.DSI[1]) = DSI[1];
+		static_cast<Cm::SpatialVector&>(r.DSI[2]) = DSI[2];
+
+		inertia[matrix.parent[i]] += Fns::translate(getParentOffset(jv), Fns::multiplySubtract(inertia[i], DSI, IS));
+	}
+
+	FsInertia& m = getRootInverseInertia(matrix);
+	m = FsInertia(Fns::invertInertia(inertia[0]));
+}
+
+// no need to compile this ecxcept for verification, and it consumes huge amounts of stack space
+void PxcFsComputeJointLoadsScalar(const FsData& matrix,
+							      const FsInertia*PX_RESTRICT baseInertia,
+							      Mat33V*PX_RESTRICT load,
+   							      const PxReal*PX_RESTRICT isf,
+							      PxU32 linkCount,
+							      PxU32 maxIterations)
+{
+	typedef ArticulationFnsScalar Fns;
+
+	// the childward S
+	SpInertia leafwardInertia[DY_ARTICULATION_MAX_SIZE];
+	SpInertia rootwardInertia[DY_ARTICULATION_MAX_SIZE];
+	SpInertia inertia[DY_ARTICULATION_MAX_SIZE];
+	SpInertia contribToParent[DY_ARTICULATION_MAX_SIZE];
+
+	// total articulated inertia assuming the articulation is rooted here
+
+	const FsRow* row = getFsRows(matrix);
+	const FsRowAux* aux = getAux(matrix);
+	const FsJointVectors* jointVectors = getJointVectors(matrix);
+
+	PX_UNUSED(row);
+
+	PxMat33 load_[DY_ARTICULATION_MAX_SIZE];
+
+	for(PxU32 iter=0;iter<maxIterations;iter++)
+	{
+		for(PxU32 i=0;i<linkCount;i++)
+			inertia[i] = ArticulationFnsDebug::unsimdify(baseInertia[i]);
+			
+		for(PxU32 i=linkCount;i-->1;)
+		{
+			const FsJointVectors& j = jointVectors[i];
+
+			leafwardInertia[i] = inertia[i];
+			contribToParent[i] = Fns::propagate(inertia[i], &static_cast<const Cm::SpatialVector&>(*aux[i].S), load_[i], isf[i]);
+			inertia[matrix.parent[i]] += Fns::translate((PxVec3&)j.parentOffset, contribToParent[i]);
+		}
+
+		for(PxU32 i=1;i<linkCount;i++)
+		{
+			rootwardInertia[i] = Fns::translate(-(PxVec3&)jointVectors[i].parentOffset, inertia[matrix.parent[i]]) - contribToParent[i];				
+			inertia[i] += Fns::propagate(rootwardInertia[i], &static_cast<const Cm::SpatialVector&>(*aux[i].S), load_[i], isf[i]);
+		}
+
+		for(PxU32 i=1;i<linkCount;i++)
+		{
+			load_[i] = Fns::computeDriveInertia(leafwardInertia[i], rootwardInertia[i], &static_cast<const Cm::SpatialVector&>(*aux[i].S));
+			PX_ASSERT(load_[i][0].isFinite() && load_[i][1].isFinite() && load_[2][i].isFinite());
+		}					
+	}
+	for(PxU32 i=1;i<linkCount;i++)
+		load[i] = Mat33V_From_PxMat33(load_[i]);
+}
+
+
+void PxcFsApplyImpulse(const FsData& matrix, 
+					   PxU32 linkID, 
+					   const Cm::SpatialVector& impulse)
+{
+#if DY_ARTICULATION_DEBUG_VERIFY
+	PxcFsRefApplyImpulse(matrix, state.refVelocity, linkID, impulse);
+#endif
+
+	Cm::SpatialVector Z = -impulse;
+
+	for(PxU32 i = linkID; i!=0; i = matrix.row[i].parent)
+	{
+		PxVec3 SZ;
+		Z = propagateImpulse(matrix.row[i], SZ, Z, matrix.aux[i]);
+		deferredSZRef(state,i) += SZ;
+	}
+
+	static_cast<Cm::SpatialVector &>(state.deferredZ) += Z;
+	state.dirty |= matrix.row[linkID].pathToRoot;
+}
+
+Cm::SpatialVector PxcFsGetVelocity(const FsData& matrix,
+								  PxU32 linkID)
+{
+	// find the dirty node on the path (including the root) with the lowest index
+	ArticulationBitField toUpdate = matrix.row[linkID].pathToRoot & state.dirty;
+
+	if(toUpdate)
+	{
+		ArticulationBitField ignoreNodes = (toUpdate & (0-toUpdate))-1;
+		ArticulationBitField path = matrix.row[linkID].pathToRoot & ~ignoreNodes, p = path;
+		ArticulationBitField newDirty = 0;
+
+		Cm::SpatialVector dV = Cm::SpatialVector::zero();
+		if(p & 1)
+		{
+			dV = getRootDeltaV(matrix, -deferredZ(state));
+
+			velocityRef(state, 0) += dV;
+			for(ArticulationBitField defer = matrix.row[0].children & ~path; defer; defer &= (defer-1))
+				deferredVelRef(state, ArticulationLowestSetBit(defer)) += dV;
+
+			deferredZRef(state) = Cm::SpatialVector::zero();
+			newDirty = matrix.row[0].children;
+			p--;
+		}
+
+		for(; p; p &= (p-1))
+		{
+			PxU32 i = ArticulationLowestSetBit(p);
+
+			dV = propagateVelocity(matrix.row[i], deferredSZ(state,i), dV + state.deferredVel[i], matrix.aux[i]);
+
+			velocityRef(state,i) += dV;
+			for(ArticulationBitField defer = matrix.row[i].children & ~path; defer; defer &= (defer-1))
+				deferredVelRef(state,ArticulationLowestSetBit(defer)) += dV;
+
+			newDirty |= matrix.row[i].children;
+			deferredVelRef(state,i) = Cm::SpatialVector::zero();
+			deferredSZRef(state,i) = PxVec3(0);
+		}
+
+		state.dirty = (state.dirty | newDirty)&~path;
+	}
+#if DY_ARTICULATION_DEBUG_VERIFY
+	Cm::SpatialVector v = state.velocity[linkID];
+	Cm::SpatialVector rv = state.refVelocity[linkID];
+	PX_ASSERT((v-rv).magnitude()<1e-4f * rv.magnitude());
+#endif
+
+	return state.velocity[linkID];
+}
+
+void PxcFsFlushVelocity(const FsData& matrix)
+{
+	Cm::SpatialVector V = getRootDeltaV(matrix, -deferredZ(state));
+	deferredZRef(state) = Cm::SpatialVector::zero();
+	velocityRef(state,0) += V;
+	for(ArticulationBitField defer = matrix.row[0].children; defer; defer &= (defer-1))
+		deferredVelRef(state,ArticulationLowestSetBit(defer)) += V;
+
+	for(PxU32 i = 1; i<matrix.linkCount; i++)
+	{
+		Cm::SpatialVector V = propagateVelocity(matrix.row[i], deferredSZ(state,i), state.deferredVel[i], matrix.aux[i]);
+		deferredVelRef(state,i) = Cm::SpatialVector::zero();
+		deferredSZRef(state,i) = PxVec3(0);
+		velocityRef(state,i) += V;
+		for(ArticulationBitField defer = matrix.row[i].children; defer; defer &= (defer-1))
+			deferredVelRef(state,ArticulationLowestSetBit(defer)) += V;
+	}
+
+	state.dirty = 0;
+}
+
+void PxcFsPropagateDrivenInertiaScalar(FsData& matrix,
+									   const FsInertia* baseInertia,
+									   const PxReal* isf,
+									   const Mat33V* load,
+									   PxcFsScratchAllocator allocator)
+{
+	typedef ArticulationFnsSimd<ArticulationFnsSimdBase> Fns;
+
+	Cm::SpatialVectorV IS[3];
+	PxMat33 D;
+
+	FsRow* rows = getFsRows(matrix);
+	const FsRowAux* aux = getAux(matrix);
+	const FsJointVectors* jointVectors = getJointVectors(matrix);
+
+	FsInertia *inertia = allocator.alloc<FsInertia>(matrix.linkCount);
+	PxMemCopy(inertia, baseInertia, matrix.linkCount*sizeof(FsInertia));
+
+	for(PxU32 i=matrix.linkCount; --i>0;)
+	{
+		FsRow& r = rows[i];
+		const FsRowAux& a = aux[i];
+		const FsJointVectors& jv = jointVectors[i];
+
+		Mat33V m = Fns::computeSIS(inertia[i], a.S, IS);
+		FloatV f = FLoad(isf[i]);
+
+		Mat33V D = Fns::invertSym33(Mat33V(V3ScaleAdd(load[i].col0, f, m.col0),
+										   V3ScaleAdd(load[i].col1, f, m.col1),
+										   V3ScaleAdd(load[i].col2, f, m.col2)));
+		r.D = D;
+
+		inertia[matrix.parent[i]] = Fns::addInertia(inertia[matrix.parent[i]], 
+													Fns::translateInertia(jv.parentOffset, Fns::multiplySubtract(inertia[i], D,  IS,  r.DSI)));
+	}
+
+	getRootInverseInertia(matrix) = Fns::invertInertia(inertia[0]);
+}
+
+void PxcLtbSolve(const FsData& m, 
+				 Vec3V* c,					// rhs error to solve for
+				 Cm::SpatialVector* y)		// velocity delta output
+{
+	typedef ArticulationFnsScalar Fns;
+
+	PxVec4* b = reinterpret_cast<PxVec4*>(c);
+	const LtbRow* rows = getLtbRows(m);
+	PxMemZero(y, m.linkCount*sizeof(Cm::SpatialVector));
+
+	for(PxU32 i=m.linkCount;i-->1;)
+	{
+		PxU32 p = m.parent[i];
+		const LtbRow& r = rows[i];
+		b[i] -= PxVec4(Fns::axisDot(&static_cast<const Cm::SpatialVector&>(*r.j1), y[i]),0);
+		y[p] -= Fns::axisMultiply(&static_cast<const Cm::SpatialVector&>(*r.j0), b[i].getXYZ());
+	}
+
+	y[0] = Fns::multiply(rows[0].inertia,y[0]);
+
+	for(PxU32 i=1; i<m.linkCount; i++)
+	{
+		PxU32 p = m.parent[i];
+		const LtbRow& r = rows[i];
+		PxVec3 t = Fns::multiply(r.jResponse, b[i].getXYZ()) - Fns::axisDot(&static_cast<const Cm::SpatialVector&>(*r.j0), y[p]);
+		y[i] = Fns::multiply(r.inertia, y[i]) - Fns::axisMultiply(&static_cast<const Cm::SpatialVector&>(*r.j1), t);
+	}
+}
+
+
+#endif
+
+
+#if DY_ARTICULATION_DEBUG_VERIFY
+void PxcLtbFactorScalar(FsData& m)
+{
+	typedef ArticulationFnsScalar Fns;
+	LtbRow* rows = getLtbRows(m);
+
+	SpInertia inertia[DY_ARTICULATION_MAX_SIZE];
+	for(PxU32 i=0;i<m.linkCount;i++)
+		inertia[i] = ArticulationFnsDebug::unsimdify(rows[i].inertia);
+
+	Cm::SpatialVector j[3];
+	for(PxU32 i=m.linkCount; --i>0;)
+	{
+		LtbRow& b = rows[i];
+		inertia[i] = Fns::invertInertia(inertia[i]);
+		PxU32 p = m.parent[i];
+
+		Cm::SpatialVector* j0 = &reinterpret_cast<Cm::SpatialVector&>(*b.j0),
+						 * j1 = &reinterpret_cast<Cm::SpatialVector&>(*b.j1);
+
+		Fns::multiply(j, inertia[i], j1);
+		PxMat33 jResponse = Fns::invertSym33(-Fns::multiplySym(j, j1));
+		j1[0] = j[0]; j1[1] = j[1]; j1[2] = j[2];
+
+		b.jResponse = Mat33V_From_PxMat33(jResponse);
+		Fns::multiply(j, j0, jResponse);
+		inertia[p] = Fns::multiplySubtract(inertia[p], j, j0);
+		j0[0] = j[0]; j0[1] = j[1]; j0[2] = j[2];
+	}
+
+	rows[0].inertia = Fns::invertInertia(inertia[0]);
+	for(PxU32 i=1;i<m.linkCount;i++)
+		rows[i].inertia = inertia[i];
+}
+
+void PxcFsPropagateDrivenInertiaScalar(FsData& matrix,
+									   const FsInertia* baseInertia,
+									   const PxReal* isf,
+									   const Mat33V* load)
+{
+	typedef ArticulationFnsScalar Fns;
+
+	Cm::SpatialVector IS[3], DSI[3];
+	PxMat33 D;
+
+	FsRow* rows = getFsRows(matrix);
+	const FsRowAux* aux = getAux(matrix);
+	const FsJointVectors* jointVectors = getJointVectors(matrix);
+
+	SpInertia inertia[DY_ARTICULATION_MAX_SIZE];
+	for(PxU32 i=0;i<matrix.linkCount;i++)
+		inertia[i] = ArticulationFnsDebug::unsimdify(baseInertia[i]);
+
+	for(PxU32 i=matrix.linkCount; --i>0;)
+	{
+		FsRow& r = rows[i];
+		const FsRowAux& a = aux[i];
+		const FsJointVectors& jv = jointVectors[i];
+
+		Fns::multiply(IS, inertia[i], &reinterpret_cast<const Cm::SpatialVector&>(*a.S));
+
+		PX_ALIGN(16, PxMat33) L;
+		PxMat33_From_Mat33V(load[i], L);
+		D = Fns::invertSym33(Fns::multiplySym(&reinterpret_cast<const Cm::SpatialVector&>(*a.S), IS) + L*isf[i]);
+
+		Fns::multiply(DSI, IS, D);
+
+		r.D = Mat33V_From_PxMat33(D);
+		reinterpret_cast<Cm::SpatialVector&>(r.DSI[0]) = DSI[0];
+		reinterpret_cast<Cm::SpatialVector&>(r.DSI[1]) = DSI[1];
+		reinterpret_cast<Cm::SpatialVector&>(r.DSI[2]) = DSI[2];
+
+		inertia[matrix.parent[i]] += Fns::translate(getParentOffset(jv), Fns::multiplySubtract(inertia[i], DSI, IS));
+	}
+
+	FsInertia& m = getRootInverseInertia(matrix);
+	m = FsInertia(Fns::invertInertia(inertia[0]));
+}
+
+// no need to compile this ecxcept for verification, and it consumes huge amounts of stack space
+void PxcFsComputeJointLoadsScalar(const FsData& matrix,
+								  const FsInertia*PX_RESTRICT baseInertia,
+								  Mat33V*PX_RESTRICT load,
+								  const PxReal*PX_RESTRICT isf,
+								  PxU32 linkCount,
+								  PxU32 maxIterations)
+{
+	typedef ArticulationFnsScalar Fns;
+
+	// the childward S
+	SpInertia leafwardInertia[DY_ARTICULATION_MAX_SIZE];
+	SpInertia rootwardInertia[DY_ARTICULATION_MAX_SIZE];
+	SpInertia inertia[DY_ARTICULATION_MAX_SIZE];
+	SpInertia contribToParent[DY_ARTICULATION_MAX_SIZE];
+
+	// total articulated inertia assuming the articulation is rooted here
+
+	const FsRow* row = getFsRows(matrix);
+	const FsRowAux* aux = getAux(matrix);
+	const FsJointVectors* jointVectors = getJointVectors(matrix);
+
+	PX_UNUSED(row);
+
+	PxMat33 load_[DY_ARTICULATION_MAX_SIZE];
+
+	for(PxU32 iter=0;iter<maxIterations;iter++)
+	{
+		for(PxU32 i=0;i<linkCount;i++)
+			inertia[i] = ArticulationFnsDebug::unsimdify(baseInertia[i]);
+			
+		for(PxU32 i=linkCount;i-->1;)
+		{
+			const FsJointVectors& j = jointVectors[i];
+
+			leafwardInertia[i] = inertia[i];
+			contribToParent[i] = Fns::propagate(inertia[i], &reinterpret_cast<const Cm::SpatialVector&>(*aux[i].S), load_[i], isf[i]);
+			inertia[matrix.parent[i]] += Fns::translate((PxVec3&)j.parentOffset, contribToParent[i]);
+		}
+
+		for(PxU32 i=1;i<linkCount;i++)
+		{
+			rootwardInertia[i] = Fns::translate(-(PxVec3&)jointVectors[i].parentOffset, inertia[matrix.parent[i]]) - contribToParent[i];				
+			inertia[i] += Fns::propagate(rootwardInertia[i], &reinterpret_cast<const Cm::SpatialVector&>(*aux[i].S), load_[i], isf[i]);
+		}
+
+		for(PxU32 i=1;i<linkCount;i++)
+		{
+			load_[i] = Fns::computeDriveInertia(leafwardInertia[i], rootwardInertia[i], &reinterpret_cast<const Cm::SpatialVector&>(*aux[i].S));
+			PX_ASSERT(load_[i][0].isFinite() && load_[i][1].isFinite() && load_[2][i].isFinite());
+		}					
+	}
+	for(PxU32 i=1;i<linkCount;i++)
+		load[i] = Mat33V_From_PxMat33(load_[i]);
+}
+#endif
+
+}
+
+}