//
// 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-2019 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 "NpArticulationLink.h"
#include "NpArticulationJoint.h"
#include "NpWriteCheck.h"
#include "NpReadCheck.h"
#include "ScbArticulation.h"
#include "CmVisualization.h"
#include "CmConeLimitHelper.h"
#include "CmUtils.h"
#include "NpArticulation.h"

using namespace physx;

// PX_SERIALIZATION
void NpArticulationLink::requiresObjects(PxProcessPxBaseCallback& c)
{
	NpArticulationLinkT::requiresObjects(c);
	
	if(mInboundJoint)
		c.process(*mInboundJoint);
}

void NpArticulationLink::exportExtraData(PxSerializationContext& stream)
{
	NpArticulationLinkT::exportExtraData(stream);
	Cm::exportInlineArray(mChildLinks, stream);
}

void NpArticulationLink::importExtraData(PxDeserializationContext& context)
{
	NpArticulationLinkT::importExtraData(context);
	Cm::importInlineArray(mChildLinks, context);
}

void NpArticulationLink::resolveReferences(PxDeserializationContext& context)
{	
    context.translatePxBase(mRoot);
    context.translatePxBase(mInboundJoint);
    context.translatePxBase(mParent);
       
    NpArticulationLinkT::resolveReferences(context);

    const PxU32 nbLinks = mChildLinks.size();
    for(PxU32 i=0;i<nbLinks;i++)
        context.translatePxBase(mChildLinks[i]);
}

NpArticulationLink* NpArticulationLink::createObject(PxU8*& address, PxDeserializationContext& context)
{
	NpArticulationLink* obj = new (address) NpArticulationLink(PxBaseFlags(0));
	address += sizeof(NpArticulationLink);	
	obj->importExtraData(context);
	obj->resolveReferences(context);
	return obj;
}
//~PX_SERIALIZATION

NpArticulationLink::NpArticulationLink(const PxTransform& bodyPose, PxArticulationBase& root, NpArticulationLink* parent) :
	NpArticulationLinkT	(PxConcreteType::eARTICULATION_LINK, PxBaseFlag::eOWNS_MEMORY, PxActorType::eARTICULATION_LINK, bodyPose),
	mRoot				(&root),
	mInboundJoint		(NULL),
	mParent				(parent),
	mLLIndex			(0xffffffff),
	mInboundJointDof	(0xffffffff)
{
	PX_ASSERT(mBody.getScbType() == ScbType::eBODY);
	mBody.setScbType(ScbType::eBODY_FROM_ARTICULATION_LINK);

	if (parent)
		parent->addToChildList(*this);
}

NpArticulationLink::~NpArticulationLink()
{
}

void NpArticulationLink::releaseInternal()
{
	NpPhysics::getInstance().notifyDeletionListenersUserRelease(this, userData);

	NpArticulationLinkT::release();

	PxArticulationImpl* impl = reinterpret_cast<PxArticulationImpl*>(mRoot->getImpl());
	impl->removeLinkFromList(*this);

	if (mParent)
		mParent->removeFromChildList(*this);

	if (mInboundJoint)
		mInboundJoint->release();

	NpScene* npScene = NpActor::getAPIScene(*this);
	if (npScene)
	{
		npScene->getScene().removeActor(mBody, true, false);
		//Removing this link may displace the link IDs for the other links in the articulation, so recompute them all
		impl->recomputeLinkIDs();
	}

	mBody.destroy();
}

void NpArticulationLink::release()
{
	NP_WRITE_CHECK(NpActor::getOwnerScene(*this));

	PxArticulationImpl* impl = reinterpret_cast<PxArticulationImpl*>(mRoot->getImpl());

	PX_UNUSED(impl);

	if (impl->getRoot() == this && NpActor::getOwnerScene(*this) != NULL)
	{
		Ps::getFoundation().error(PxErrorCode::eINVALID_OPERATION, __FILE__, __LINE__, "PxArticulationLink::release(): root link may not be released while articulation is in a scene");
		return;
	}

	//! this function doesn't get called when the articulation root is released
	// therefore, put deregistration code etc. into dtor, not here

	if (mChildLinks.empty())
	{
		releaseInternal();
	}
	else
	{
		Ps::getFoundation().error(PxErrorCode::eINVALID_OPERATION, __FILE__, __LINE__, "PxArticulationLink::release(): Only leaf articulation links can be released. Release call failed");
	}
}

PxTransform NpArticulationLink::getGlobalPose() const
{
	NP_READ_CHECK(NpActor::getOwnerScene(*this));

	//!!!AL TODO: Need to start from root and compute along the branch to reflect double buffered state of root link
	return getScbBodyFast().getBody2World() * getScbBodyFast().getBody2Actor().getInverse();
}

void NpArticulationLink::setLinearDamping(PxReal linearDamping)
{
	NP_WRITE_CHECK(NpActor::getOwnerScene(*this));
	PX_CHECK_AND_RETURN(PxIsFinite(linearDamping), "NpArticulationLink::setLinearDamping: invalid float");
	PX_CHECK_AND_RETURN(linearDamping >= 0, "NpArticulationLink::setLinearDamping: The linear damping must be nonnegative!");

	getScbBodyFast().setLinearDamping(linearDamping);
}

PxReal NpArticulationLink::getLinearDamping() const
{
	NP_READ_CHECK(NpActor::getOwnerScene(*this));

	return getScbBodyFast().getLinearDamping();
}

void NpArticulationLink::setAngularDamping(PxReal angularDamping)
{
	NP_WRITE_CHECK(NpActor::getOwnerScene(*this));
	PX_CHECK_AND_RETURN(PxIsFinite(angularDamping), "NpArticulationLink::setAngularDamping: invalid float");
	PX_CHECK_AND_RETURN(angularDamping >= 0, "NpArticulationLink::setAngularDamping: The angular damping must be nonnegative!")

	getScbBodyFast().setAngularDamping(angularDamping);
}

PxReal NpArticulationLink::getAngularDamping() const
{
	NP_READ_CHECK(NpActor::getOwnerScene(*this));

	return getScbBodyFast().getAngularDamping();
}

PxArticulationBase& NpArticulationLink::getArticulation() const
{
	NP_READ_CHECK(NpActor::getOwnerScene(*this));
	return *mRoot;
}

PxArticulationJointBase* NpArticulationLink::getInboundJoint() const
{
	NP_READ_CHECK(NpActor::getOwnerScene(*this));
	return mInboundJoint;
}

PxU32 NpArticulationLink::getInboundJointDof() const
{
	NP_READ_CHECK(NpActor::getOwnerScene(*this));
	return mInboundJointDof;
}

PxU32 NpArticulationLink::getNbChildren() const
{
	NP_READ_CHECK(NpActor::getOwnerScene(*this));
	return mChildLinks.size();
}

PxU32 NpArticulationLink::getChildren(PxArticulationLink** userBuffer, PxU32 bufferSize, PxU32 startIndex) const
{
	NP_READ_CHECK(NpActor::getOwnerScene(*this));
	return Cm::getArrayOfPointers(userBuffer, bufferSize, startIndex, mChildLinks.begin(), mChildLinks.size());
}

PxU32 NpArticulationLink::getLinkIndex() const
{
	NP_READ_CHECK(NpActor::getOwnerScene(*this));
	return mLLIndex;
}

void NpArticulationLink::setCMassLocalPose(const PxTransform& pose)
{
	NP_WRITE_CHECK(NpActor::getOwnerScene(*this));
	PX_CHECK_AND_RETURN(pose.isSane(), "PxArticulationLink::setCMassLocalPose: invalid parameter");

	const PxTransform p = pose.getNormalized();
	const PxTransform oldpose = getScbBodyFast().getBody2Actor();
	const PxTransform comShift = p.transformInv(oldpose);

	NpArticulationLinkT::setCMassLocalPoseInternal(p);

	if(mInboundJoint)
	{
		Scb::ArticulationJoint &j = mInboundJoint->getImpl()->getScbArticulationJoint();
		j.setChildPose(comShift.transform(j.getChildPose()));
	}

	for(PxU32 i=0; i<mChildLinks.size(); i++)
	{
		Scb::ArticulationJoint &j = static_cast<NpArticulationJoint*>(mChildLinks[i]->getInboundJoint())->getScbArticulationJoint();
		j.setParentPose(comShift.transform(j.getParentPose()));
	}
}

void NpArticulationLink::addForce(const PxVec3& force, PxForceMode::Enum mode, bool autowake)
{
	NpScene* scene = NpActor::getOwnerScene(*this);
	PX_UNUSED(scene);

	PX_CHECK_AND_RETURN(force.isFinite(), "NpArticulationLink::addForce: force is not valid.");
	NP_WRITE_CHECK(scene);
	PX_CHECK_AND_RETURN(scene, "NpArticulationLink::addForce: articulation link must be in a scene!");

	addSpatialForce(&force, 0, mode);

	reinterpret_cast<PxArticulationImpl*>(mRoot->getImpl())->wakeUpInternal((!force.isZero()), autowake);
}

void NpArticulationLink::addTorque(const PxVec3& torque, PxForceMode::Enum mode, bool autowake)
{
	NpScene* scene = NpActor::getOwnerScene(*this);
	PX_UNUSED(scene);

	PX_CHECK_AND_RETURN(torque.isFinite(), "NpArticulationLink::addTorque: force is not valid.");
	NP_WRITE_CHECK(scene);
	PX_CHECK_AND_RETURN(scene, "NpArticulationLink::addTorque: articulation link must be in a scene!");

	addSpatialForce(0, &torque, mode);

	reinterpret_cast<PxArticulationImpl*>(mRoot->getImpl())->wakeUpInternal((!torque.isZero()), autowake);
}

void NpArticulationLink::setForceAndTorque(const PxVec3& force, const PxVec3& torque, PxForceMode::Enum mode)
{
	NpScene* scene = NpActor::getOwnerScene(*this);
	PX_UNUSED(scene);

	PX_CHECK_AND_RETURN(torque.isFinite(), "NpArticulationLink::setForceAndTorque: torque is not valid.");
	PX_CHECK_AND_RETURN(force.isFinite(), "NpArticulationLink::setForceAndTorque: force is not valid.");
	NP_WRITE_CHECK(scene);
	PX_CHECK_AND_RETURN(scene, "NpArticulationLink::addTorque: articulation link must be in a scene!");

	setSpatialForce(&force, &torque, mode);

	reinterpret_cast<PxArticulationImpl*>(mRoot->getImpl())->wakeUpInternal((!torque.isZero()), true);
}

void NpArticulationLink::clearForce(PxForceMode::Enum mode)
{
	NpScene* scene = NpActor::getOwnerScene(*this);
	PX_UNUSED(scene);
	NP_WRITE_CHECK(scene);
	PX_CHECK_AND_RETURN(scene, "NpArticulationLink::clearForce: articulation link must be in a scene!");

	clearSpatialForce(mode, true, false);
}

void NpArticulationLink::clearTorque(PxForceMode::Enum mode)
{
	NpScene* scene = NpActor::getOwnerScene(*this);
	PX_UNUSED(scene);
	NP_WRITE_CHECK(scene);
	PX_CHECK_AND_RETURN(scene, "NpArticulationLink::clearTorque: articulation link must be in a scene!");

	clearSpatialForce(mode, false, true);
}

void NpArticulationLink::setGlobalPoseInternal(const PxTransform& pose, bool autowake)
{
	NpScene* scene = NpActor::getOwnerScene(*this);

	PX_CHECK_AND_RETURN(pose.isValid(), "NpArticulationLink::setGlobalPose pose is not valid.");

	NP_WRITE_CHECK(scene);

#if PX_CHECKED
	if (scene)
		scene->checkPositionSanity(*this, pose, "PxArticulationLink::setGlobalPose");
#endif

	PxTransform body2World = pose * getScbBodyFast().getBody2Actor();
	getScbBodyFast().setBody2World(body2World, false);

	if (scene && autowake)
		reinterpret_cast<PxArticulationImpl*>(mRoot->getImpl())->wakeUpInternal(false, true);

	if (scene)
		reinterpret_cast<PxArticulationImpl*>(mRoot->getImpl())->setGlobalPose();
}

void NpArticulationLink::setGlobalPose(const PxTransform& pose)
{
	// clow: no need to test inputs here, it's done in the setGlobalPose function already
	setGlobalPose(pose, true);
}

void NpArticulationLink::setGlobalPose(const PxTransform& pose, bool autowake)
{
	PX_CHECK_AND_RETURN(mRoot->getConcreteType() == PxConcreteType::eARTICULATION, "NpArticulationLink::setGlobalPose teleport isn't allowed in the reduced coordinate system.");
	setGlobalPoseInternal(pose, autowake);
}

void NpArticulationLink::setLinearVelocity(const PxVec3& velocity, bool autowake)
{
	NpScene* scene = NpActor::getOwnerScene(*this);

	PX_CHECK_AND_RETURN(velocity.isFinite(), "NpArticulationLink::setLinearVelocity velocity is not valid.");

	NP_WRITE_CHECK(scene);
	
	getScbBodyFast().setLinearVelocity(velocity);

	if (scene)
		reinterpret_cast<PxArticulationImpl*>(mRoot->getImpl())->wakeUpInternal((!velocity.isZero()), autowake);
}

void NpArticulationLink::setAngularVelocity(const PxVec3& velocity, bool autowake)
{
	NpScene* scene = NpActor::getOwnerScene(*this);

	PX_CHECK_AND_RETURN(velocity.isFinite(), "NpArticulationLink::setAngularVelocity velocity is not valid.");

	NP_WRITE_CHECK(scene);

	getScbBodyFast().setAngularVelocity(velocity);

	if (scene)
		reinterpret_cast<PxArticulationImpl*>(mRoot->getImpl())->wakeUpInternal((!velocity.isZero()), autowake);
}

void NpArticulationLink::setMaxAngularVelocity(PxReal maxAngularVelocity)
{
	NP_WRITE_CHECK(NpActor::getOwnerScene(*this));
	PX_CHECK_AND_RETURN(PxIsFinite(maxAngularVelocity), "NpArticulationLink::setMaxAngularVelocity: invalid float");
	PX_CHECK_AND_RETURN(maxAngularVelocity >= 0.0f, "NpArticulationLink::setMaxAngularVelocity: threshold must be non-negative!");

	getScbBodyFast().setMaxAngVelSq(maxAngularVelocity * maxAngularVelocity);
}

PxReal NpArticulationLink::getMaxAngularVelocity() const
{
	NP_READ_CHECK(NpActor::getOwnerScene(*this));

	return PxSqrt(getScbBodyFast().getMaxAngVelSq());
}

void NpArticulationLink::setMaxLinearVelocity(PxReal maxLinearVelocity)
{
	NP_WRITE_CHECK(NpActor::getOwnerScene(*this));
	PX_CHECK_AND_RETURN(PxIsFinite(maxLinearVelocity), "NpArticulationLink::setMaxAngularVelocity: invalid float");
	PX_CHECK_AND_RETURN(maxLinearVelocity >= 0.0f, "NpArticulationLink::setMaxAngularVelocity: threshold must be non-negative!");

	getScbBodyFast().setMaxLinVelSq(maxLinearVelocity * maxLinearVelocity);
}

PxReal NpArticulationLink::getMaxLinearVelocity() const
{
	NP_READ_CHECK(NpActor::getOwnerScene(*this));

	return PxSqrt(getScbBodyFast().getMaxLinVelSq());
}

#if PX_ENABLE_DEBUG_VISUALIZATION
void NpArticulationLink::visualize(Cm::RenderOutput& out, NpScene* scene)
{
	NpArticulationLinkT::visualize(out, scene);

	if (getScbBodyFast().getActorFlags() & PxActorFlag::eVISUALIZATION)
	{
		PX_ASSERT(getScene());
		PxReal scale = getScene()->getVisualizationParameter(PxVisualizationParameter::eSCALE);

		PxReal massAxes = scale * getScene()->getVisualizationParameter(PxVisualizationParameter::eBODY_MASS_AXES);
		if (massAxes != 0)
		{
			PxU32 color = 0xff;
			color = (color<<16 | color<<8 | color);
			PxVec3 dims = invertDiagInertia(getScbBodyFast().getInverseInertia());
			dims = getDimsFromBodyInertia(dims, 1.0f / getScbBodyFast().getInverseMass());

			out << color << getScbBodyFast().getBody2World() << Cm::DebugBox(dims * 0.5f);
		}	
		PxReal frameScale = scale * getScene()->getVisualizationParameter(PxVisualizationParameter::eJOINT_LOCAL_FRAMES);
		PxReal limitScale = scale * getScene()->getVisualizationParameter(PxVisualizationParameter::eJOINT_LIMITS);
		if ( frameScale != 0.0f || limitScale != 0.0f )
		{
			Cm::ConstraintImmediateVisualizer viz( frameScale, limitScale, out );
			visualizeJoint( viz );
		}
	}
}

static PX_FORCE_INLINE PxReal computePhi(const PxQuat& q)
{
	PxQuat twist = q;
	twist.normalize();

	PxReal angle = twist.getAngle();
	if (twist.x<0.0f)
		angle = -angle;
	return angle;
}

// PT: TODO: don't duplicate this, it should be available in MathUtils or something
static 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
}

static PX_FORCE_INLINE void separateSwingTwist(const PxQuat& q, PxQuat& twist, PxQuat& swing1, PxQuat& swing2)
{
	twist = q.x != 0.0f ? PxQuat(q.x, 0, 0, q.w).getNormalized() : PxQuat(PxIdentity);
	PxQuat swing = q * twist.getConjugate();
	swing1 = swing.y != 0.f ? PxQuat(0.f, swing.y, 0.f, swing.w).getNormalized() : PxQuat(PxIdentity);
	swing = swing * swing1.getConjugate();
	swing2 = swing.z != 0.f ? PxQuat(0.f, 0.f, swing.z, swing.w).getNormalized() : PxQuat(PxIdentity);
}

void NpArticulationLink::setKinematicLink(const bool value)
{
	NP_WRITE_CHECK(NpActor::getOwnerScene(*this));

	getScbBodyFast().getScBody().setKinematicLink(value);

}

void NpArticulationLink::visualizeJoint(PxConstraintVisualizer& jointViz)
{
	NpArticulationLink* parent = getParent();
	if(parent)
	{
		PxTransform cA2w = getGlobalPose().transform(mInboundJoint->getChildPose());
		PxTransform cB2w = parent->getGlobalPose().transform(mInboundJoint->getParentPose());
	
		jointViz.visualizeJointFrames(cA2w, cB2w);

		PxArticulationJointImpl* impl = mInboundJoint->getImpl();

		if (getArticulation().getConcreteType() == PxConcreteType::eARTICULATION)
		{
			PxTransform parentFrame = cB2w;

			if (cA2w.q.dot(cB2w.q) < 0)
				cB2w.q = -cB2w.q;

			PxTransform cB2cA = cA2w.transformInv(cB2w);

			PxQuat swing, twist;
			Ps::separateSwingTwist(cB2cA.q, swing, twist);

			PxMat33 cA2w_m(cA2w.q), cB2w_m(cB2w.q);

			PxReal tqPhi = Ps::tanHalf(twist.x, twist.w);		// always support (-pi, +pi)

			PxReal lower, upper, yLimit, zLimit;

			impl->getScbArticulationJoint().getTwistLimit(lower, upper);
			impl->getScbArticulationJoint().getSwingLimit(yLimit, zLimit);
			PxReal swingPad = impl->getScbArticulationJoint().getSwingLimitContactDistance(), twistPad = impl->getScbArticulationJoint().getTwistLimitContactDistance();
			jointViz.visualizeAngularLimit(parentFrame, lower, upper, PxAbs(tqPhi) > PxTan(upper - twistPad));

			PxVec3 tanQSwing = PxVec3(0, Ps::tanHalf(swing.z, swing.w), -Ps::tanHalf(swing.y, swing.w));
			Cm::ConeLimitHelper coneHelper(PxTan(yLimit / 4), PxTan(zLimit / 4), PxTan(swingPad / 4));
			jointViz.visualizeLimitCone(parentFrame, PxTan(yLimit / 4), PxTan(zLimit / 4), !coneHelper.contains(tanQSwing));
		}
		else
		{
			PX_ASSERT(getArticulation().getConcreteType() == PxConcreteType::eARTICULATION_REDUCED_COORDINATE);
			//(1) visualize any angular dofs/limits...

			const PxMat33 cA2w_m(cA2w.q), cB2w_m(cB2w.q);

			PxTransform parentFrame = cB2w;

			if (cA2w.q.dot(cB2w.q) < 0)
				cB2w.q = -cB2w.q;

			//const PxTransform cB2cA = cA2w.transformInv(cB2w);

			const PxTransform cA2cB = cB2w.transformInv(cA2w);

			Sc::ArticulationJointCore& joint = impl->getScbArticulationJoint().getScArticulationJoint();

			PxQuat swing1, swing2, twist;
			separateSwingTwist(cA2cB.q, twist, swing1, swing2);

			const PxReal pad = 0.01f;

			if(joint.getMotion(PxArticulationAxis::eTWIST))
			{
				PxReal lowLimit, highLimit;

				const PxReal angle = computePhi(twist);
				joint.getLimit(PxArticulationAxis::Enum(PxArticulationAxis::eTWIST), lowLimit, highLimit);

				bool active = (angle-pad) < lowLimit || (angle+pad) > highLimit;

				PxTransform tmp = parentFrame;

				jointViz.visualizeAngularLimit(tmp, lowLimit, highLimit, active);
			}

			if (joint.getMotion(PxArticulationAxis::eSWING1))
			{
				PxReal lowLimit, highLimit;

				joint.getLimit(PxArticulationAxis::Enum(PxArticulationAxis::eSWING1), lowLimit, highLimit);

				const PxReal angle = computeSwingAngle(swing1.y, swing1.w);

				bool active = (angle - pad) < lowLimit || (angle + pad) > highLimit;

				PxTransform tmp = parentFrame;
				tmp.q = tmp.q * PxQuat(-PxPiDivTwo, PxVec3(0.f, 0.f, 1.f));

				
				jointViz.visualizeAngularLimit(tmp, -highLimit, -lowLimit, active);
			}

			if (joint.getMotion(PxArticulationAxis::eSWING2))
			{
				PxReal lowLimit, highLimit;

				joint.getLimit(PxArticulationAxis::Enum(PxArticulationAxis::eSWING2), lowLimit, highLimit);

				const PxReal angle = computeSwingAngle(swing2.z, swing2.w);

				bool active = (angle - pad) < lowLimit || (angle + pad) > highLimit;

				PxTransform tmp = parentFrame;
				tmp.q = tmp.q * PxQuat(PxPiDivTwo, PxVec3(0.f, 1.f, 0.f));

				jointViz.visualizeAngularLimit(tmp, -highLimit, -lowLimit, active);
			}

			for (PxU32 i = PxArticulationAxis::eX; i <= PxArticulationAxis::eZ; ++i)
			{
				if (joint.getMotion(PxArticulationAxis::Enum(i)) == PxArticulationMotion::eLIMITED)
				{
					PxU32 index = i - PxArticulationAxis::eX;
					PxReal lowLimit, highLimit;
					joint.getLimit(PxArticulationAxis::Enum(i), lowLimit, highLimit);
					PxReal ordinate = cA2cB.p[index];
					PxVec3 origin = cB2w.p;
					PxVec3 axis = cA2w_m[index];
					const bool active = ordinate < lowLimit || ordinate > highLimit;
					const PxVec3 p0 = origin + axis * lowLimit;
					const PxVec3 p1 = origin + axis * highLimit;
					jointViz.visualizeLine(p0, p1, active ? 0xff0000u : 0xffffffu);
				}
			}
		}
	}	
}
#endif  // PX_ENABLE_DEBUG_VISUALIZATION