+Unity Runtime codeHEADmaster

1 files changed, 1097 insertions, 0 deletions

diff --git a/Runtime/Dynamics/Rigidbody.cpp b/Runtime/Dynamics/Rigidbody.cpp
new file mode 100644
index 0000000..6ca8af6
--- /dev/null
+++ b/Runtime/Dynamics/Rigidbody.cpp
@@ -0,0 +1,1097 @@
+#include "UnityPrefix.h"
+#if ENABLE_PHYSICS
+#include "RigidBody.h"
+#include "Collider.h"
+#include "Joint.h"
+#include "PhysicsManager.h"
+#include "Runtime/Graphics/Transform.h"
+#include "Runtime/Serialize/TransferFunctions/SerializeTransfer.h"
+#include "Runtime/BaseClasses/SupportedMessageOptimization.h"
+#include "External/PhysX/builds/SDKs/Physics/include/NxPhysics.h"
+#include "NxWrapperUtility.h"
+#include "Runtime/Utilities/Utility.h"
+#include "Runtime/BaseClasses/MessageHandler.h"
+#include "Runtime/Profiler/Profiler.h"
+#include "Runtime/Utilities/ValidateArgs.h"
+#include "Runtime/GameCode/RootMotionData.h"
+#include "Runtime/Input/TimeManager.h"
+#include "Runtime/Misc/BuildSettings.h"
+
+using namespace Unity;
+using namespace std;
+
+PROFILER_INFORMATION(gSweepTestProfile, "Rigidbody.SweepTest", kProfilerPhysics)
+PROFILER_INFORMATION(gSweepTestAllProfile, "Rigidbody.SweepTestAll", kProfilerPhysics)
+
+/*
+ - Rigid bodies can't change the inertia tensor
+ - Transfer function should be optimize to serialize into desc data if rigid body doesn't exit yet (Possibly always)
+ 
+ - Make it so you can have multiple rigidbodies inside of each other and they update in the right order!
+   What about sleeping??????
+ 
+*/
+
+
+inline Quaternionf QuatFromNx(const NxQuat& q)
+{
+        return Quaternionf(q.x, q.y, q.z, q.w);
+}
+
+
+
+Rigidbody::Rigidbody (MemLabelId label, ObjectCreationMode mode)
+	: Super(label, mode),
+	m_SortedNode(this)
+{
+	m_Actor = NULL;
+	m_ImplicitTensor = true;
+	m_InterpolationInfo = NULL;
+	m_DisableReadUpdateTransform = 0;
+	m_CollisionDetection = kCCDModeOff;
+	m_CachedCollisionDetection = kCCDModeOff;
+}
+
+Rigidbody::~Rigidbody ()
+{
+	CleanupInternal (false);
+}
+
+void Rigidbody::Reset ()
+{
+	Super::Reset ();
+
+	if (m_Actor)
+	{
+		SetMass(1.0F);
+		SetAngularDrag(0.05f);
+		SetDrag(0.0F);
+		SetConstraints(kFreezeNone);
+		SetIsKinematic(false);
+		SetUseGravity(true);
+		SetCollisionDetectionMode(kCCDModeOff);
+	}
+	else
+	{
+		m_Mass = 1.0F;
+		m_AngularDrag = 0.05f;
+		m_Drag = 0.0f;
+		m_Constraints = kFreezeNone;
+		m_IsKinematic = false;
+		m_UseGravity = true;
+		m_CollisionDetection = kCCDModeOff;
+		m_CachedCollisionDetection = kCCDModeOff;
+	}
+	m_Interpolate = kNoInterpolation;
+}
+
+void Rigidbody::CheckConsistency ()
+{
+	Super::CheckConsistency ();
+	m_Mass = max(0.0000001F, m_Mass);
+}
+
+// Because Component -> Deactivate can be called but IsActive might still return true, we need to pass the active state!
+void Rigidbody::Create (bool isActive)
+{
+	if (m_Actor == NULL || (bool)m_ActiveScene != isActive)
+	{
+		NxBodyDesc bodyDesc;
+		bodyDesc.solverIterationCount = GetPhysicsManager ().GetSolverIterationCount ();
+		NxActorDesc actorDesc;
+		
+		if (m_Actor)
+		{
+			m_Actor->saveBodyToDesc (bodyDesc);
+			CleanupInternal (true);
+		}
+		else
+		{
+			bodyDesc.massSpaceInertia = NxVec3 (1.0F, 1.0F, 1.0F);
+			bodyDesc.mass = m_Mass;
+			bodyDesc.linearDamping = m_Drag;
+			bodyDesc.angularDamping = m_AngularDrag;
+			bodyDesc.mass = m_Mass;
+			if (!m_UseGravity)
+				bodyDesc.flags |= NX_BF_DISABLE_GRAVITY;
+			if (m_IsKinematic)
+				bodyDesc.flags |= NX_BF_KINEMATIC;
+			bodyDesc.flags |= m_Constraints;
+		}
+		actorDesc.body = &bodyDesc;
+		actorDesc.userData = this;
+
+		if (isActive)
+		{
+			//;;printf_console ("Creating active physics actor\n");
+			m_Actor = GetDynamicsScene ().createActor (actorDesc);
+			SupportedMessagesDidChange (GetGameObject ().GetSupportedMessages ());
+			m_ActiveScene = true;
+		}
+		else
+		{
+			//;;printf_console ("Creating inactive physics actor\n");
+			m_Actor = GetInactiveDynamicsScene ().createActor (actorDesc);
+		
+			m_ActiveScene = false;
+		}
+
+		UpdateInterpolationNode();
+	}
+	
+	if (!isActive && m_Actor != NULL)
+	{
+		Assert(m_Actor->getNbShapes() == 0);
+	}
+}
+
+
+void Rigidbody::SupportedMessagesDidChange (int supported)
+{
+	if (m_Actor)
+	{
+		if (supported & kHasCollisionStay)
+			m_Actor->setGroup (kContactTouchGroup);
+		else if (supported & (kHasCollisionStay | kHasCollisionEnterExit))
+			m_Actor->setGroup (kContactEnterExitGroup);
+		else
+			m_Actor->setGroup (kContactNothingGroup);
+	}
+}
+
+void Rigidbody::CleanupInternal (bool recreateColliders)
+{
+	if (m_Actor)
+	{
+		int shapeCount = m_Actor->getNbShapes ();
+		NxShape*const * shapes = m_Actor->getShapes ();
+		
+		Collider** colliders;
+		ALLOC_TEMP(colliders, Collider*, shapeCount)
+
+		for (int i=0;i<shapeCount;i++)
+		{
+			Collider* collider = (Collider*)shapes[i]->userData;
+			AssertIf (collider == NULL);
+			colliders[i] = collider;
+			collider->Cleanup ();
+		}
+		
+		if (m_ActiveScene)
+		{
+			//;;printf_console ("Deleting active physics actor");
+			GetDynamicsScene ().releaseActor (*m_Actor);
+		}
+		else
+		{
+			//;;printf_console ("Deleting inactive physics actor");
+			GetInactiveDynamicsScene ().releaseActor (*m_Actor);
+		}
+		m_Actor = NULL;
+		
+		if (recreateColliders)
+		{
+			for (int i=0;i<shapeCount;i++)
+			{
+				colliders[i]->RecreateCollider (this);
+			}
+		}
+
+		delete m_InterpolationInfo;
+		m_InterpolationInfo = NULL;
+		m_CachedCollisionDetection = m_CollisionDetection;
+	}
+	Assert(m_Actor == NULL);
+	m_SortedNode.RemoveFromList();
+}
+
+Vector3f Rigidbody::GetWorldCenterOfMass () const
+{
+	return Vec3FromNx (m_Actor->getCMassGlobalPosition());
+}
+
+void Rigidbody::Deactivate (DeactivateOperation operation)
+{
+	// When we are about to destroy a rigidbody, we don't destroy it immediately
+	// Instead we destroy it in the rigidbody destructor.
+	// This way scripts have no chance of accessing the colliders.
+	if (operation == kWillDestroyGameObjectDeactivate)
+		;
+	else
+	{	
+		Create (false);
+	}
+	m_SortedNode.RemoveFromList();
+	
+	Super::Deactivate (operation);
+}
+
+void Rigidbody::UpdateSortedBody ()
+{
+	m_SortedNode.RemoveFromList();
+	if (m_ActiveScene)
+		GetPhysicsManager().AddBody(GetTransformDepth(GetComponent(Transform)), m_SortedNode); 
+}
+
+void Rigidbody::AwakeFromLoad (AwakeFromLoadMode awakeMode)
+{
+	Super::AwakeFromLoad (awakeMode);
+	
+	AssertIf (GameObject::GetMessageHandler ().HasMessageCallback (ClassID (Rigidbody), kTransformChanged.messageID) == false);
+	
+	Create (IsActive ());
+	
+	// When modifying already loaded rigidbody
+	// Apply properties through immediate mode function
+	if (!(awakeMode & kDidLoadFromDisk))
+	{
+		SetIsKinematic(m_IsKinematic);
+		SetMass(m_Mass);
+		SetDrag(m_Drag);
+		SetAngularDrag(m_AngularDrag);
+		SetUseGravity(m_UseGravity);
+		SetCollisionDetectionMode(m_CollisionDetection);
+		SetConstraints(m_Constraints);
+	}
+	
+	UpdateInterpolationNode();
+	
+	if (IsActive())
+		FetchPoseFromTransform();
+	
+	if (!GetIsKinematic())
+		m_DisableReadUpdateTransform = 0;
+	
+	if (awakeMode & kActivateAwakeFromLoad)
+	{
+		MessageData data;
+		GetComponent(Transform).BroadcastMessageAny(kForceRecreateCollider, data);
+	}
+	
+	UpdateSortedBody ();
+}
+
+void Rigidbody::ClosestPointOnBounds (const Vector3f& position, Vector3f& outPosition, float& outSqrDistance)
+{
+	// No collider - just the distance to the center of mass
+	int count = m_Actor->getNbShapes();
+	if (count == 0)
+	{
+		outPosition = GetWorldCenterOfMass();
+		outSqrDistance = SqrMagnitude(position - outPosition);
+		return;
+	}
+	
+	outSqrDistance = std::numeric_limits<float>::infinity();
+
+	NxShape*const * shapes = m_Actor->getShapes();
+	for (int i=0;i<count;i++)
+	{
+		NxBounds3 bounds;
+		shapes[i]->getWorldBounds(bounds);
+		AABB aabb;
+		bounds.getCenter((NxVec3&)aabb.GetCenter());
+		bounds.getExtents((NxVec3&)aabb.GetExtent());
+		
+		Vector3f closest;
+		float sqrDistance;
+		
+		CalculateClosestPoint(position, aabb, closest, sqrDistance);
+		if (sqrDistance < outSqrDistance)
+		{
+			outPosition = closest;
+			outSqrDistance = sqrDistance;
+		}
+	}
+}
+
+void Rigidbody::AddExplosionForce (float force, const Vector3f& position, float radius, float upwardsModifier, int forceMode)
+{
+	Vector3f pointOnSurface;
+	float sqrDistance;
+
+	Vector3f offsetPosition = position - Vector3f(0.0F, upwardsModifier, 0.0F);
+	if (upwardsModifier == 0.0F)
+	{
+		ClosestPointOnBounds(position, pointOnSurface, sqrDistance);
+	}
+	else
+	{
+		/// Upwards modifier will not modify the distance
+		/// But it will modify the point on the surface
+		ClosestPointOnBounds(position, pointOnSurface, sqrDistance);
+
+		float tmpDistance;
+		ClosestPointOnBounds(offsetPosition, pointOnSurface, tmpDistance);
+	}
+	
+	// Linear distance fall off
+	float distanceScale;
+	if (radius > Vector3f::epsilon)
+		distanceScale = 1.0F - clamp01(sqrtf(sqrDistance) / radius);
+	else
+		distanceScale = 1.0F;
+	
+	// Calculate normalized direction towards surface point
+	Vector3f direction = pointOnSurface - offsetPosition;
+	
+	float length = Magnitude(direction);
+	if (length > Vector3f::epsilon)
+		direction /= length;
+	else
+		direction = Vector3f (0.0F, 1.0F, 0.0F);
+	
+	AddForceAtPosition(force * distanceScale * direction, pointOnSurface, forceMode);
+}
+
+void Rigidbody::InitializeClass ()
+{
+	REGISTER_MESSAGE (Rigidbody, kTransformChanged, TransformChanged, int);
+	REGISTER_MESSAGE_PTR (Rigidbody, kAnimatorMoveBuiltin, ApplyRootMotionBuiltin, RootMotionData);
+}
+
+template<class TransferFunction>
+void Rigidbody::Transfer (TransferFunction& transfer) 
+{
+	Super::Transfer (transfer);
+	transfer.SetVersion (2);
+	TRANSFER_SIMPLE (m_Mass);
+	TRANSFER_SIMPLE (m_Drag);
+	TRANSFER_SIMPLE (m_AngularDrag);
+	TRANSFER_SIMPLE (m_UseGravity);
+	TRANSFER (m_IsKinematic);
+
+	transfer.Transfer (m_Interpolate, "m_Interpolate");
+	if (transfer.IsOldVersion(1))
+	{
+		bool freezeRotation;
+		transfer.Transfer (freezeRotation, "m_FreezeRotation");
+		if (freezeRotation)
+			m_Constraints = kFreezeRotation;
+		else
+			m_Constraints = kFreezeNone;
+	}
+	else
+	{
+		transfer.Align();
+
+		// Hide in editor and show using custom inspector instead.
+		transfer.Transfer (m_Constraints, "m_Constraints", kHideInEditorMask | kGenerateBitwiseDifferences);
+	}
+	
+	TRANSFER (m_CollisionDetection);
+}
+
+void Rigidbody::SetUseGravity (bool value)
+{
+	AssertIf (m_Actor == NULL);
+	SetDirty ();
+	if (value)
+	{
+		m_Actor->clearBodyFlag (NX_BF_DISABLE_GRAVITY);
+		m_Actor->wakeUp();
+	}
+	else
+		m_Actor->raiseBodyFlag (NX_BF_DISABLE_GRAVITY);
+	m_UseGravity = value;
+}
+
+bool Rigidbody::GetUseGravity () const
+{
+	AssertIf (m_Actor == NULL);
+	return !m_Actor->readBodyFlag (NX_BF_DISABLE_GRAVITY);
+}
+
+void Rigidbody::SetFreezeRotation (bool value)
+{
+	if (value)
+		SetConstraints (GetConstraints() | kFreezeRotation);
+	else
+		SetConstraints (GetConstraints() & ~kFreezeRotation);
+}
+
+bool Rigidbody::GetFreezeRotation () const
+{
+	return (GetConstraints () & kFreezeRotation) == kFreezeRotation;
+}
+
+void Rigidbody::SetConstraints (int value)
+{
+	AssertIf (m_Actor == NULL);
+
+	// If we are removing constraints, wake up the rigidbody, so it can start moving.
+	if (IS_CONTENT_NEWER_OR_SAME (kUnityVersion4_3_a1) && m_Constraints & ~value)
+		WakeUp();
+	
+	SetDirty ();
+	value &= NX_BF_FROZEN;
+	m_Actor->clearBodyFlag (NX_BF_FROZEN);
+	m_Actor->raiseBodyFlag ((NxBodyFlag)value);
+	m_Constraints = value;
+}
+
+int Rigidbody::GetConstraints () const
+{
+	AssertIf (m_Actor == NULL);
+	
+	int value = 0;
+	for (int i = kFreezePositionX; i <= kFreezeRotationZ; i<<=1)
+	{
+		if (m_Actor->readBodyFlag ((NxBodyFlag)i))
+			value |= i;
+	}
+	return value;
+}
+
+void Rigidbody::SetIsKinematic (bool value)
+{
+	AssertIf (m_Actor == NULL);
+
+	SetDirty ();
+	if (value)
+		m_Actor->raiseBodyFlag (NX_BF_KINEMATIC);
+	else
+		m_Actor->clearBodyFlag (NX_BF_KINEMATIC);
+	m_IsKinematic = value;
+	UpdateInterpolationNode ();
+	
+	m_DisableReadUpdateTransform = 0;
+}
+
+bool Rigidbody::GetIsKinematic () const
+{
+	AssertIf (m_Actor == NULL);
+	return m_Actor->readBodyFlag (NX_BF_KINEMATIC);
+}
+
+void Rigidbody::UpdateMassDistribution ()
+{
+	AssertIf (m_Actor == NULL);
+
+	if (m_ImplicitTensor)
+	{	
+		NxShape*const * shapes = m_Actor->getShapes();
+		int count = m_Actor->getNbShapes();
+		for (int i=0;i<count;i++)
+		{
+			Collider* coll = (Collider*)(shapes[i]->userData);
+			
+			// Triggers don't have mass, and RaycastColliders are too thing to give good mathematical useful results.
+			if (!shapes[i]->getFlag(NX_TRIGGER_ENABLE) && !(coll && coll->GetClassID() == 140))
+			{
+				m_Actor->updateMassFromShapes (0.0F, m_Mass);		
+				return;
+			}
+		}
+
+		// no usable shapes - reset actor CoG and inertia
+		m_Actor->setMass (m_Mass);
+		m_Actor->setCMassOffsetLocalPosition ((const NxVec3&)Vector3f::zero);
+		m_Actor->setMassSpaceInertiaTensor ((const NxVec3&)Vector3f::one);		
+	}	
+}
+
+void Rigidbody::SetMass (float mass)
+{
+	AssertIf (m_Actor == NULL);
+	SetDirty ();
+	m_Mass = mass;
+
+	if (m_ImplicitTensor)
+		UpdateMassDistribution();
+	else
+		m_Actor->setMass (mass);
+}
+
+float Rigidbody::GetMass () const
+{
+	AssertIf (m_Actor == NULL);
+	return m_Actor->getMass ();
+}
+
+void Rigidbody::SetCenterOfMass (const Vector3f& centerOfMass)
+{
+	m_Actor->setCMassOffsetLocalPosition ((const NxVec3&)centerOfMass);
+	m_ImplicitTensor = false;
+}
+
+Vector3f Rigidbody::GetCenterOfMass () const
+{
+	return Vec3FromNx(m_Actor->getCMassLocalPosition ());
+}
+
+void Rigidbody::SetInertiaTensorRotation (const Quaternionf& inertia)
+{
+	m_ImplicitTensor = false;
+	NxMat33 matrix ((const NxQuat&)inertia);
+	m_Actor->setCMassOffsetLocalOrientation (matrix);
+}
+
+Quaternionf Rigidbody::GetInertiaTensorRotation () const
+{
+	NxMat33 matrix = m_Actor->getCMassLocalOrientation ();
+	NxQuat quat (matrix);
+	return (const Quaternionf&)quat;
+}
+
+void Rigidbody::SetInertiaTensor (const Vector3f& inertia)
+{
+	m_ImplicitTensor = false;
+	if (inertia.x > std::numeric_limits<float>::epsilon() &&
+		inertia.y > std::numeric_limits<float>::epsilon() &&
+		inertia.z > std::numeric_limits<float>::epsilon())
+		m_Actor->setMassSpaceInertiaTensor ((const NxVec3&)inertia);
+	else
+		ErrorStringObject ("Inertia tensor must be larger then zero in all coordinates.", this);
+}
+
+Vector3f Rigidbody::GetInertiaTensor () const
+{
+	return Vec3FromNx(m_Actor->getMassSpaceInertiaTensor ());
+}
+
+Vector3f Rigidbody::GetVelocity () const
+{
+	AssertIf (m_Actor == NULL);
+	return Vec3FromNx(m_Actor->getLinearVelocity ());
+}
+
+void Rigidbody::SetVelocity (const Vector3f& velocity)
+{
+	AssertIf (m_Actor == NULL);
+	ABORT_INVALID_VECTOR3 (velocity, velocity, rigidbody);
+	m_Actor->setLinearVelocity ((const NxVec3&)velocity);
+}
+
+Vector3f Rigidbody::GetAngularVelocity () const
+{
+	AssertIf (m_Actor == NULL);
+	return Vec3FromNx(m_Actor->getAngularVelocity ());
+}
+
+void Rigidbody::SetAngularVelocity (const Vector3f& velocity)
+{
+	AssertIf (m_Actor == NULL);
+	ABORT_INVALID_VECTOR3 (velocity, angularVelocity, rigidbody);
+	m_Actor->setAngularVelocity ((const NxVec3&)velocity);
+}
+
+void Rigidbody::SetDrag (float damping)
+{
+	AssertIf (m_Actor == NULL);
+	SetDirty ();
+	m_Drag = damping;
+	m_Actor->setLinearDamping (damping);
+}
+
+float Rigidbody::GetDrag () const
+{
+	AssertIf (m_Actor == NULL);
+	return m_Actor->getLinearDamping ();
+}
+
+void Rigidbody::SetAngularDrag (float damping)
+{
+	AssertIf (m_Actor == NULL);
+	SetDirty ();
+	m_AngularDrag = damping;
+	m_Actor->setAngularDamping (damping);
+}
+
+float Rigidbody::GetAngularDrag () const
+{
+	AssertIf (m_Actor == NULL);
+	return m_Actor->getAngularDamping ();
+}
+
+void Rigidbody::SetMaxAngularVelocity (float maxAngularVelocity)
+{
+	AssertIf (m_Actor == NULL);
+	return m_Actor->setMaxAngularVelocity (maxAngularVelocity);
+}
+
+float Rigidbody::GetMaxAngularVelocity () const
+{
+	AssertIf (m_Actor == NULL);
+	return m_Actor->getMaxAngularVelocity ();
+}
+
+void Rigidbody::SetSolverIterationCount (int iterationCount)
+{
+	return m_Actor->setSolverIterationCount(iterationCount);	
+}
+
+int Rigidbody::GetSolverIterationCount () const
+{
+	return m_Actor->getSolverIterationCount();	
+}
+
+void Rigidbody::SetSleepVelocity (float value)
+{
+	m_Actor->setSleepLinearVelocity(value);	
+}
+
+float Rigidbody::GetSleepVelocity () const
+{
+	return m_Actor->getSleepLinearVelocity();	
+}
+
+void Rigidbody::SetSleepAngularVelocity (float value)
+{
+	m_Actor->setSleepAngularVelocity(value);	
+}
+
+float Rigidbody::GetSleepAngularVelocity () const
+{
+	return m_Actor->getSleepAngularVelocity();	
+}
+
+void Rigidbody::SetInterpolation (RigidbodyInterpolation interpolation)
+{
+	m_Interpolate = interpolation;
+	UpdateInterpolationNode();
+}
+
+Vector3f Rigidbody::GetPointVelocity (const Vector3f& worldPoint) const
+{
+	AssertIf (m_Actor == NULL);
+	return Vec3FromNx(m_Actor->getPointVelocity ((const NxVec3&)worldPoint));
+}
+
+Vector3f Rigidbody::GetRelativePointVelocity (const Vector3f& localPoint) const
+{
+	AssertIf (m_Actor == NULL);
+	return Vec3FromNx(m_Actor->getLocalPointVelocity ((const NxVec3&)localPoint));
+}
+
+void Rigidbody::AddForceAtPosition (const Vector3f& force, const Vector3f& position, int mode)
+{
+	ABORT_INVALID_VECTOR3 (force, force, rigidbody)
+	ABORT_INVALID_VECTOR3 (position, position, rigidbody)
+	AssertIf (m_Actor == NULL);
+	if (!m_Actor->readBodyFlag(NX_BF_KINEMATIC))
+		m_Actor->addForceAtPos ((const NxVec3&)force, (const NxVec3&)position, (NxForceMode)mode);
+}
+
+void Rigidbody::AddForce (const Vector3f& force, int mode)
+{
+	ABORT_INVALID_VECTOR3 (force, force, rigidbody)
+	AssertIf (m_Actor == NULL);
+	if (!m_Actor->readBodyFlag(NX_BF_KINEMATIC))
+		m_Actor->addForce ((const NxVec3&)force, (NxForceMode)mode);
+}
+
+void Rigidbody::AddRelativeForce (const Vector3f& force, int mode)
+{
+	ABORT_INVALID_VECTOR3 (force, force, rigidbody)
+	AssertIf (m_Actor == NULL);
+	if (!m_Actor->readBodyFlag(NX_BF_KINEMATIC))
+		m_Actor->addLocalForce ((const NxVec3&)force, (NxForceMode)mode);
+}
+
+void Rigidbody::AddTorque (const Vector3f& torque, int mode)
+{
+	ABORT_INVALID_VECTOR3 (torque, torque, rigidbody)
+	AssertIf (m_Actor == NULL);
+	if (!m_Actor->readBodyFlag(NX_BF_KINEMATIC))
+		m_Actor->addTorque ((const NxVec3&)torque, (NxForceMode)mode);
+}
+
+void Rigidbody::AddRelativeTorque (const Vector3f& torque, int mode)
+{
+	ABORT_INVALID_VECTOR3 (torque, torque, rigidbody)
+	AssertIf (m_Actor == NULL);
+	if (!m_Actor->readBodyFlag(NX_BF_KINEMATIC))
+		m_Actor->addLocalTorque ((const NxVec3&)torque, (NxForceMode)mode);
+}
+
+void Rigidbody::FetchPoseFromTransform ()
+{
+	AssertIf (m_Actor == NULL);
+	Transform& transform = GetComponent (Transform);
+	Vector3f pos = transform.GetPosition ();
+	Quaternionf rot = transform.GetRotation ();
+	NxMat33 nxrot ((const NxQuat&)rot);
+	NxMat34 pose (nxrot, Vec3ToNx(pos));
+	
+	AssertFiniteParameter(pos)
+	AssertFiniteParameter(rot)
+	
+	if (GetIsKinematic())
+	{
+		m_Actor->setGlobalPose (pose);
+		// Novodex workaround ->Move global pose needs to always be called in order to make sure triggers get activated!
+		m_Actor->moveGlobalPose (pose);
+		m_DisableReadUpdateTransform = 1;
+	}
+	else
+	{
+		m_Actor->setGlobalPose (pose);
+	}
+}
+
+
+void Rigidbody::TransformChanged (int mask)
+{	
+	if (m_Actor)
+	{
+		bool kine = GetIsKinematic();
+		
+		// When reading transform positions back from PhysX, we don't want to write back to PhysX again.
+		// However, when a Rigidbody is kinematic or sleeping, and is moved because a parent rigidbody has changed,
+		// we still want to update the position.
+		if (GetPhysicsManager().IsRigidbodyTransformMessageEnabled() || kine || m_Actor->isSleeping())
+		{
+			Transform& transform = GetComponent (Transform);
+			const int posRotMask = Transform::kRotationChanged | Transform::kPositionChanged;
+			
+			if ((mask & posRotMask) == posRotMask || (mask & Transform::kScaleChanged))
+			{
+				Vector3f pos = transform.GetPosition ();
+				Quaternionf rot = transform.GetRotation ();
+				NxMat33 nxrot ((const NxQuat&)rot);
+				NxMat34 pose (nxrot, Vec3ToNx(pos));
+				
+				AssertFiniteParameter(pos)
+				AssertFiniteParameter(rot)
+
+				
+				if (kine)
+				{
+					if ((mask & Transform::kAnimatePhysics) == 0)
+						m_Actor->setGlobalPose (pose);
+					// Novodex workaround ->Move global pose needs to always be called in order to make sure triggers get activated!
+					m_Actor->moveGlobalPose (pose);
+					m_DisableReadUpdateTransform = 1;
+				}
+				else
+				{
+					m_Actor->setGlobalPose (pose);
+					if (m_InterpolationInfo)
+						m_InterpolationInfo->disabled = 1;
+				}
+			}
+			else if (mask & Transform::kRotationChanged)		
+			{
+				Quaternionf rot = transform.GetRotation ();
+				AssertFiniteParameter(rot)
+				if (kine)
+				{
+					if ((mask & Transform::kAnimatePhysics) == 0)
+						m_Actor->setGlobalOrientationQuat ((const NxQuat&)rot);
+					// Novodex workaround ->Move global pose needs to always be called in order to make sure triggers get activated!
+					m_Actor->moveGlobalOrientationQuat ((const NxQuat&)rot);
+					m_DisableReadUpdateTransform = 1;
+				}
+				else
+				{
+					m_Actor->setGlobalOrientationQuat ((const NxQuat&)rot);
+								
+					if (m_InterpolationInfo)
+						m_InterpolationInfo->disabled = 1;
+				}
+			}
+			else if (mask & Transform::kPositionChanged)
+			{
+				Vector3f pos = transform.GetPosition ();
+				AssertFiniteParameter(pos)
+
+				if (kine)
+				{
+					if ((mask & Transform::kAnimatePhysics) == 0)
+						m_Actor->setGlobalPosition ((const NxVec3&)pos);
+					// Novodex workaround ->Move global pose needs to always be called in order to make sure triggers get activated!
+					m_Actor->moveGlobalPosition ((const NxVec3&)pos);
+					m_DisableReadUpdateTransform = 1;
+				}
+				else
+				{
+					m_Actor->setGlobalPosition ((const NxVec3&)pos);
+					if (m_InterpolationInfo)
+						m_InterpolationInfo->disabled = 1;
+				}
+					
+			}
+		}
+	}
+	//printf_console ("Changed transform :%s\n", GetName ().c_str ());
+}
+
+void Rigidbody::ApplyRootMotionBuiltin (RootMotionData* rootMotion)
+{	
+	if (m_Actor == NULL || rootMotion->didApply)
+		return;
+
+	if(GetIsKinematic())
+	{
+		SetPosition(GetPosition() + rootMotion->deltaPosition);
+		SetRotation(rootMotion->targetRotation);
+	}
+	else
+	{
+		Quaternionf rotation = GetRotation();
+		Quaternionf invRotation = Inverse(rotation);
+	
+		// Get the physics velocity in local space
+		Vector3f physicsVelocityLocal = RotateVectorByQuat(invRotation, GetVelocity());
+
+		// Get the local space velocity and blend it with the physics velocity on the y-axis based on gravity weight
+		// We do this in local space in order to support moving on a curve earth with gravity changing direction
+		Vector3f animVelocityGlobal = rootMotion->deltaPosition * GetInvDeltaTime();
+		Vector3f localVelocity = RotateVectorByQuat (invRotation, animVelocityGlobal);
+		localVelocity.y = Lerp (localVelocity.y, physicsVelocityLocal.y, rootMotion->gravityWeight);
+	
+		// If we use gravity, when we are in a jumping root motion, we have to cancel out the gravity
+		// applied by physX by default. When doing for example a jump the only thing affecting velocity should be the animation data.
+		// The animation already has gravity applied in the animation data so to speak...
+		if (GetUseGravity())
+			AddForce(GetPhysicsManager().GetGravity() * -Lerp(1.0F, 0.0F, rootMotion->gravityWeight));
+	
+		// Apply velocity & rotation
+		Vector3f globalVelocity = RotateVectorByQuat(rotation, localVelocity);
+		SetVelocity(globalVelocity);
+		MoveRotation(rootMotion->targetRotation);
+	}	
+	
+	rootMotion->didApply = true;
+}
+
+
+Vector3f Rigidbody::GetPosition () const
+{
+	AssertIf (m_Actor == NULL);
+	return Vec3FromNx (m_Actor->getGlobalPosition ());
+}
+
+Quaternionf Rigidbody::GetRotation () const
+{
+	AssertIf (m_Actor == NULL);
+	return QuatFromNx(m_Actor->getGlobalOrientationQuat ());
+}
+
+void Rigidbody::SetPosition (const Vector3f& position)
+{
+	ABORT_INVALID_VECTOR3 (position, position, rigidbody);
+	if (GetIsKinematic())
+	{
+		m_Actor->setGlobalPosition ((const NxVec3&)position);
+		// Novodex workaround ->Move global pose needs to always be called in order to make sure triggers get activated!		
+		m_Actor->moveGlobalPosition ((const NxVec3&)position);
+		m_DisableReadUpdateTransform = 0;
+	}
+	else
+	{
+		if (m_InterpolationInfo)
+			m_InterpolationInfo->disabled = 1;
+
+		m_Actor->setGlobalPosition ((const NxVec3&)position);
+	}
+}
+
+void Rigidbody::SetRotation (const Quaternionf& rotation)
+{
+	ABORT_INVALID_QUATERNION (rotation, rotation, rigidbody);
+
+	if (GetIsKinematic())
+	{
+		m_Actor->setGlobalOrientationQuat ((const NxQuat&)rotation);
+		// Novodex workaround ->Move global pose needs to always be called in order to make sure triggers get activated!		
+		m_Actor->moveGlobalOrientationQuat ((const NxQuat&)rotation);
+		m_DisableReadUpdateTransform = 0;
+	}
+	else
+	{
+		if (m_InterpolationInfo)
+			m_InterpolationInfo->disabled = 1;
+
+		m_Actor->setGlobalOrientationQuat ((const NxQuat&)rotation);
+	}
+}
+
+void Rigidbody::MovePosition (const Vector3f& position)
+{
+	ABORT_INVALID_VECTOR3 (position, position, rigidbody);
+	if (GetIsKinematic())
+	{
+		m_Actor->moveGlobalPosition ((const NxVec3&)position);
+		m_DisableReadUpdateTransform = 0;
+	}
+	else
+	{
+		m_Actor->setGlobalPosition ((const NxVec3&)position);
+	}
+}
+
+void Rigidbody::MoveRotation (const Quaternionf& rotation)
+{
+	ABORT_INVALID_QUATERNION (rotation, rotation, rigidbody);
+
+	if (GetIsKinematic())
+	{
+		m_Actor->moveGlobalOrientation ((const NxQuat&)rotation);
+		m_DisableReadUpdateTransform = 0;
+	}
+	else
+	{
+		m_Actor->setGlobalOrientation ((const NxQuat&)rotation);
+	}
+}
+
+void Rigidbody::SetDensity (float density)
+{
+	if (m_Actor)
+		m_Actor->updateMassFromShapes (density, 0.0F);
+}
+
+bool Rigidbody::IsSleeping ()
+{
+	return m_Actor->isSleeping();
+}
+
+void Rigidbody::Sleep ()
+{
+	return m_Actor->putToSleep();
+}
+
+void Rigidbody::WakeUp ()
+{
+	return m_Actor->wakeUp();
+}
+
+
+bool Rigidbody::GetDetectCollisions() const
+{
+	if (m_Actor)
+		return !m_Actor->readActorFlag(NX_AF_DISABLE_COLLISION);
+	else
+		return true;
+}
+
+void Rigidbody::SetDetectCollisions(bool enable)
+{
+	if (m_Actor)
+	{
+		if (enable)
+			m_Actor->clearActorFlag (NX_AF_DISABLE_COLLISION);	
+		else
+			m_Actor->raiseActorFlag (NX_AF_DISABLE_COLLISION);	
+	}
+}
+
+bool Rigidbody::GetUseConeFriction() const
+{
+	if (m_Actor)
+		return !m_Actor->readActorFlag(NX_AF_FORCE_CONE_FRICTION);
+	else
+		return true;
+}
+
+void Rigidbody::SetUseConeFriction(bool enable)
+{
+	if (m_Actor)
+	{
+		if (enable)
+			m_Actor->clearActorFlag (NX_AF_FORCE_CONE_FRICTION);	
+		else
+			m_Actor->raiseActorFlag (NX_AF_FORCE_CONE_FRICTION);	
+	}
+}
+
+void Rigidbody::UpdateInterpolationNode ()
+{
+	if (m_Interpolate == kNoInterpolation || !m_ActiveScene)
+	{
+		delete m_InterpolationInfo;
+		m_InterpolationInfo = NULL;
+	}
+	else
+	{
+		if (m_InterpolationInfo == NULL)
+		{
+			m_InterpolationInfo = new RigidbodyInterpolationInfo();
+			RigidbodyInterpolationInfo& info = *m_InterpolationInfo;
+			info.body = this;
+			info.disabled = 1;
+			info.position = Vector3f::zero;
+			info.rotation = Quaternionf::identity();
+			GetPhysicsManager().GetInterpolatedBodies().push_back(*m_InterpolationInfo);
+		}
+	}
+}
+
+void Rigidbody::SetCollisionDetectionMode (int ccd)
+{
+	if (ccd != m_CachedCollisionDetection)
+	{
+		m_CollisionDetection = ccd;
+		m_CachedCollisionDetection = ccd;
+		
+		if (m_Actor)
+		{
+			int shapeCount = m_Actor->getNbShapes ();
+			NxShape*const * shapes = m_Actor->getShapes ();
+			
+			for (int i=0;i<shapeCount;i++)
+			{
+				Collider* collider = (Collider*)shapes[i]->userData;
+				collider->ReCreate();
+			}
+		}
+		
+		SetDirty();
+	}
+}
+
+
+bool Rigidbody::SweepTest (const Vector3f &direction, float distance, RaycastHit& outHit)
+{
+	AssertIf (!IsNormalized (direction));
+	PROFILER_AUTO(gSweepTestProfile, NULL)
+		
+	if (m_Actor)
+	{
+		if (distance == std::numeric_limits<float>::infinity())
+			// CapsuleCasts fail when using NX_MAX_F32 here.
+			// So pick a lower "high" number instead.
+			distance = 1000000.0f;
+			
+		NxSweepQueryHit hit;
+		NxU32 nb = m_Actor->linearSweep ((const NxVec3&)direction * distance, NX_SF_DYNAMICS|NX_SF_STATICS, NULL, 1, &hit, NULL);
+		if (nb)
+		{
+			NxToRaycastHit(hit, distance, outHit);
+			return true;
+		}
+	}
+	return false;
+}
+
+#define kSweepMaxHits 128
+const PhysicsManager::RaycastHits& Rigidbody::SweepTestAll (const Vector3f &direction, float distance)
+{
+	AssertIf (!IsNormalized (direction));
+	PROFILER_AUTO(gSweepTestAllProfile, NULL)
+	
+	if (distance == std::numeric_limits<float>::infinity())
+		// CapsuleCasts fail when using NX_MAX_F32 here.
+		// So pick a lower "high" number instead.
+		distance = 1000000.0f;
+	
+	static vector<RaycastHit> outHits;
+
+	if (m_Actor)
+	{
+		NxSweepQueryHit hits[kSweepMaxHits];
+		NxU32 nb = m_Actor->linearSweep ((const NxVec3&)direction * distance, NX_SF_DYNAMICS|NX_SF_STATICS|NX_SF_ALL_HITS, NULL, kSweepMaxHits, hits, NULL);
+
+		outHits.resize(nb);
+		for (int i=0; i<nb; i++)
+			NxToRaycastHit(hits[i], distance, outHits[i]);
+	}
+	return outHits;
+}
+
+
+IMPLEMENT_CLASS_HAS_INIT (Rigidbody)
+IMPLEMENT_OBJECT_SERIALIZE (Rigidbody)
+#endif // ENABLE_PHYSICS