Initial commit:

PhysX 3.4.0 Update @ 21294896
APEX 1.4.0 Update @ 21275617

[CL 21300167]

1 files changed, 2950 insertions, 0 deletions

diff --git a/PhysX_3.4/Source/LowLevelDynamics/src/DyDynamics.cpp b/PhysX_3.4/Source/LowLevelDynamics/src/DyDynamics.cpp
new file mode 100644
index 00000000..07f3b642
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevelDynamics/src/DyDynamics.cpp
@@ -0,0 +1,2950 @@
+// 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 "PsTime.h"
+#include "PsAtomic.h"
+#include "PxvDynamics.h"
+
+#include "foundation/PxProfiler.h"
+#include "PxsRigidBody.h"
+#include "PxsContactManager.h"
+#include "DyDynamics.h"
+#include "DyBodyCoreIntegrator.h"
+#include "DySolverCore.h"
+#include "DySolverControl.h"
+#include "DySolverContact.h"
+#include "DySolverContactPF.h"
+#include "DyArticulationContactPrep.h"
+#include "DySolverBody.h"
+
+#include "DyConstraintPrep.h"
+#include "DyConstraintPartition.h"
+#include "DyArticulation.h"
+
+#include "CmFlushPool.h"
+#include "DyArticulationPImpl.h"
+#include "PxsMaterialManager.h"
+#include "DySolverContactPF4.h"
+#include "DyContactReduction.h"
+#include "PxcNpContactPrepShared.h"
+#include "DyContactPrep.h"
+#include "DySolverControlPF.h"
+#include "PxSceneDesc.h"
+#include "PxsSimpleIslandManager.h"
+#include "PxvNphaseImplementationContext.h"
+#include "PxsContactManagerState.h"
+#include "PxsDefaultMemoryManager.h"
+#include "DyContactPrepShared.h"
+  
+//KS - used to turn on/off batched SIMD constraints.
+#define DY_BATCH_CONSTRAINTS 1
+//KS - used to specifically turn on/off batches 1D SIMD constraints.
+#define DY_BATCH_1D 1
+
+namespace physx
+{
+namespace Dy
+{
+
+struct SolverIslandObjects
+{
+	PxsRigidBody**				bodies;	
+	Articulation**				articulations;
+	Dy::Articulation**			articulationOwners;
+	PxsIndexedContactManager*	contactManagers;
+	//PxsIndexedConstraint*		constraints;
+
+	const IG::IslandId*			islandIds;
+	PxU32						numIslands;
+	PxU32*						bodyRemapTable;
+	PxU32*						nodeIndexArray;
+
+	PxSolverConstraintDesc*	constraintDescs;
+	PxSolverConstraintDesc*	orderedConstraintDescs;
+	PxSolverConstraintDesc*	tempConstraintDescs;
+	PxConstraintBatchHeader*	constraintBatchHeaders;
+	Cm::SpatialVector*			motionVelocities;
+	PxsBodyCore**				bodyCoreArray;
+
+	SolverIslandObjects() : bodies(NULL), articulations(NULL), articulationOwners(NULL),
+		contactManagers(NULL), islandIds(NULL), numIslands(0), nodeIndexArray(NULL), constraintDescs(NULL), orderedConstraintDescs(NULL), 
+		tempConstraintDescs(NULL), constraintBatchHeaders(NULL), motionVelocities(NULL), bodyCoreArray(NULL)
+	{
+	}
+};
+
+Context* createDynamicsContext(	PxcNpMemBlockPool* memBlockPool,
+								PxcScratchAllocator& scratchAllocator, Cm::FlushPool& taskPool,
+								PxvSimStats& simStats, PxTaskManager* taskManager, Ps::VirtualAllocatorCallback* allocatorCallback, PxsMaterialManager* materialManager,
+								IG::IslandSim* accurateIslandSim, PxU64 contextID,
+								const bool enableStabilization, const bool useEnhancedDeterminism, const bool useAdaptiveForce
+								)
+{
+	return DynamicsContext::create(	memBlockPool, scratchAllocator, taskPool, simStats, taskManager, allocatorCallback, materialManager, accurateIslandSim,
+									contextID, enableStabilization, useEnhancedDeterminism, useAdaptiveForce);
+}
+
+// PT: TODO: consider removing this function. We already have "createDynamicsContext".
+DynamicsContext* DynamicsContext::create(	PxcNpMemBlockPool* memBlockPool,
+											PxcScratchAllocator& scratchAllocator,
+											Cm::FlushPool& taskPool,
+											PxvSimStats& simStats,
+											PxTaskManager* taskManager,
+											Ps::VirtualAllocatorCallback* allocatorCallback,
+											PxsMaterialManager* materialManager,
+											IG::IslandSim* accurateIslandSim,
+											PxU64 contextID,
+											const bool enableStabilization,
+											const bool useEnhancedDeterminism,
+											const bool useAdaptiveForce
+											)
+{
+	// PT: TODO: inherit from UserAllocated, remove placement new
+	DynamicsContext* dc = reinterpret_cast<DynamicsContext*>(PX_ALLOC(sizeof(DynamicsContext), "DynamicsContext"));
+	if(dc)
+	{
+		new(dc)DynamicsContext(memBlockPool, scratchAllocator, taskPool, simStats, taskManager, allocatorCallback, materialManager, accurateIslandSim, contextID, enableStabilization, useEnhancedDeterminism, useAdaptiveForce);
+	}
+	return dc;
+}
+
+
+void DynamicsContext::destroy()
+{
+	this->~DynamicsContext();
+	PX_FREE(this);
+}
+
+void DynamicsContext::resetThreadContexts()
+{
+	PxcThreadCoherentCacheIterator<ThreadContext, PxcNpMemBlockPool> threadContextIt(mThreadContextPool);
+	ThreadContext* threadContext = threadContextIt.getNext();
+
+	while(threadContext != NULL)
+	{
+		threadContext->reset();
+		threadContext = threadContextIt.getNext();
+	}
+}
+
+
+// =========================== Basic methods
+
+
+DynamicsContext::DynamicsContext(	PxcNpMemBlockPool* memBlockPool,
+									PxcScratchAllocator& scratchAllocator,
+									Cm::FlushPool& taskPool,
+									PxvSimStats& simStats,
+									PxTaskManager* taskManager,
+									Ps::VirtualAllocatorCallback* allocatorCallback,
+									PxsMaterialManager* materialManager,
+									IG::IslandSim* accurateIslandSim,
+									PxU64 contextID,
+									const bool enableStabilization,
+									const bool useEnhancedDeterminism,
+									const bool useAdaptiveForce
+									) : 
+	Dy::Context			(accurateIslandSim, allocatorCallback, simStats, enableStabilization, useEnhancedDeterminism, useAdaptiveForce),
+	mThreadContextPool	(memBlockPool),
+	mMaterialManager	(materialManager),
+	mScratchAllocator	(scratchAllocator),
+	mTaskPool			(taskPool),
+	mTaskManager		(taskManager),
+	mContextID			(contextID)
+{
+	createThresholdStream(*allocatorCallback);
+	createForceChangeThresholdStream(*allocatorCallback);
+	mExceededForceThresholdStream[0] = PX_PLACEMENT_NEW(PX_ALLOC(sizeof(ThresholdStream), PX_DEBUG_EXP("ExceededForceThresholdStream[0]")), ThresholdStream(*allocatorCallback));
+	mExceededForceThresholdStream[1] = PX_PLACEMENT_NEW(PX_ALLOC(sizeof(ThresholdStream), PX_DEBUG_EXP("ExceededForceThresholdStream[1]")), ThresholdStream(*allocatorCallback));
+	mThresholdStreamOut = 0;
+	mCurrentIndex = 0;
+	mWorldSolverBody.linearVelocity = PxVec3(0);
+	mWorldSolverBody.angularState = PxVec3(0);
+	mWorldSolverBodyData.invMass = 0;
+	mWorldSolverBodyData.sqrtInvInertia = PxMat33(PxZero);
+	mWorldSolverBodyData.nodeIndex = IG_INVALID_NODE;
+	mWorldSolverBodyData.reportThreshold = PX_MAX_REAL;
+	mWorldSolverBodyData.penBiasClamp = -PX_MAX_REAL;
+	mWorldSolverBodyData.maxContactImpulse = PX_MAX_REAL;
+	mWorldSolverBody.solverProgress=MAX_PERMITTED_SOLVER_PROGRESS;
+	mWorldSolverBody.maxSolverNormalProgress=MAX_PERMITTED_SOLVER_PROGRESS;
+	mWorldSolverBody.maxSolverFrictionProgress=MAX_PERMITTED_SOLVER_PROGRESS;
+	mWorldSolverBodyData.linearVelocity = mWorldSolverBodyData.angularVelocity = PxVec3(0.f);
+	mWorldSolverBodyData.body2World = PxTransform(PxIdentity);
+	mWorldSolverBodyData.lockFlags = 0;
+	mSolverCore[PxFrictionType::ePATCH] = SolverCoreGeneral::create();
+	mSolverCore[PxFrictionType::eONE_DIRECTIONAL] = SolverCoreGeneralPF::create();
+	mSolverCore[PxFrictionType::eTWO_DIRECTIONAL] = SolverCoreGeneralPF::create();
+}
+
+DynamicsContext::~DynamicsContext()
+{
+	for(PxU32 i = 0; i < PxFrictionType::eFRICTION_COUNT; ++i)
+	{
+		mSolverCore[i]->destroyV();
+	}
+
+	if(mExceededForceThresholdStream[0])
+	{
+		mExceededForceThresholdStream[0]->~ThresholdStream();
+		PX_FREE(mExceededForceThresholdStream[0]);
+	}
+	mExceededForceThresholdStream[0] = NULL;
+
+	if(mExceededForceThresholdStream[1])
+	{
+		mExceededForceThresholdStream[1]->~ThresholdStream();
+		PX_FREE(mExceededForceThresholdStream[1]);
+	}
+	mExceededForceThresholdStream[1] = NULL;
+
+}
+
+#if PX_ENABLE_SIM_STATS
+void DynamicsContext::addThreadStats(const ThreadContext::ThreadSimStats& stats)
+{
+	mSimStats.mNbActiveConstraints += stats.numActiveConstraints;
+	mSimStats.mNbActiveDynamicBodies += stats.numActiveDynamicBodies;
+	mSimStats.mNbActiveKinematicBodies += stats.numActiveKinematicBodies;
+	mSimStats.mNbAxisSolverConstraints += stats.numAxisSolverConstraints;
+}
+#endif
+
+// =========================== Solve methods!
+
+void DynamicsContext::setDescFromIndices(PxSolverConstraintDesc& desc, const PxsIndexedInteraction& constraint, const PxU32 solverBodyOffset)
+{
+	PX_COMPILE_TIME_ASSERT(PxsIndexedInteraction::eBODY == 0);
+	PX_COMPILE_TIME_ASSERT(PxsIndexedInteraction::eKINEMATIC == 1);
+	const PxU32 offsetMap[] = {solverBodyOffset, 0};
+	//const PxU32 offsetMap[] = {mKinematicCount, 0};
+
+	if(constraint.indexType0 == PxsIndexedInteraction::eARTICULATION)
+	{
+		Articulation* a = getArticulation(constraint.articulation0);
+		desc.articulationA = a->getFsDataPtr();
+		desc.articulationALength = Ps::to16(a->getSolverDataSize());
+		PX_ASSERT(0==(desc.articulationALength & 0x0f));
+		desc.linkIndexA = Ps::to16(a->getLinkIndex(constraint.articulation0));
+	}
+	else
+	{
+		desc.linkIndexA = PxSolverConstraintDesc::NO_LINK;
+		//desc.articulationALength = 0; //this is unioned with bodyADataIndex
+		/*desc.bodyA = constraint.indexType0 == PxsIndexedInteraction::eWORLD ? &mWorldSolverBody
+																			: &mSolverBodyPool[(PxU32)constraint.solverBody0 + offsetMap[constraint.indexType0]];
+		desc.bodyADataIndex = PxU16(constraint.indexType0 == PxsIndexedInteraction::eWORLD ? 0
+																			: (PxU16)constraint.solverBody0 + 1 + offsetMap[constraint.indexType0]);*/
+
+		desc.bodyA = constraint.indexType0 == PxsIndexedInteraction::eWORLD ? &mWorldSolverBody
+																			: &mSolverBodyPool[PxU32(constraint.solverBody0) + offsetMap[constraint.indexType0]];
+		desc.bodyADataIndex = PxU16(constraint.indexType0 == PxsIndexedInteraction::eWORLD ? 0
+																			: PxU16(constraint.solverBody0) + 1 + offsetMap[constraint.indexType0]);
+	}
+
+	if(constraint.indexType1 == PxsIndexedInteraction::eARTICULATION)
+	{
+		Articulation* a = getArticulation(constraint.articulation1);
+		desc.articulationB = a->getFsDataPtr();
+		desc.articulationBLength = Ps::to16(a->getSolverDataSize());
+		PX_ASSERT(0==(desc.articulationBLength & 0x0f));
+		desc.linkIndexB = Ps::to16(a->getLinkIndex(constraint.articulation1));
+	}
+	else
+	{
+		desc.linkIndexB = PxSolverConstraintDesc::NO_LINK;
+		//desc.articulationBLength = 0; //this is unioned with bodyBDataIndex
+		desc.bodyB = constraint.indexType1 == PxsIndexedInteraction::eWORLD ? &mWorldSolverBody
+																			: &mSolverBodyPool[PxU32(constraint.solverBody1) + offsetMap[constraint.indexType1]];
+		desc.bodyBDataIndex = PxU16(constraint.indexType1 == PxsIndexedInteraction::eWORLD ? 0
+																			: PxU16(constraint.solverBody1) + 1 + offsetMap[constraint.indexType1]);
+	}
+}
+
+void DynamicsContext::setDescFromIndices(PxSolverConstraintDesc& desc, IG::EdgeIndex edgeIndex, const IG::SimpleIslandManager& islandManager,
+	PxU32* bodyRemap, const PxU32 solverBodyOffset)
+{
+	PX_COMPILE_TIME_ASSERT(PxsIndexedInteraction::eBODY == 0);
+	PX_COMPILE_TIME_ASSERT(PxsIndexedInteraction::eKINEMATIC == 1);
+
+	const IG::IslandSim& islandSim = islandManager.getAccurateIslandSim();
+
+	IG::NodeIndex node1 = islandSim.getNodeIndex1(edgeIndex);
+	if (node1.isStaticBody())
+	{
+		desc.bodyA = &mWorldSolverBody;
+		desc.bodyADataIndex = 0;
+		desc.linkIndexA = PxSolverConstraintDesc::NO_LINK;
+	}
+	else
+	{
+		const IG::Node& node = islandSim.getNode(node1);
+		if (node.getNodeType() == IG::Node::eARTICULATION_TYPE)
+		{
+			Dy::Articulation* a = islandSim.getLLArticulation(node1);
+			desc.articulationA = a->getFsDataPtr();
+			desc.articulationALength = Ps::to16(a->getSolverDataSize());
+			PX_ASSERT(0 == (desc.articulationALength & 0x0f));
+			desc.linkIndexA = Ps::to16(node1.articulationLinkId());
+		}
+		else
+		{
+			PxU32 activeIndex = islandSim.getActiveNodeIndex(node1);
+			PxU32 index = node.isKinematic() ? activeIndex : bodyRemap[activeIndex] + solverBodyOffset;
+			desc.bodyA = &mSolverBodyPool[index];
+			desc.bodyADataIndex = Ps::to16(index + 1);
+			desc.linkIndexA = PxSolverConstraintDesc::NO_LINK;
+		}
+	}
+
+	IG::NodeIndex node2 = islandSim.getNodeIndex2(edgeIndex);
+	if (node2.isStaticBody())
+	{
+		desc.bodyB = &mWorldSolverBody;
+		desc.bodyBDataIndex = 0;
+		desc.linkIndexB = PxSolverConstraintDesc::NO_LINK;
+	}
+	else
+	{
+		const IG::Node& node = islandSim.getNode(node2);
+		if (node.getNodeType() == IG::Node::eARTICULATION_TYPE)
+		{
+			Dy::Articulation* a = islandSim.getLLArticulation(node2);
+			desc.articulationB = a->getFsDataPtr();
+			desc.articulationBLength = Ps::to16(a->getSolverDataSize());
+			PX_ASSERT(0 == (desc.articulationBLength & 0x0f));
+			desc.linkIndexB = Ps::to16(node2.articulationLinkId());
+		}
+		else
+		{
+			PxU32 activeIndex = islandSim.getActiveNodeIndex(node2);
+			PxU32 index = node.isKinematic() ? activeIndex : bodyRemap[activeIndex] + solverBodyOffset;
+			desc.bodyB = &mSolverBodyPool[index];
+			desc.bodyBDataIndex = Ps::to16(index + 1);
+			desc.linkIndexB = PxSolverConstraintDesc::NO_LINK;
+		}
+	}
+}
+
+
+class PxsPreIntegrateTask : public Cm::Task
+{
+	PxsPreIntegrateTask& operator=(const PxsPreIntegrateTask&);
+public:
+	PxsPreIntegrateTask(	DynamicsContext&		context,
+							PxsBodyCore*const*		bodyArray,
+							PxsRigidBody*const*		originalBodyArray,
+							PxU32 const*				nodeIndexArray,
+							PxSolverBody*					solverBodies,
+							PxSolverBodyData*				solverBodyDataPool,
+							PxF32						dt,
+							PxU32						numBodies,
+							volatile PxU32*				maxSolverPositionIterations,
+							volatile PxU32*				maxSolverVelocityIterations,
+							const PxU32					startIndex,
+							const PxU32					numToIntegrate,
+							const PxVec3&				gravity) :
+		mContext(context),
+		mBodyArray(bodyArray),
+		mOriginalBodyArray(originalBodyArray),
+		mNodeIndexArray(nodeIndexArray),
+		mSolverBodies(solverBodies),
+		mSolverBodyDataPool(solverBodyDataPool),
+		mDt(dt),
+		mNumBodies(numBodies),
+		mMaxSolverPositionIterations(maxSolverPositionIterations),
+		mMaxSolverVelocityIterations(maxSolverVelocityIterations),
+		mStartIndex(startIndex),
+		mNumToIntegrate(numToIntegrate),
+		mGravity(gravity)
+	{}
+
+	virtual void runInternal();
+
+	virtual const char* getName() const
+	{
+		return "PxsDynamics.preIntegrate";
+	}
+
+public:
+	DynamicsContext&			mContext;
+	PxsBodyCore*const*			mBodyArray;
+	PxsRigidBody*const*			mOriginalBodyArray;
+	PxU32 const*			mNodeIndexArray;
+	PxSolverBody*				mSolverBodies;
+	PxSolverBodyData*			mSolverBodyDataPool;
+	PxF32						mDt;
+	PxU32						mNumBodies;
+	volatile PxU32*				mMaxSolverPositionIterations;
+	volatile PxU32*				mMaxSolverVelocityIterations;
+	PxU32						mStartIndex;
+	PxU32						mNumToIntegrate;
+	PxVec3						mGravity;
+
+};
+
+
+
+class PxsParallelSolverTask : public Cm::Task
+{
+	PxsParallelSolverTask& operator=(PxsParallelSolverTask&);
+public:
+
+	PxsParallelSolverTask(SolverIslandParams& params, DynamicsContext& context, PxFrictionType::Enum frictionType, IG::IslandSim& islandSim)
+		: mParams(params), mContext(context), mFrictionType(frictionType), mIslandSim(islandSim)
+	{
+	}
+
+	virtual void runInternal()
+	{
+		solveParallel(mContext, mParams, mIslandSim);
+	}
+
+	virtual const char* getName() const
+	{
+		return "PxsDynamics.parallelSolver";
+	}
+
+	SolverIslandParams&						mParams;
+	DynamicsContext&						mContext;
+	PxFrictionType::Enum					mFrictionType;
+	IG::IslandSim&							mIslandSim;
+};
+
+
+#define PX_CONTACT_REDUCTION 1
+
+class PxsSolverConstraintPostProcessTask : public Cm::Task
+{
+	PxsSolverConstraintPostProcessTask& operator=(const PxsSolverConstraintPostProcessTask&);
+public:
+
+	PxsSolverConstraintPostProcessTask(DynamicsContext& context,
+		ThreadContext& threadContext,
+		const SolverIslandObjects& objects,				  
+		const PxU32 solverBodyOffset,
+		PxU32 startIndex,
+		PxU32 stride,
+		PxsMaterialManager* materialManager,
+		PxsContactManagerOutputIterator& iterator) :
+		mContext(context), 
+		mThreadContext(threadContext),
+		mObjects(objects),
+		mSolverBodyOffset(solverBodyOffset),
+		mStartIndex(startIndex),
+		mStride(stride),
+		mMaterialManager(materialManager),
+		mOutputs(iterator)
+	{}
+
+	void mergeContacts(CompoundContactManager& header, ThreadContext& threadContext)
+	{
+		Gu::ContactBuffer& buffer = threadContext.mContactBuffer;
+		PxsMaterialInfo materialInfo[Gu::ContactBuffer::MAX_CONTACTS];
+		PxU32 size = 0;
+
+		for(PxU32 a = 0; a < header.mStride; ++a)
+		{
+			PxsContactManager* manager = mThreadContext.orderedContactList[a+header.mStartIndex]->contactManager;
+			PxcNpWorkUnit& unit = manager->getWorkUnit();
+			PxsContactManagerOutput& output = mOutputs.getContactManager(unit.mNpIndex);
+			PxContactStreamIterator iter(output.contactPatches, output.contactPoints, output.getInternalFaceIndice(), output.nbPatches, output.nbContacts);
+
+			PxU32 origSize = size;
+			PX_UNUSED(origSize);
+			if(!iter.forceNoResponse)
+			{
+				while(iter.hasNextPatch())
+				{
+					iter.nextPatch();
+					while(iter.hasNextContact())
+					{
+						PX_ASSERT(size < Gu::ContactBuffer::MAX_CONTACTS);
+						iter.nextContact();
+						PxsMaterialInfo& info = materialInfo[size];
+						Gu::ContactPoint& point = buffer.contacts[size++];
+						point.dynamicFriction = iter.getDynamicFriction();
+						point.staticFriction = iter.getStaticFriction();
+						point.restitution = iter.getRestitution();
+						point.internalFaceIndex1 = iter.getFaceIndex1();
+						point.materialFlags = PxU8(iter.getMaterialFlags());
+						point.maxImpulse = iter.getMaxImpulse();
+						point.targetVel = iter.getTargetVel();
+						point.normal = iter.getContactNormal();
+						point.point = iter.getContactPoint();
+						point.separation = iter.getSeparation();
+						info.mMaterialIndex0 = iter.getMaterialIndex0();
+						info.mMaterialIndex1 = iter.getMaterialIndex1();
+					}
+				}
+				PX_ASSERT(output.nbContacts == (size - origSize));
+			}	
+		}
+
+		PxU32 origSize = size;
+#if PX_CONTACT_REDUCTION
+		ContactReduction<6> reduction(buffer.contacts, materialInfo, size);
+		reduction.reduceContacts();
+		//OK, now we write back the contacts...
+
+		PxU8 histo[Gu::ContactBuffer::MAX_CONTACTS];
+		PxMemZero(histo, sizeof(histo));
+
+		size = 0;
+		for(PxU32 a = 0; a < reduction.mNumPatches; ++a)
+		{
+			ReducedContactPatch& patch = reduction.mPatches[a];
+			for(PxU32 b = 0; b < patch.numContactPoints; ++b)
+			{
+				histo[patch.contactPoints[b]] = 1;
+				++size;
+			}
+		}
+#endif
+
+		PxU16* PX_RESTRICT data = reinterpret_cast<PxU16*>(threadContext.mConstraintBlockStream.reserve(size * sizeof(PxU16), mThreadContext.mConstraintBlockManager));
+		header.forceBufferList = data;
+		
+
+#if PX_CONTACT_REDUCTION
+		const PxU32 reservedSize = size;
+		PX_UNUSED(reservedSize);
+		size = 0;
+		for(PxU32 a = 0; a < origSize; ++a)
+		{
+			if(histo[a])
+			{
+				if(size != a)
+				{
+					buffer.contacts[size] = buffer.contacts[a];
+					materialInfo[size] = materialInfo[a];
+				}
+				data[size] = Ps::to16(a);
+				size++;
+			}
+		}
+		PX_ASSERT(reservedSize >= size);
+#else
+		for(PxU32 a = 0; a < size; ++a)
+			data[a] = a;
+#endif
+
+
+		PxU32 contactForceByteSize = size * sizeof(PxReal);
+
+
+		PxsContactManagerOutput& output = mOutputs.getContactManager(header.unit->mNpIndex);
+
+		PxU16 compressedContactSize;
+
+		physx::writeCompressedContact(buffer.contacts, size, NULL, output.nbContacts, output.contactPatches, output.contactPoints, compressedContactSize,
+			reinterpret_cast<PxReal*&>(output.contactForces), contactForceByteSize, mMaterialManager, false, 
+			false, materialInfo, output.nbPatches, 0, &mThreadContext.mConstraintBlockManager, &threadContext.mConstraintBlockStream, false);
+	}
+
+	virtual void runInternal()
+	{
+		PxU32 endIndex = mStartIndex + mStride;
+
+		ThreadContext* threadContext = mContext.getThreadContext();
+		//TODO - we need to do this somewhere else
+		//threadContext->mContactBlockStream.reset();
+		threadContext->mConstraintBlockStream.reset();
+
+		for(PxU32 a = mStartIndex; a < endIndex; ++a)
+		{
+			mergeContacts(mThreadContext.compoundConstraints[a], *threadContext);
+		}
+		mContext.putThreadContext(threadContext);
+	}
+
+	virtual const char* getName() const { return "PxsDynamics.solverConstraintPostProcess"; }
+
+
+	DynamicsContext&			mContext;
+	ThreadContext&			mThreadContext;
+	const SolverIslandObjects		mObjects;
+	PxU32						mSolverBodyOffset;
+	PxU32						mStartIndex;
+	PxU32						mStride;
+	PxsMaterialManager*			mMaterialManager;
+	PxsContactManagerOutputIterator& mOutputs;
+};
+
+class PxsForceThresholdTask  : public Cm::Task
+{
+	DynamicsContext&		mDynamicsContext;
+
+	PxsForceThresholdTask& operator=(const PxsForceThresholdTask&);
+public:
+
+	PxsForceThresholdTask(DynamicsContext& context): mDynamicsContext(context) 
+	{
+	}
+
+	void createForceChangeThresholdStream()
+	{
+		ThresholdStream& thresholdStream = mDynamicsContext.getThresholdStream();
+		//bool haveThresholding = thresholdStream.size()!=0;
+
+		ThresholdTable& thresholdTable = mDynamicsContext.getThresholdTable();
+		thresholdTable.build(thresholdStream);
+
+		//generate current force exceeded threshold stream
+		ThresholdStream& curExceededForceThresholdStream = *mDynamicsContext.mExceededForceThresholdStream[mDynamicsContext.mCurrentIndex];
+		ThresholdStream& preExceededForceThresholdStream = *mDynamicsContext.mExceededForceThresholdStream[1 - mDynamicsContext.mCurrentIndex];
+		curExceededForceThresholdStream.forceSize_Unsafe(0);
+
+		//fill in the currrent exceeded force threshold stream
+		for(PxU32 i=0; i<thresholdTable.mPairsSize; ++i)
+		{
+			ThresholdTable::Pair& pair = thresholdTable.mPairs[i];
+			ThresholdStreamElement& elem = thresholdStream[pair.thresholdStreamIndex];
+			if(pair.accumulatedForce > elem.threshold * mDynamicsContext.mDt)
+			{
+				elem.accumulatedForce = pair.accumulatedForce;
+				curExceededForceThresholdStream.pushBack(elem);
+			}
+		}
+
+		ThresholdStream& forceChangeThresholdStream = mDynamicsContext.getForceChangedThresholdStream();
+		forceChangeThresholdStream.forceSize_Unsafe(0);
+		Ps::Array<PxU32>& forceChangeMask = mDynamicsContext.mExceededForceThresholdStreamMask;
+
+		const PxU32 nbPreExceededForce = preExceededForceThresholdStream.size();
+		const PxU32 nbCurExceededForce = curExceededForceThresholdStream.size();
+
+		//generate force change thresholdStream
+		if(nbPreExceededForce)
+		{
+			thresholdTable.build(preExceededForceThresholdStream);
+
+			//set force change mask
+			const PxU32 nbTotalExceededForce = nbPreExceededForce + nbCurExceededForce;
+			forceChangeMask.reserve(nbTotalExceededForce);
+			forceChangeMask.forceSize_Unsafe(nbTotalExceededForce);
+			
+			//initialize the forceChangeMask
+			for (PxU32 i = 0; i < nbTotalExceededForce; ++i)
+				forceChangeMask[i] = 1;
+
+			for(PxU32 i=0; i< nbCurExceededForce; ++i)
+			{
+				ThresholdStreamElement& curElem = curExceededForceThresholdStream[i];
+				
+				PxU32 pos;
+				if(thresholdTable.check(preExceededForceThresholdStream, curElem, pos))
+				{
+					forceChangeMask[pos] = 0;
+					forceChangeMask[i + nbPreExceededForce] = 0;
+				}
+			}
+
+			//create force change threshold stream
+			for(PxU32 i=0; i<nbTotalExceededForce; ++i)
+			{
+				const PxU32 hasForceChange = forceChangeMask[i];
+				if(hasForceChange)
+				{
+					bool lostPair = (i < nbPreExceededForce);
+					ThresholdStreamElement& elem = lostPair ?  preExceededForceThresholdStream[i] : curExceededForceThresholdStream[i - nbPreExceededForce];
+					ThresholdStreamElement elt;
+					elt = elem;
+					elt.accumulatedForce = lostPair ? 0.f : elem.accumulatedForce;
+					forceChangeThresholdStream.pushBack(elt);
+				}
+				else
+				{
+					//persistent pair
+					if (i < nbPreExceededForce)
+					{
+						ThresholdStreamElement& elem = preExceededForceThresholdStream[i];
+						ThresholdStreamElement elt;
+						elt = elem;
+						elt.accumulatedForce = elem.accumulatedForce;
+						forceChangeThresholdStream.pushBack(elt);
+					}
+				}
+			}
+		}
+		else
+		{
+			forceChangeThresholdStream.reserve(nbCurExceededForce);
+			forceChangeThresholdStream.forceSize_Unsafe(nbCurExceededForce);
+			PxMemCopy(forceChangeThresholdStream.begin(), curExceededForceThresholdStream.begin(), sizeof(ThresholdStreamElement) * nbCurExceededForce);
+		}
+	}
+
+	virtual void runInternal()
+	{
+		mDynamicsContext.getThresholdStream().forceSize_Unsafe(PxU32(mDynamicsContext.mThresholdStreamOut));
+		createForceChangeThresholdStream();
+	}
+
+	virtual const char* getName() const { return "PxsDynamics.createForceChangeThresholdStream"; }
+};
+
+
+struct ConstraintLess
+{
+	bool operator()(const PxSolverConstraintDesc& left, const PxSolverConstraintDesc& right) const
+	{
+		return reinterpret_cast<Constraint*>(left.constraint)->index > reinterpret_cast<Constraint*>(right.constraint)->index;
+	}
+};
+
+struct ArticulationSortPredicate
+{
+	bool operator()(const PxsIndexedContactManager*& left, const PxsIndexedContactManager*& right) const
+	{
+		return left->contactManager->getWorkUnit().index < right->contactManager->getWorkUnit().index;
+	}
+};
+
+class SolverArticulationUpdateTask : public Cm::Task
+{
+	
+
+	ThreadContext& mIslandThreadContext;
+
+	Articulation** mArticulations;
+	ArticulationSolverDesc* mArticulationDescArray;
+	PxU32 mNbToProcess;
+
+	Dy::DynamicsContext& mContext;
+	PxU32 mStartIdx;
+
+public:
+
+	static const PxU32 NbArticulationsPerTask = 8;
+
+	SolverArticulationUpdateTask(ThreadContext& islandThreadContext, Articulation** articulations, ArticulationSolverDesc* articulationDescArray, PxU32 nbToProcess, Dy::DynamicsContext& context,
+		PxU32 startIdx):
+		mIslandThreadContext(islandThreadContext), mArticulations(articulations), mArticulationDescArray(articulationDescArray), mNbToProcess(nbToProcess), mContext(context), mStartIdx(startIdx)
+	{
+	}
+
+	virtual const char* getName() const { return "SolverArticulationUpdateTask"; }
+
+	virtual void runInternal()
+	{
+		ThreadContext& threadContext = *mContext.getThreadContext();
+
+		threadContext.mConstraintBlockStream.reset(); //Clear in case there's some left-over memory in this context, for which the block has already been freed 
+		PxU32 maxVelIters = 0;
+		PxU32 maxPosIters = 0;
+		PxU32 maxArticulationLength = 0;
+		PxU32 maxSolverArticLength = 0;
+
+		PxU32 startIdx = mStartIdx;
+		for(PxU32 i=0;i<mNbToProcess; i++)
+		{
+			Articulation& a = *(mArticulations[i]);
+			a.getSolverDesc(mArticulationDescArray[i]);
+
+			PxU32 acCount, descCount;
+			
+			descCount = ArticulationPImpl::computeUnconstrainedVelocities(mArticulationDescArray[i], mContext.mDt, threadContext.mConstraintBlockStream, 
+				mIslandThreadContext.mContactDescPtr + startIdx, acCount, mContext.getScratchAllocator(), 
+				mIslandThreadContext.mConstraintBlockManager, mContext.getGravity(), mContext.getContextId());
+
+			mArticulationDescArray[i].numInternalConstraints = Ps::to8(descCount);
+
+			maxArticulationLength = PxMax(maxArticulationLength, PxU32(mArticulationDescArray[i].totalDataSize));
+			maxSolverArticLength = PxMax(maxSolverArticLength, PxU32(mArticulationDescArray[i].solverDataSize));
+
+			const PxU16 iterWord = a.getIterationCounts();
+			maxVelIters = PxMax<PxU32>(PxU32(iterWord >> 8),	maxVelIters);
+			maxPosIters = PxMax<PxU32>(PxU32(iterWord & 0xff), maxPosIters);
+			startIdx += DY_ARTICULATION_MAX_SIZE;
+		}
+		Ps::atomicMax(reinterpret_cast<PxI32*>(&mIslandThreadContext.mMaxSolverPositionIterations), PxI32(maxPosIters));
+		Ps::atomicMax(reinterpret_cast<PxI32*>(&mIslandThreadContext.mMaxSolverVelocityIterations), PxI32(maxVelIters));
+		Ps::atomicMax(reinterpret_cast<PxI32*>(&mIslandThreadContext.mMaxArticulationLength), PxI32(maxArticulationLength));
+		Ps::atomicMax(reinterpret_cast<PxI32*>(&mIslandThreadContext.mMaxArticulationSolverLength), PxI32(maxSolverArticLength));
+
+		mContext.putThreadContext(&threadContext);
+	}
+
+private:
+	PX_NOCOPY(SolverArticulationUpdateTask)
+};
+
+
+struct EnhancedSortPredicate
+{
+	bool operator()(const PxsIndexedContactManager& left, const PxsIndexedContactManager& right) const
+	{
+		PxcNpWorkUnit& unit0 = left.contactManager->getWorkUnit();
+		PxcNpWorkUnit& unit1 = right.contactManager->getWorkUnit();
+		return (unit0.mTransformCache0 < unit1.mTransformCache0) ||
+			((unit0.mTransformCache0 == unit1.mTransformCache0) && (unit0.mTransformCache1 < unit1.mTransformCache1));
+	}
+};
+
+
+class PxsSolverStartTask : public Cm::Task
+{
+	PxsSolverStartTask& operator=(const PxsSolverStartTask&);
+public:
+
+	PxsSolverStartTask(DynamicsContext& context,
+		IslandContext& islandContext,
+		const SolverIslandObjects& objects,
+		const PxU32 solverBodyOffset,
+		const PxU32 kinematicCount,
+		IG::SimpleIslandManager& islandManager,
+		PxU32* bodyRemapTable,
+		PxsMaterialManager* materialManager,
+		PxsContactManagerOutputIterator& iterator,
+		bool enhancedDeterminism
+		) :
+		mContext				(context), 
+		mIslandContext			(islandContext),
+		mObjects				(objects),
+		mSolverBodyOffset		(solverBodyOffset),
+		mKinematicCount			(kinematicCount),
+		mIslandManager			(islandManager),
+		mBodyRemapTable			(bodyRemapTable),
+		mMaterialManager		(materialManager),
+		mOutputs				(iterator),
+		mEnhancedDeterminism	(enhancedDeterminism)
+	{}
+
+	void startTasks()
+	{
+		PX_PROFILE_ZONE("Dynamics.solveGroup", mContext.getContextId());
+		{
+			ThreadContext& mThreadContext = *mContext.getThreadContext();
+
+			mIslandContext.mThreadContext = &mThreadContext;
+
+			mThreadContext.mMaxSolverPositionIterations = 0;
+			mThreadContext.mMaxSolverVelocityIterations = 0;
+			mThreadContext.mAxisConstraintCount = 0;
+			mThreadContext.mContactDescPtr = mThreadContext.contactConstraintDescArray;
+			mThreadContext.mFrictionDescPtr = mThreadContext.frictionConstraintDescArray.begin();
+			mThreadContext.mNumDifferentBodyConstraints = 0;
+			mThreadContext.mNumSelfConstraintBlocks = 0;
+			mThreadContext.mNumSelfConstraints = 0;
+			mThreadContext.mNumDifferentBodyFrictionConstraints = 0;
+			mThreadContext.mNumSelfConstraintFrictionBlocks = 0;
+			mThreadContext.mNumSelfFrictionConstraints = 0;
+			mThreadContext.numContactConstraintBatches = 0;
+			mThreadContext.contactDescArraySize = 0;
+
+
+			mThreadContext.contactConstraintDescArray = mObjects.constraintDescs;
+			mThreadContext.orderedContactConstraints = mObjects.orderedConstraintDescs;
+			mThreadContext.mContactDescPtr = mObjects.constraintDescs;
+			mThreadContext.tempConstraintDescArray = mObjects.tempConstraintDescs;
+			mThreadContext.contactConstraintBatchHeaders = mObjects.constraintBatchHeaders;
+			mThreadContext.motionVelocityArray = mObjects.motionVelocities;
+			mThreadContext.mBodyCoreArray = mObjects.bodyCoreArray;
+			mThreadContext.mRigidBodyArray = mObjects.bodies;
+			mThreadContext.mArticulationArray = mObjects.articulations;
+			mThreadContext.bodyRemapTable = mObjects.bodyRemapTable;
+			mThreadContext.mNodeIndexArray = mObjects.nodeIndexArray;
+
+			const PxU32 frictionConstraintCount = mContext.getFrictionType() == PxFrictionType::ePATCH ? 0 : PxU32(mIslandContext.mCounts.contactManagers);
+			mThreadContext.resizeArrays(frictionConstraintCount, mIslandContext.mCounts.articulations);
+
+			PxsBodyCore** PX_RESTRICT bodyArrayPtr = mThreadContext.mBodyCoreArray;
+			PxsRigidBody** PX_RESTRICT rigidBodyPtr = mThreadContext.mRigidBodyArray;
+			Articulation** PX_RESTRICT articulationPtr = mThreadContext.mArticulationArray;
+			PxU32* PX_RESTRICT bodyRemapTable = mThreadContext.bodyRemapTable;
+			PxU32* PX_RESTRICT nodeIndexArray = mThreadContext.mNodeIndexArray;
+
+			PxU32 nbIslands = mObjects.numIslands;
+			const IG::IslandId* const islandIds = mObjects.islandIds;
+
+			const IG::IslandSim& islandSim = mIslandManager.getAccurateIslandSim();
+
+			PxU32 bodyIndex = 0, articIndex = 0;
+			for(PxU32 i = 0; i < nbIslands; ++i)
+			{
+				const IG::Island& island = islandSim.getIsland(islandIds[i]);
+
+				IG::NodeIndex currentIndex = island.mRootNode;
+
+				while(currentIndex.isValid())
+				{
+					const IG::Node& node = islandSim.getNode(currentIndex);
+
+					if(node.getNodeType() == IG::Node::eARTICULATION_TYPE)
+					{
+						articulationPtr[articIndex++] = node.getArticulation();
+					}
+					else
+					{
+						PxsRigidBody* rigid = node.getRigidBody();
+						PX_ASSERT(bodyIndex < (mIslandContext.mCounts.bodies + mContext.mKinematicCount + 1));
+						rigidBodyPtr[bodyIndex] = rigid;
+						bodyArrayPtr[bodyIndex] = &rigid->getCore();
+						nodeIndexArray[bodyIndex] = currentIndex.index();
+						bodyRemapTable[islandSim.getActiveNodeIndex(currentIndex)] = bodyIndex++;
+					}
+
+					currentIndex = node.mNextNode;
+				}
+			}
+
+
+			PxsIndexedContactManager* indexedManagers = mObjects.contactManagers;
+
+			PxU32 currentContactIndex = 0;
+			for(PxU32 i = 0; i < nbIslands; ++i)
+			{
+				const IG::Island& island = islandSim.getIsland(islandIds[i]);
+
+				IG::EdgeIndex contactEdgeIndex = island.mFirstEdge[IG::Edge::eCONTACT_MANAGER];
+
+				while(contactEdgeIndex != IG_INVALID_EDGE)
+				{
+					const IG::Edge& edge = islandSim.getEdge(contactEdgeIndex);
+
+					PxsContactManager* contactManager = mIslandManager.getContactManager(contactEdgeIndex);
+
+					if(contactManager)
+					{
+						const IG::NodeIndex nodeIndex1 = islandSim.getNodeIndex1(contactEdgeIndex);
+						const IG::NodeIndex nodeIndex2 = islandSim.getNodeIndex2(contactEdgeIndex);
+
+						PxsIndexedContactManager& indexedManager = indexedManagers[currentContactIndex++];
+						indexedManager.contactManager = contactManager;
+
+						PX_ASSERT(!nodeIndex1.isStaticBody());
+						{
+							const IG::Node& node1 = islandSim.getNode(nodeIndex1);
+
+							//Is it an articulation or not???
+							if(node1.getNodeType() == IG::Node::eARTICULATION_TYPE)
+							{
+								indexedManager.indexType0 = PxsIndexedInteraction::eARTICULATION;
+								indexedManager.solverBody0 = size_t(node1.getArticulation()) | nodeIndex1.articulationLinkId();
+							}
+							else
+							{
+								if(node1.isKinematic())
+								{
+									indexedManager.indexType0 = PxsIndexedInteraction::eKINEMATIC;
+									indexedManager.solverBody0 = islandSim.getActiveNodeIndex(nodeIndex1);
+								}
+								else
+								{
+									indexedManager.indexType0 = PxsIndexedInteraction::eBODY;
+									indexedManager.solverBody0 = bodyRemapTable[islandSim.getActiveNodeIndex(nodeIndex1)];
+								}
+								PX_ASSERT(indexedManager.solverBody0 < (mIslandContext.mCounts.bodies + mContext.mKinematicCount + 1));
+							}
+
+						}
+
+						if(nodeIndex2.isStaticBody())
+						{
+							indexedManager.indexType1 = PxsIndexedInteraction::eWORLD;
+						}
+						else
+						{
+							const IG::Node& node2 = islandSim.getNode(nodeIndex2);
+
+							//Is it an articulation or not???
+							if(node2.getNodeType() == IG::Node::eARTICULATION_TYPE)
+							{
+								indexedManager.indexType1 = PxsIndexedInteraction::eARTICULATION;
+								indexedManager.solverBody1 = size_t(node2.getArticulation()) | nodeIndex2.articulationLinkId();
+							}
+							else
+							{
+								if(node2.isKinematic())
+								{
+									indexedManager.indexType1 = PxsIndexedInteraction::eKINEMATIC;
+									indexedManager.solverBody1 = islandSim.getActiveNodeIndex(nodeIndex2);
+								}
+								else
+								{
+									indexedManager.indexType1 = PxsIndexedInteraction::eBODY;
+									indexedManager.solverBody1 = bodyRemapTable[islandSim.getActiveNodeIndex(nodeIndex2)];
+								}
+								PX_ASSERT(indexedManager.solverBody1 < (mIslandContext.mCounts.bodies + mContext.mKinematicCount + 1));
+							}
+						}
+
+					}
+					contactEdgeIndex = edge.mNextIslandEdge;
+				}
+			}
+
+			if (mEnhancedDeterminism)
+			{
+				Ps::sort(indexedManagers, currentContactIndex, EnhancedSortPredicate());
+			}
+
+			mIslandContext.mCounts.contactManagers = currentContactIndex;
+		}
+	}
+
+	void integrate()
+	{
+		ThreadContext& mThreadContext = *mIslandContext.mThreadContext;
+		PxSolverBody* solverBodies = mContext.mSolverBodyPool.begin() + mSolverBodyOffset;
+		PxSolverBodyData* solverBodyData = mContext.mSolverBodyDataPool.begin() + mSolverBodyOffset;
+
+		{			
+			PX_PROFILE_ZONE("Dynamics.updateVelocities", mContext.getContextId());
+
+			mContext.preIntegrationParallel(	
+				mContext.mDt,
+				mThreadContext.mBodyCoreArray,
+				mObjects.bodies,
+				mThreadContext.mNodeIndexArray,
+				mIslandContext.mCounts.bodies,
+				solverBodies,
+				solverBodyData,
+				mThreadContext.motionVelocityArray,
+				mThreadContext.mMaxSolverPositionIterations,
+				mThreadContext.mMaxSolverVelocityIterations,
+				*mCont
+				);
+		}
+	}
+
+	void articulationTask()
+	{
+		ThreadContext& mThreadContext = *mIslandContext.mThreadContext;
+		ArticulationSolverDesc* articulationDescArray = mThreadContext.getArticulations().begin();
+
+		for(PxU32 i=0;i<mIslandContext.mCounts.articulations; i+= SolverArticulationUpdateTask::NbArticulationsPerTask)
+		{
+
+			SolverArticulationUpdateTask* task = PX_PLACEMENT_NEW(mContext.getTaskPool().allocate(sizeof(SolverArticulationUpdateTask)), SolverArticulationUpdateTask)(mThreadContext, 
+				&mObjects.articulations[i], &articulationDescArray[i], PxMin(SolverArticulationUpdateTask::NbArticulationsPerTask, mIslandContext.mCounts.articulations - i), mContext,
+				i*DY_ARTICULATION_MAX_SIZE);
+
+			task->setContinuation(mCont);
+			task->removeReference();
+	
+		}
+	}
+
+	void setupDescTask()
+	{
+		ThreadContext& mThreadContext = *mIslandContext.mThreadContext;
+		PxSolverConstraintDesc* contactDescPtr = mThreadContext.mContactDescPtr;
+
+		//PxU32 constraintCount = mCounts.constraints + mCounts.contactManagers;
+
+		PxU32 nbIslands = mObjects.numIslands;
+		const IG::IslandId* const islandIds = mObjects.islandIds;
+
+		const IG::IslandSim& islandSim = mIslandManager.getAccurateIslandSim();
+
+		for(PxU32 i = 0; i < nbIslands; ++i)
+		{
+			const IG::Island& island = islandSim.getIsland(islandIds[i]);
+
+			IG::EdgeIndex edgeId = island.mFirstEdge[IG::Edge::eCONSTRAINT];
+
+			while(edgeId != IG_INVALID_EDGE)
+			{
+				PxSolverConstraintDesc& desc = *contactDescPtr;
+				
+				const IG::Edge& edge = islandSim.getEdge(edgeId);
+				Dy::Constraint* constraint = mIslandManager.getConstraint(edgeId);
+				mContext.setDescFromIndices(desc, edgeId, mIslandManager, mBodyRemapTable, mSolverBodyOffset);
+				desc.constraint = reinterpret_cast<PxU8*>(constraint);
+				desc.constraintLengthOver16 = DY_SC_TYPE_RB_1D;
+				contactDescPtr++;
+				edgeId = edge.mNextIslandEdge;
+			}
+
+		}
+
+#if 1
+		Ps::sort(mThreadContext.mContactDescPtr, PxU32(contactDescPtr - mThreadContext.mContactDescPtr), ConstraintLess());
+#endif
+
+
+		mThreadContext.orderedContactList.forceSize_Unsafe(0);
+		mThreadContext.orderedContactList.reserve(mIslandContext.mCounts.contactManagers);
+		mThreadContext.orderedContactList.forceSize_Unsafe(mIslandContext.mCounts.contactManagers);
+		mThreadContext.tempContactList.forceSize_Unsafe(0);
+		mThreadContext.tempContactList.reserve(mIslandContext.mCounts.contactManagers);
+		mThreadContext.tempContactList.forceSize_Unsafe(mIslandContext.mCounts.contactManagers);
+
+		const PxsIndexedContactManager** constraints = mThreadContext.orderedContactList.begin();
+
+
+		//OK, we sort the orderedContactList 
+
+		mThreadContext.compoundConstraints.forceSize_Unsafe(0);
+		if(mIslandContext.mCounts.contactManagers)
+		{
+			{
+				mThreadContext.sortIndexArray.forceSize_Unsafe(0);
+
+				PX_COMPILE_TIME_ASSERT(PxsIndexedInteraction::eBODY == 0);
+				PX_COMPILE_TIME_ASSERT(PxsIndexedInteraction::eKINEMATIC == 1);
+
+				const PxI32 offsetMap[] = {PxI32(mContext.mKinematicCount), 0};
+
+				const PxU32 totalBodies = mContext.mKinematicCount + mIslandContext.mCounts.bodies+1;
+
+				mThreadContext.sortIndexArray.reserve(totalBodies);
+				mThreadContext.sortIndexArray.forceSize_Unsafe(totalBodies);
+				PxMemZero(mThreadContext.sortIndexArray.begin(), totalBodies * 4);
+
+				//Iterate over the array based on solverBodyDatapool, creating a list of sorted constraints (in order of body pair)
+				//We only do this with contacts. It's important that this is done this way because we don't want to break our rules that all joints
+				//appear before all contacts in the constraint list otherwise we will lose all guarantees about sorting joints.
+				
+				for(PxU32 a = 0; a < mIslandContext.mCounts.contactManagers; ++a)
+				{
+					PX_ASSERT(mObjects.contactManagers[a].indexType0 != PxsIndexedInteraction::eWORLD);
+					//Index first body...
+					PxU8 indexType = mObjects.contactManagers[a].indexType0;
+					if(indexType != PxsIndexedInteraction::eARTICULATION && mObjects.contactManagers[a].indexType1 != PxsIndexedInteraction::eARTICULATION)
+					{
+						PX_ASSERT((indexType == PxsIndexedInteraction::eBODY) || (indexType == PxsIndexedInteraction::eKINEMATIC));
+
+						PxI32 index = PxI32(mObjects.contactManagers[a].solverBody0 + offsetMap[indexType]);
+						PX_ASSERT(index >= 0);
+						mThreadContext.sortIndexArray[PxU32(index)]++;
+					}
+				}
+
+				PxU32 accumulatedCount = 0;
+
+				for(PxU32 a = mThreadContext.sortIndexArray.size(); a > 0; --a)
+				{
+					PxU32 ind = a - 1;
+					PxU32 val = mThreadContext.sortIndexArray[ind];
+					mThreadContext.sortIndexArray[ind] = accumulatedCount;
+					accumulatedCount += val;
+				}
+
+				//OK, now copy across data to orderedConstraintDescs, pushing articulations to the end...
+				for(PxU32 a = 0; a < mIslandContext.mCounts.contactManagers; ++a)
+				{
+					//Index first body...
+					PxU8 indexType = mObjects.contactManagers[a].indexType0;
+					if(indexType != PxsIndexedInteraction::eARTICULATION && mObjects.contactManagers[a].indexType1 != PxsIndexedInteraction::eARTICULATION)
+					{
+						PX_ASSERT((indexType == PxsIndexedInteraction::eBODY) || (indexType == PxsIndexedInteraction::eKINEMATIC));
+
+						PxI32 index = PxI32(mObjects.contactManagers[a].solverBody0 + offsetMap[indexType]);
+						PX_ASSERT(index >= 0);
+						mThreadContext.tempContactList[mThreadContext.sortIndexArray[PxU32(index)]++] = &mObjects.contactManagers[a];
+					}
+					else
+					{
+						mThreadContext.tempContactList[accumulatedCount++] = &mObjects.contactManagers[a];
+					}
+				}
+
+				//Now do the same again with bodyB, being careful not to overwrite the joints
+				PxMemZero(mThreadContext.sortIndexArray.begin(), totalBodies * 4);
+
+
+				for(PxU32 a = 0; a < mIslandContext.mCounts.contactManagers; ++a)
+				{
+					//Index first body...
+					PxU8 indexType = mThreadContext.tempContactList[a]->indexType1;
+					if(indexType != PxsIndexedInteraction::eARTICULATION && mObjects.contactManagers[a].indexType0 != PxsIndexedInteraction::eARTICULATION)
+					{
+						PX_ASSERT((indexType == PxsIndexedInteraction::eBODY) || (indexType == PxsIndexedInteraction::eKINEMATIC) || (indexType == PxsIndexedInteraction::eWORLD));
+
+						PxI32 index = (indexType == PxsIndexedInteraction::eWORLD) ? 0 : PxI32(mThreadContext.tempContactList[a]->solverBody1 + offsetMap[indexType]);
+						PX_ASSERT(index >= 0);
+						mThreadContext.sortIndexArray[PxU32(index)]++;
+					}
+				}
+
+				accumulatedCount = 0;
+				for(PxU32 a = mThreadContext.sortIndexArray.size(); a > 0; --a)
+				{
+					PxU32 ind = a - 1;
+					PxU32 val = mThreadContext.sortIndexArray[ind];
+					mThreadContext.sortIndexArray[ind] = accumulatedCount;
+					accumulatedCount += val;
+				}
+
+				PxU32 articulationStartIndex = accumulatedCount;
+
+				//OK, now copy across data to orderedConstraintDescs, pushing articulations to the end...
+				for(PxU32 a = 0; a < mIslandContext.mCounts.contactManagers; ++a)
+				{
+					//Index first body...
+					PxU8 indexType = mThreadContext.tempContactList[a]->indexType1;
+					if(indexType != PxsIndexedInteraction::eARTICULATION && mObjects.contactManagers[a].indexType0 != PxsIndexedInteraction::eARTICULATION)
+					{
+						PX_ASSERT((indexType == PxsIndexedInteraction::eBODY) || (indexType == PxsIndexedInteraction::eKINEMATIC) || (indexType == PxsIndexedInteraction::eWORLD));
+
+						PxI32 index = (indexType == PxsIndexedInteraction::eWORLD) ? 0 : PxI32(mThreadContext.tempContactList[a]->solverBody1 + offsetMap[indexType]);
+						PX_ASSERT(index >= 0);
+						constraints[mThreadContext.sortIndexArray[PxU32(index)]++] = mThreadContext.tempContactList[a];
+					}
+					else
+					{
+						constraints[accumulatedCount++] = mThreadContext.tempContactList[a];
+					}
+				}
+
+#if 1
+				Ps::sort(constraints + articulationStartIndex, accumulatedCount - articulationStartIndex, ArticulationSortPredicate());
+#endif
+			}
+
+			mThreadContext.mStartContactDescPtr = contactDescPtr;
+
+			mThreadContext.compoundConstraints.reserve(1024);
+			mThreadContext.compoundConstraints.forceSize_Unsafe(0);
+			//mThreadContext.compoundConstraints.forceSize_Unsafe(mCounts.contactManagers);
+
+			PxSolverConstraintDesc* startDesc = contactDescPtr;
+			mContext.setDescFromIndices(*startDesc, *constraints[0], mSolverBodyOffset);
+			startDesc->constraint = reinterpret_cast<PxU8*>(constraints[0]->contactManager);
+			startDesc->constraintLengthOver16 = DY_SC_TYPE_RB_CONTACT;
+
+			PxsContactManagerOutput* startManagerOutput = &mOutputs.getContactManager(constraints[0]->contactManager->getWorkUnit().mNpIndex);
+			PxU32 contactCount = startManagerOutput->nbContacts;
+			PxU32 startIndex = 0;
+			PxU32 numHeaders = 0;
+			for(PxU32 a = 1; a < mIslandContext.mCounts.contactManagers; ++a)
+			{
+				PxSolverConstraintDesc& desc = *(contactDescPtr+1);
+				mContext.setDescFromIndices(desc, *constraints[a], mSolverBodyOffset);
+
+				PxsContactManager* manager = constraints[a]->contactManager;
+				PxsContactManagerOutput& output = mOutputs.getContactManager(manager->getWorkUnit().mNpIndex);
+
+				desc.constraint = reinterpret_cast<PxU8*>(constraints[a]->contactManager);
+				desc.constraintLengthOver16 = DY_SC_TYPE_RB_CONTACT;
+
+				if (contactCount == 0)
+				{
+					//This is the first object in the pair
+					*startDesc = *(contactDescPtr + 1);
+					startIndex = a;
+					startManagerOutput = &output;
+				}
+				
+				if(startDesc->bodyA != desc.bodyA || startDesc->bodyB != desc.bodyB 
+					|| startDesc->linkIndexA != PxSolverConstraintDesc::NO_LINK || startDesc->linkIndexB != PxSolverConstraintDesc::NO_LINK
+					|| contactCount + output.nbContacts > Gu::ContactBuffer::MAX_CONTACTS
+					|| manager->isChangeable()
+					) //If this is the first thing and no contacts...then we skip
+				{
+					PxU32 stride = a - startIndex;
+					if(contactCount > 0)
+					{
+						if(stride > 1)
+						{
+							++numHeaders;
+							CompoundContactManager& header = mThreadContext.compoundConstraints.insert();
+							header.mStartIndex = startIndex;
+							header.mStride = Ps::to16(stride);	
+							header.mReducedContactCount = Ps::to16(contactCount);
+							PxsContactManager* manager1 = constraints[startIndex]->contactManager;
+							PxcNpWorkUnit& unit = manager1->getWorkUnit();
+
+							PX_ASSERT(startManagerOutput == &mOutputs.getContactManager(unit.mNpIndex));
+
+							header.unit = &unit;
+							header.cmOutput = startManagerOutput;
+							header.originalContactPatches = startManagerOutput->contactPatches;
+							header.originalContactPoints = startManagerOutput->contactPoints;
+							header.originalContactCount = startManagerOutput->nbContacts;
+							header.originalPatchCount	= startManagerOutput->nbPatches;
+							header.originalForceBuffer = reinterpret_cast<PxReal*>(startManagerOutput->contactForces);
+							header.originalStatusFlags = startManagerOutput->statusFlag;
+						}
+						startDesc = ++contactDescPtr;
+					}
+					else
+					{
+						//Copy back next contactDescPtr
+						*startDesc = *(contactDescPtr+1);
+					}
+					contactCount = 0;
+					startIndex = a;
+					startManagerOutput = &output;
+				}
+				contactCount += output.nbContacts;
+				
+			}
+			PxU32 stride = mIslandContext.mCounts.contactManagers - startIndex;
+			if(contactCount > 0)
+			{
+				if(stride > 1)
+				{
+					++numHeaders;
+					CompoundContactManager& header = mThreadContext.compoundConstraints.insert();
+					header.mStartIndex = startIndex;
+					header.mStride = Ps::to16(stride);
+					header.mReducedContactCount = Ps::to16(contactCount);
+					PxsContactManager* manager = constraints[startIndex]->contactManager;
+					PxcNpWorkUnit& unit = manager->getWorkUnit();
+					header.unit = &unit;
+					header.cmOutput = startManagerOutput;
+					header.originalContactPatches = startManagerOutput->contactPatches;
+					header.originalContactPoints = startManagerOutput->contactPoints;
+					header.originalContactCount = startManagerOutput->nbContacts;
+					header.originalPatchCount	= startManagerOutput->nbPatches;
+					header.originalForceBuffer = reinterpret_cast<PxReal*>(startManagerOutput->contactForces);
+					header.originalStatusFlags = startManagerOutput->statusFlag;
+				}
+				contactDescPtr++;
+			}
+
+			if(numHeaders)
+			{
+				const PxU32 unrollSize = 8;
+				for(PxU32 a = 0; a < numHeaders; a+= unrollSize)
+				{
+					PxsSolverConstraintPostProcessTask* postProcessTask = PX_PLACEMENT_NEW( mContext.getTaskPool().allocate(sizeof(PxsSolverConstraintPostProcessTask)), 
+						PxsSolverConstraintPostProcessTask)(mContext, mThreadContext, mObjects, mSolverBodyOffset, a, PxMin(unrollSize, numHeaders - a), mMaterialManager,
+						mOutputs);
+					postProcessTask->setContinuation(mCont);
+					postProcessTask->removeReference();
+				}
+			}
+		}
+		mThreadContext.contactDescArraySize = PxU32(contactDescPtr - mThreadContext.contactConstraintDescArray);
+		mThreadContext.mContactDescPtr = contactDescPtr;
+	}
+
+	virtual void runInternal()
+	{
+		startTasks();
+		integrate();
+		setupDescTask();
+		articulationTask();
+	}
+
+	virtual const char* getName() const
+	{
+		return "PxsDynamics.solverStart";
+	}
+
+private:
+	DynamicsContext&			mContext;
+	IslandContext&				mIslandContext;
+	const SolverIslandObjects	mObjects;
+	const PxU32					mSolverBodyOffset;
+	const PxU32					mKinematicCount;
+	IG::SimpleIslandManager&	mIslandManager;
+	PxU32*						mBodyRemapTable;
+	PxsMaterialManager*			mMaterialManager;
+	PxsContactManagerOutputIterator& mOutputs;
+	bool						mEnhancedDeterminism;
+};
+
+class PxsSolverConstraintPartitionTask : public Cm::Task
+{
+	PxsSolverConstraintPartitionTask& operator=(const PxsSolverConstraintPartitionTask&);
+public:
+
+	PxsSolverConstraintPartitionTask(DynamicsContext& context,
+		IslandContext& islandContext,
+		const SolverIslandObjects& objects,				  
+		const PxU32 solverBodyOffset, bool enhancedDeterminism) :
+		mContext(context), 
+		mIslandContext(islandContext),
+		mObjects(objects),
+		mSolverBodyOffset(solverBodyOffset),
+		mEnhancedDeterminism(enhancedDeterminism)
+	{}
+
+	virtual void runInternal()
+	{
+
+		ThreadContext& mThreadContext = *mIslandContext.mThreadContext;
+
+		//Compact articulation pairs...
+		ArticulationSolverDesc* artics = mThreadContext.getArticulations().begin();
+
+		if(mIslandContext.mCounts.articulations)
+		{
+			PxU32 nbArticConstraints = artics[0].numInternalConstraints;
+
+			PxSolverConstraintDesc* currDesc = mThreadContext.mContactDescPtr;
+			for(PxU32 a = 1; a < mIslandContext.mCounts.articulations; ++a)
+			{
+				//Compact pairs...
+				const PxU32 nbInternalConstraints = artics[a].numInternalConstraints;
+				const PxU32 startIdx = a * DY_ARTICULATION_MAX_SIZE;
+				const PxU32 endIdx = startIdx + nbInternalConstraints;
+
+				for(PxU32 b = startIdx; b < endIdx; ++b)
+				{
+					currDesc[nbArticConstraints++] = currDesc[b];
+				}
+			}
+
+			mThreadContext.contactDescArraySize += nbArticConstraints;
+		}
+
+		PxSolverConstraintDesc* descBegin = mThreadContext.contactConstraintDescArray;
+		PxU32 descCount = mThreadContext.contactDescArraySize;
+
+		PxSolverBody* solverBodies = mContext.mSolverBodyPool.begin() + mSolverBodyOffset;
+		
+		mThreadContext.mNumDifferentBodyConstraints = descCount;
+
+		{
+			mThreadContext.mNumDifferentBodyConstraints = 0;
+			mThreadContext.mNumSelfConstraints = 0;
+			mThreadContext.mNumSelfConstraintBlocks = 0;
+			mThreadContext.mNumDifferentBodyFrictionConstraints = 0;
+			mThreadContext.mNumSelfConstraintFrictionBlocks = 0;
+			mThreadContext.mNumSelfFrictionConstraints = 0;
+
+			if(descCount > 0)
+			{
+				ConstraintPartitionArgs args;
+				args.mBodies = solverBodies;
+				args.mArticulationPtrs = artics;
+				args.mContactConstraintDescriptors = descBegin;
+				args.mNumArticulationPtrs = mThreadContext.getArticulations().size();
+				args.mNumBodies = mIslandContext.mCounts.bodies;
+				args.mNumContactConstraintDescriptors = descCount;
+				args.mOrderedContactConstraintDescriptors = mThreadContext.orderedContactConstraints;
+				args.mTempContactConstraintDescriptors = mThreadContext.tempConstraintDescArray;
+				args.mNumDifferentBodyConstraints = args.mNumSelfConstraints = args.mNumSelfConstraintBlocks = 0;
+				args.mConstraintsPerPartition = &mThreadContext.mConstraintsPerPartition;
+				args.mBitField = &mThreadContext.mPartitionNormalizationBitmap;
+				args.enhancedDeterminism = mEnhancedDeterminism;
+				
+				mThreadContext.mMaxPartitions = partitionContactConstraints(args);
+				mThreadContext.mNumDifferentBodyConstraints = args.mNumDifferentBodyConstraints;
+				mThreadContext.mNumSelfConstraints = args.mNumSelfConstraints;
+				mThreadContext.mNumSelfConstraintBlocks = args.mNumSelfConstraintBlocks;
+			}
+			else
+			{
+				PxMemZero(mThreadContext.mConstraintsPerPartition.begin(), sizeof(PxU32)*mThreadContext.mConstraintsPerPartition.capacity());
+			}
+
+			PX_ASSERT((mThreadContext.mNumDifferentBodyConstraints + mThreadContext.mNumSelfConstraints) == descCount);
+		}
+
+	}
+
+	virtual const char* getName() const { return "PxsDynamics.solverConstraintPartition"; }
+
+	DynamicsContext&			mContext;
+	IslandContext&			mIslandContext;
+	const SolverIslandObjects		mObjects;
+	PxU32						mSolverBodyOffset;
+	bool						mEnhancedDeterminism;
+};
+
+
+class PxsSolverSetupSolveTask : public Cm::Task
+{
+	PxsSolverSetupSolveTask& operator=(const PxsSolverSetupSolveTask&);
+public:
+
+	PxsSolverSetupSolveTask(
+		DynamicsContext& context,
+		IslandContext& islandContext,
+		const SolverIslandObjects& objects,				  
+		const PxU32 solverBodyOffset,
+		IG::IslandSim& islandSim) :
+		mContext(context), 
+		mIslandContext(islandContext),
+		mObjects(objects),
+		mSolverBodyOffset(solverBodyOffset),
+		mIslandSim(islandSim)
+	{}
+
+
+	virtual void runInternal()
+	{
+		ThreadContext& mThreadContext = *mIslandContext.mThreadContext;
+
+		PxSolverConstraintDesc* contactDescBegin = mThreadContext.orderedContactConstraints;
+		PxSolverConstraintDesc* contactDescPtr = mThreadContext.orderedContactConstraints;
+
+		PxSolverBody* solverBodies = mContext.mSolverBodyPool.begin() + mSolverBodyOffset;
+		PxSolverBodyData* solverBodyDatas = mContext.mSolverBodyDataPool.begin();
+
+		PxU32 frictionDescCount = mThreadContext.mNumDifferentBodyFrictionConstraints;
+
+		PxU32 j = 0, i = 0;
+		
+		//On PS3, self-constraints will be bumped to the end of the constraint list
+		//and processed separately. On PC/360, they will be mixed in the array and
+		//classed as "different body" constraints regardless of the fact that they're self-constraints.
+		//PxU32 numBatches = mThreadContext.numDifferentBodyBatchHeaders;
+		// TODO: maybe replace with non-null joints from end of the array
+
+		PxU32 numBatches = 0;
+
+		PxU32 currIndex = 0;
+		for(PxU32 a = 0; a < mThreadContext.mConstraintsPerPartition.size(); ++a)
+		{
+			PxU32 endIndex = currIndex + mThreadContext.mConstraintsPerPartition[a];
+
+			PxU32 numBatchesInPartition = 0;
+			for(PxU32 b = currIndex; b < endIndex; ++b)
+			{
+				PxConstraintBatchHeader& _header = mThreadContext.contactConstraintBatchHeaders[b];
+				PxU16 stride = _header.mStride, newStride = _header.mStride;
+				PxU32 startIndex = j;
+				for(PxU16 c = 0; c < stride; ++c)
+				{
+					if(getConstraintLength(contactDescBegin[i]) == 0)
+					{
+						newStride--;
+						i++;
+					}
+					else
+					{
+						if(i!=j)
+							contactDescBegin[j] = contactDescBegin[i];
+						i++;
+						j++;
+						contactDescPtr++;
+					}
+				}
+
+				if(newStride != 0)
+				{
+					mThreadContext.contactConstraintBatchHeaders[numBatches].mStartIndex = startIndex;
+					mThreadContext.contactConstraintBatchHeaders[numBatches].mStride = newStride;
+					PxU8 type = *contactDescBegin[startIndex].constraint;
+					if(type == DY_SC_TYPE_STATIC_CONTACT)
+					{
+						//Check if any block of constraints is classified as type static (single) contact constraint.
+						//If they are, iterate over all constraints grouped with it and switch to "dynamic" contact constraint
+						//type if there's a dynamic contact constraint in the group.
+						for(PxU32 c = 1; c < newStride; ++c)
+						{
+							if(*contactDescBegin[startIndex+c].constraint == DY_SC_TYPE_RB_CONTACT)
+							{
+								type = DY_SC_TYPE_RB_CONTACT;
+							}
+						}
+					}
+
+					mThreadContext.contactConstraintBatchHeaders[numBatches].mConstraintType = type;
+					numBatches++;
+					numBatchesInPartition++;
+				}
+			}
+			PxU32 numHeaders = numBatchesInPartition;
+			currIndex += mThreadContext.mConstraintsPerPartition[a];
+			mThreadContext.mConstraintsPerPartition[a] = numHeaders;
+		}
+
+		PxU32 contactDescCount = PxU32(contactDescPtr - contactDescBegin);
+
+		mThreadContext.mNumDifferentBodyConstraints = contactDescCount;
+
+		PxU32 numSelfConstraintBlocks = mThreadContext.mNumSelfConstraintBlocks;
+
+		//Remap self constraint array. Self-constraint blocks exists on PS3 as an optimization for SPU solver.
+		for(PxU32 a = 0; a < numSelfConstraintBlocks; ++a)
+		{
+			PX_ASSERT(mThreadContext.mSelfConstraintBlocks[a].startId == i);
+			PxU32 origNumSelfConstraints = mThreadContext.mSelfConstraintBlocks[a].numSelfConstraints;
+			PxU32 startId = j;
+
+			for(PxU32 b = 0; b < origNumSelfConstraints; ++b)
+			{
+				PxSolverConstraintDesc& desc = contactDescBegin[i];
+
+				if(getConstraintLength(desc))
+				{
+					PxConstraintBatchHeader& header = mThreadContext.contactConstraintBatchHeaders[numBatches++];
+					header.mStride = 1;
+					header.mStartIndex = j;
+					header.mConstraintType = *desc.constraint;
+					if(i != j)
+						contactDescBegin[j] = contactDescBegin[i];
+					j++;
+				}
+				i++;
+			}
+			mThreadContext.mSelfConstraintBlocks[a].startId = startId;
+			mThreadContext.mSelfConstraintBlocks[a].numSelfConstraints = j - startId;
+		}
+
+		mThreadContext.numContactConstraintBatches = numBatches;
+		mThreadContext.mNumSelfConstraints = j - contactDescCount; //self constraint count
+		contactDescCount = j;
+		mThreadContext.mOrderedContactDescCount = j;
+
+		//Now do the friction constraints if we're not using the sticky model
+		if(mContext.getFrictionType() != PxFrictionType::ePATCH)
+		{
+			PxSolverConstraintDesc* frictionDescBegin = mThreadContext.frictionConstraintDescArray.begin();
+			PxSolverConstraintDesc* frictionDescPtr = frictionDescBegin;
+
+			Ps::Array<PxConstraintBatchHeader>& frictionHeaderArray = mThreadContext.frictionConstraintBatchHeaders;
+			frictionHeaderArray.forceSize_Unsafe(0);
+			frictionHeaderArray.reserve(mThreadContext.numContactConstraintBatches);
+			PxConstraintBatchHeader* headers = frictionHeaderArray.begin();
+
+			Ps::Array<PxU32>& constraintsPerPartition = mThreadContext.mConstraintsPerPartition;
+			Ps::Array<PxU32>& frictionConstraintsPerPartition = mThreadContext.mFrictionConstraintsPerPartition;
+			frictionConstraintsPerPartition.forceSize_Unsafe(0);
+			frictionConstraintsPerPartition.reserve(constraintsPerPartition.capacity());
+			
+
+			PxU32 fricI = 0;
+			PxU32 startIndex = 0;
+			PxU32 fricHeaders = 0;
+			for(PxU32 k = 0; k < constraintsPerPartition.size(); ++k)
+			{
+				PxU32 numBatchesInK = constraintsPerPartition[k];
+				PxU32 endIndex = startIndex + numBatchesInK;
+
+				PxU32 startFricH = fricHeaders;
+
+				for(PxU32 a = startIndex; a < endIndex; ++a)
+				{
+					PxConstraintBatchHeader& _header = mThreadContext.contactConstraintBatchHeaders[a];
+					PxU16 stride = _header.mStride;
+					if(_header.mConstraintType == DY_SC_TYPE_RB_CONTACT || _header.mConstraintType == DY_SC_TYPE_EXT_CONTACT || 
+						_header.mConstraintType == DY_SC_TYPE_STATIC_CONTACT)
+					{
+						PxU8 type = 0;
+						//Extract friction from this constraint
+						for(PxU16 b = 0; b < stride; ++b)
+						{
+							//create the headers...
+							PxSolverConstraintDesc& desc = contactDescBegin[_header.mStartIndex + b];
+							PX_ASSERT(desc.constraint);
+							SolverContactCoulombHeader* header = reinterpret_cast<SolverContactCoulombHeader*>(desc.constraint);
+							PxU32 frictionOffset = header->frictionOffset;
+							PxU8* PX_RESTRICT constraint =  reinterpret_cast<PxU8*>(header) + frictionOffset;
+							const PxU32 origLength = getConstraintLength(desc);
+							const PxU32 length = (origLength - frictionOffset);
+
+							setConstraintLength(*frictionDescPtr, length);
+							frictionDescPtr->constraint	= constraint;
+							frictionDescPtr->bodyA = desc.bodyA;
+							frictionDescPtr->bodyB = desc.bodyB;
+							frictionDescPtr->bodyADataIndex = desc.bodyADataIndex;
+							frictionDescPtr->bodyBDataIndex = desc.bodyBDataIndex;
+							frictionDescPtr->linkIndexA = desc.linkIndexA;
+							frictionDescPtr->linkIndexB = desc.linkIndexB;
+							frictionDescPtr->writeBack = NULL;
+							frictionDescPtr->writeBackLengthOver4 = 0;
+							type = *constraint;
+							frictionDescPtr++;
+						}
+						headers->mStartIndex = fricI;
+						headers->mStride = stride;
+						headers->mConstraintType = type;
+						headers++;
+						fricHeaders++;
+						fricI += stride;
+					}
+					else if(_header.mConstraintType == DY_SC_TYPE_BLOCK_RB_CONTACT || _header.mConstraintType == DY_SC_TYPE_BLOCK_STATIC_RB_CONTACT)
+					{
+						//KS - TODO - Extract block of 4 contacts from this constraint. This isn't implemented yet for coulomb friction model
+						PX_ASSERT(contactDescBegin[_header.mStartIndex].constraint);
+						SolverContactCoulombHeader4* head = reinterpret_cast<SolverContactCoulombHeader4*>(contactDescBegin[_header.mStartIndex].constraint);
+						PxU32 frictionOffset = head->frictionOffset;
+						PxU8* PX_RESTRICT constraint =  reinterpret_cast<PxU8*>(head) + frictionOffset;
+						const PxU32 origLength = getConstraintLength(contactDescBegin[_header.mStartIndex]);
+						const PxU32 length = (origLength - frictionOffset);
+						PxU8 type = *constraint;
+						PX_ASSERT(type == DY_SC_TYPE_BLOCK_FRICTION || type == DY_SC_TYPE_BLOCK_STATIC_FRICTION);
+						for(PxU32 b = 0; b < 4; ++b)
+						{
+							PxSolverConstraintDesc& desc = contactDescBegin[_header.mStartIndex+b];
+							setConstraintLength(*frictionDescPtr, length);
+							frictionDescPtr->constraint	= constraint;
+							frictionDescPtr->bodyA = desc.bodyA;
+							frictionDescPtr->bodyB = desc.bodyB;
+							frictionDescPtr->bodyADataIndex = desc.bodyADataIndex;
+							frictionDescPtr->bodyBDataIndex = desc.bodyBDataIndex;
+							frictionDescPtr->linkIndexA = desc.linkIndexA;
+							frictionDescPtr->linkIndexB = desc.linkIndexB;
+							frictionDescPtr->writeBack = NULL;
+							frictionDescPtr->writeBackLengthOver4 = 0;
+							frictionDescPtr++;
+						}
+						headers->mStartIndex = fricI;
+						headers->mStride = stride;
+						headers->mConstraintType = type;
+						headers++;
+						fricHeaders++;
+						fricI += stride;
+					}
+				}
+				startIndex += numBatchesInK;
+				if(startFricH < fricHeaders)
+				{
+					frictionConstraintsPerPartition.pushBack(fricHeaders - startFricH);
+				}
+			}
+		
+
+			frictionDescCount = PxU32(frictionDescPtr - frictionDescBegin);
+			
+			mThreadContext.mNumDifferentBodyFrictionConstraints = frictionDescCount;
+
+			frictionHeaderArray.forceSize_Unsafe(PxU32(headers - frictionHeaderArray.begin()));
+
+			mThreadContext.mNumSelfFrictionConstraints = fricI - frictionDescCount; //self constraint count
+			mThreadContext.mNumDifferentBodyFrictionConstraints = frictionDescCount;
+			frictionDescCount = fricI;
+			mThreadContext.mOrderedFrictionDescCount = frictionDescCount;
+
+
+		}
+
+		{
+			{
+				PX_PROFILE_ZONE("Dynamics.solver", mContext.getContextId());
+
+				PxSolverConstraintDesc* contactDescs = mThreadContext.orderedContactConstraints;
+				PxSolverConstraintDesc* frictionDescs = mThreadContext.frictionConstraintDescArray.begin();
+
+				PxI32* thresholdPairsOut = &mContext.mThresholdStreamOut;
+
+				SolverIslandParams& params = *reinterpret_cast<SolverIslandParams*>(mContext.getTaskPool().allocate(sizeof(SolverIslandParams)));
+				params.positionIterations = mThreadContext.mMaxSolverPositionIterations;
+				params.velocityIterations = mThreadContext.mMaxSolverVelocityIterations;
+				params.bodyListStart = solverBodies;
+				params.bodyDataList = solverBodyDatas;
+				params.solverBodyOffset = mSolverBodyOffset;
+				params.bodyListSize = mIslandContext.mCounts.bodies;
+				params.articulationListStart = mThreadContext.getArticulations().begin();
+				params.articulationListSize = mThreadContext.getArticulations().size();
+				params.constraintList = contactDescs;
+				params.constraintIndex = 0;
+				params.constraintIndex2 = 0;
+				params.bodyListIndex = 0;
+				params.bodyListIndex2 = 0;
+				params.bodyIntegrationListIndex = 0;
+				params.thresholdStream = mContext.getThresholdStream().begin();
+				params.thresholdStreamLength = mContext.getThresholdStream().size();
+				params.outThresholdPairs = thresholdPairsOut;
+				params.motionVelocityArray = mThreadContext.motionVelocityArray;
+				params.bodyArray = mThreadContext.mBodyCoreArray;
+				params.numObjectsIntegrated = 0;
+				params.constraintBatchHeaders = mThreadContext.contactConstraintBatchHeaders;
+				params.numConstraintHeaders = mThreadContext.numContactConstraintBatches;
+				params.headersPerPartition = mThreadContext.mConstraintsPerPartition.begin();
+				params.nbPartitions = mThreadContext.mConstraintsPerPartition.size();
+				params.rigidBodies = const_cast<PxsRigidBody**>(mObjects.bodies);
+				params.frictionHeadersPerPartition = mThreadContext.mFrictionConstraintsPerPartition.begin();
+				params.nbFrictionPartitions = mThreadContext.mFrictionConstraintsPerPartition.size();
+				params.frictionConstraintBatches = mThreadContext.frictionConstraintBatchHeaders.begin();
+				params.numFrictionConstraintHeaders = mThreadContext.frictionConstraintBatchHeaders.size();
+				params.frictionConstraintIndex = 0;
+				params.frictionConstraintList = frictionDescs;
+
+				const PxU32 unrollSize = 8;
+				const PxU32 denom = PxMax(1u, (mThreadContext.mMaxPartitions*unrollSize));
+				const PxU32 MaxTasks = getTaskManager()->getCpuDispatcher()->getWorkerCount();
+				const PxU32 idealThreads = mThreadContext.numContactConstraintBatches/denom;
+				const PxU32 numTasks = PxMax(1u, PxMin(idealThreads, MaxTasks));
+				
+				if(numTasks > 1)
+				{
+					const PxU32 idealBatchSize = PxMax(unrollSize, idealThreads*unrollSize/(numTasks*2));
+
+					params.batchSize = idealBatchSize; //assigning ideal batch size for the solver to grab work at. Only needed by the multi-threaded island solver.
+
+					for(PxU32 a = 1; a < numTasks; ++a)
+					{
+						void* tsk = mContext.getTaskPool().allocate(sizeof(PxsParallelSolverTask));
+						PxsParallelSolverTask* pTask = PX_PLACEMENT_NEW(tsk, PxsParallelSolverTask)(
+							params, mContext, mContext.getFrictionType(), mIslandSim);
+
+						//Force to complete before merge task!
+						pTask->setContinuation(mCont);
+						
+						pTask->removeReference();
+					}
+
+					//Avoid kicking off one parallel task when we can do the work inline in this function
+					{						
+						PX_PROFILE_ZONE("Dynamics.parallelSolve", mContext.getContextId());
+
+						solveParallel(mContext, params, mIslandSim);
+					}
+					const PxI32 numBodiesPlusArtics = PxI32( mIslandContext.mCounts.bodies + mIslandContext.mCounts.articulations );
+
+					PxI32* numObjectsIntegrated = &params.numObjectsIntegrated;
+
+					WAIT_FOR_PROGRESS_NO_TIMER(numObjectsIntegrated, numBodiesPlusArtics);
+
+				}
+				else
+				{
+					
+					//Only one task - a small island so do a sequential solve (avoid the atomic overheads)
+					solveVBlock(mContext.mSolverCore[mContext.getFrictionType()], params);
+
+					const PxU32 bodyCountMin1 = mIslandContext.mCounts.bodies - 1u;
+					PxSolverBodyData* solverBodyData2 = solverBodyDatas + mSolverBodyOffset + 1;
+					for(PxU32 k=0; k < mIslandContext.mCounts.bodies; k++)
+					{
+						const PxU32 prefetchAddress = PxMin(k+4, bodyCountMin1);
+						Ps::prefetchLine(mThreadContext.mBodyCoreArray[prefetchAddress]);
+						Ps::prefetchLine(&mThreadContext.motionVelocityArray[k], 128);
+						Ps::prefetchLine(&mThreadContext.mBodyCoreArray[prefetchAddress], 128);
+						Ps::prefetchLine(&mObjects.bodies[prefetchAddress]);
+
+						PxSolverBodyData& solverBodyData = solverBodyData2[k];
+
+						integrateCore(mThreadContext.motionVelocityArray[k].linear, mThreadContext.motionVelocityArray[k].angular,
+							solverBodies[k], solverBodyData, mContext.mDt);
+
+						PxsRigidBody& rBody = *mObjects.bodies[k];
+						PxsBodyCore& core = rBody.getCore();
+						rBody.mLastTransform = core.body2World;
+						core.body2World = solverBodyData.body2World;
+						core.linearVelocity = solverBodyData.linearVelocity;
+						core.angularVelocity = solverBodyData.angularVelocity;
+
+
+						bool hasStaticTouch = mIslandSim.getIslandStaticTouchCount(IG::NodeIndex(solverBodyData.nodeIndex)) != 0;
+						sleepCheck(const_cast<PxsRigidBody*>(mObjects.bodies[k]), mContext.mDt, mContext.mInvDt, mContext.mEnableStabilization, mContext.mUseAdaptiveForce, mThreadContext.motionVelocityArray[k],
+							hasStaticTouch);
+					}
+
+					for(PxU32 cnt=0;cnt<mIslandContext.mCounts.articulations;cnt++)
+					{
+						ArticulationSolverDesc &d = mThreadContext.getArticulations()[cnt];
+						PX_PROFILE_ZONE("Articulations.integrate", mContext.getContextId());
+
+						ArticulationPImpl::updateBodies(d, mContext.getDt());
+					}
+				}
+			}
+		}
+	}
+
+	virtual const char* getName() const { return "PxsDynamics.solverSetupSolve"; }
+
+	DynamicsContext&			mContext;
+	IslandContext&				mIslandContext;
+	const SolverIslandObjects	mObjects;
+	PxU32						mSolverBodyOffset;
+	IG::IslandSim&				mIslandSim;
+};
+
+class PxsSolverEndTask : public Cm::Task
+{
+	PxsSolverEndTask& operator=(const PxsSolverEndTask&);
+public:
+
+	PxsSolverEndTask(DynamicsContext& context,
+		IslandContext& islandContext,
+		const SolverIslandObjects& objects,				  
+		const PxU32 solverBodyOffset,
+		PxsContactManagerOutputIterator& cmOutputs) :
+		mContext			(context), 
+		mIslandContext		(islandContext),
+		mObjects			(objects),
+		mSolverBodyOffset	(solverBodyOffset),
+		mOutputs			(cmOutputs)
+	{}
+
+	virtual void runInternal()
+	{		
+		ThreadContext& mThreadContext = *mIslandContext.mThreadContext;
+#if PX_ENABLE_SIM_STATS
+		mThreadContext.getSimStats().numAxisSolverConstraints += mThreadContext.mAxisConstraintCount;
+#endif
+		//Patch up the contact managers (TODO - fix up force writeback)
+		PxU32 numCompoundConstraints = mThreadContext.compoundConstraints.size();
+		for(PxU32 i = 0; i < numCompoundConstraints; ++i)
+		{
+			CompoundContactManager& manager = mThreadContext.compoundConstraints[i];
+			PxsContactManagerOutput* cmOutput = manager.cmOutput;
+
+			PxReal* contactForces = reinterpret_cast<PxReal*>(cmOutput->contactForces);
+			PxU32 contactCount = cmOutput->nbContacts;
+
+			cmOutput->contactPatches = manager.originalContactPatches;
+			cmOutput->contactPoints = manager.originalContactPoints;
+			cmOutput->nbContacts = manager.originalContactCount;
+			cmOutput->nbPatches = manager.originalPatchCount;
+			cmOutput->statusFlag = manager.originalStatusFlags;
+			cmOutput->contactForces = manager.originalForceBuffer;
+			
+			for(PxU32 a = 1; a < manager.mStride; ++a)
+			{
+				PxsContactManager* pManager = mThreadContext.orderedContactList[manager.mStartIndex + a]->contactManager;
+				pManager->getWorkUnit().frictionDataPtr = manager.unit->frictionDataPtr;
+				pManager->getWorkUnit().frictionPatchCount = manager.unit->frictionPatchCount;
+				//pManager->getWorkUnit().prevFrictionPatchCount = manager.unit->prevFrictionPatchCount;
+			}
+
+			//This is a stride-based contact force writer. The assumption is that we may have skipped certain unimportant contacts reported by the 
+			//discrete narrow phase
+			if(contactForces)
+			{
+				PxU32 currentContactIndex = 0;
+				PxU32 currentManagerIndex = manager.mStartIndex;
+				PxU32 currentManagerContactIndex = 0;
+
+				for(PxU32 a = 0; a < contactCount; ++a)
+				{
+					PxU32 index = manager.forceBufferList[a];
+					PxsContactManager* pManager = mThreadContext.orderedContactList[currentManagerIndex]->contactManager;
+					PxsContactManagerOutput* output = &mOutputs.getContactManager(pManager->getWorkUnit().mNpIndex);
+					while(currentContactIndex < index || output->nbContacts == 0)
+					{
+						//Step forwards...first in this manager...
+						
+						PxU32 numToStep = PxMin(index - currentContactIndex, PxU32(output->nbContacts) - currentManagerContactIndex);
+						currentContactIndex += numToStep;
+						currentManagerContactIndex += numToStep;
+						if(currentManagerContactIndex == output->nbContacts)
+						{
+							currentManagerIndex++;
+							currentManagerContactIndex = 0;
+							pManager = mThreadContext.orderedContactList[currentManagerIndex]->contactManager;
+							output = &mOutputs.getContactManager(pManager->getWorkUnit().mNpIndex);
+						}
+					}
+					if(output->nbContacts > 0 && output->contactForces)
+						output->contactForces[currentManagerContactIndex] = contactForces[a];
+				}
+			}
+		}
+
+		mThreadContext.compoundConstraints.forceSize_Unsafe(0);
+
+		mThreadContext.mConstraintBlockManager.reset();
+
+		mContext.putThreadContext(&mThreadContext);
+	}
+
+
+	virtual const char* getName() const
+	{
+		return "PxsDynamics.solverEnd";
+	}
+
+	DynamicsContext&					mContext;	
+	IslandContext&						mIslandContext;
+	const SolverIslandObjects			mObjects;
+	const PxU32							mSolverBodyOffset;
+	PxsContactManagerOutputIterator&	mOutputs;
+};
+
+class PxsSolverCreateFinalizeConstraintsTask : public Cm::Task
+{
+	PxsSolverCreateFinalizeConstraintsTask& operator=(const PxsSolverCreateFinalizeConstraintsTask&);
+public:
+
+	PxsSolverCreateFinalizeConstraintsTask(
+		DynamicsContext& context,
+		IslandContext& islandContext,
+		PxU32 solverDataOffset,
+		PxsContactManagerOutputIterator& outputs,
+		bool enhancedDeterminism) : 
+		mContext				(context),
+		mIslandContext			(islandContext),
+		mSolverDataOffset		(solverDataOffset),
+		mOutputs				(outputs),
+		mEnhancedDeterminism	(enhancedDeterminism)
+	{
+	}
+	
+	virtual void runInternal();
+
+	virtual const char* getName() const { return "PxsDynamics.solverCreateFinalizeConstraints"; }
+
+	DynamicsContext&					mContext;
+	IslandContext&						mIslandContext;
+	PxU32								mSolverDataOffset;
+	PxsContactManagerOutputIterator&	mOutputs;
+	bool								mEnhancedDeterminism;
+};
+
+
+// helper function to join two tasks together and ensure ref counts are correct
+void chainTasks(PxLightCpuTask* first, PxLightCpuTask* next)
+{
+	first->setContinuation(next);
+	next->removeReference();
+}
+
+PxBaseTask* createSolverTaskChain(DynamicsContext& dynamicContext,
+										const SolverIslandObjects& objects,				  
+										const PxsIslandIndices& counts,
+										const PxU32 solverBodyOffset, 
+										IG::SimpleIslandManager& islandManager, 
+										PxU32* bodyRemapTable, PxsMaterialManager* materialManager, PxBaseTask* continuation,
+										PxsContactManagerOutputIterator& iterator, bool useEnhancedDeterminism)
+{
+	Cm::FlushPool& taskPool =  dynamicContext.getTaskPool();
+
+	taskPool.lock();
+
+
+	IslandContext* islandContext = reinterpret_cast<IslandContext*>(taskPool.allocate(sizeof(IslandContext)));
+	islandContext->mThreadContext = NULL;
+	islandContext->mCounts = counts;
+
+
+	// create lead task
+	PxsSolverStartTask* startTask = PX_PLACEMENT_NEW(taskPool.allocateNotThreadSafe(sizeof(PxsSolverStartTask)), PxsSolverStartTask)(dynamicContext, *islandContext, objects, solverBodyOffset, dynamicContext.getKinematicCount(), 
+		islandManager, bodyRemapTable, materialManager, iterator, useEnhancedDeterminism);
+	PxsSolverEndTask* endTask = PX_PLACEMENT_NEW(taskPool.allocateNotThreadSafe(sizeof(PxsSolverEndTask)), PxsSolverEndTask)(dynamicContext, *islandContext, objects, solverBodyOffset, iterator);	
+
+
+	PxsSolverCreateFinalizeConstraintsTask* createFinalizeConstraintsTask = PX_PLACEMENT_NEW(taskPool.allocateNotThreadSafe(sizeof(PxsSolverCreateFinalizeConstraintsTask)), PxsSolverCreateFinalizeConstraintsTask)(dynamicContext, *islandContext, solverBodyOffset, iterator, useEnhancedDeterminism);
+	PxsSolverSetupSolveTask* setupSolveTask = PX_PLACEMENT_NEW(taskPool.allocateNotThreadSafe(sizeof(PxsSolverSetupSolveTask)), PxsSolverSetupSolveTask)(dynamicContext, *islandContext, objects, solverBodyOffset, islandManager.getAccurateIslandSim());
+
+	PxsSolverConstraintPartitionTask* partitionConstraintsTask = PX_PLACEMENT_NEW(taskPool.allocateNotThreadSafe(sizeof(PxsSolverConstraintPartitionTask)), PxsSolverConstraintPartitionTask)(dynamicContext, *islandContext, objects, solverBodyOffset, useEnhancedDeterminism);
+
+	endTask->setContinuation(continuation);
+
+	// set up task chain in reverse order
+	chainTasks(setupSolveTask, endTask);
+	chainTasks(createFinalizeConstraintsTask, setupSolveTask);
+	chainTasks(partitionConstraintsTask, createFinalizeConstraintsTask);
+	chainTasks(startTask, partitionConstraintsTask);
+
+	taskPool.unlock();
+
+	return startTask;
+}
+
+
+void DynamicsContext::update(IG::SimpleIslandManager& simpleIslandManager, PxBaseTask* /*continuation*/, PxBaseTask* lostTouchTask,
+							 PxsContactManager** /*foundPatchManagers*/, PxU32 /*nbFoundPatchManagers*/, 
+							 PxsContactManager** /*lostPatchManagers*/, PxU32 /*nbLostPatchManagers*/,
+							 PxU32 /*maxPatchesPerCM*/,
+							 PxsContactManagerOutputIterator& iterator,
+							 PxsContactManagerOutput*,
+							 const PxReal dt, const PxVec3& gravity, const PxU32 /*bitMapWordCounts*/)
+{		
+	PX_PROFILE_ZONE("Dynamics.solverQueueTasks", mContextID);
+
+	PX_UNUSED(simpleIslandManager);
+
+	mOutputIterator = iterator;
+
+	mDt = dt;
+	mInvDt = dt == 0.0f ? 0.0f : 1.0f/dt;
+	mGravity = gravity;
+
+	const IG::IslandSim& islandSim = simpleIslandManager.getAccurateIslandSim();
+
+	const PxU32 islandCount = islandSim.getNbActiveIslands();
+
+	const PxU32 activatedContactCount = islandSim.getNbActivatedEdges(IG::Edge::eCONTACT_MANAGER);
+	const IG::EdgeIndex* const activatingEdges = islandSim.getActivatedEdges(IG::Edge::eCONTACT_MANAGER);
+
+	for(PxU32 a = 0; a < activatedContactCount; ++a)
+	{
+		PxsContactManager* cm = simpleIslandManager.getContactManager(activatingEdges[a]);
+		if(cm)
+		{
+			cm->getWorkUnit().frictionPatchCount = 0; //KS - zero the friction patch count on any activating edges
+		}
+	}
+
+#if PX_ENABLE_SIM_STATS
+	if(islandCount > 0)
+	{
+		mSimStats.mNbActiveKinematicBodies = islandSim.getNbActiveKinematics();
+		mSimStats.mNbActiveDynamicBodies = islandSim.getNbActiveNodes(IG::Node::eRIGID_BODY_TYPE);	
+		mSimStats.mNbActiveConstraints = islandSim.getNbActiveEdges(IG::Edge::eCONSTRAINT);
+	}
+	else
+	{
+		mSimStats.mNbActiveKinematicBodies = islandSim.getNbActiveKinematics();
+		mSimStats.mNbActiveDynamicBodies = 0;	
+		mSimStats.mNbActiveConstraints = 0;
+	}
+#endif
+
+	mThresholdStreamOut = 0;
+
+	resetThreadContexts();
+
+	//If there is no work to do then we can do nothing at all.
+	if(0 == islandCount)
+	{
+		return;
+	}
+
+	//KS - test that world solver body's velocities are finite and 0, then set it to 0.
+	//Technically, the velocity should always be 0 but can be stomped if a NAN creeps into the simulation.
+	PX_ASSERT(mWorldSolverBody.linearVelocity == PxVec3(0.f));
+	PX_ASSERT(mWorldSolverBody.angularState == PxVec3(0.f));
+	PX_ASSERT(mWorldSolverBody.linearVelocity.isFinite());
+	PX_ASSERT(mWorldSolverBody.angularState.isFinite());
+
+	mWorldSolverBody.linearVelocity = mWorldSolverBody.angularState = PxVec3(0.f);
+
+	const PxU32 kinematicCount = islandSim.getNbActiveKinematics();
+	const IG::NodeIndex* const kinematicIndices = islandSim.getActiveKinematics();
+	mKinematicCount = kinematicCount;
+
+	const PxU32 bodyCount = islandSim.getNbActiveNodes(IG::Node::eRIGID_BODY_TYPE);
+
+	PxU32 numArtics = islandSim.getNbActiveNodes(IG::Node::eARTICULATION_TYPE);
+
+	{
+		if(kinematicCount + bodyCount > mSolverBodyPool.capacity())
+		{
+			mSolverBodyPool.reserve((kinematicCount + bodyCount + 31) & ~31); // pad out to 32 * 128 = 4k to prevent alloc churn
+			mSolverBodyDataPool.reserve((kinematicCount + bodyCount + 31 + 1) & ~31); // pad out to 32 * 128 = 4k to prevent alloc churn
+			mSolverBodyRemapTable.reserve((kinematicCount + bodyCount + 31 + 1) & ~31);
+		}
+
+		{
+			PxSolverBody emptySolverBody;
+			PxMemZero(&emptySolverBody, sizeof(PxSolverBody));
+			mSolverBodyPool.resize(kinematicCount + bodyCount, emptySolverBody);
+			PxSolverBodyData emptySolverBodyData;
+			PxMemZero(&emptySolverBodyData, sizeof(PxSolverBodyData));
+			mSolverBodyDataPool.resize(kinematicCount + bodyCount + 1, emptySolverBodyData);
+			mSolverBodyRemapTable.resize(bodyCount);
+		}
+
+		// integrate and copy all the kinematics - overkill, since not all kinematics
+		// need solver bodies
+
+		mSolverBodyDataPool[0] = mWorldSolverBodyData;
+
+
+		{			
+			PX_PROFILE_ZONE("Dynamics.updateKinematics", mContextID);
+			PxMemZero(mSolverBodyPool.begin(), kinematicCount*sizeof(PxSolverBody));
+			for(PxU32 i=0;i<kinematicCount;i++)
+			{
+				PxsRigidBody* rigidBody = islandSim.getRigidBody(kinematicIndices[i]);
+				const PxsBodyCore& core = rigidBody->getCore();
+				copyToSolverBodyData(core.linearVelocity, core.angularVelocity, core.inverseMass, core.inverseInertia, core.body2World, core.maxPenBias, 
+					core.maxContactImpulse, kinematicIndices[i].index(), core.contactReportThreshold, mSolverBodyDataPool[i + 1], core.lockFlags);
+				rigidBody->saveLastCCDTransform();
+				// Only really necessary for PS3 at the moment (for the cross island parallel constraint solver
+				// but we might switch to the same on other platforms)
+				mSolverBodyPool[i].solverProgress=MAX_PERMITTED_SOLVER_PROGRESS;
+				mSolverBodyPool[i].maxSolverNormalProgress=MAX_PERMITTED_SOLVER_PROGRESS;
+				mSolverBodyPool[i].maxSolverFrictionProgress=MAX_PERMITTED_SOLVER_PROGRESS;
+			}
+		}
+	}
+
+	PxU32 solverBatchMax = mSolverBatchSize;
+	PxU32 articulationBatchMax = 2;
+	PxU32 minimumConstraintCount = 1;
+
+
+	//Resize arrays of solver constraints...
+	PxU32 numArticulationConstraints=numArtics*Dy::DY_ARTICULATION_MAX_SIZE; //Just allocate enough memory to fit worst-case maximum size articulations...
+
+	const PxU32 nbActiveContactManagers = islandSim.getNbActiveEdges(IG::Edge::eCONTACT_MANAGER);
+	const PxU32 nbActiveConstraints = islandSim.getNbActiveEdges(IG::Edge::eCONSTRAINT);
+
+	PxU32 totalConstraintCount = nbActiveConstraints + nbActiveContactManagers + numArticulationConstraints;
+
+	mSolverConstraintDescPool.forceSize_Unsafe(0);
+	mSolverConstraintDescPool.reserve((totalConstraintCount + 63) & (~63));
+	mSolverConstraintDescPool.forceSize_Unsafe(totalConstraintCount);
+
+	mOrderedSolverConstraintDescPool.forceSize_Unsafe(0);
+	mOrderedSolverConstraintDescPool.reserve((totalConstraintCount + 63) & (~63));
+	mOrderedSolverConstraintDescPool.forceSize_Unsafe(totalConstraintCount);
+
+	mTempSolverConstraintDescPool.forceSize_Unsafe(0);
+	mTempSolverConstraintDescPool.reserve((totalConstraintCount + 63) & (~63));
+	mTempSolverConstraintDescPool.forceSize_Unsafe(totalConstraintCount);
+
+	mContactConstraintBatchHeaders.forceSize_Unsafe(0);
+	mContactConstraintBatchHeaders.reserve((totalConstraintCount + 63) & (~63));
+	mContactConstraintBatchHeaders.forceSize_Unsafe(totalConstraintCount);
+
+	mContactList.forceSize_Unsafe(0);
+	mContactList.reserve((nbActiveContactManagers +63u) & (~63u));
+	mContactList.forceSize_Unsafe(nbActiveContactManagers);
+
+	mMotionVelocityArray.forceSize_Unsafe(0);
+	mMotionVelocityArray.reserve((bodyCount + 63u) & (~63u));
+	mMotionVelocityArray.forceSize_Unsafe(bodyCount);
+
+	mBodyCoreArray.forceSize_Unsafe(0);
+	mBodyCoreArray.reserve((bodyCount + 63u) & (~63u));
+	mBodyCoreArray.forceSize_Unsafe(bodyCount);
+
+	mRigidBodyArray.forceSize_Unsafe(0);
+	mRigidBodyArray.reserve((bodyCount + 63u) & (~63u));
+	mRigidBodyArray.forceSize_Unsafe(bodyCount);
+
+	mArticulationArray.forceSize_Unsafe(0);
+	mArticulationArray.reserve((numArtics + 63u) & (~63u));
+	mArticulationArray.forceSize_Unsafe(numArtics);
+
+	mNodeIndexArray.forceSize_Unsafe(0);
+	mNodeIndexArray.reserve((bodyCount + 63u) & (~63u));
+	mNodeIndexArray.forceSize_Unsafe(bodyCount);
+
+
+	ThresholdStream& stream = getThresholdStream();
+	stream.forceSize_Unsafe(0);
+	stream.reserve(Ps::nextPowerOfTwo(nbActiveContactManagers != 0 ? nbActiveContactManagers-1 : nbActiveContactManagers));
+
+	PxU32 constraintIndex = 0;
+
+	//flip exceeded force threshold buffer
+	mCurrentIndex = 1 - mCurrentIndex;
+
+	//create force threshold tasks to produce force change events
+	PxsForceThresholdTask* forceThresholdTask =  PX_PLACEMENT_NEW(getTaskPool().allocateNotThreadSafe(sizeof(PxsForceThresholdTask)), PxsForceThresholdTask)(*this);
+	forceThresholdTask->setContinuation(lostTouchTask);
+
+	const IG::IslandId*const islandIds = islandSim.getActiveIslands();
+
+	PxU32 currentIsland = 0;
+	PxU32 currentBodyIndex = 0;
+	PxU32 currentArticulation = 0;
+	PxU32 currentContact = 0;
+	//while(start<sentinel)
+	while(currentIsland < islandCount)
+	{
+		SolverIslandObjects objectStarts;
+		objectStarts.articulations				= mArticulationArray.begin()+ currentArticulation;
+		objectStarts.bodies						= mRigidBodyArray.begin()	+ currentBodyIndex;
+		objectStarts.contactManagers			= mContactList.begin()	+ currentContact;
+		objectStarts.constraintDescs			= mSolverConstraintDescPool.begin() + constraintIndex;
+		objectStarts.orderedConstraintDescs		= mOrderedSolverConstraintDescPool.begin() + constraintIndex;
+		objectStarts.tempConstraintDescs		= mTempSolverConstraintDescPool.begin() + constraintIndex;
+		objectStarts.constraintBatchHeaders		= mContactConstraintBatchHeaders.begin() + constraintIndex;
+		objectStarts.motionVelocities			= mMotionVelocityArray.begin() + currentBodyIndex;
+		objectStarts.bodyCoreArray				= mBodyCoreArray.begin() + currentBodyIndex;
+		objectStarts.islandIds					= islandIds + currentIsland;
+		objectStarts.bodyRemapTable				= mSolverBodyRemapTable.begin();
+		objectStarts.nodeIndexArray				= mNodeIndexArray.begin() + currentBodyIndex;
+
+		PxU32 startIsland = currentIsland;
+		PxU32 constraintCount = 0;
+
+		PxU32 nbArticulations = 0;
+		PxU32 nbBodies = 0;
+		PxU32 nbConstraints = 0;
+		PxU32 nbContactManagers =0;
+
+		//KS - logic is a bit funky here. We will keep rolling the island together provided currentIsland < islandCount AND either we haven't exceeded the max number of bodies or we have
+		//zero constraints AND we haven't exceeded articulation batch counts (it's still currently beneficial to keep articulations in separate islands but this is only temporary).
+		while((currentIsland < islandCount && (nbBodies < solverBatchMax || constraintCount < minimumConstraintCount)) && nbArticulations < articulationBatchMax)
+		{
+			const IG::Island& island = islandSim.getIsland(islandIds[currentIsland]);
+			nbBodies += island.mSize[IG::Node::eRIGID_BODY_TYPE];
+			nbArticulations += island.mSize[IG::Node::eARTICULATION_TYPE];
+			nbConstraints += island.mEdgeCount[IG::Edge::eCONSTRAINT];
+			nbContactManagers += island.mEdgeCount[IG::Edge::eCONTACT_MANAGER];
+			constraintCount = nbConstraints + nbContactManagers;
+			currentIsland++;
+		}
+
+		
+		objectStarts.numIslands = currentIsland - startIsland;
+
+		constraintIndex += nbArticulations*Dy::DY_ARTICULATION_MAX_SIZE;
+
+		PxsIslandIndices counts;
+		
+		counts.articulations	= nbArticulations;
+		counts.bodies			= nbBodies;
+
+		counts.constraints		= nbConstraints;
+		counts.contactManagers	= nbContactManagers;
+		if(counts.articulations + counts.bodies > 0)
+		{
+			PxBaseTask* task = createSolverTaskChain(*this, objectStarts, counts, 
+				kinematicCount + currentBodyIndex, simpleIslandManager, mSolverBodyRemapTable.begin(), mMaterialManager, forceThresholdTask, mOutputIterator, mUseEnhancedDeterminism);		
+			task->removeReference();
+		}
+
+		currentBodyIndex += nbBodies;
+		currentArticulation += nbArticulations;
+		currentContact += nbContactManagers;
+
+		constraintIndex += constraintCount;
+	}
+
+	//kick off forceThresholdTask
+	forceThresholdTask->removeReference();
+}
+
+void DynamicsContext::updateBodyCore(PxBaseTask* continuation)
+{
+	PX_UNUSED(continuation);
+}
+
+void DynamicsContext::mergeResults()
+{	
+	PX_PROFILE_ZONE("Dynamics.solverMergeResults", mContextID);
+	//OK. Sum up sim stats here...
+
+#if PX_ENABLE_SIM_STATS
+	PxcThreadCoherentCacheIterator<ThreadContext, PxcNpMemBlockPool> threadContextIt(mThreadContextPool);
+	ThreadContext* threadContext = threadContextIt.getNext();
+
+	while(threadContext != NULL)
+	{
+		ThreadContext::ThreadSimStats& threadStats = threadContext->getSimStats();
+		addThreadStats(threadStats);
+		threadStats.clear();
+		threadContext = threadContextIt.getNext();
+	}	
+#endif
+}
+
+
+static void preIntegrationParallel(
+   const PxF32 dt,
+   PxsBodyCore*const* bodyArray,					// INOUT: core body attributes
+  PxsRigidBody*const* originalBodyArray,			// IN: original bodies (LEGACY - DON'T deref the ptrs!!)
+   PxU32 const* nodeIndexArray,					// IN: island node index
+   PxU32 bodyCount,									// IN: body count
+   PxSolverBody* solverBodyPool,					// IN: solver body pool (space preallocated)
+   PxSolverBodyData* solverBodyDataPool,			// IN: solver body data pool (space preallocated)
+   volatile PxU32* maxSolverPositionIterations,
+   volatile PxU32* maxSolverVelocityIterations,
+   const PxVec3& gravity)
+{
+	PxU32 localMaxPosIter = 0;
+	PxU32 localMaxVelIter = 0;
+
+
+	for(PxU32 a = 1; a < bodyCount; ++a)
+	{
+		PxU32 i = a-1;
+		Ps::prefetchLine(bodyArray[a]);
+		Ps::prefetchLine(bodyArray[a],128);
+		Ps::prefetchLine(&solverBodyDataPool[a]);
+		Ps::prefetchLine(&solverBodyDataPool[a],128);
+
+		PxsBodyCore& core = *bodyArray[i];
+		const PxsRigidBody& rBody = *originalBodyArray[i];
+		
+		PxU16 iterWord = core.solverIterationCounts;
+		localMaxPosIter = PxMax<PxU32>(PxU32(iterWord & 0xff), localMaxPosIter);
+		localMaxVelIter = PxMax<PxU32>(PxU32(iterWord >> 8), localMaxVelIter);
+
+		//const Cm::SpatialVector& accel = originalBodyArray[i]->getAccelerationV();
+		bodyCoreComputeUnconstrainedVelocity(gravity, dt, core.linearDamping, core.angularDamping, rBody.accelScale, core.maxLinearVelocitySq, core.maxAngularVelocitySq, 
+			core.linearVelocity, core.angularVelocity, !!(rBody.mInternalFlags & PxcRigidBody::eDISABLE_GRAVITY));
+
+		copyToSolverBodyData(core.linearVelocity, core.angularVelocity, core.inverseMass, core.inverseInertia, core.body2World, core.maxPenBias, core.maxContactImpulse, nodeIndexArray[i], 
+			core.contactReportThreshold, solverBodyDataPool[i + 1], core.lockFlags);
+		solverBodyPool[i].solverProgress = 0;
+		solverBodyPool[i].maxSolverNormalProgress = 0;
+		solverBodyPool[i].maxSolverFrictionProgress = 0;
+	}
+	const PxU32 i = bodyCount - 1;
+	PxsBodyCore& core = *bodyArray[i];
+	const PxsRigidBody& rBody = *originalBodyArray[i];
+		
+	PxU16 iterWord = core.solverIterationCounts;
+	localMaxPosIter = PxMax<PxU32>(PxU32(iterWord & 0xff), localMaxPosIter);
+	localMaxVelIter = PxMax<PxU32>(PxU32(iterWord >> 8), localMaxVelIter);
+
+	bodyCoreComputeUnconstrainedVelocity(gravity, dt, core.linearDamping, core.angularDamping, rBody.accelScale, core.maxLinearVelocitySq, core.maxAngularVelocitySq,
+		core.linearVelocity, core.angularVelocity, !!(rBody.mInternalFlags & PxcRigidBody::eDISABLE_GRAVITY));
+
+	copyToSolverBodyData(core.linearVelocity, core.angularVelocity, core.inverseMass, core.inverseInertia, core.body2World, core.maxPenBias, core.maxContactImpulse, nodeIndexArray[i], 
+		core.contactReportThreshold, solverBodyDataPool[i + 1], core.lockFlags);
+	solverBodyPool[i].solverProgress = 0;
+	solverBodyPool[i].maxSolverNormalProgress = 0;
+	solverBodyPool[i].maxSolverFrictionProgress = 0;
+
+	physx::shdfnd::atomicMax(reinterpret_cast<volatile PxI32*>(maxSolverPositionIterations), PxI32(localMaxPosIter));
+	physx::shdfnd::atomicMax(reinterpret_cast<volatile PxI32*>(maxSolverVelocityIterations), PxI32(localMaxVelIter));
+}
+
+
+void PxsPreIntegrateTask::runInternal()
+{
+	{
+		preIntegrationParallel(mDt, mBodyArray + mStartIndex, mOriginalBodyArray + mStartIndex, mNodeIndexArray + mStartIndex, mNumToIntegrate,
+							mSolverBodies + mStartIndex, mSolverBodyDataPool + mStartIndex,
+							mMaxSolverPositionIterations, mMaxSolverVelocityIterations, mGravity);
+	}
+}
+
+void DynamicsContext::preIntegrationParallel(
+   const PxF32 dt,
+   PxsBodyCore*const* bodyArray,					// INOUT: core body attributes
+   PxsRigidBody*const* originalBodyArray,			// IN: original bodies (LEGACY - DON'T deref the ptrs!!)
+   PxU32 const* nodeIndexArray,						// IN: island node index
+   PxU32 bodyCount,									// IN: body count
+   PxSolverBody* solverBodyPool,					// IN: solver body pool (space preallocated)
+   PxSolverBodyData* solverBodyDataPool,			// IN: solver body data pool (space preallocated)
+   Cm::SpatialVector* /*motionVelocityArray*/,			// OUT: motion velocities
+   PxU32& maxSolverPositionIterations,
+   PxU32& maxSolverVelocityIterations,
+   PxBaseTask& task
+   )
+{
+	//TODO - make this based on some variables so we can try different configurations
+	const PxU32 IntegrationPerThread = 256;
+
+	const PxU32 numTasks = ((bodyCount + IntegrationPerThread-1)/IntegrationPerThread);
+	const PxU32 taskBatchSize = 64;
+
+	for(PxU32 i = 0; i < numTasks; i+=taskBatchSize)
+	{
+		const PxU32 nbTasks = PxMin(numTasks - i, taskBatchSize);
+		PxsPreIntegrateTask* tasks = reinterpret_cast<PxsPreIntegrateTask*>(getTaskPool().allocate(sizeof(PxsPreIntegrateTask)*nbTasks));
+		for(PxU32 a = 0; a < nbTasks; ++a)
+		{
+			PxU32 startIndex = (i+a)*IntegrationPerThread;
+			PxU32 nbToIntegrate = PxMin((bodyCount-startIndex), IntegrationPerThread);
+			PxsPreIntegrateTask* pTask = PX_PLACEMENT_NEW(&tasks[a], PxsPreIntegrateTask)(*this, bodyArray,
+							originalBodyArray, nodeIndexArray, solverBodyPool, solverBodyDataPool, dt, bodyCount,
+							&maxSolverPositionIterations, &maxSolverVelocityIterations, startIndex, 
+							nbToIntegrate, mGravity);
+
+			pTask->setContinuation(&task);
+			pTask->removeReference();
+		}
+	}
+
+	PxMemZero(solverBodyPool, bodyCount * sizeof(PxSolverBody));
+}
+
+inline void WaitBodyRequiredState(volatile PxU32* state, PxU32 requiredState)
+{
+	while(requiredState != *state );
+}
+
+void solveParallel(SOLVER_PARALLEL_METHOD_ARGS)
+{
+	context.solveParallel(params, islandSim);
+}
+
+
+void DynamicsContext::solveParallel(SolverIslandParams& params, IG::IslandSim& islandSim)
+{
+	PxI32 targetCount = mSolverCore[mFrictionType]->solveVParallelAndWriteBack(params);
+
+	PxI32* solveCount = &params.constraintIndex2;
+
+	//PxI32 targetCount = (PxI32)(params.numConstraintHeaders * (params.velocityIterations + params.positionIterations));
+
+	WAIT_FOR_PROGRESS_NO_TIMER(solveCount, targetCount);
+
+	integrateCoreParallel(params, islandSim);
+}
+
+void DynamicsContext::integrateCoreParallel(SolverIslandParams& params, IG::IslandSim& islandSim)
+{
+	const PxI32 unrollCount = 128;
+
+	PxI32* bodyIntegrationListIndex = &params.bodyIntegrationListIndex;
+
+	PxI32 index = physx::shdfnd::atomicAdd(bodyIntegrationListIndex, unrollCount) - unrollCount;
+
+	const PxI32 numBodies = PxI32(params.bodyListSize);
+	const PxI32 numArtics = PxI32(params.articulationListSize);
+
+	Cm::SpatialVector* PX_RESTRICT motionVelocityArray = params.motionVelocityArray;
+	PxsBodyCore*const* bodyArray = params.bodyArray;
+	PxsRigidBody** PX_RESTRICT rigidBodies = params.rigidBodies;
+	ArticulationSolverDesc* PX_RESTRICT articulationListStart = params.articulationListStart;
+
+
+	PxI32 numIntegrated = 0;
+
+	PxI32 bodyRemainder = unrollCount;
+
+	while(index < numArtics)
+	{
+		const PxI32 remainder = PxMin(numArtics - index, unrollCount);
+		bodyRemainder -= remainder;
+
+		for(PxI32 a = 0; a < remainder; ++a, index++)
+		{
+			const PxI32 i = index;
+			{
+				PX_PROFILE_ZONE("Articulations.integrate", mContextID);
+
+				ArticulationPImpl::updateBodies(articulationListStart[i], mDt);
+			}
+
+			++numIntegrated;
+		}
+		if(bodyRemainder == 0)
+		{
+			index = physx::shdfnd::atomicAdd(bodyIntegrationListIndex, unrollCount) - unrollCount;
+			bodyRemainder = unrollCount;
+		}
+	}	
+
+	index -= numArtics;
+
+	const PxI32 unrollPlusArtics = unrollCount + numArtics;
+
+	PxSolverBody* PX_RESTRICT solverBodies = params.bodyListStart;
+	PxSolverBodyData* PX_RESTRICT solverBodyData = params.bodyDataList + params.solverBodyOffset+1;
+
+	while(index < numBodies)
+	{
+		const PxI32 remainder = PxMin(numBodies - index, bodyRemainder);
+		bodyRemainder -= remainder;
+		for(PxI32 a = 0; a < remainder; ++a, index++)
+		{
+			const PxI32 prefetch = PxMin(index+4, numBodies - 1);
+			Ps::prefetchLine(bodyArray[prefetch]);
+			Ps::prefetchLine(bodyArray[prefetch],128);
+			Ps::prefetchLine(&solverBodies[index],128);
+			Ps::prefetchLine(&motionVelocityArray[index],128);
+			Ps::prefetchLine(&bodyArray[index+32]);
+			Ps::prefetchLine(&rigidBodies[prefetch]);
+			
+			PxSolverBodyData& data = solverBodyData[index];
+
+			integrateCore(motionVelocityArray[index].linear, motionVelocityArray[index].angular,
+				solverBodies[index], data, mDt);
+
+			PxsRigidBody& rBody = *rigidBodies[index];
+			PxsBodyCore& core = rBody.getCore();
+			rBody.mLastTransform = core.body2World;
+			core.body2World = data.body2World;
+			core.linearVelocity = data.linearVelocity;
+			core.angularVelocity = data.angularVelocity;
+
+			bool hasStaticTouch = islandSim.getIslandStaticTouchCount(IG::NodeIndex(data.nodeIndex)) != 0;
+			sleepCheck(rigidBodies[index], mDt, mInvDt, mEnableStabilization, mUseAdaptiveForce, motionVelocityArray[index], hasStaticTouch);
+
+			++numIntegrated;
+		}
+
+		{
+			index = physx::shdfnd::atomicAdd(bodyIntegrationListIndex, unrollCount) - unrollPlusArtics;
+			bodyRemainder = unrollCount;
+		}
+	}
+
+	Ps::memoryBarrier();
+	physx::shdfnd::atomicAdd(&params.numObjectsIntegrated, numIntegrated);
+}
+
+class BlockAllocator : public PxConstraintAllocator
+{
+	PxsConstraintBlockManager& mConstraintBlockManager;
+	PxcConstraintBlockStream& mConstraintBlockStream;
+	FrictionPatchStreamPair& mFrictionPatchStreamPair;
+	PxU32& mTotalConstraintByteSize;
+public:
+
+	BlockAllocator(PxsConstraintBlockManager& constraintBlockManager, PxcConstraintBlockStream& constraintBlockStream, FrictionPatchStreamPair& frictionPatchStreamPair,
+		PxU32& totalConstraintByteSize) :
+		mConstraintBlockManager(constraintBlockManager), mConstraintBlockStream(constraintBlockStream), mFrictionPatchStreamPair(frictionPatchStreamPair), 
+		mTotalConstraintByteSize(totalConstraintByteSize)
+	{
+	}
+
+	virtual PxU8* reserveConstraintData(const PxU32 size)
+	{
+		mTotalConstraintByteSize += size;
+		return mConstraintBlockStream.reserve(size, mConstraintBlockManager);
+	}
+
+	virtual PxU8* reserveFrictionData(const PxU32 size)
+	{
+		return mFrictionPatchStreamPair.reserve<PxU8>(size);
+	}
+
+	virtual PxU8* findInputPatches(PxU8* frictionCookie)
+	{
+		return frictionCookie;
+	}
+
+	PX_NOCOPY(BlockAllocator)
+
+};
+
+
+
+static PxU32 createFinalizeContacts_Parallel(PxSolverBodyData* solverBodyData, ThreadContext& mThreadContext, DynamicsContext& context,
+									  PxU32 startIndex, PxU32 endIndex, PxsContactManagerOutputIterator& outputs)
+{
+	const PxFrictionType::Enum frictionType = context.getFrictionType();
+	const PxReal bounceThreshold = context.getBounceThreshold();
+	const PxReal frictionOffsetThreshold = context.getFrictionOffsetThreshold();
+	const PxReal dt = context.getDt();
+	const PxReal invDt = context.getInvDt();
+
+	PxSolverConstraintDesc* contactDescPtr = mThreadContext.orderedContactConstraints;
+
+	PxConstraintBatchHeader* headers = mThreadContext.contactConstraintBatchHeaders;
+	
+	PxI32 axisConstraintCount = 0;
+	ThreadContext* threadContext = context.getThreadContext();
+	threadContext->mConstraintBlockStream.reset(); //ensure there's no left-over memory that belonged to another island
+
+	PxTransform idt(PxIdentity);
+
+	BlockAllocator blockAllocator(mThreadContext.mConstraintBlockManager, threadContext->mConstraintBlockStream, threadContext->mFrictionPatchStreamPair, threadContext->mConstraintSize);
+
+	const PxReal ccdMaxSeparation = context.getCCDSeparationThreshold();
+
+	for(PxU32 a = startIndex; a < endIndex; ++a)
+	{
+
+		PxConstraintBatchHeader& header = headers[a];
+
+		if(contactDescPtr[header.mStartIndex].constraintLengthOver16 == DY_SC_TYPE_RB_CONTACT)
+		{
+			SolverConstraintPrepState::Enum state = SolverConstraintPrepState::eUNBATCHABLE;
+
+			PxSolverContactDesc blockDescs[4];
+			PxsContactManagerOutput* cmOutputs[4];
+			PxsContactManager* cms[4];
+			for (PxU32 i = 0; i < header.mStride; ++i)
+			{
+				PxSolverConstraintDesc& desc = contactDescPtr[header.mStartIndex + i];
+				PxSolverContactDesc& blockDesc = blockDescs[i];
+				PxsContactManager* cm = reinterpret_cast<PxsContactManager*>(desc.constraint);
+
+				cms[i] = cm;
+
+				PxcNpWorkUnit& unit = cm->getWorkUnit();
+
+				cmOutputs[i] = &outputs.getContactManager(unit.mNpIndex);
+
+				PxSolverBodyData& data0 = desc.linkIndexA != 0xffff ? solverBodyData[0] : solverBodyData[desc.bodyADataIndex];
+				PxSolverBodyData& data1 = desc.linkIndexB != 0xffff ? solverBodyData[0] : solverBodyData[desc.bodyBDataIndex];
+
+				blockDesc.data0 = &data0;
+				blockDesc.data1 = &data1;
+
+				PxU8 flags = unit.rigidCore0->mFlags;
+				if (unit.rigidCore1)
+					flags |= PxU8(unit.rigidCore1->mFlags);
+
+				blockDesc.bodyFrame0 = unit.rigidCore0->body2World;
+				blockDesc.bodyFrame1 = unit.rigidCore1 ? unit.rigidCore1->body2World : idt;
+				blockDesc.shapeInteraction = cm->getShapeInteraction();
+				blockDesc.contactForces = cmOutputs[i]->contactForces;
+				blockDesc.desc = &desc;
+				blockDesc.body0 = desc.bodyA;
+				blockDesc.body1 = desc.bodyB;
+				blockDesc.hasForceThresholds = !!(unit.flags & PxcNpWorkUnitFlag::eFORCE_THRESHOLD);
+				blockDesc.disableStrongFriction = !!(unit.flags & PxcNpWorkUnitFlag::eDISABLE_STRONG_FRICTION);
+				blockDesc.bodyState0 = (unit.flags & PxcNpWorkUnitFlag::eARTICULATION_BODY0) ? PxSolverContactDesc::eARTICULATION : PxSolverContactDesc::eDYNAMIC_BODY;
+				blockDesc.bodyState1 = (unit.flags & PxcNpWorkUnitFlag::eARTICULATION_BODY1) ? PxSolverContactDesc::eARTICULATION : (unit.flags & PxcNpWorkUnitFlag::eHAS_KINEMATIC_ACTOR) ? PxSolverContactDesc::eKINEMATIC_BODY :
+					((unit.flags & PxcNpWorkUnitFlag::eDYNAMIC_BODY1) ? PxSolverContactDesc::eDYNAMIC_BODY : PxSolverContactDesc::eSTATIC_BODY);
+				//blockDesc.flags = unit.flags;
+
+				PxReal dominance0 = unit.dominance0 ? 1.f : 0.f;
+				PxReal dominance1 = unit.dominance1 ? 1.f : 0.f;
+
+				blockDesc.mInvMassScales.linear0 = blockDesc.mInvMassScales.angular0 = dominance0;
+				blockDesc.mInvMassScales.linear1 = blockDesc.mInvMassScales.angular1 = dominance1;
+				blockDesc.restDistance = unit.restDistance;
+				blockDesc.frictionPtr = unit.frictionDataPtr;
+				blockDesc.frictionCount = unit.frictionPatchCount;
+				blockDesc.maxCCDSeparation = (flags & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD) ? ccdMaxSeparation : PX_MAX_F32;
+
+			}
+
+			if(header.mStride == 4)
+			{
+				//KS - todo - plumb in axisConstraintCount into this method to keep track of the number of axes
+				state = createFinalizeMethods4[frictionType](cmOutputs, *threadContext,
+					 blockDescs,
+					 invDt,
+					 bounceThreshold,
+					 frictionOffsetThreshold,
+					 context.getCorrelationDistance(),
+					 blockAllocator);
+
+			}
+			if(SolverConstraintPrepState::eSUCCESS != state)
+			{
+				for(PxU32 i = 0; i < header.mStride; ++i)
+				{
+					PxSolverConstraintDesc& desc = contactDescPtr[header.mStartIndex+i];
+					PxsContactManager* cm = reinterpret_cast<PxsContactManager*>(desc.constraint);
+					PxcNpWorkUnit& n = cm->getWorkUnit();
+
+					PxsContactManagerOutput& output = outputs.getContactManager(n.mNpIndex);
+					
+					createFinalizeMethods[frictionType](blockDescs[i], output, *threadContext,
+						invDt, bounceThreshold, frictionOffsetThreshold, context.getCorrelationDistance(), blockAllocator);
+			
+					getContactManagerConstraintDesc(output,*cm,desc);
+				}
+			}
+
+			for (PxU32 i = 0; i < header.mStride; ++i)
+			{
+				PxsContactManager* cm = cms[i];
+
+				PxcNpWorkUnit& unit = cm->getWorkUnit();
+				unit.frictionDataPtr = blockDescs[i].frictionPtr;
+				unit.frictionPatchCount = blockDescs[i].frictionCount;
+				axisConstraintCount += blockDescs[i].axisConstraintCount;
+
+			}
+		}
+		else if(contactDescPtr[header.mStartIndex].constraintLengthOver16 == DY_SC_TYPE_RB_1D)
+		{
+
+			SolverConstraintShaderPrepDesc shaderDescs[4];
+			PxSolverConstraintPrepDesc descs[4];
+
+			PxTransform id(PxIdentity);
+
+			for (PxU32 i = 0; i < header.mStride; ++i)
+			{
+				PxSolverConstraintDesc& desc = contactDescPtr[header.mStartIndex + i];
+				const Constraint* constraint = reinterpret_cast<const Constraint*>(desc.constraint);
+
+				SolverConstraintShaderPrepDesc& shaderPrepDesc = shaderDescs[i];
+				PxSolverConstraintPrepDesc& prepDesc = descs[i];
+
+				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::NO_LINK ? 0 : desc.bodyADataIndex];
+				PxSolverBodyData* sbodyData1 = &solverBodyData[desc.linkIndexB != PxSolverConstraintDesc::NO_LINK ? 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 = &context.getConstraintWriteBackPool()[constraint->index];
+				prepDesc.disablePreprocessing = !!(constraint->flags & PxConstraintFlag::eDISABLE_PREPROCESSING);				
+				prepDesc.improvedSlerp = !!(constraint->flags & PxConstraintFlag::eIMPROVED_SLERP);
+				prepDesc.driveLimitsAreForces = !!(constraint->flags & PxConstraintFlag::eDRIVE_LIMITS_ARE_FORCES);
+				prepDesc.minResponseThreshold = constraint->minResponseThreshold;
+			}
+
+#if DY_BATCH_1D
+			SolverConstraintPrepState::Enum state = SolverConstraintPrepState::eUNBATCHABLE;
+			if(header.mStride == 4)
+			{
+				PxU32 totalRows;
+				state = setupSolverConstraint4
+					(shaderDescs, descs, dt, invDt, totalRows,
+					blockAllocator);
+
+				axisConstraintCount += totalRows;
+			}
+			if(state != SolverConstraintPrepState::eSUCCESS)
+#endif
+			{
+				for(PxU32 i = 0; i < header.mStride; ++i)
+				{
+					axisConstraintCount += SetupSolverConstraint(shaderDescs[i], descs[i], blockAllocator, dt, invDt);
+				}
+			}
+		}
+	}
+
+	threadContext->getSimStats().numAxisSolverConstraints += axisConstraintCount;
+
+	context.putThreadContext(threadContext);
+	return PxU32(axisConstraintCount); //Can't write to mThreadContext as it's shared!!!!
+}
+
+class PxsCreateFinalizeContactsTask : public Cm::Task
+{
+	PxsCreateFinalizeContactsTask& operator=(const PxsCreateFinalizeContactsTask&);
+public:
+	PxsCreateFinalizeContactsTask( const PxU32 numConstraints, PxSolverConstraintDesc* descArray, PxSolverBodyData* solverBodyData,
+		ThreadContext& threadContext, DynamicsContext& context, PxU32 startIndex, PxU32 endIndex, PxsContactManagerOutputIterator& outputs) :
+			mNumConstraints(numConstraints), mDescArray(descArray), mSolverBodyData(solverBodyData),
+			mThreadContext(threadContext), mDynamicsContext(context),
+			mOutputs(outputs),
+			mStartIndex(startIndex), mEndIndex(endIndex)
+	{}
+
+	virtual void runInternal()
+	{
+		createFinalizeContacts_Parallel(mSolverBodyData, mThreadContext, mDynamicsContext, mStartIndex, mEndIndex, mOutputs);
+	}
+
+	virtual const char* getName() const
+	{
+		return "PxsDynamics.createFinalizeContacts";
+	}
+
+public:
+	const PxU32 mNumConstraints;
+	PxSolverConstraintDesc* mDescArray;
+	PxSolverBodyData* mSolverBodyData;
+	ThreadContext& mThreadContext;
+	DynamicsContext& mDynamicsContext;
+	PxsContactManagerOutputIterator& mOutputs;
+	PxU32 mStartIndex;
+	PxU32 mEndIndex;
+};
+
+void PxsSolverCreateFinalizeConstraintsTask::runInternal()
+{
+	ThreadContext& mThreadContext = *mIslandContext.mThreadContext;
+
+
+	
+	PxU32 descCount = mThreadContext.mNumDifferentBodyConstraints;
+	PxU32 selfConstraintDescCount = mThreadContext.contactDescArraySize - mThreadContext.mNumDifferentBodyConstraints;
+
+	Ps::Array<PxU32>& accumulatedConstraintsPerPartition = mThreadContext.mConstraintsPerPartition;
+
+	PxU32 numHeaders = 0;
+	PxU32 currentPartition = 0;
+	PxU32 maxJ = descCount == 0 ? 0 : accumulatedConstraintsPerPartition[0];
+
+	const PxU32 maxBatchPartition = 0xFFFFFFFF;
+
+	const PxU32 maxBatchSize = mEnhancedDeterminism ? 1u : 4u;
+
+	PxU32 headersPerPartition = 0;
+	for(PxU32 a = 0; a < descCount;)
+	{
+		
+
+		PxU32 loopMax = PxMin(maxJ - a, maxBatchSize);
+		PxU16 j = 0;
+		if(loopMax > 0)
+		{
+			PxConstraintBatchHeader& header = mThreadContext.contactConstraintBatchHeaders[numHeaders++];
+			
+			j=1;
+			PxSolverConstraintDesc& desc = mThreadContext.orderedContactConstraints[a];
+			if(!isArticulationConstraint(desc) && (desc.constraintLengthOver16 == DY_SC_TYPE_RB_CONTACT || 
+				desc.constraintLengthOver16 == DY_SC_TYPE_RB_1D) && currentPartition < maxBatchPartition)
+			{
+				for(; j < loopMax && desc.constraintLengthOver16 == mThreadContext.orderedContactConstraints[a+j].constraintLengthOver16 && 
+					!isArticulationConstraint(mThreadContext.orderedContactConstraints[a+j]); ++j);
+			}
+			header.mStartIndex = a;
+			header.mStride = j;
+			headersPerPartition++;
+		}
+		if(maxJ == (a + j) && maxJ != descCount)
+		{
+			//Go to next partition!
+			accumulatedConstraintsPerPartition[currentPartition] = headersPerPartition;
+			headersPerPartition = 0;
+			currentPartition++;
+			maxJ = accumulatedConstraintsPerPartition[currentPartition];
+		}
+		a+= j;
+	}
+	if(descCount)
+		accumulatedConstraintsPerPartition[currentPartition] = headersPerPartition;
+
+	
+
+	accumulatedConstraintsPerPartition.forceSize_Unsafe(mThreadContext.mMaxPartitions);
+
+	PxU32 numDifferentBodyBatchHeaders = numHeaders;
+
+	for(PxU32 a = 0; a < selfConstraintDescCount; ++a)
+	{
+		PxConstraintBatchHeader& header = mThreadContext.contactConstraintBatchHeaders[numHeaders++];
+		header.mStartIndex = a + descCount;
+		header.mStride = 1;
+	}
+
+	PxU32 numSelfConstraintBatchHeaders = numHeaders - numDifferentBodyBatchHeaders;
+
+	mThreadContext.numDifferentBodyBatchHeaders = numDifferentBodyBatchHeaders;
+	mThreadContext.numSelfConstraintBatchHeaders = numSelfConstraintBatchHeaders;
+	mThreadContext.numContactConstraintBatches = numHeaders;
+
+	PX_UNUSED(descCount);
+
+	{
+		PxSolverConstraintDesc* descBegin = mThreadContext.orderedContactConstraints;
+
+		const PxU32 numThreads = getTaskManager()->getCpuDispatcher()->getWorkerCount();
+				
+		//Choose an appropriate number of constraint prep tasks. This must be proportionate to the number of constraints to prep and the number
+		//of worker threads available.
+		const PxU32 TaskBlockSize = 16;
+		const PxU32 TaskBlockLargeSize = 64;
+		const PxU32 BlockAllocationSize = 64;
+
+		PxU32 numTasks = (numHeaders+TaskBlockLargeSize-1)/TaskBlockLargeSize;
+
+		if(numTasks)
+		{
+
+			if(numTasks < numThreads)
+				numTasks = PxMax(1u, (numHeaders+TaskBlockSize-1)/TaskBlockSize);
+
+			const PxU32 constraintsPerTask = (numHeaders + numTasks-1)/numTasks;
+
+			for(PxU32 i = 0; i < numTasks; i+=BlockAllocationSize)
+			{
+				PxU32 blockSize = PxMin(numTasks - i, BlockAllocationSize);
+
+				PxsCreateFinalizeContactsTask* tasks = reinterpret_cast<PxsCreateFinalizeContactsTask*>(mContext.getTaskPool().allocate(sizeof(PxsCreateFinalizeContactsTask)*blockSize));
+
+				for(PxU32 a = 0; a < blockSize; ++a)
+				{
+					PxU32 startIndex = (a + i) * constraintsPerTask;
+					PxU32 endIndex = PxMin(startIndex + constraintsPerTask, numHeaders);
+					PxsCreateFinalizeContactsTask* pTask = PX_PLACEMENT_NEW(&tasks[a], PxsCreateFinalizeContactsTask( descCount, descBegin, mContext.mSolverBodyDataPool.begin(), mThreadContext, mContext, startIndex, endIndex, mOutputs));
+
+					pTask->setContinuation(mCont);
+					pTask->removeReference();
+				}
+			}
+		}
+	}
+}
+
+}
+}
+
+