// 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.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  

#ifndef EXT_JOINT_H
#define EXT_JOINT_H

#include "PxPhysics.h"
#include "extensions/PxConstraintExt.h"
#include "PxRigidStatic.h"
#include "PxRigidDynamic.h"
#include "PxArticulationLink.h"
#include "PxArticulationReducedCoordinate.h"
#include "PxScene.h"

#include "foundation/PxAllocator.h"
#include "foundation/PxMathUtils.h"
#include "CmUtils.h"
#include "ExtJointData.h"

#if PX_SUPPORT_PVD
	#include "pvd/PxPvdSceneClient.h"
	#include "ExtPvd.h"
	#include "PxPvdClient.h"
#endif

#include "omnipvd/ExtOmniPvdSetData.h"

namespace physx
{
// PX_SERIALIZATION
	class PxDeserializationContext;

PxConstraint* resolveConstraintPtr(PxDeserializationContext& v, PxConstraint* old, PxConstraintConnector* connector, PxConstraintShaderTable& shaders);

// ~PX_SERIALIZATION

namespace Ext
{
	PX_FORCE_INLINE float computeSwingAngle(float swingYZ, float swingW)
	{
		return 4.0f * PxAtan2(swingYZ, 1.0f + swingW);	// tan (t/2) = sin(t)/(1+cos t), so this is the quarter angle
	}

	template<class JointType, class DataType>
	PX_FORCE_INLINE	JointType* createJointT(PxPhysics& physics, PxRigidActor* actor0, const PxTransform& localFrame0, PxRigidActor* actor1, const PxTransform& localFrame1, const PxConstraintShaderTable& shaders)
	{
		JointType* j;
		PX_NEW_SERIALIZED(j, JointType)(physics.getTolerancesScale(), actor0, localFrame0, actor1, localFrame1);

		if(!physics.createConstraint(actor0, actor1, *j, shaders, sizeof(DataType)))
			PX_DELETE(j);

#if PX_SUPPORT_OMNI_PVD
		if (j)
		{
			omniPvdInitJoint(*j);
		}
#endif

		return j;
	}

// PX_SERIALIZATION
	template<class JointType>
	PX_FORCE_INLINE	JointType*	createJointObject(PxU8*& address, PxDeserializationContext& context)
	{
		JointType* obj = PX_PLACEMENT_NEW(address, JointType(PxBaseFlag::eIS_RELEASABLE));
		address += sizeof(JointType);	
		obj->importExtraData(context);
		obj->resolveReferences(context);
		return obj;
	}
//~PX_SERIALIZATION

	template <class Base, class DataClass, class ValueStruct>
	class JointT : public Base, public PxConstraintConnector, public PxUserAllocated
	{
    public:
// PX_SERIALIZATION
						JointT(PxBaseFlags baseFlags) : Base(baseFlags)	{}

				void	exportExtraData(PxSerializationContext& stream)
				{
					if(mData)
					{
						stream.alignData(PX_SERIAL_ALIGN);
						stream.writeData(mData, sizeof(DataClass));
					}
					stream.writeName(mName);
				}

				void	importExtraData(PxDeserializationContext& context)
				{
					if(mData)
						mData = context.readExtraData<DataClass, PX_SERIAL_ALIGN>();
					context.readName(mName);
				}

		virtual void	preExportDataReset(){}
		virtual	void	requiresObjects(PxProcessPxBaseCallback& c)
		{			
			c.process(*mPxConstraint);
			
			{
				PxRigidActor* a0 = NULL;
				PxRigidActor* a1 = NULL;
				mPxConstraint->getActors(a0,a1);
				
				if (a0)
				{
					c.process(*a0);
				}
				if (a1)
				{
					c.process(*a1);
				}
			}
		}	
//~PX_SERIALIZATION
		
#if PX_SUPPORT_PVD
		// PxConstraintConnector
		virtual bool updatePvdProperties(physx::pvdsdk::PvdDataStream& pvdConnection, const PxConstraint* c, PxPvdUpdateType::Enum updateType) const	PX_OVERRIDE
		{
			if(updateType == PxPvdUpdateType::UPDATE_SIM_PROPERTIES)
			{
				Ext::Pvd::simUpdate<Base>(pvdConnection, *this);
				return true;
			}
			else if(updateType == PxPvdUpdateType::UPDATE_ALL_PROPERTIES)
			{
				Ext::Pvd::updatePvdProperties<Base, ValueStruct>(pvdConnection, *this);
				return true;
			}
			else if(updateType == PxPvdUpdateType::CREATE_INSTANCE)
			{
				Ext::Pvd::createPvdInstance<Base>(pvdConnection, *c, *this);
				return true;
			}
			else if(updateType == PxPvdUpdateType::RELEASE_INSTANCE)
			{
				Ext::Pvd::releasePvdInstance(pvdConnection, *c, *this);
				return true;
			}
			return false;
		}
#else
		virtual bool updatePvdProperties(physx::pvdsdk::PvdDataStream&, const PxConstraint*, PxPvdUpdateType::Enum) const	PX_OVERRIDE
		{
			return false;
		}
#endif

		// PxConstraintConnector
		virtual void updateOmniPvdProperties() const PX_OVERRIDE
		{
		}

		// PxJoint
		virtual void setActors(PxRigidActor* actor0, PxRigidActor* actor1)	PX_OVERRIDE
		{	
			//TODO SDK-DEV
			//You can get the debugger stream from the NpScene
			//Ext::Pvd::setActors( stream, this, mPxConstraint, actor0, actor1 );
			PX_CHECK_AND_RETURN(actor0 != actor1, "PxJoint::setActors: actors must be different");
			PX_CHECK_AND_RETURN((actor0 && !actor0->is<PxRigidStatic>()) || (actor1 && !actor1->is<PxRigidStatic>()), "PxJoint::setActors: at least one actor must be non-static");

#if PX_SUPPORT_PVD
			PxScene* scene = getScene();
			if(scene)
			{
				//if pvd not connect data stream is NULL
				physx::pvdsdk::PvdDataStream* conn = scene->getScenePvdClient()->getClientInternal()->getDataStream();
				if( conn != NULL )
					Ext::Pvd::setActors(
					*conn,
					*this,
					*mPxConstraint,
					actor0, 
					actor1
					);
			}
#endif
			mPxConstraint->setActors(actor0, actor1);
			mData->c2b[0] = getCom(actor0).transformInv(mLocalPose[0]);
			mData->c2b[1] = getCom(actor1).transformInv(mLocalPose[1]);
			mPxConstraint->markDirty();

			OMNI_PVD_WRITE_SCOPE_BEGIN(pvdWriter, pvdRegData)
			OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxJoint, actor0, static_cast<PxJoint&>(*this), actor0)
			OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxJoint, actor1, static_cast<PxJoint&>(*this), actor1)
			OMNI_PVD_WRITE_SCOPE_END
		}

		// PxJoint
		virtual	void getActors(PxRigidActor*& actor0, PxRigidActor*& actor1)	const	PX_OVERRIDE
		{	
			if(mPxConstraint)
				mPxConstraint->getActors(actor0,actor1);
			else
			{
				actor0 = NULL;
				actor1 = NULL;
			}
		}

		// this is the local pose relative to the actor, and we store internally the local
		// pose relative to the body 

		// PxJoint
		virtual void setLocalPose(PxJointActorIndex::Enum actor, const PxTransform& pose)	PX_OVERRIDE
		{
			PX_CHECK_AND_RETURN(pose.isSane(), "PxJoint::setLocalPose: transform is invalid");
			const PxTransform p = pose.getNormalized();
			mLocalPose[actor] = p;
			mData->c2b[actor] = getCom(actor).transformInv(p); 
			mPxConstraint->markDirty();

			OMNI_PVD_WRITE_SCOPE_BEGIN(pvdWriter, pvdRegData)
			OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxJoint, actor0LocalPose, static_cast<PxJoint&>(*this), mLocalPose[0])
			OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxJoint, actor1LocalPose, static_cast<PxJoint&>(*this), mLocalPose[1])
			OMNI_PVD_WRITE_SCOPE_END
		}

		// PxJoint
		virtual	PxTransform	getLocalPose(PxJointActorIndex::Enum actor) const	PX_OVERRIDE
		{	
			return mLocalPose[actor];
		}

		static	PxTransform	getGlobalPose(const PxRigidActor* actor)
		{
			if(!actor)
				return PxTransform(PxIdentity);
			return actor->getGlobalPose();
		}

		static	void	getActorVelocity(const PxRigidActor* actor, PxVec3& linear, PxVec3& angular)
		{
			if(!actor || actor->is<PxRigidStatic>())
			{
				linear = angular = PxVec3(0.0f);
				return;
			}
			
			linear = static_cast<const PxRigidBody*>(actor)->getLinearVelocity();
			angular = static_cast<const PxRigidBody*>(actor)->getAngularVelocity();
		}

		// PxJoint
		virtual	PxTransform	getRelativeTransform()	const	PX_OVERRIDE
		{
			PxRigidActor* actor0, * actor1;
			mPxConstraint->getActors(actor0, actor1);
			const PxTransform t0 = getGlobalPose(actor0) * mLocalPose[0];
			const PxTransform t1 = getGlobalPose(actor1) * mLocalPose[1];
			return t0.transformInv(t1);
		}

		// PxJoint
		virtual	PxVec3	getRelativeLinearVelocity()	const	PX_OVERRIDE
		{
			PxRigidActor* actor0, * actor1;
			PxVec3 l0, a0, l1, a1;
			mPxConstraint->getActors(actor0, actor1);

			const PxTransform actor0ToWorld = getGlobalPose(actor0);
			const PxTransform actor1ToWorld = getGlobalPose(actor1);

			const PxTransform body0ToActor = getCom(actor0);
			const PxTransform body1ToActor = getCom(actor1);

			getActorVelocity(actor0, l0, a0);
			getActorVelocity(actor1, l1, a1);

			// the offset from center of mass to joint frame origin (in the
			// actor frame)
			const PxVec3 jointFrame0CoMOffsetLocal = mLocalPose[0].p - body0ToActor.p;
			const PxVec3 jointFrame1CoMOffsetLocal = mLocalPose[1].p - body1ToActor.p;

			const PxVec3 jointFrame0CoMOffsetWorld = actor0ToWorld.rotate(jointFrame0CoMOffsetLocal);
			const PxVec3 jointFrame1CoMOffsetWorld = actor1ToWorld.rotate(jointFrame1CoMOffsetLocal);

			const PxVec3 relativeVelWorld = (l1 + a1.cross(jointFrame1CoMOffsetWorld)) -
				(l0 + a0.cross(jointFrame0CoMOffsetWorld));

			const PxQuat jointFrame0ToWorld = actor0ToWorld.q * mLocalPose[0].q;
			const PxVec3 relativeVelJointFrame0 = jointFrame0ToWorld.rotateInv(relativeVelWorld);

			return relativeVelJointFrame0;
		}

		// PxJoint
		virtual	PxVec3	getRelativeAngularVelocity()	const	PX_OVERRIDE
		{
			PxRigidActor* actor0, * actor1;
			PxVec3 l0, a0, l1, a1;
			mPxConstraint->getActors(actor0, actor1);

			const PxTransform actor0ToWorld = getGlobalPose(actor0);

			getActorVelocity(actor0, l0, a0);
			getActorVelocity(actor1, l1, a1);

			const PxVec3 relativeVelWorld = a1 - a0;

			const PxQuat jointFrame0ToWorld = actor0ToWorld.q * mLocalPose[0].q;
			const PxVec3 relativeVelJointFrame0 = jointFrame0ToWorld.rotateInv(relativeVelWorld);

			return relativeVelJointFrame0;
		}

		// PxJoint
		virtual	void setBreakForce(PxReal force, PxReal torque)	PX_OVERRIDE
		{
			PX_CHECK_AND_RETURN(PxIsFinite(force) && PxIsFinite(torque), "PxJoint::setBreakForce: invalid float");
			mPxConstraint->setBreakForce(force,torque);

			OMNI_PVD_WRITE_SCOPE_BEGIN(pvdWriter, pvdRegData)
			OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxJoint, breakForce, static_cast<PxJoint&>(*this), force)
			OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxJoint, breakTorque, static_cast<PxJoint&>(*this), torque)
			OMNI_PVD_WRITE_SCOPE_END
		}

		// PxJoint
		virtual	void getBreakForce(PxReal& force, PxReal& torque)	const	PX_OVERRIDE
		{
			mPxConstraint->getBreakForce(force,torque);
		}

		// PxJoint
		virtual	void setConstraintFlags(PxConstraintFlags flags)	PX_OVERRIDE
		{
			mPxConstraint->setFlags(flags);

			OMNI_PVD_SET(OMNI_PVD_CONTEXT_HANDLE, PxJoint, constraintFlags, static_cast<PxJoint&>(*this), flags)
		}

		// PxJoint
		virtual	void setConstraintFlag(PxConstraintFlag::Enum flag, bool value)	PX_OVERRIDE
		{
			mPxConstraint->setFlag(flag, value);

			OMNI_PVD_SET(OMNI_PVD_CONTEXT_HANDLE, PxJoint, constraintFlags, static_cast<PxJoint&>(*this), getConstraintFlags())
		}

		// PxJoint
		virtual	PxConstraintFlags getConstraintFlags()	const	PX_OVERRIDE
		{
			return mPxConstraint->getFlags();
		}

		// PxJoint
		virtual	void setInvMassScale0(PxReal invMassScale)	PX_OVERRIDE
		{
			PX_CHECK_AND_RETURN(PxIsFinite(invMassScale) && invMassScale>=0, "PxJoint::setInvMassScale0: scale must be non-negative");
			mData->invMassScale.linear0 = invMassScale;
			mPxConstraint->markDirty();

			OMNI_PVD_SET(OMNI_PVD_CONTEXT_HANDLE, PxJoint, invMassScale0, static_cast<PxJoint&>(*this), invMassScale)
		}

		// PxJoint
		virtual	PxReal getInvMassScale0() const	PX_OVERRIDE
		{
			return mData->invMassScale.linear0;
		}

		// PxJoint
		virtual	void setInvInertiaScale0(PxReal invInertiaScale)	PX_OVERRIDE
		{
			PX_CHECK_AND_RETURN(PxIsFinite(invInertiaScale) && invInertiaScale>=0, "PxJoint::setInvInertiaScale0: scale must be non-negative");
			mData->invMassScale.angular0 = invInertiaScale;
			mPxConstraint->markDirty();

			OMNI_PVD_SET(OMNI_PVD_CONTEXT_HANDLE, PxJoint, invInertiaScale0, static_cast<PxJoint&>(*this), invInertiaScale)
		}

		// PxJoint
		virtual	PxReal getInvInertiaScale0() const	PX_OVERRIDE
		{
			return mData->invMassScale.angular0;
		}

		// PxJoint
		virtual	void setInvMassScale1(PxReal invMassScale)	PX_OVERRIDE
		{
			PX_CHECK_AND_RETURN(PxIsFinite(invMassScale) && invMassScale>=0, "PxJoint::setInvMassScale1: scale must be non-negative");
			mData->invMassScale.linear1 = invMassScale;
			mPxConstraint->markDirty();

			OMNI_PVD_SET(OMNI_PVD_CONTEXT_HANDLE, PxJoint, invMassScale1, static_cast<PxJoint&>(*this), invMassScale)
		}

		// PxJoint
		virtual	PxReal getInvMassScale1() const	PX_OVERRIDE
		{
			return mData->invMassScale.linear1;
		}

		// PxJoint
		virtual	void setInvInertiaScale1(PxReal invInertiaScale)	PX_OVERRIDE
		{
			PX_CHECK_AND_RETURN(PxIsFinite(invInertiaScale) && invInertiaScale>=0, "PxJoint::setInvInertiaScale: scale must be non-negative");
			mData->invMassScale.angular1 = invInertiaScale;
			mPxConstraint->markDirty();

			OMNI_PVD_SET(OMNI_PVD_CONTEXT_HANDLE, PxJoint, invInertiaScale1, static_cast<PxJoint&>(*this), invInertiaScale)
		}

		// PxJoint
		virtual	PxReal getInvInertiaScale1() const	PX_OVERRIDE
		{
			return mData->invMassScale.angular1;
		}

		// PxJoint
		virtual	PxConstraint* getConstraint()	const	PX_OVERRIDE
		{
			return mPxConstraint;
		}

		// PxJoint
		virtual	void setName(const char* name)	PX_OVERRIDE
		{
			mName = name;
#if PX_SUPPORT_OMNI_PVD
			const char* n = name ? name : "";
			PxU32 nLen = PxU32(strlen(n)) + 1;
			OMNI_PVD_SET_ARRAY(OMNI_PVD_CONTEXT_HANDLE, PxJoint, name, static_cast<PxJoint&>(*this), n, nLen)
#endif
		}

		// PxJoint
		virtual	const char* getName()	const	PX_OVERRIDE
		{
			return mName;
		}

		// PxJoint
		virtual	void release()	PX_OVERRIDE
		{
			mPxConstraint->release();
		}

		// PxJoint
		virtual	PxScene* getScene() const	PX_OVERRIDE
		{
			return mPxConstraint ? mPxConstraint->getScene() : NULL;
		}

		// PxConstraintConnector
		virtual	void onComShift(PxU32 actor)	PX_OVERRIDE
		{
			mData->c2b[actor] = getCom(actor).transformInv(mLocalPose[actor]); 
			markDirty();
		}

		// PxConstraintConnector
		virtual	void onOriginShift(const PxVec3& shift)	PX_OVERRIDE
		{
			PxRigidActor* a[2];
			mPxConstraint->getActors(a[0], a[1]);

			if (!a[0])
			{
				mLocalPose[0].p -= shift;
				mData->c2b[0].p -= shift;
				markDirty();
			}
			else if (!a[1])
			{
				mLocalPose[1].p -= shift;
				mData->c2b[1].p -= shift;
				markDirty();
			}
		}

		// PxConstraintConnector
		virtual	void* prepareData()	PX_OVERRIDE
		{
			return mData;
		}

		// PxConstraintConnector
		virtual	void* getExternalReference(PxU32& typeID)	PX_OVERRIDE
		{
			typeID = PxConstraintExtIDs::eJOINT;
			return static_cast<PxJoint*>( this );
		}

		// PxConstraintConnector
		virtual	PxBase* getSerializable()	PX_OVERRIDE
		{
			return this;
		}

		// PxConstraintConnector
		virtual	void onConstraintRelease()	PX_OVERRIDE
		{
			PX_FREE(mData);
			PX_DELETE_THIS;
		}

		// PxConstraintConnector
		virtual const void* getConstantBlock()	const	PX_OVERRIDE
		{
			return mData; 
		}

		virtual	void		connectToConstraint(PxConstraint* c)	PX_OVERRIDE
		{
			mPxConstraint = c;
		}

	private:
		PxTransform getCom(PxU32 index) const
		{
			PxRigidActor* a[2];
			mPxConstraint->getActors(a[0],a[1]);
			return getCom(a[index]);
		}

		PxTransform getCom(PxRigidActor* actor) const
		{
			if (!actor)
				return PxTransform(PxIdentity);
			else if (actor->getType() == PxActorType::eRIGID_DYNAMIC || actor->getType() == PxActorType::eARTICULATION_LINK)
				return static_cast<PxRigidBody*>(actor)->getCMassLocalPose();
			else
			{
				PX_ASSERT(actor->getType() == PxActorType::eRIGID_STATIC);
				return static_cast<PxRigidStatic*>(actor)->getGlobalPose().getInverse();
			}
		}

	protected:

		JointT(PxType concreteType, PxRigidActor* actor0, const PxTransform& localFrame0, PxRigidActor* actor1, const PxTransform& localFrame1, const char* name) :
			Base			(concreteType, PxBaseFlag::eOWNS_MEMORY | PxBaseFlag::eIS_RELEASABLE),
			mName			(NULL),
			mPxConstraint	(NULL)
		{
			PX_UNUSED(name);
			Base::userData = NULL;

			const PxU32 size = sizeof(DataClass);
			JointData* data = reinterpret_cast<JointData*>(PX_ALLOC(size, name));
			PxMarkSerializedMemory(data, size);

			mLocalPose[0]				= localFrame0.getNormalized();
			mLocalPose[1]				= localFrame1.getNormalized();
			data->c2b[0]				= getCom(actor0).transformInv(mLocalPose[0]);
			data->c2b[1]				= getCom(actor1).transformInv(mLocalPose[1]);
			data->invMassScale.linear0	= 1.0f;
			data->invMassScale.angular0	= 1.0f;
			data->invMassScale.linear1	= 1.0f;
			data->invMassScale.angular1	= 1.0f;

			mData = data;
		}

		virtual ~JointT()
		{
			if(Base::getBaseFlags() & PxBaseFlag::eOWNS_MEMORY)
				PX_FREE(mData);

			OMNI_PVD_DESTROY(OMNI_PVD_CONTEXT_HANDLE, PxJoint, static_cast<Base&>(*this))
		}

		PX_FORCE_INLINE DataClass& data() const
		{
			return *static_cast<DataClass*>(mData);
		}

		PX_FORCE_INLINE	void markDirty()
		{ 
			mPxConstraint->markDirty();
		}

		void wakeUpActors()
		{
			PxRigidActor* a[2];
			mPxConstraint->getActors(a[0], a[1]);
			for(PxU32 i = 0; i < 2; i++)
			{
				if(a[i] && a[i]->getScene())
				{
					if(a[i]->getType() == PxActorType::eRIGID_DYNAMIC)
					{
						PxRigidDynamic* rd = static_cast<PxRigidDynamic*>(a[i]);
						if(!(rd->getRigidBodyFlags() & PxRigidBodyFlag::eKINEMATIC))
						{
							const PxScene* scene = rd->getScene();
							const PxReal wakeCounterResetValue = scene->getWakeCounterResetValue();
							const PxReal wakeCounter = rd->getWakeCounter();
							if(wakeCounter < wakeCounterResetValue)
							{
								rd->wakeUp();
							}
						}
					}
					else if(a[i]->getType() == PxActorType::eARTICULATION_LINK)
					{
						PxArticulationLink* link = reinterpret_cast<PxArticulationLink*>(a[i]);
						PxArticulationReducedCoordinate& articulation = link->getArticulation();
						const PxReal wakeCounter = articulation.getWakeCounter();
						const PxScene* scene = link->getScene();
						const PxReal wakeCounterResetValue = scene->getWakeCounterResetValue();
						if(wakeCounter < wakeCounterResetValue)
						{
							articulation.wakeUp();
						}
					}
				}
			}
		}

		PX_FORCE_INLINE	PxQuat	getTwistOrSwing(bool needTwist) const
		{
			const PxQuat q = getRelativeTransform().q;
			// PT: TODO: we don't need to compute both quats here
			PxQuat swing, twist;
			PxSeparateSwingTwist(q, swing, twist);
			return needTwist ? twist : swing;
		}

		PxReal	getTwistAngle_Internal() const
		{
			const PxQuat twist = getTwistOrSwing(true);

			// PT: the angle-axis formulation creates the quat like this:
			//
			//	const float a = angleRadians * 0.5f;
			//	const float s = PxSin(a);
			//	w = PxCos(a);
			//	x = unitAxis.x * s;
			//	y = unitAxis.y * s;
			//	z = unitAxis.z * s;
			//
			// With the twist axis = (1;0;0) this gives:
			//
			//	w = PxCos(angleRadians * 0.5f);
			//	x = PxSin(angleRadians * 0.5f);
			//	y = 0.0f;
			//	z = 0.0f;
			//
			// Thus the quat's "getAngle" function returns:
			//
			//	angle = PxAcos(w) * 2.0f;
			//
			// PxAcos will return an angle between 0 and PI in radians, so "getAngle" will return an angle between 0 and PI*2.

			PxReal angle = twist.getAngle();

			if(twist.x<0.0f)
				angle = -angle;

			return angle;
		}

		PxReal	getSwingYAngle_Internal()	const
		{
			PxQuat swing = getTwistOrSwing(false);

			if(swing.w < 0.0f)		// choose the shortest rotation
				swing = -swing;

			const PxReal angle = computeSwingAngle(swing.y, swing.w);
			PX_ASSERT(angle>-PxPi && angle<=PxPi);				// since |y| < w+1, the atan magnitude is < PI/4
			return angle;
		}

		PxReal	getSwingZAngle_Internal()	const
		{
			PxQuat swing = getTwistOrSwing(false);

			if(swing.w < 0.0f)		// choose the shortest rotation
				swing = -swing;

			const PxReal angle = computeSwingAngle(swing.z, swing.w);
			PX_ASSERT(angle>-PxPi && angle <= PxPi);			// since |y| < w+1, the atan magnitude is < PI/4
			return angle;
		}

		const char*		mName;
		PxTransform		mLocalPose[2];
		PxConstraint*	mPxConstraint;
		JointData*		mData;
	};

#if PX_SUPPORT_OMNI_PVD
	void omniPvdSetBaseJointParams(const PxJoint& joint, PxJointConcreteType::Enum cType);
	template<typename JointType> void omniPvdInitJoint(JointType& joint);
#endif

} // namespace Ext

}

#endif