//
// 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-2021 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 "ScBodySim.h"
#include "ScShapeSim.h"
#include "ScScene.h"
#include "ScArticulationSim.h"
#include "PxsContext.h"
#include "PxsSimpleIslandManager.h"
#include "PxsSimulationController.h"

using namespace physx;
using namespace physx::Dy;
using namespace Sc;

#define PX_FREEZE_INTERVAL 1.5f
#define PX_FREE_EXIT_THRESHOLD 4.f
#define PX_FREEZE_TOLERANCE 0.25f

#define PX_SLEEP_DAMPING	0.5f
#define PX_FREEZE_SCALE		0.9f

BodySim::BodySim(Scene& scene, BodyCore& core, bool compound) :
	RigidSim				(scene, core),
	mLLBody					(&core.getCore(), PX_FREEZE_INTERVAL),
	mNodeIndex				(IG_INVALID_NODE),
	mInternalFlags			(0),
	mVelModState			(VMF_GRAVITY_DIRTY),
	mActiveListIndex		(SC_NOT_IN_SCENE_INDEX),
	mActiveCompoundListIndex(SC_NOT_IN_SCENE_INDEX),
	mArticulation			(NULL),
	mConstraintGroup		(NULL)
{
	core.getCore().numCountedInteractions = 0;
	core.getCore().numBodyInteractions = 0;
	core.getCore().disableGravity = core.getActorFlags() & PxActorFlag::eDISABLE_GRAVITY;
	if(core.getFlags() & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD)
		mLLBody.mInternalFlags |= PxsRigidBody::eSPECULATIVE_CCD;

	//If a body pending insertion was given a force/torque then it will have 
	//the dirty flags stored in a separate structure.  Copy them across
	//so we can use them now that the BodySim is constructed.
	SimStateData* simStateData = core.getSimStateData(false);
	bool hasPendingForce = false;
	if(simStateData)
	{
		VelocityMod* velmod = simStateData->getVelocityModData();
		hasPendingForce = (velmod->flags != 0) && 
			(!velmod->getLinearVelModPerSec().isZero() || !velmod->getAngularVelModPerSec().isZero() ||
			 !velmod->getLinearVelModPerStep().isZero() || !velmod->getAngularVelModPerStep().isZero());
		mVelModState = velmod->flags;	
		velmod->flags = 0;
	}

	// PT: don't read the core ptr we just wrote, use input param
	// PT: at time of writing we get a big L2 here because even though bodycore has been prefetched, the wake counter is 160 bytes away
	const bool isAwake =	(core.getWakeCounter() > 0) || 
							(!core.getLinearVelocity().isZero()) ||
							(!core.getAngularVelocity().isZero()) || 
							hasPendingForce;

	const bool isKine = isKinematic();

	IG::SimpleIslandManager* simpleIslandManager = scene.getSimpleIslandManager();
	if (!isArticulationLink())
	{
		mNodeIndex = simpleIslandManager->addRigidBody(&mLLBody, isKine, isAwake);
	}
	else
	{
		if(mArticulation)
		{
			const ArticulationLinkHandle articLinkhandle = mArticulation->getLinkHandle(*this);
			IG::NodeIndex index = mArticulation->getIslandNodeIndex();
			mNodeIndex.setIndices(index.index(), articLinkhandle & (DY_ARTICULATION_MAX_SIZE-1));
		}
	}

	//If a user add force or torque before the body is inserted into the scene,
	//this logic will make sure pre solver stage apply external force/torque to the body
	if(hasPendingForce && !isArticulationLink())
		scene.getVelocityModifyMap().growAndSet(mNodeIndex.index());

	PX_ASSERT(mActiveListIndex == SC_NOT_IN_SCENE_INDEX);

	// A.B. need to set the compound rigid flag early enough, so that we add the rigid into 
	// active list and do not create the shape bounds
	if(compound)
		raiseInternalFlag(BF_IS_COMPOUND_RIGID);

	setActive(isAwake, ActorSim::AS_PART_OF_CREATION);

	if (isAwake)
	{
		scene.addToActiveBodyList(*this);
		PX_ASSERT(isActive());
	}
	else
	{
		mActiveListIndex = SC_NOT_IN_ACTIVE_LIST_INDEX;
		mActiveCompoundListIndex = SC_NOT_IN_ACTIVE_LIST_INDEX;
		PX_ASSERT(!isActive());

		simpleIslandManager->deactivateNode(mNodeIndex);
	}

	if (isKine)
	{
		initKinematicStateBase(core, true);

		const SimStateData* kd = core.getSimStateData(true);
		if (!kd)
		{
			core.setupSimStateData(scene.getSimStateDataPool(), true, false);
			notifyPutToSleep();  // sleep state of kinematics is fully controlled by the simulation controller not the island manager
		}
		else
		{
			PX_ASSERT(kd->isKine());
			PX_ASSERT(kd->getKinematicData()->targetValid);  // the only reason for the kinematic data to exist at that point already is if the target has been set
			PX_ASSERT(isAwake);  // the expectation is that setting a target also sets the wake counter to a positive value
			postSetKinematicTarget();
		}
	}
}

BodySim::~BodySim()
{
	Scene& scene = getScene();
	const bool active = isActive();

	getBodyCore().tearDownSimStateData(scene.getSimStateDataPool(), isKinematic() ? true : false);

	PX_ASSERT(!readInternalFlag(BF_ON_DEATHROW)); // Before 3.0 it could happen that destroy could get called twice. Assert to make sure this is fixed.
	raiseInternalFlag(BF_ON_DEATHROW);

	scene.removeBody(*this);
	PX_ASSERT(!getConstraintGroup());  // Removing from scene should erase constraint group node if it existed

	if(mArticulation)
		mArticulation->removeBody(*this);

	//Articulations are represented by a single node, so they must only be removed by the articulation and not the links!
	if(mArticulation == NULL && mNodeIndex.articulationLinkId() == 0) //If it wasn't an articulation link, then we can remove it
		scene.getSimpleIslandManager()->removeNode(mNodeIndex);

	PX_ASSERT(mActiveListIndex != SC_NOT_IN_SCENE_INDEX);

	if (active)
		scene.removeFromActiveBodyList(*this);

	mActiveListIndex = SC_NOT_IN_SCENE_INDEX;
	mActiveCompoundListIndex = SC_NOT_IN_SCENE_INDEX;

	mCore.setSim(NULL);
}

void BodySim::updateCached(Cm::BitMapPinned* shapeChangedMap)
{
	if(!(mLLBody.mInternalFlags & PxsRigidBody::eFROZEN))
	{
		ElementSim* current = getElements_();
		while(current)
		{
			static_cast<ShapeSim*>(current)->updateCached(0, shapeChangedMap);
			current = current->mNextInActor;
		}
	}
}

void BodySim::updateCached(PxsTransformCache& transformCache, Bp::BoundsArray& boundsArray)
{
	PX_ASSERT(!(mLLBody.mInternalFlags & PxsRigidBody::eFROZEN));	// PT: should not be called otherwise

	ElementSim* current = getElements_();
	while(current)
	{
		static_cast<ShapeSim*>(current)->updateCached(transformCache, boundsArray);
		current = current->mNextInActor;
	}
}

void BodySim::updateContactDistance(PxReal* contactDistance, const PxReal dt, Bp::BoundsArray& boundsArray)
{
	if (getLowLevelBody().getCore().mFlags & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD
		&& !(getLowLevelBody().mInternalFlags & PxsRigidBody::eFROZEN))
	{
		const PxVec3 linVel = getLowLevelBody().getLinearVelocity();
		const PxVec3 aVel = getLowLevelBody().getAngularVelocity();
		const PxReal inflation = linVel.magnitude() * dt;

		ElementSim* current = getElements_();
		while(current)
		{
			static_cast<ShapeSim*>(current)->updateContactDistance(contactDistance, inflation, aVel, dt, boundsArray);
			current = current->mNextInActor;
		}
	}
}

//--------------------------------------------------------------
//
// BodyCore interface implementation
//
//--------------------------------------------------------------

void BodySim::notifyAddSpatialAcceleration()
{
	//The dirty flag is stored separately in the BodySim so that we query the dirty flag before going to 
	//the expense of querying the simStateData for the velmod values.
	raiseVelocityModFlag(VMF_ACC_DIRTY);

	if(!isArticulationLink())
		getScene().getVelocityModifyMap().growAndSet(getNodeIndex().index());
}

void BodySim::notifyClearSpatialAcceleration()
{
	//The dirty flag is stored separately in the BodySim so that we query the dirty flag before going to 
	//the expense of querying the simStateData for the velmod values.
	raiseVelocityModFlag(VMF_ACC_DIRTY);
	if (!isArticulationLink())
		getScene().getVelocityModifyMap().growAndSet(getNodeIndex().index());
}

void BodySim::notifyAddSpatialVelocity()
{
	//The dirty flag is stored separately in the BodySim so that we query the dirty flag before going to 
	//the expense of querying the simStateData for the velmod values.
	raiseVelocityModFlag(VMF_VEL_DIRTY);
	if (!isArticulationLink())
		getScene().getVelocityModifyMap().growAndSet(getNodeIndex().index());
}

void BodySim::notifyClearSpatialVelocity()
{
	//The dirty flag is stored separately in the BodySim so that we query the dirty flag before going to 
	//the expense of querying the simStateData for the velmod values.
	raiseVelocityModFlag(VMF_VEL_DIRTY);
	if (!isArticulationLink())
		getScene().getVelocityModifyMap().growAndSet(getNodeIndex().index());
}

void BodySim::postActorFlagChange(PxU32 oldFlags, PxU32 newFlags)
{
	// PT: don't convert to bool if not needed
	const PxU32 wasWeightless = oldFlags & PxActorFlag::eDISABLE_GRAVITY;
	const PxU32 isWeightless = newFlags & PxActorFlag::eDISABLE_GRAVITY;

	if (isWeightless != wasWeightless)
	{
		if (mVelModState == 0)
			raiseVelocityModFlag(VMF_GRAVITY_DIRTY);

		getBodyCore().getCore().disableGravity = isWeightless!=0;
	}
}

void BodySim::postBody2WorldChange()
{
	mLLBody.saveLastCCDTransform();
	notifyShapesOfTransformChange();
}

void BodySim::postSetWakeCounter(PxReal t, bool forceWakeUp)
{
	if ((t > 0.0f) || forceWakeUp)
		notifyNotReadyForSleeping();
	else
	{
		const bool readyForSleep = checkSleepReadinessBesidesWakeCounter();
		if (readyForSleep)
			notifyReadyForSleeping();
	}
}

void BodySim::postSetKinematicTarget()
{
	PX_ASSERT(getBodyCore().getSimStateData(true));
	PX_ASSERT(getBodyCore().getSimStateData(true)->isKine());
	PX_ASSERT(getBodyCore().getSimStateData(true)->getKinematicData()->targetValid);

	raiseInternalFlag(BF_KINEMATIC_MOVED);	// Important to set this here already because trigger interactions need to have this information when being activated.
	clearInternalFlag(BF_KINEMATIC_SURFACE_VELOCITY);
}

static void updateBPGroup(ElementSim* current)
{
	while(current)
	{
		static_cast<ShapeSim*>(current)->updateBPGroup();
		current = current->mNextInActor;
	}
}

void BodySim::postSwitchToKinematic()
{
	initKinematicStateBase(getBodyCore(), false);

	// - interactions need to get refiltered to make sure that kinematic-kinematic and kinematic-static pairs get suppressed
	// - unlike postSwitchToDynamic(), constraint interactions are not marked dirty here because a transition to kinematic will put the object asleep which in turn 
	//   triggers onDeactivate_() on the constraint pairs that are active. If such pairs get deactivated, they will get removed from the list of active breakable
	//   constraints automatically.
	setActorsInteractionsDirty(InteractionDirtyFlag::eBODY_KINEMATIC, NULL, InteractionFlag::eFILTERABLE);

	getScene().getSimpleIslandManager()->setKinematic(mNodeIndex);

	//
	updateBPGroup(getElements_());
}

void BodySim::postSwitchToDynamic()
{
	mScene.getSimpleIslandManager()->setDynamic(mNodeIndex);

	setForcesToDefaults(true);

	if(getConstraintGroup())
		getConstraintGroup()->markForProjectionTreeRebuild(mScene.getProjectionManager());

	// - interactions need to get refiltered to make sure that former kinematic-kinematic and kinematic-static pairs get enabled
	// - switching from kinematic to dynamic does not change the sleep state of the body. The constraint interactions are marked dirty
	//   to check later whether they need to be activated plus potentially tracked for constraint break testing. This special treatment
	//   is necessary because constraints between two kinematic bodies are considered inactive, no matter whether one of the kinematics
	//   is active (has a target) or not.
	setActorsInteractionsDirty(InteractionDirtyFlag::eBODY_KINEMATIC, NULL, InteractionFlag::eFILTERABLE | InteractionFlag::eCONSTRAINT);

	clearInternalFlag(BF_KINEMATIC_MOVE_FLAGS);

	if(isActive())
		mScene.swapInActiveBodyList(*this);

	//
	updateBPGroup(getElements_());
}

void BodySim::postPosePreviewChange(const PxU32 posePreviewFlag)
{
	if (isActive())
	{
		if (posePreviewFlag & PxRigidBodyFlag::eENABLE_POSE_INTEGRATION_PREVIEW)
			getScene().addToPosePreviewList(*this);
		else
			getScene().removeFromPosePreviewList(*this);
	}
	else
		PX_ASSERT(!getScene().isInPosePreviewList(*this));
}

//--------------------------------------------------------------
//
// Sleeping
//
//--------------------------------------------------------------

void BodySim::activate()
{
	// Activate body
	{
		PX_ASSERT((!isKinematic()) || notInScene() || readInternalFlag(InternalFlags(BF_KINEMATIC_MOVED | BF_KINEMATIC_SURFACE_VELOCITY)));	// kinematics should only get activated when a target is set.
																								// exception: object gets newly added, then the state change will happen later
		if(!isArticulationLink())
		{
			mLLBody.mInternalFlags &= (~PxsRigidBody::eFROZEN);
			// Put in list of activated bodies. The list gets cleared at the end of a sim step after the sleep callbacks have been fired.
			getScene().onBodyWakeUp(this);
		}

		BodyCore& core = getBodyCore();
		if(core.getFlags() & PxRigidBodyFlag::eENABLE_POSE_INTEGRATION_PREVIEW)
		{
			PX_ASSERT(!getScene().isInPosePreviewList(*this));
			getScene().addToPosePreviewList(*this);
		}
		createSqBounds();
	}

	// Activate interactions
	{
		const PxU32 nbInteractions = getActorInteractionCount();

		for(PxU32 i=0; i<nbInteractions; ++i)
		{
			Ps::prefetchLine(mInteractions[PxMin(i+1,nbInteractions-1)]);
			Interaction* interaction = mInteractions[i];

			const bool isNotIGControlled = interaction->getType() != InteractionType::eOVERLAP &&
				interaction->getType() != InteractionType::eMARKER;

			if(!interaction->readInteractionFlag(InteractionFlag::eIS_ACTIVE) && (isNotIGControlled))
			{
				const bool proceed = activateInteraction(interaction, NULL);

				if (proceed && (interaction->getType() < InteractionType::eTRACKED_IN_SCENE_COUNT))
					getScene().notifyInteractionActivated(interaction);
			}
		}
	}

	//set speculative CCD bit map if speculative CCD flag is on
	{
		BodyCore& core = getBodyCore();
		if (core.getFlags() & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD)
		{
			if (isArticulationLink())
			{
				if (getNodeIndex().isValid())
					getScene().setSpeculativeCCDArticulationLink(getNodeIndex().index());
			}
			else
				getScene().setSpeculativeCCDRigidBody(getNodeIndex().index());
		}
	}
}

void BodySim::deactivate()
{
	// Deactivate interactions
	{
		const PxU32 nbInteractions = getActorInteractionCount();

		for(PxU32 i=0; i<nbInteractions; ++i)
		{
			Ps::prefetchLine(mInteractions[PxMin(i+1,nbInteractions-1)]);
			Interaction* interaction = mInteractions[i];

			const bool isNotIGControlled = interaction->getType() != InteractionType::eOVERLAP &&
				interaction->getType() != InteractionType::eMARKER;

			if (interaction->readInteractionFlag(InteractionFlag::eIS_ACTIVE) && isNotIGControlled)
			{
				const bool proceed = deactivateInteraction(interaction);
				if (proceed && (interaction->getType() < InteractionType::eTRACKED_IN_SCENE_COUNT))
					getScene().notifyInteractionDeactivated(interaction);
			}
		}
	}

	// Deactivate body
	{
		PX_ASSERT((!isKinematic()) || notInScene() || !readInternalFlag(BF_KINEMATIC_MOVED));	// kinematics should only get deactivated when no target is set.
																								// exception: object gets newly added, then the state change will happen later
		BodyCore& core = getBodyCore();
		if(!readInternalFlag(BF_ON_DEATHROW))
		{
			// Set velocity to 0.
			// Note: this is also fine if the method gets called because the user puts something to sleep (this behavior is documented in the API)
			PX_ASSERT(core.getWakeCounter() == 0.0f);
			const PxVec3 zero(0.0f);
			core.setLinearVelocityInternal(zero);
			core.setAngularVelocityInternal(zero);
	
			setForcesToDefaults(!core.getCore().disableGravity);
		}

		if(!isArticulationLink())  // Articulations have their own sleep logic.
			getScene().onBodySleep(this);

		if(core.getFlags() & PxRigidBodyFlag::eENABLE_POSE_INTEGRATION_PREVIEW)
		{
			PX_ASSERT(getScene().isInPosePreviewList(*this));
			getScene().removeFromPosePreviewList(*this);
		}
		destroySqBounds();
	}

	// reset speculative CCD bit map if speculative CCD flag is on
	{
		BodyCore& core = getBodyCore();
		if (core.getFlags() & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD)
		{
			if (isArticulationLink())
			{
				if (getNodeIndex().isValid())
					getScene().resetSpeculativeCCDArticulationLink(getNodeIndex().index());
			}
			else
				getScene().resetSpeculativeCCDRigidBody(getNodeIndex().index());
		}
	}
}

void BodySim::setActive(bool active, PxU32 infoFlag)
{
	PX_ASSERT(!active || isDynamicRigid());  // Currently there should be no need to activate an actor that does not take part in island generation

	const PxU32 asPartOfCreation = infoFlag & ActorSim::AS_PART_OF_CREATION;
	if (asPartOfCreation || isActive() != active)
	{
		PX_ASSERT(!asPartOfCreation || (getActorInteractionCount() == 0)); // On creation or destruction there should be no interactions

		if (active)
		{
			if (!asPartOfCreation)
			{
				// Inactive => Active
				getScene().addToActiveBodyList(*this);
			}

			activate();

			PX_ASSERT(asPartOfCreation || isActive());
		}
		else
		{
			if (!asPartOfCreation)
			{
				// Active => Inactive
				getScene().removeFromActiveBodyList(*this);
			}

			deactivate();

			PX_ASSERT(asPartOfCreation || (!isActive()));
		}
	}
}

void BodySim::wakeUp()
{
	setActive(true);
	notifyWakeUp(true);
}

void BodySim::putToSleep()
{
	PX_ASSERT(getBodyCore().getWakeCounter() == 0.0f);
	PX_ASSERT(getBodyCore().getLinearVelocity().isZero());
	PX_ASSERT(getBodyCore().getAngularVelocity().isZero());
#ifdef _DEBUG
	// pending forces should have been cleared at this point
	const SimStateData* sd = getBodyCore().getSimStateData(false);
	if (sd)
	{
		const VelocityMod* vm = sd->getVelocityModData();
		PX_ASSERT(vm->linearPerSec.isZero() && vm->linearPerStep.isZero() && vm->angularPerSec.isZero() && vm->angularPerStep.isZero());
	}
#endif

	setActive(false);
	notifyPutToSleep();

	clearInternalFlag(InternalFlags(BF_KINEMATIC_SETTLING | BF_KINEMATIC_SETTLING_2));	// putToSleep is used when a kinematic gets removed from the scene while the sim is running and then gets re-inserted immediately.
												// We can move this code when we look into the open task of making buffered re-insertion more consistent with the non-buffered case.
}

void BodySim::internalWakeUp(PxReal wakeCounterValue)
{
	if(mArticulation)
		mArticulation->internalWakeUp(wakeCounterValue);
	else
		internalWakeUpBase(wakeCounterValue);
}

void BodySim::internalWakeUpArticulationLink(PxReal wakeCounterValue)
{
	PX_ASSERT(mArticulation);
	internalWakeUpBase(wakeCounterValue);
}

void BodySim::internalWakeUpBase(PxReal wakeCounterValue)	//this one can only increase the wake counter, not decrease it, so it can't be used to put things to sleep!
{
	if ((!isKinematic()) && (getBodyCore().getWakeCounter() < wakeCounterValue))
	{
		PX_ASSERT(wakeCounterValue > 0.0f);
		getBodyCore().setWakeCounterFromSim(wakeCounterValue);

		//we need to update the gpu body sim because we reset the wake counter for the body core
		mScene.getSimulationController()->updateDynamic(isArticulationLink(), mNodeIndex);
		setActive(true);
		notifyWakeUp(false);
		mLLBody.mInternalFlags &= (~PxsRigidBody::eFROZEN);
	}
}

void BodySim::notifyReadyForSleeping()
{
	if(mArticulation == NULL)
		getScene().getSimpleIslandManager()->deactivateNode(mNodeIndex);
}

void BodySim::notifyNotReadyForSleeping()
{
	getScene().getSimpleIslandManager()->activateNode(mNodeIndex);
}

void BodySim::notifyWakeUp(bool /*wakeUpInIslandGen*/)
{
	getScene().getSimpleIslandManager()->activateNode(mNodeIndex);
}

void BodySim::notifyPutToSleep()
{
	getScene().getSimpleIslandManager()->putNodeToSleep(mNodeIndex);
}

void BodySim::resetSleepFilter()
{
	mLLBody.sleepAngVelAcc = PxVec3(0.0f);
	mLLBody.sleepLinVelAcc = PxVec3(0.0f);
}

//This function will be called by CPU sleepCheck code
PxReal BodySim::updateWakeCounter(PxReal dt, PxReal energyThreshold, const Cm::SpatialVector& motionVelocity)
{
	// update the body's sleep state and 
	BodyCore& core = getBodyCore();

	const PxReal wakeCounterResetTime = ScInternalWakeCounterResetValue;

	PxReal wc = core.getWakeCounter();
	
	{
		PxVec3 bcSleepLinVelAcc = mLLBody.sleepLinVelAcc;
		PxVec3 bcSleepAngVelAcc = mLLBody.sleepAngVelAcc;

		if(wc < wakeCounterResetTime * 0.5f || wc < dt)
		{
			const PxTransform& body2World = getBody2World();

			// calculate normalized energy: kinetic energy divided by mass
			const PxVec3 t = core.getInverseInertia();
			const PxVec3 inertia(t.x > 0.0f ? 1.0f/t.x : 1.0f, t.y > 0.0f ? 1.0f/t.y : 1.0f, t.z > 0.0f ? 1.0f/t.z : 1.0f);

			PxVec3 sleepLinVelAcc =motionVelocity.linear;
			PxVec3 sleepAngVelAcc = body2World.q.rotateInv(motionVelocity.angular);

			bcSleepLinVelAcc += sleepLinVelAcc;
			bcSleepAngVelAcc += sleepAngVelAcc;

			PxReal invMass = core.getInverseMass();
			if(invMass == 0.0f)
				invMass = 1.0f;

			const PxReal angular = bcSleepAngVelAcc.multiply(bcSleepAngVelAcc).dot(inertia) * invMass;
			const PxReal linear = bcSleepLinVelAcc.magnitudeSquared();
			PxReal normalizedEnergy = 0.5f * (angular + linear);

			// scale threshold by cluster factor (more contacts => higher sleep threshold)
			const PxReal clusterFactor = PxReal(1 + getNumCountedInteractions());
			const PxReal threshold = clusterFactor*energyThreshold;
		
			if (normalizedEnergy >= threshold)
			{
				PX_ASSERT(isActive());
				resetSleepFilter();
				const float factor = threshold == 0.0f ? 2.0f : PxMin(normalizedEnergy/threshold, 2.0f);
				PxReal oldWc = wc;
				wc = factor * 0.5f * wakeCounterResetTime + dt * (clusterFactor - 1.0f);
				core.setWakeCounterFromSim(wc);
				if (oldWc == 0.0f)  // for the case where a sleeping body got activated by the system (not the user) AND got processed by the solver as well
					notifyNotReadyForSleeping();
				
				return wc;
			}
		}

		mLLBody.sleepLinVelAcc = bcSleepLinVelAcc;
		mLLBody.sleepAngVelAcc = bcSleepAngVelAcc;
	}

	wc = PxMax(wc-dt, 0.0f);
	core.setWakeCounterFromSim(wc);
	return wc;
}

//--------------------------------------------------------------
//
// Kinematics
//
//--------------------------------------------------------------

PX_FORCE_INLINE void BodySim::initKinematicStateBase(BodyCore&, bool asPartOfCreation)
{
	PX_ASSERT(!readInternalFlag(BF_KINEMATIC_MOVED));

	if (!asPartOfCreation && isActive())
		getScene().swapInActiveBodyList(*this);

	//mLLBody.setAccelerationV(Cm::SpatialVector::zero());

	// Need to be before setting setRigidBodyFlag::KINEMATIC

	if (getConstraintGroup())
		getConstraintGroup()->markForProjectionTreeRebuild(getScene().getProjectionManager());
}

void BodySim::calculateKinematicVelocity(PxReal oneOverDt)
{
	PX_ASSERT(isKinematic());
	
	/*------------------------------------------------\
	| kinematic bodies are moved directly by the user and are not influenced by external forces
	| we simply determine the distance moved since the last simulation frame and 
	| assign the appropriate delta to the velocity. This vel will be used to shove dynamic
	| objects in the solver.
	| We have to do this like so in a delayed way, because when the user sets the target pos the dt is not
	| yet known.
	\------------------------------------------------*/
	PX_ASSERT(isActive());

	BodyCore& core = getBodyCore();

	if (readInternalFlag(BF_KINEMATIC_MOVED))
	{
		clearInternalFlag(InternalFlags(BF_KINEMATIC_SETTLING | BF_KINEMATIC_SETTLING_2));
		const SimStateData* kData = core.getSimStateData(true);
		PX_ASSERT(kData);
		PX_ASSERT(kData->isKine());
		PX_ASSERT(kData->getKinematicData()->targetValid);
		PxVec3 linVelLL, angVelLL;
		const PxTransform targetPose = kData->getKinematicData()->targetPose;
		const PxTransform& currBody2World = getBody2World();

		//the kinematic target pose is now the target of the body (CoM) and not the actor.

		PxVec3 deltaPos = targetPose.p;
		deltaPos -= currBody2World.p;
		linVelLL = deltaPos * oneOverDt;

		PxQuat q = targetPose.q * currBody2World.q.getConjugate();

		if (q.w < 0)	//shortest angle.
			q = -q;

		PxReal angle;
 		PxVec3 axis;
		q.toRadiansAndUnitAxis(angle, axis);
		angVelLL = axis * angle * oneOverDt;

		core.getCore().linearVelocity = linVelLL;
		core.getCore().angularVelocity = angVelLL;

		// Moving a kinematic should trigger a wakeUp call on a higher level.
		PX_ASSERT(core.getWakeCounter()>0);
		PX_ASSERT(isActive());
		
	}
	else if (!readInternalFlag(BF_KINEMATIC_SURFACE_VELOCITY))
	{
		core.setLinearVelocity(PxVec3(0));
		core.setAngularVelocity(PxVec3(0));
	}
}

void BodySim::updateKinematicPose()
{
	/*------------------------------------------------\
	| kinematic bodies are moved directly by the user and are not influenced by external forces
	| we simply determine the distance moved since the last simulation frame and 
	| assign the appropriate delta to the velocity. This vel will be used to shove dynamic
	| objects in the solver.
	| We have to do this like so in a delayed way, because when the user sets the target pos the dt is not
	| yet known.
	\------------------------------------------------*/

	PX_ASSERT(isKinematic());
	PX_ASSERT(isActive());

	BodyCore& core = getBodyCore();

	if (readInternalFlag(BF_KINEMATIC_MOVED))
	{
		clearInternalFlag(InternalFlags(BF_KINEMATIC_SETTLING | BF_KINEMATIC_SETTLING_2));
		const SimStateData* kData = core.getSimStateData(true);
		PX_ASSERT(kData);
		PX_ASSERT(kData->isKine());
		PX_ASSERT(kData->getKinematicData()->targetValid);
		
		const PxTransform targetPose = kData->getKinematicData()->targetPose;
		getBodyCore().getCore().body2World = targetPose;
	}
}

bool BodySim::deactivateKinematic()
{
	BodyCore& core = getBodyCore();
	if(readInternalFlag(BF_KINEMATIC_SETTLING_2))
	{
		clearInternalFlag(BF_KINEMATIC_SETTLING_2);
		core.setWakeCounterFromSim(0);	// For sleeping objects the wake counter must be 0. This needs to hold for kinematics too.
		notifyReadyForSleeping();
		notifyPutToSleep();
		setActive(false);
		return true;
	}
	else if (readInternalFlag(BF_KINEMATIC_SETTLING))
	{
		clearInternalFlag(BF_KINEMATIC_SETTLING);
		raiseInternalFlag(BF_KINEMATIC_SETTLING_2);
	}
	else if (!readInternalFlag(BF_KINEMATIC_SURFACE_VELOCITY))
	{
		clearInternalFlag(BF_KINEMATIC_MOVED);
		raiseInternalFlag(BF_KINEMATIC_SETTLING);
	}
	return false;
}

//--------------------------------------------------------------
//
// Miscellaneous
//
//--------------------------------------------------------------

void BodySim::updateForces(PxReal dt, PxsRigidBody** updatedBodySims, PxU32* updatedBodyNodeIndices, PxU32& index, Cm::SpatialVector* acceleration, const bool useAccelerations, bool simUsesAdaptiveForce)
{
	PxVec3 linAcc(0.0f), angAcc(0.0f);

	const bool accDirty = readVelocityModFlag(VMF_ACC_DIRTY);
	const bool velDirty = readVelocityModFlag(VMF_VEL_DIRTY);

	BodyCore& bodyCore = getBodyCore();
	SimStateData* simStateData = NULL;

	//if we change the logic like this, which means we don't need to have two seperate variables in the pxgbodysim to represent linAcc and angAcc. However, this
	//means angAcc will be always 0
	if( (accDirty || velDirty) &&  ((simStateData = bodyCore.getSimStateData(false)) != NULL) )
	{
		VelocityMod* velmod = simStateData->getVelocityModData();

		//we don't have support for articulation yet
		if (updatedBodySims)
		{
			updatedBodySims[index] = &getLowLevelBody();
			updatedBodyNodeIndices[index++] = getNodeIndex().index();
		}

		if(velDirty)
		{
			linAcc = velmod->getLinearVelModPerStep();
			angAcc = velmod->getAngularVelModPerStep();

			if (useAccelerations)
			{
				acceleration->linear = linAcc / dt;
				acceleration->angular = angAcc / dt;

			}
			else
			{
				getBodyCore().updateVelocities(linAcc, angAcc);
			}
		}
		
		if (accDirty)
		{
			linAcc = velmod->getLinearVelModPerSec();
			angAcc = velmod->getAngularVelModPerSec();

			if (acceleration)
			{
				acceleration->linear = linAcc;
				acceleration->angular = angAcc;
			}
			else
			{
				PxReal scale = dt;
				if (simUsesAdaptiveForce)
				{
					if (getScene().getSimpleIslandManager()->getAccurateIslandSim().getIslandStaticTouchCount(mNodeIndex) != 0)
					{
						scale *= mLLBody.accelScale;
					}
				}
				getBodyCore().updateVelocities(linAcc*scale, angAcc*scale);
			}
		}		
	}

	setForcesToDefaults(readVelocityModFlag(VMF_ACC_DIRTY));
}

void BodySim::onConstraintDetach()
{
	PX_ASSERT(readInternalFlag(BF_HAS_CONSTRAINTS));

	PxU32 size = getActorInteractionCount();
	Interaction** interactions = getActorInteractions();
	unregisterCountedInteraction();

	while(size--)
	{
		const Interaction* interaction = *interactions++;
		if(interaction->getType() == InteractionType::eCONSTRAINTSHADER)
			return;
	}

	clearInternalFlag(BF_HAS_CONSTRAINTS);  // There are no other constraint interactions left
}

void BodySim::setArticulation(ArticulationSim* a, PxReal wakeCounter, bool asleep, PxU32 bodyIndex)
{
	mArticulation = a; 
	if(a)
	{
		IG::NodeIndex index = mArticulation->getIslandNodeIndex();
		mNodeIndex.setIndices(index.index(), bodyIndex);
		getBodyCore().setWakeCounterFromSim(wakeCounter);

		if (getFlagsFast() & PxRigidBodyFlag::eENABLE_SPECULATIVE_CCD)
			getScene().setSpeculativeCCDArticulationLink(mNodeIndex.index());

		if (!asleep)
		{
			setActive(true);
			notifyWakeUp(false);
		}
		else
		{
			notifyReadyForSleeping();
			notifyPutToSleep();
			setActive(false);
		}
	}
	else
	{
		//Setting a 1 in the articulation ID to avoid returning the node Index to the node index
		//manager
		mNodeIndex.setIndices(IG_INVALID_NODE, 1);
	}
}

void BodySim::createSqBounds()
{
	if(!isActive() || usingSqKinematicTarget() || readInternalFlag(BF_IS_COMPOUND_RIGID))
		return;

	PX_ASSERT(!isFrozen());
	
	ElementSim* current = getElements_();
	while(current)
	{
		static_cast<ShapeSim*>(current)->createSqBounds();
		current = current->mNextInActor;
	}
}

void BodySim::destroySqBounds()
{
	ElementSim* current = getElements_();
	while(current)
	{
		static_cast<ShapeSim*>(current)->destroySqBounds();
		current = current->mNextInActor;
	}
}

void BodySim::freezeTransforms(Cm::BitMapPinned* shapeChangedMap)
{
	ElementSim* current = getElements_();
	while(current)
	{
		ShapeSim* sim = static_cast<ShapeSim*>(current);
		sim->updateCached(PxsTransformFlag::eFROZEN, shapeChangedMap);
		sim->destroySqBounds();
		current = current->mNextInActor;
	}
}

void BodySim::disableCompound()
{
	if(isActive())
		getScene().removeFromActiveCompoundBodyList(*this);
	clearInternalFlag(BF_IS_COMPOUND_RIGID);
}

