// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of NVIDIA CORPORATION nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// Copyright (c) 2008-2025 NVIDIA Corporation. All rights reserved.  

// ****************************************************************************
// This snippet illustrates the use of PhysX immediate mode.
//
// It creates a number of box stacks on a plane, and if rendering, allows the
// user to create new stacks and fire a ball from the camera position
// ****************************************************************************

#include <ctype.h>
#include "PxPhysicsAPI.h"
#include "../snippetcommon/SnippetPVD.h"
#include "../snippetutils/SnippetUtils.h"
#include "../snippetutils/SnippetImmUtils.h"
#include "foundation/PxArray.h"
#include "PxImmediateMode.h"
#include "extensions/PxMassProperties.h"
#include "../snippetcommon/SnippetPrint.h"
#include "extensions/PxRigidActorExt.h"

#define USE_TGS 1

//Enables whether we want persistent state caching (contact cache, friction caching) or not. Avoiding persistency results in one-shot collision detection and zero friction 
//correlation but simplifies code by not longer needing to cache persistent pairs.
#define WITH_PERSISTENCY 1

//Toggles between using low-level inertia computation code or using the RigidBodyExt inertia computation code. The former can operate without the need for PxRigidDynamics being constructed.
#define USE_LOWLEVEL_INERTIA_COMPUTATION 1

//Toggles whether we batch constraints or not. Constraint batching is an optional process which can improve performance by grouping together independent constraints. These independent constraints
//can be solved in parallel by using multiple lanes of SIMD registers.
#define BATCH_CONTACTS 1

// Toggles whether we use PhysX' "immediate broadphase", or not. If we don't, a simple O(n^2) broadphase is used.
#define USE_IMMEDIATE_BROADPHASE	1

// Toggles profiling for the full step. Only for Windows.
#define PROFILE_STEP	0

using namespace physx;
using namespace immediate;
using namespace SnippetImmUtils;

static PxDefaultAllocator		gAllocator;
static PxDefaultErrorCallback	gErrorCallback;
static PxFoundation*			gFoundation = NULL;
static PxPhysics*				gPhysics	= NULL;
static PxDefaultCpuDispatcher*	gDispatcher = NULL;
static PxScene*					gScene		= NULL;
static PxMaterial*				gMaterial	= NULL;
static PxPvd*					gPvd = NULL;

static physx::PxArray<PxConstraint*>* gConstraints = NULL;

static PxReal gStackZ = 10.0f;

//Enable to 1 to use centimeter units instead of meter units.
//Instructive to demonstrate which values used in immediate mode are unit-dependent
#define USE_CM_UNITS 0

#if !USE_CM_UNITS
	static const PxReal gUnitScale = 1.0f;
	//static float cameraSpeed = 1.0f;
#else
	static const PxReal gUnitScale = 100.0f;
	//static float cameraSpeed = 100.0f;
#endif

#if USE_IMMEDIATE_BROADPHASE && WITH_PERSISTENCY
	static PxU32 gFrameIndex = 0;
#endif

#if WITH_PERSISTENCY
	#if USE_IMMEDIATE_BROADPHASE
		static PxAABBManager* gAABBManager = NULL;

		struct PersistentPair
		{
			PersistentPair()	{}
			PersistentPair(PxU32 id0, PxU32 id1) : mID0(id0), mID1(id1)	{}

			PX_INLINE bool operator == (const PersistentPair& a) const
			{
				return a.mID0 == mID0 && a.mID1 == mID1;
			}

			PxU32	mID0;
			PxU32	mID1;
		};

		struct PersistentPairData
		{
			PxRigidActor*	actor0;
			PxRigidActor*	actor1;

			PxCache	cache;
			PxU8*	frictions;
			PxU32	nbFrictions;
		};

		static PX_INLINE uint32_t PxComputeHash(const PersistentPair& p)
		{
			return PxComputeHash(uint64_t(p.mID0)|(uint64_t(p.mID1)<<32));
		}

		static PxHashMap<PersistentPair, PersistentPairData>*	gPersistentPairs = NULL;
	#else
		namespace
		{
		struct PersistentContactPair
		{
			PxCache cache;
			PxU8* frictions;
			PxU32 nbFrictions;
		};
		}

		static PxArray<PersistentContactPair>* allContactCache = NULL;
	#endif
#endif

static TestCacheAllocator* gCacheAllocator = NULL;
static TestConstraintAllocator* gConstraintAllocator = NULL;

namespace
{
struct ContactPair
{
	PxRigidActor* actor0;
	PxRigidActor* actor1;

	PxU32 idx0, idx1;

	PxU32 startContactIndex;
	PxU32 nbContacts;
};

class TestContactRecorder : public immediate::PxContactRecorder
{
	PxArray<ContactPair>& mContactPairs;
	PxArray<PxContactPoint>& mContactPoints;
	PxRigidActor& mActor0;
	PxRigidActor& mActor1;
	PxU32 mIdx0, mIdx1;
	bool mHasContacts;
public:
	
	TestContactRecorder(PxArray<ContactPair>& contactPairs, PxArray<PxContactPoint>& contactPoints, PxRigidActor& actor0, 
	PxRigidActor& actor1, PxU32 idx0, PxU32 idx1) : mContactPairs(contactPairs), mContactPoints(contactPoints),
		mActor0(actor0), mActor1(actor1), mIdx0(idx0), mIdx1(idx1), mHasContacts(false)
	{
	}

	virtual bool recordContacts(const PxContactPoint* contactPoints, PxU32 nbContacts, PxU32 index)
	{
		PX_UNUSED(index);
		{
			ContactPair pair;
			pair.actor0				= &mActor0;
			pair.actor1				= &mActor1;
			pair.nbContacts			= nbContacts;
			pair.startContactIndex	= mContactPoints.size();
			pair.idx0				= mIdx0;
			pair.idx1				= mIdx1;
			mContactPairs.pushBack(pair);
			mHasContacts = true;
		}

		for (PxU32 c = 0; c < nbContacts; ++c)
		{
			//Fill in solver-specific data that our contact gen does not produce...
			PxContactPoint point = contactPoints[c];
			point.maxImpulse		= PX_MAX_F32;
			point.targetVel			= PxVec3(0.0f);
			point.staticFriction	= 0.5f;
			point.dynamicFriction	= 0.5f;
			point.restitution		= 0.0f;
			point.damping			= 0.0f;
			point.materialFlags		= 0;
			mContactPoints.pushBack(point);
		}
		return true;
	}

	PX_FORCE_INLINE bool hasContacts() const	{ return mHasContacts; }
private:
	PX_NOCOPY(TestContactRecorder)
};
}

static bool generateContacts(	const PxGeometryHolder& geom0, const PxGeometryHolder& geom1, PxRigidActor& actor0, PxRigidActor& actor1, PxCacheAllocator& cacheAllocator,
								PxArray<PxContactPoint>& contactPoints, PxArray<ContactPair>& contactPairs, PxU32 idx0, PxU32 idx1, PxCache& cache)
{
	const PxTransform tr0 = actor0.getGlobalPose();
	const PxTransform tr1 = actor1.getGlobalPose();

	TestContactRecorder recorder(contactPairs, contactPoints, actor0, actor1, idx0, idx1);

	const PxGeometry* pxGeom0 = &geom0.any();
	const PxGeometry* pxGeom1 = &geom1.any();

	physx::immediate::PxGenerateContacts(&pxGeom0, &pxGeom1, &tr0, &tr1, &cache, 1, recorder, gUnitScale*0.04f, gUnitScale*0.01f, gUnitScale, cacheAllocator);

	return recorder.hasContacts();
}

static PxRigidDynamic* createDynamic(const PxTransform& t, const PxGeometry& geometry, const PxVec3& velocity=PxVec3(0))
{
	PxRigidDynamic* dynamic = PxCreateDynamic(*gPhysics, t, geometry, *gMaterial, 10.0f);
	dynamic->setAngularDamping(0.5f);
	dynamic->setLinearVelocity(velocity);
	gScene->addActor(*dynamic);
	return dynamic;
}

static void updateInertia(PxRigidBody* body, PxReal density)
{
#if !USE_LOWLEVEL_INERTIA_COMPUTATION
	PX_UNUSED(density);
	//Compute the inertia of the rigid body using the helper function in PxRigidBodyExt
	PxRigidBodyExt::updateMassAndInertia(*body, 10.0f);
#else
	//Compute the inertia/mass of the bodies using the more low-level PxMassProperties interface
	//This was written for readability rather than performance. Complexity can be avoided if you know that you are dealing with a single shape body

	PxU32 nbShapes = body->getNbShapes();

	//Keep track of an array of inertia tensors and local poses.
	physx::PxArray<PxMassProperties> inertias;
	physx::PxArray<PxTransform> localPoses;

	for (PxU32 a = 0; a < nbShapes; ++a)
	{
		PxShape* shape;

		body->getShapes(&shape, 1, a);
		//(1) initialize an inertia tensor based on the shape's geometry
		PxMassProperties inertia(shape->getGeometry());
		//(2) Scale the inertia tensor based on density. If you use a single density instead of a density per-shape, this could be performed just prior to
		//extracting the massSpaceInertiaTensor
		inertia = inertia * density;

		inertias.pushBack(inertia);
		localPoses.pushBack(shape->getLocalPose());
	}

	//(3)Sum all the inertia tensors - can be skipped if the shape count is 1
	PxMassProperties inertia = PxMassProperties::sum(inertias.begin(), localPoses.begin(), inertias.size());
	//(4)Get the diagonalized inertia component and frame of the mass space orientation
	PxQuat orient;
	const PxVec3 diagInertia = PxMassProperties::getMassSpaceInertia(inertia.inertiaTensor, orient);
	//(4) Set properties on the rigid body
	body->setMass(inertia.mass);
	body->setCMassLocalPose(PxTransform(inertia.centerOfMass, orient));
	body->setMassSpaceInertiaTensor(diagInertia);
#endif
}

/*static*/ void createStack(const PxTransform& t, PxU32 size, PxReal halfExtent)
{
	PxShape* shape = gPhysics->createShape(PxBoxGeometry(halfExtent, halfExtent, halfExtent), *gMaterial);
	for (PxU32 i = 0; i<size; i++)
	{
		for (PxU32 j = 0; j<size - i; j++)
		{
			PxTransform localTm(PxVec3(PxReal(j * 2) - PxReal(size - i), PxReal(i * 2 + 1), 0) * halfExtent);
			PxRigidDynamic* body = gPhysics->createRigidDynamic(t.transform(localTm));
			body->attachShape(*shape);
			
			updateInertia(body, 10.f);

			gScene->addActor(*body);
		}
	}
	shape->release();
}

struct Triangle
{
	PxU32 ind0, ind1, ind2;
};

static PxTriangleMesh* createMeshGround()
{
	const PxU32 gridSize = 8;
	const PxReal gridStep = 512.f / (gridSize-1);

	PxVec3 verts[gridSize * gridSize];

	const PxU32 nbTriangles = 2 * (gridSize - 1) * (gridSize-1);

	Triangle indices[nbTriangles];

	for (PxU32 a = 0; a < gridSize; ++a)
	{
		for (PxU32 b = 0; b < gridSize; ++b)
		{
			verts[a * gridSize + b] = PxVec3(-400.f + b*gridStep, 0.f, -400.f + a*gridStep);
		}
	}

	for (PxU32 a = 0; a < (gridSize-1); ++a)
	{
		for (PxU32 b = 0; b < (gridSize-1); ++b)
		{
			Triangle& tri0 = indices[(a * (gridSize-1) + b) * 2];
			Triangle& tri1 = indices[((a * (gridSize-1) + b) * 2) + 1];

			tri0.ind0 = a * gridSize + b + 1;
			tri0.ind1 = a * gridSize + b;
			tri0.ind2 = (a + 1) * gridSize + b + 1;

			tri1.ind0 = (a + 1) * gridSize + b + 1;
			tri1.ind1 = a * gridSize + b;
			tri1.ind2 = (a + 1) * gridSize + b;
		}
	}

	PxTriangleMeshDesc meshDesc;
	meshDesc.points.data = verts;
	meshDesc.points.count = gridSize * gridSize;
	meshDesc.points.stride = sizeof(PxVec3);
	meshDesc.triangles.count = nbTriangles;
	meshDesc.triangles.data = indices;
	meshDesc.triangles.stride = sizeof(Triangle);

	PxCookingParams cookingParams(gPhysics->getTolerancesScale());
	PxTriangleMesh* triMesh = PxCreateTriangleMesh(cookingParams, meshDesc, gPhysics->getPhysicsInsertionCallback());

	return triMesh;
}

#if WITH_PERSISTENCY && USE_IMMEDIATE_BROADPHASE
static void createBroadPhase()
{
	PxBroadPhaseDesc bpDesc(PxBroadPhaseType::eABP);
	PxBroadPhase* bp = PxCreateBroadPhase(bpDesc);
	gAABBManager = PxCreateAABBManager(*bp);

	gPersistentPairs = new PxHashMap<PersistentPair, PersistentPairData>;
}

static void releaseBroadPhase()
{
	PxBroadPhase* bp = &gAABBManager->getBroadPhase();
	PX_RELEASE(gAABBManager);
	PX_RELEASE(bp);
	delete gPersistentPairs;
}
#endif

static void updateContactPairs()
{
#if WITH_PERSISTENCY
	#if USE_IMMEDIATE_BROADPHASE
	// In this simple snippet we just recreate the broadphase structure here
	releaseBroadPhase();
	createBroadPhase();
	gFrameIndex = 0;
	#else
	allContactCache->clear();

	const PxU32 nbDynamic = gScene->getNbActors(PxActorTypeFlag::eRIGID_DYNAMIC);
	const PxU32 nbStatic = gScene->getNbActors(PxActorTypeFlag::eRIGID_STATIC);

	const PxU32 totalPairs = (nbDynamic * (nbDynamic - 1)) / 2 + nbDynamic * nbStatic;

	allContactCache->resize(totalPairs);

	for (PxU32 a = 0; a < totalPairs; ++a)
	{
		(*allContactCache)[a].frictions = NULL;
		(*allContactCache)[a].nbFrictions = 0;
		(*allContactCache)[a].cache.mCachedData = 0;
		(*allContactCache)[a].cache.mCachedSize = 0;
		(*allContactCache)[a].cache.mManifoldFlags = 0;
		(*allContactCache)[a].cache.mPairData = 0;
	}
	#endif
#endif
}

void initPhysics(bool /*interactive*/)
{
	gFoundation = PxCreateFoundation(PX_PHYSICS_VERSION, gAllocator, gErrorCallback);
	gPvd = PxCreatePvd(*gFoundation);
	PxPvdTransport* transport = PxDefaultPvdSocketTransportCreate(PVD_HOST, 5425, 10);
	gPvd->connect(*transport, PxPvdInstrumentationFlag::ePROFILE);

	gPhysics = PxCreatePhysics(PX_PHYSICS_VERSION, *gFoundation, PxTolerancesScale(), true, gPvd);

	PxSceneDesc sceneDesc(gPhysics->getTolerancesScale());
	sceneDesc.gravity = PxVec3(0.0f, -9.81f, 0.0f)*gUnitScale;
	gDispatcher = PxDefaultCpuDispatcherCreate(0);
	sceneDesc.cpuDispatcher	= gDispatcher;
	sceneDesc.filterShader	= PxDefaultSimulationFilterShader;

	gScene = gPhysics->createScene(sceneDesc);

	gMaterial = gPhysics->createMaterial(0.5f, 0.5f, 0.2f);

	gConstraints = new physx::PxArray<PxConstraint*>();

	const bool useGroundMesh = true;	// Use a triangle mesh or a plane for the ground
	if(useGroundMesh)
	{
		PxTriangleMesh* mesh = createMeshGround();

		PxTriangleMeshGeometry geom(mesh);

		PxRigidStatic* groundMesh = gPhysics->createRigidStatic(PxTransform(PxVec3(0, 2, 0)));
		PxRigidActorExt::createExclusiveShape(*groundMesh, geom, *gMaterial);
		gScene->addActor(*groundMesh);
	}
	else
	{
		PxRigidStatic* groundPlane = PxCreatePlane(*gPhysics, PxPlane(0,1,0,0), *gMaterial);
		gScene->addActor(*groundPlane);
	}

	for (PxU32 i = 0; i<4; i++)
		createStack(PxTransform(PxVec3(0, 2, gStackZ -= 10.0f*gUnitScale)), 20, gUnitScale);

	PxRigidDynamic* ball = createDynamic(PxTransform(PxVec3(0, 20, 100)*gUnitScale), PxSphereGeometry(2 * gUnitScale), PxVec3(0, -25, -100)*gUnitScale);
	PxRigidDynamic* ball2 = createDynamic(PxTransform(PxVec3(0, 24, 100)*gUnitScale), PxSphereGeometry(2 * gUnitScale), PxVec3(0, -25, -100)*gUnitScale);
	PxRigidDynamic* ball3 = createDynamic(PxTransform(PxVec3(0, 27, 100)*gUnitScale), PxSphereGeometry(2 * gUnitScale), PxVec3(0, -25, -100)*gUnitScale);
	updateInertia(ball, 1000.f);
	updateInertia(ball2, 1000.f);

	PxD6Joint* joint = PxD6JointCreate(*gPhysics, ball, PxTransform(PxVec3(0, 4, 0)*gUnitScale), ball2, PxTransform(PxVec3(0, -2, 0)*gUnitScale));
	PxD6Joint* joint2 = PxD6JointCreate(*gPhysics, ball2, PxTransform(PxVec3(0, 4, 0)*gUnitScale), ball3, PxTransform(PxVec3(0, -2, 0)*gUnitScale));
	
	gConstraints->pushBack(joint->getConstraint());
	gConstraints->pushBack(joint2->getConstraint());

	gCacheAllocator = new TestCacheAllocator;
	gConstraintAllocator = new TestConstraintAllocator;
#if WITH_PERSISTENCY
	#if USE_IMMEDIATE_BROADPHASE
		createBroadPhase();
	#else
		allContactCache = new PxArray<PersistentContactPair>;
	#endif
#endif

	updateContactPairs();
}

void stepPhysics(bool /*interactive*/)
{
#if PROFILE_STEP
	PxU64 time = __rdtsc();
#endif
	gCacheAllocator->reset();
	gConstraintAllocator->release();

	const PxU32 nbStatics = gScene->getNbActors(PxActorTypeFlag::eRIGID_STATIC);
	const PxU32 nbDynamics = gScene->getNbActors(PxActorTypeFlag::eRIGID_DYNAMIC);

	const PxU32 totalActors = nbDynamics + nbStatics;

	PxArray<ContactPair> activeContactPairs;
	PxArray<PxContactPoint> contactPoints;

	activeContactPairs.reserve(4 * totalActors);
	
	PxArray<PxActor*> actors(totalActors);
	PxArray<PxBounds3> shapeBounds(totalActors+1);
	PxArray<PxGeometryHolder> mGeometries(totalActors);

#if USE_TGS
	PxArray<PxTGSSolverBodyVel> bodies(totalActors);
	PxArray<PxTGSSolverBodyData> bodyData(totalActors);
	PxArray<PxTGSSolverBodyTxInertia> txInertia(totalActors);
	PxArray<PxTransform> globalPoses(totalActors);
#else
	PxArray<PxSolverBody> bodies(totalActors);
	PxArray<PxSolverBodyData> bodyData(totalActors);
#endif

#if USE_IMMEDIATE_BROADPHASE && !WITH_PERSISTENCY
	PxArray<PxBpIndex> handles(totalActors);
	PxArray<PxBpFilterGroup> groups(totalActors);
	PxArray<float> distances(totalActors);
#endif

	gScene->getActors(PxActorTypeFlag::eRIGID_DYNAMIC, actors.begin(), nbDynamics);

	gScene->getActors(PxActorTypeFlag::eRIGID_STATIC, actors.begin() + nbDynamics, nbStatics);

#if USE_IMMEDIATE_BROADPHASE && WITH_PERSISTENCY
	gFrameIndex++;
#endif

	//Now do collision detection...Brute force every dynamic against every dynamic/static
	for (PxU32 a = 0; a < totalActors; ++a)
	{
		PxRigidActor* actor = actors[a]->is<PxRigidActor>();

		PxShape* shape;
		actor->getShapes(&shape, 1);

		//Compute the AABBs. We inflate these by 2cm margins
		shapeBounds[a] = PxShapeExt::getWorldBounds(*shape, *actor, 1.f);
		shapeBounds[a].minimum -= PxVec3(0.02f)*gUnitScale;
		shapeBounds[a].maximum += PxVec3(0.02f)*gUnitScale;

		mGeometries[a].storeAny(shape->getGeometry());

#if USE_IMMEDIATE_BROADPHASE
	#if WITH_PERSISTENCY
		if(gFrameIndex==1)
		{
			const PxBpFilterGroup group = a < nbDynamics ? PxGetBroadPhaseDynamicFilterGroup(a) : PxGetBroadPhaseStaticFilterGroup();
			gAABBManager->addObject(a, shapeBounds[a], group);
		}
		else if(a < nbDynamics)
		{
			gAABBManager->updateObject(a, &shapeBounds[a]);
		}
	#else
		handles[a] = a;
		distances[a]= 0.0f;
		if(a < nbDynamics)
			groups[a] = PxGetBroadPhaseDynamicFilterGroup(a);
		else
			groups[a] = PxGetBroadPhaseStaticFilterGroup();
	#endif
#endif
	}

#if USE_IMMEDIATE_BROADPHASE
	{
		PxBroadPhaseResults results;
	#if WITH_PERSISTENCY
		gAABBManager->updateAndFetchResults(results);

		const PxU32 nbCreatedPairs = results.mNbCreatedPairs;
		for(PxU32 i=0;i<nbCreatedPairs;i++)
		{
			const PxU32 id0 = results.mCreatedPairs[i].mID0;
			const PxU32 id1 = results.mCreatedPairs[i].mID1;

			const PersistentPair currentPair(id0, id1);

			PersistentPairData data;
			data.actor0 = actors[id0]->is<PxRigidActor>();
			data.actor1 = actors[id1]->is<PxRigidActor>();
			data.frictions = NULL;
			data.nbFrictions = 0;
			data.cache = PxCache();

			bool b = gPersistentPairs->insert(currentPair, data);
			PX_ASSERT(b);
			PX_UNUSED(b);
		}

		const PxU32 nbDeletedPairs = results.mNbDeletedPairs;
		for(PxU32 i=0;i<nbDeletedPairs;i++)
		{
			const PxU32 id0 = results.mDeletedPairs[i].mID0;
			const PxU32 id1 = results.mDeletedPairs[i].mID1;
			const PersistentPair currentPair(id0, id1);

			PxHashMap<PersistentPair, PersistentPairData>::Entry removedEntry;
			bool b = gPersistentPairs->erase(currentPair, removedEntry);
			PX_ASSERT(b);
			PX_UNUSED(b);
		}
	#else
		PxBroadPhaseDesc bpDesc(PxBroadPhaseType::eABP);
		PxBroadPhase* bp = PxCreateBroadPhase(bpDesc);

		const PxBroadPhaseUpdateData updateData(handles.begin(), totalActors, NULL, 0, NULL, 0, shapeBounds.begin(), groups.begin(), distances.begin(), totalActors);
		bp->update(results, updateData);

		const PxU32 nbPairs = results.mNbCreatedPairs;
		for(PxU32 i=0;i<nbPairs;i++)
		{
			const PxU32 id0 = results.mCreatedPairs[i].mID0;
			const PxU32 id1 = results.mCreatedPairs[i].mID1;

			ContactPair pair;
			pair.actor0 = actors[id0]->is<PxRigidActor>();
			pair.actor1 = actors[id1]->is<PxRigidActor>();
			pair.idx0 = id0;
			pair.idx1 = id1;

			activeContactPairs.pushBack(pair);
		}
		PX_RELEASE(bp);
	#endif
	}
#else
	{
		//Broad phase for active pairs...
		for (PxU32 a = 0; a < nbDynamics; ++a)
		{
			PxRigidDynamic* dyn0 = actors[a]->is<PxRigidDynamic>();
			for (PxU32 b = a + 1; b < totalActors; ++b)
			{
				PxRigidActor* actor1 = actors[b]->is<PxRigidActor>();

				if (shapeBounds[a].intersects(shapeBounds[b]))
				{
					ContactPair pair;
					pair.actor0 = dyn0;
					pair.actor1 = actor1;
					pair.idx0 = a;
					pair.idx1 = b;

					activeContactPairs.pushBack(pair);
				}
	#if WITH_PERSISTENCY
				else
				{
					const PxU32 startIndex = a == 0 ? 0 : (a * totalActors) - (a * (a + 1)) / 2;

					PersistentContactPair& persistentData = (*allContactCache)[startIndex + (b - a - 1)];

					//No collision detection performed at all so clear contact cache and friction data
					persistentData.frictions = NULL;
					persistentData.nbFrictions = 0;
					persistentData.cache = PxCache();
				}
	#endif
			}
		}
	}
#endif

#if USE_IMMEDIATE_BROADPHASE && WITH_PERSISTENCY
	const PxU32 nbActivePairs = gPersistentPairs->size();
	activeContactPairs.forceSize_Unsafe(0);
	contactPoints.reserve(4 * nbActivePairs);

	for(PxHashMap<PersistentPair, PersistentPairData>::Iterator iter = gPersistentPairs->getIterator(); !iter.done(); ++iter)
	{
		const PxU32 id0 = iter->first.mID0;
		const PxU32 id1 = iter->first.mID1;
		PxRigidActor* dyn0 = iter->second.actor0;
		const PxGeometryHolder& holder0 = mGeometries[id0];

		PxRigidActor* actor1 = iter->second.actor1;
		const PxGeometryHolder& holder1 = mGeometries[id1];

		if (!generateContacts(holder0, holder1, *dyn0, *actor1, *gCacheAllocator, contactPoints, activeContactPairs, id0, id1, iter->second.cache))
		{
			//Contact generation run but no touches found so clear cached friction data
			iter->second.frictions = NULL;
			iter->second.nbFrictions = 0;
		}
	}
#else
	const PxU32 nbActivePairs = activeContactPairs.size();
	ContactPair* activePairs = activeContactPairs.begin();

	activeContactPairs.forceSize_Unsafe(0);

	contactPoints.reserve(4 * nbActivePairs);

	for (PxU32 a = 0; a < nbActivePairs; ++a)
	{
		const ContactPair& pair = activePairs[a];

		PxRigidActor* dyn0 = pair.actor0;
		const PxGeometryHolder& holder0 = mGeometries[pair.idx0];

		PxRigidActor* actor1 = pair.actor1;
		const PxGeometryHolder& holder1 = mGeometries[pair.idx1];

	#if WITH_PERSISTENCY
		const PxU32 startIndex = pair.idx0 == 0 ? 0 : (pair.idx0 * totalActors) - (pair.idx0 * (pair.idx0 + 1)) / 2;

		PersistentContactPair& persistentData = (*allContactCache)[startIndex + (pair.idx1 - pair.idx0 - 1)];

		if (!generateContacts(holder0, holder1, *dyn0, *actor1, *gCacheAllocator, contactPoints, activeContactPairs, pair.idx0, pair.idx1, persistentData.cache))
		{
			//Contact generation run but no touches found so clear cached friction data
			persistentData.frictions = NULL;
			persistentData.nbFrictions = 0;
		}
	#else
		PxCache cache;
		generateContacts(holder0, holder1, *dyn0, *actor1, *gCacheAllocator, contactPoints, activeContactPairs, pair.idx0, pair.idx1, cache);
	#endif
	}
#endif
	
	const PxReal dt = 1.f / 60.f;
	const PxReal invDt = 60.f;

	const PxU32 nbPositionIterations = 4;
	const PxU32 nbVelocityIterations = 1;

#if USE_TGS
	const PxReal stepDt = dt/PxReal(nbPositionIterations);
	const PxReal invStepDt = invDt * PxReal(nbPositionIterations);
#endif

	const PxVec3 gravity(0.f, -9.8f* gUnitScale, 0.f);

	//Construct solver bodies
	for (PxU32 a = 0; a < nbDynamics; ++a)
	{
		PxRigidDynamic* dyn = actors[a]->is<PxRigidDynamic>();

		immediate::PxRigidBodyData data;
		data.linearVelocity				= dyn->getLinearVelocity();
		data.angularVelocity			= dyn->getAngularVelocity();
		data.invMass					= dyn->getInvMass();
		data.invInertia					= dyn->getMassSpaceInvInertiaTensor();		
		data.body2World					= dyn->getGlobalPose();
		data.maxDepenetrationVelocity	= dyn->getMaxDepenetrationVelocity();
		data.maxContactImpulse			= dyn->getMaxContactImpulse();
		data.linearDamping				= dyn->getLinearDamping();
		data.angularDamping				= dyn->getAngularDamping();
		data.maxLinearVelocitySq		= 100.f*100.f*gUnitScale*gUnitScale;
		data.maxAngularVelocitySq		= 7.f*7.f;

#if USE_TGS
		physx::immediate::PxConstructSolverBodiesTGS(&data, &bodies[a], &txInertia[a], &bodyData[a], 1, gravity, dt);
		globalPoses[a] = data.body2World;
#else
		physx::immediate::PxConstructSolverBodies(&data, &bodyData[a], 1, gravity, dt);
#endif
		dyn->userData = reinterpret_cast<void*>(size_t(a));
	}

	//Construct static bodies
	for (PxU32 a = nbDynamics; a < totalActors; ++a)
	{
		PxRigidStatic* sta = actors[a]->is<PxRigidStatic>();
#if USE_TGS
		physx::immediate::PxConstructStaticSolverBodyTGS(sta->getGlobalPose(), bodies[a], txInertia[a], bodyData[a]);
		globalPoses[a] = sta->getGlobalPose();
#else
		physx::immediate::PxConstructStaticSolverBody(sta->getGlobalPose(), bodyData[a]);
#endif
		sta->userData = reinterpret_cast<void*>(size_t(a));
	}

	PxArray<PxSolverConstraintDesc> descs(activeContactPairs.size() + gConstraints->size());

	for (PxU32 a = 0; a < activeContactPairs.size(); ++a)
	{
		PxSolverConstraintDesc& desc = descs[a];
		ContactPair& pair = activeContactPairs[a];

		//Set body pointers and bodyData idxs
#if USE_TGS
		desc.tgsBodyA = &bodies[pair.idx0];
		desc.tgsBodyB = &bodies[pair.idx1];
#else
		desc.bodyA = &bodies[pair.idx0];
		desc.bodyB = &bodies[pair.idx1];
#endif

		desc.bodyADataIndex = pair.idx0;
		desc.bodyBDataIndex = pair.idx1;
		desc.linkIndexA = PxSolverConstraintDesc::RIGID_BODY;
		desc.linkIndexB = PxSolverConstraintDesc::RIGID_BODY;

		//Cache pointer to our contact data structure and identify which type of constraint this is. We'll need this later after batching.
		//We store our pointer in the desc.constraint field as this field is ignored by PxBatchConstraints. It will later be overwritten by PxCreateContactConstraints.
		//If we choose not to perform batching and instead just create a single header per-pair, then this would not be necessary because
		//the constraintDescs would not have been reordered
		desc.constraint = reinterpret_cast<PxU8*>(&pair);
		desc.constraintType = PxSolverConstraintDesc::eCONTACT_CONSTRAINT;
	}

	for (PxU32 a = 0; a < gConstraints->size(); ++a)
	{
		PxConstraint* constraint = (*gConstraints)[a];

		PxSolverConstraintDesc& desc = descs[activeContactPairs.size() + a];

		PxRigidActor* actor0, *actor1;
		constraint->getActors(actor0, actor1);

		const PxU32 id0 = PxU32(size_t(actor0->userData));
		const PxU32 id1 = PxU32(size_t(actor1->userData));

#if USE_TGS
		desc.tgsBodyA = &bodies[id0];
		desc.tgsBodyB = &bodies[id1];
#else
		desc.bodyA = &bodies[id0];
		desc.bodyB = &bodies[id1];
#endif

		desc.bodyADataIndex = PxU16(id0);
		desc.bodyBDataIndex = PxU16(id1);
		desc.linkIndexA = PxSolverConstraintDesc::RIGID_BODY;
		desc.linkIndexB = PxSolverConstraintDesc::RIGID_BODY;

		desc.constraint = reinterpret_cast<PxU8*>(constraint);
		desc.constraintType = PxSolverConstraintDesc::eJOINT_CONSTRAINT;
	}

	PxArray<PxConstraintBatchHeader> headers(descs.size());
	PxArray<PxReal> contactForces(contactPoints.size());

	//Technically, you can batch the contacts and joints all at once using a single call but doing so mixes them in the orderedDescs array, which means that it is impossible to 
	//batch all contact or all joint dispatches into a single call. While we don't do this in this snippet (we instead process a single header at a time), our approach could be extended to
	//dispatch all contact headers at once if that was necessary.

#if BATCH_CONTACTS
	PxArray<PxSolverConstraintDesc> tempOrderedDescs(descs.size());
	physx::PxArray<PxSolverConstraintDesc>& orderedDescs = tempOrderedDescs;
	#if USE_TGS
	//1 batch the contacts
	const PxU32 nbContactHeaders = physx::immediate::PxBatchConstraintsTGS(descs.begin(), activeContactPairs.size(), bodies.begin(), nbDynamics, headers.begin(), orderedDescs.begin());

	//2 batch the joints...
	const PxU32 nbJointHeaders = physx::immediate::PxBatchConstraintsTGS(descs.begin() + activeContactPairs.size(), gConstraints->size(), bodies.begin(), nbDynamics, headers.begin() + nbContactHeaders, orderedDescs.begin() + activeContactPairs.size());
	#else
	//1 batch the contacts
	const PxU32 nbContactHeaders = physx::immediate::PxBatchConstraints(descs.begin(), activeContactPairs.size(), bodies.begin(), nbDynamics, headers.begin(), orderedDescs.begin());

	//2 batch the joints...
	const PxU32 nbJointHeaders = physx::immediate::PxBatchConstraints(descs.begin() + activeContactPairs.size(), gConstraints->size(), bodies.begin(), nbDynamics, headers.begin() + nbContactHeaders, orderedDescs.begin() + activeContactPairs.size());
	#endif
#else	
	physx::PxArray<PxSolverConstraintDesc>& orderedDescs = descs;
	
	//We are bypassing the constraint batching so we create dummy PxConstraintBatchHeaders
	const PxU32 nbContactHeaders = activeContactPairs.size();
	const PxU32 nbJointHeaders = gConstraints->size();

	for (PxU32 i = 0; i < nbContactHeaders; ++i)
	{
		PxConstraintBatchHeader& hdr = headers[i];
		hdr.startIndex = i;
		hdr.stride = 1;
		hdr.constraintType = PxSolverConstraintDesc::eCONTACT_CONSTRAINT;
	}

	for (PxU32 i = 0; i < nbJointHeaders; ++i)
	{
		PxConstraintBatchHeader& hdr = headers[nbContactHeaders+i];
		hdr.startIndex = i;
		hdr.stride = 1;
		hdr.constraintType = PxSolverConstraintDesc::eJOINT_CONSTRAINT;
	}
#endif

	const PxU32 totalHeaders = nbContactHeaders + nbJointHeaders;

	headers.forceSize_Unsafe(totalHeaders);

	//1 - Create all the contact constraints. We do this by looping over all the headers and, for each header, constructing the PxSolverContactDesc objects, then creating that contact constraint.
	//We could alternatively create all the PxSolverContactDesc objects in a single pass, then create batch create that constraint
	for (PxU32 i = 0; i < nbContactHeaders; ++i)
	{
		PxConstraintBatchHeader& header = headers[i];

		PX_ASSERT(header.constraintType == PxSolverConstraintDesc::eCONTACT_CONSTRAINT);
#if USE_TGS
		PxTGSSolverContactDesc contactDescs[4];
#else
		PxSolverContactDesc contactDescs[4];
#endif

#if WITH_PERSISTENCY
		const ContactPair* pairs[4];
#endif
		for (PxU32 a = 0; a < header.stride; ++a)
		{
			PxSolverConstraintDesc& constraintDesc = orderedDescs[header.startIndex + a];
#if USE_TGS
			PxTGSSolverContactDesc& contactDesc = contactDescs[a];
#else
			PxSolverContactDesc& contactDesc = contactDescs[a];
#endif
			PxMemZero(&contactDesc, sizeof(contactDesc));
			//Extract the contact pair that we saved in this structure earlier.
			const ContactPair& pair = *reinterpret_cast<const ContactPair*>(constraintDesc.constraint);

#if WITH_PERSISTENCY
			pairs[a] = &pair;
#endif
#if USE_TGS
			contactDesc.body0 = constraintDesc.tgsBodyA;
			contactDesc.body1 = constraintDesc.tgsBodyB;
			contactDesc.bodyData0 = &bodyData[constraintDesc.bodyADataIndex];
			contactDesc.bodyData1 = &bodyData[constraintDesc.bodyBDataIndex];
			contactDesc.body0TxI = &txInertia[constraintDesc.bodyADataIndex];
			contactDesc.body1TxI = &txInertia[constraintDesc.bodyBDataIndex];

			//This may seem redundant but the bodyFrame is not defined by the bodyData object when using articulations. This
			//example does not use articulations.
			contactDesc.bodyFrame0 = globalPoses[constraintDesc.bodyADataIndex];
			contactDesc.bodyFrame1 = globalPoses[constraintDesc.bodyBDataIndex];
#else
			contactDesc.body0 = constraintDesc.bodyA;
			contactDesc.body1 = constraintDesc.bodyB;
			contactDesc.data0 = &bodyData[constraintDesc.bodyADataIndex];
			contactDesc.data1 = &bodyData[constraintDesc.bodyBDataIndex];

			//This may seem redundant but the bodyFrame is not defined by the bodyData object when using articulations. This
			//example does not use articulations.
			contactDesc.bodyFrame0 = contactDesc.data0->body2World;
			contactDesc.bodyFrame1 = contactDesc.data1->body2World;
#endif
			contactDesc.contactForces = &contactForces[pair.startContactIndex];
			contactDesc.contacts = &contactPoints[pair.startContactIndex];
			contactDesc.numContacts = pair.nbContacts;

#if WITH_PERSISTENCY
	#if USE_IMMEDIATE_BROADPHASE
			const PersistentPair currentPair(pair.idx0, pair.idx1);
			const PxHashMap<PersistentPair, PersistentPairData>::Entry* e = gPersistentPairs->find(currentPair);
			contactDesc.frictionPtr = e->second.frictions;
			contactDesc.frictionCount = PxU8(e->second.nbFrictions);
	#else
			const PxU32 startIndex = pair.idx0 == 0 ? 0 : (pair.idx0 * totalActors) - (pair.idx0 * (pair.idx0 + 1)) / 2;
			contactDesc.frictionPtr = (*allContactCache)[startIndex + (pair.idx1 - pair.idx0 - 1)].frictions;
			contactDesc.frictionCount = PxU8((*allContactCache)[startIndex + (pair.idx1 - pair.idx0 - 1)].nbFrictions);
	#endif
#else
			contactDesc.frictionPtr = NULL;
			contactDesc.frictionCount = 0;
#endif
			contactDesc.shapeInteraction = NULL;
			contactDesc.maxCCDSeparation = PX_MAX_F32;
			
			contactDesc.bodyState0 = PxSolverConstraintPrepDescBase::eDYNAMIC_BODY;
			contactDesc.bodyState1 = pair.actor1->is<PxRigidDynamic>() ? PxSolverConstraintPrepDescBase::eDYNAMIC_BODY : PxSolverConstraintPrepDescBase::eSTATIC_BODY;
			contactDesc.desc = &constraintDesc;
			contactDesc.invMassScales.angular0 = contactDesc.invMassScales.angular1 = contactDesc.invMassScales.linear0 = contactDesc.invMassScales.linear1 = 1.f;
		}

#if USE_TGS
		immediate::PxCreateContactConstraintsTGS(&header, 1, contactDescs, *gConstraintAllocator, invStepDt, invDt, -2.f * gUnitScale, 0.04f * gUnitScale, 0.025f * gUnitScale);
#else
		immediate::PxCreateContactConstraints(&header, 1, contactDescs, *gConstraintAllocator, invDt, -2.f * gUnitScale, 0.04f * gUnitScale, 0.025f * gUnitScale);
#endif

#if WITH_PERSISTENCY
		for (PxU32 a = 0; a < header.stride; ++a)
		{
			//Cache friction information...
	#if USE_TGS
			PxTGSSolverContactDesc& contactDesc = contactDescs[a];
	#else
			PxSolverContactDesc& contactDesc = contactDescs[a];
	#endif
			//PxSolverConstraintDesc& constraintDesc = orderedDescs[header.startIndex + a];
			const ContactPair& pair = *pairs[a];

	#if USE_IMMEDIATE_BROADPHASE
			const PersistentPair currentPair(pair.idx0, pair.idx1);
			PxHashMap<PersistentPair, PersistentPairData>::Entry* e = const_cast<PxHashMap<PersistentPair, PersistentPairData>::Entry*>(gPersistentPairs->find(currentPair));
			e->second.frictions = contactDesc.frictionPtr;
			e->second.nbFrictions = contactDesc.frictionCount;
	#else
			const PxU32 startIndex = pair.idx0 == 0 ? 0 : (pair.idx0 * totalActors) - (pair.idx0 * (pair.idx0 + 1)) / 2;
			(*allContactCache)[startIndex + (pair.idx1 - pair.idx0 - 1)].frictions = contactDesc.frictionPtr;
			(*allContactCache)[startIndex + (pair.idx1 - pair.idx0 - 1)].nbFrictions = contactDesc.frictionCount;
	#endif
		}
#endif
	}

	for (PxU32 i = nbContactHeaders; i < totalHeaders; ++i)
	{
		PxConstraintBatchHeader& header = headers[i];

		PX_ASSERT(header.constraintType == PxSolverConstraintDesc::eJOINT_CONSTRAINT);

		{
#if USE_TGS
			PxTGSSolverConstraintPrepDesc jointDescs[4];
#else
			PxSolverConstraintPrepDesc jointDescs[4];
#endif
			PxConstraint* constraints[4];

			header.startIndex += activeContactPairs.size();

			for (PxU32 a = 0; a < header.stride; ++a)
			{
				PxSolverConstraintDesc& constraintDesc = orderedDescs[header.startIndex + a];
				//Extract the contact pair that we saved in this structure earlier.
				PxConstraint& constraint = *reinterpret_cast<PxConstraint*>(constraintDesc.constraint);

				constraints[a] = &constraint;
#if USE_TGS
				PxTGSSolverConstraintPrepDesc& jointDesc = jointDescs[a];
				jointDesc.body0 = constraintDesc.tgsBodyA;
				jointDesc.body1 = constraintDesc.tgsBodyB;
				jointDesc.bodyData0 = &bodyData[constraintDesc.bodyADataIndex];
				jointDesc.bodyData1 = &bodyData[constraintDesc.bodyBDataIndex];
				jointDesc.body0TxI = &txInertia[constraintDesc.bodyADataIndex];
				jointDesc.body1TxI = &txInertia[constraintDesc.bodyBDataIndex];

				//This may seem redundant but the bodyFrame is not defined by the bodyData object when using articulations. This
				//example does not use articulations.
				jointDesc.bodyFrame0 = globalPoses[constraintDesc.bodyADataIndex];
				jointDesc.bodyFrame1 = globalPoses[constraintDesc.bodyBDataIndex];
#else
				PxSolverConstraintPrepDesc& jointDesc = jointDescs[a];
				jointDesc.body0 = constraintDesc.bodyA;
				jointDesc.body1 = constraintDesc.bodyB;
				jointDesc.data0 = &bodyData[constraintDesc.bodyADataIndex];
				jointDesc.data1 = &bodyData[constraintDesc.bodyBDataIndex];

				//This may seem redundant but the bodyFrame is not defined by the bodyData object when using articulations. This
				//example does not use articulations.
				jointDesc.bodyFrame0 = jointDesc.data0->body2World;
				jointDesc.bodyFrame1 = jointDesc.data1->body2World;
#endif
				PxRigidActor* actor0, *actor1;

				constraint.getActors(actor0, actor1);

				jointDesc.bodyState0 = PxSolverConstraintPrepDescBase::eDYNAMIC_BODY;
				jointDesc.bodyState1 = actor1 == NULL ? PxSolverConstraintPrepDescBase::eSTATIC_BODY : actor1->is<PxRigidDynamic>() ? PxSolverConstraintPrepDescBase::eDYNAMIC_BODY : PxSolverConstraintPrepDescBase::eSTATIC_BODY;
				jointDesc.desc = &constraintDesc;
				jointDesc.invMassScales.angular0 = jointDesc.invMassScales.angular1 = jointDesc.invMassScales.linear0 = jointDesc.invMassScales.linear1 = 1.f;
				jointDesc.writeback = NULL;
				constraint.getBreakForce(jointDesc.linBreakForce, jointDesc.angBreakForce);
				jointDesc.minResponseThreshold = constraint.getMinResponseThreshold();
				jointDesc.disablePreprocessing = !!(constraint.getFlags() & PxConstraintFlag::eDISABLE_PREPROCESSING);				
				jointDesc.improvedSlerp = !!(constraint.getFlags() & PxConstraintFlag::eIMPROVED_SLERP);
				jointDesc.driveLimitsAreForces = !!(constraint.getFlags() & PxConstraintFlag::eDRIVE_LIMITS_ARE_FORCES);
			}

#if USE_TGS
			immediate::PxCreateJointConstraintsWithShadersTGS(&header, 1, constraints, jointDescs, *gConstraintAllocator, stepDt, dt, invStepDt, invDt, 1.f);
#else
			immediate::PxCreateJointConstraintsWithShaders(&header, 1, constraints, jointDescs, *gConstraintAllocator, dt, invDt);
#endif
		}
	}

#if USE_TGS
	immediate::PxSolveConstraintsTGS(headers.begin(), headers.size(), orderedDescs.begin(), bodies.begin(), txInertia.begin(), nbDynamics, nbPositionIterations, nbVelocityIterations, stepDt, invStepDt);

	immediate::PxIntegrateSolverBodiesTGS(bodies.begin(), txInertia.begin(), globalPoses.begin(), nbDynamics, dt);

	for (PxU32 a = 0; a < nbDynamics; ++a)
	{
		PxRigidDynamic* dynamic = actors[a]->is<PxRigidDynamic>();

		const PxTGSSolverBodyVel& body = bodies[a];

		dynamic->setLinearVelocity(body.linearVelocity);
		dynamic->setAngularVelocity(body.angularVelocity);
		dynamic->setGlobalPose(globalPoses[a]);
	}
#else
	//Solve all the constraints produced earlier. Intermediate motion linear/angular velocity buffers are filled in. These contain intermediate delta velocity information that is used
	//the PxIntegrateSolverBody
	PxArray<PxVec3> motionLinearVelocity(nbDynamics);
	PxArray<PxVec3> motionAngularVelocity(nbDynamics);

	//Zero the bodies array. This buffer contains the delta velocities and are accumulated during the simulation. For correct behavior, it is vital
	//that this buffer is zeroed.
	PxMemZero(bodies.begin(), bodies.size() * sizeof(PxSolverBody));

	immediate::PxSolveConstraints(headers.begin(), headers.size(), orderedDescs.begin(), bodies.begin(), motionLinearVelocity.begin(), motionAngularVelocity.begin(), nbDynamics, nbPositionIterations, nbVelocityIterations);

	immediate::PxIntegrateSolverBodies(bodyData.begin(), bodies.begin(), motionLinearVelocity.begin(), motionAngularVelocity.begin(), nbDynamics, dt);

	for (PxU32 a = 0; a < nbDynamics; ++a)
	{
		PxRigidDynamic* dynamic = actors[a]->is<PxRigidDynamic>();

		const PxSolverBodyData& data = bodyData[a];

		dynamic->setLinearVelocity(data.linearVelocity);
		dynamic->setAngularVelocity(data.angularVelocity);
		dynamic->setGlobalPose(data.body2World);
	}
#endif

#if PROFILE_STEP
	time = __rdtsc() - time;
	printf("Time: %d\n", PxU32(time/1024));
#endif
}
	
void cleanupPhysics(bool /*interactive*/)
{
	PX_RELEASE(gScene);
	PX_RELEASE(gDispatcher);
	PX_RELEASE(gPhysics);

	if(gPvd)
	{
		PxPvdTransport* transport = gPvd->getTransport();
		PX_RELEASE(gPvd);
		PX_RELEASE(transport);
	}

	delete gCacheAllocator;
	delete gConstraintAllocator;

#if WITH_PERSISTENCY
	#if USE_IMMEDIATE_BROADPHASE
	releaseBroadPhase();
	#else
	delete allContactCache;
	#endif
#endif

	PX_RELEASE(gFoundation);

	printf("SnippetImmediateMode done.\n");
}

void keyPress(unsigned char key, const PxTransform& camera)
{
	switch(toupper(key))
	{
		case ' ':	createDynamic(camera, PxSphereGeometry(3.0f*gUnitScale), camera.rotate(PxVec3(0, 0, -1)) * 200*gUnitScale);	updateContactPairs();  break;
	}
}

int snippetMain(int, const char*const*)
{
#ifdef RENDER_SNIPPET
	extern void renderLoop();
	renderLoop();
#else
	static const PxU32 frameCount = 100;
	initPhysics(false);
	for(PxU32 i=0; i<frameCount; i++)
		stepPhysics(false);
	cleanupPhysics(false);
#endif

	return 0;
}
