From 3dfe2108cfab31ba3ee5527e217d0d8e99a51162 Mon Sep 17 00:00:00 2001
From: git perforce import user <a@b>
Date: Tue, 25 Oct 2016 12:29:14 -0600
Subject: Initial commit: PhysX 3.4.0 Update @ 21294896 APEX 1.4.0 Update @
 21275617

[CL 21300167]
---
 .../PhysXExtensions/src/ExtD6JointSolverPrep.cpp   | 232 +++++++++++++++++++++
 1 file changed, 232 insertions(+)
 create mode 100644 PhysX_3.4/Source/PhysXExtensions/src/ExtD6JointSolverPrep.cpp

(limited to 'PhysX_3.4/Source/PhysXExtensions/src/ExtD6JointSolverPrep.cpp')

diff --git a/PhysX_3.4/Source/PhysXExtensions/src/ExtD6JointSolverPrep.cpp b/PhysX_3.4/Source/PhysXExtensions/src/ExtD6JointSolverPrep.cpp
new file mode 100644
index 00000000..498bb0e4
--- /dev/null
+++ b/PhysX_3.4/Source/PhysXExtensions/src/ExtD6JointSolverPrep.cpp
@@ -0,0 +1,232 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 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 "ExtD6Joint.h"
+#include "ExtConstraintHelper.h"
+#include "CmConeLimitHelper.h"
+
+namespace physx
+{
+namespace Ext
+{
+	PxU32 D6JointSolverPrep(Px1DConstraint* constraints,
+		PxVec3& body0WorldOffset,
+		PxU32 maxConstraints,
+		PxConstraintInvMassScale& invMassScale,
+		const void* constantBlock,
+		const PxTransform& bA2w,
+		const PxTransform& bB2w)
+	{
+		PX_UNUSED(maxConstraints);
+
+		using namespace joint;
+
+		PX_UNUSED(maxConstraints);
+		const D6JointData& data = 
+			*reinterpret_cast<const D6JointData*>(constantBlock);
+
+		invMassScale = data.invMassScale;
+
+		const PxU32 SWING1_FLAG = 1<<PxD6Axis::eSWING1, 
+			SWING2_FLAG = 1<<PxD6Axis::eSWING2, 
+			TWIST_FLAG  = 1<<PxD6Axis::eTWIST;
+
+		const PxU32 ANGULAR_MASK = SWING1_FLAG | SWING2_FLAG | TWIST_FLAG;
+		const PxU32 LINEAR_MASK = 1<<PxD6Axis::eX | 1<<PxD6Axis::eY | 1<<PxD6Axis::eZ;
+
+		const PxD6JointDrive* drives = data.drive;
+		PxU32 locked = data.locked, limited = data.limited, driving = data.driving;
+
+		PxTransform cA2w = bA2w.transform(data.c2b[0]);
+		PxTransform cB2w = bB2w.transform(data.c2b[1]);
+
+		body0WorldOffset = cB2w.p-bA2w.p;
+		ConstraintHelper g(constraints, cB2w.p-bA2w.p, cB2w.p-bB2w.p);
+
+		if(cA2w.q.dot(cB2w.q)<0)	// minimum dist quat (equiv to flipping cB2bB.q, which we don't use anywhere)
+			cB2w.q = -cB2w.q;
+
+		PxTransform cB2cA = cA2w.transformInv(cB2w);	
+
+		PX_ASSERT(data.c2b[0].isValid());
+		PX_ASSERT(data.c2b[1].isValid());
+		PX_ASSERT(cA2w.isValid());
+		PX_ASSERT(cB2w.isValid());
+		PX_ASSERT(cB2cA.isValid());
+
+		PxMat33 cA2w_m(cA2w.q), cB2w_m(cB2w.q);
+
+		// handy for swing computation
+		PxVec3 bX = cB2w_m[0], aY = cA2w_m[1], aZ = cA2w_m[2];
+
+		if(driving & ((1<<PxD6Drive::eX)|(1<<PxD6Drive::eY)|(1<<PxD6Drive::eZ)))
+		{
+			// TODO: make drive unilateral if we are outside the limit
+			PxVec3 posErr = data.drivePosition.p - cB2cA.p;
+			for(PxU32 i = 0; i < 3; i++)
+			{
+				// -driveVelocity because velTarget is child (body1) - parent (body0) and Jacobian is 1 for body0 and -1 for parent
+				if(driving & (1<<(PxD6Drive::eX+i)))
+					g.linear(cA2w_m[i], -data.driveLinearVelocity[i], posErr[i], drives[PxD6Drive::eX+i]); 
+			}
+		}
+
+		if(driving & ((1<<PxD6Drive::eSLERP)|(1<<PxD6Drive::eSWING)|(1<<PxD6Drive::eTWIST)))
+		{
+			PxQuat d2cA_q = cB2cA.q.dot(data.drivePosition.q)>0 ? data.drivePosition.q : -data.drivePosition.q; 
+
+			const PxVec3& v = data.driveAngularVelocity;
+			PxQuat delta = d2cA_q.getConjugate() * cB2cA.q;
+
+			if(driving & (1<<PxD6Drive::eSLERP))
+			{
+				PxVec3 velTarget = -cA2w.rotate(data.driveAngularVelocity);
+
+				PxVec3 axis[3] = { PxVec3(1.f,0,0), PxVec3(0,1.f,0), PxVec3(0,0,1.f) };
+				
+				if(drives[PxD6Drive::eSLERP].stiffness!=0)
+					computeJacobianAxes(axis, cA2w.q * d2cA_q, cB2w.q);	// converges faster if there is only velocity drive
+
+				for(PxU32 i = 0; i < 3; i++)
+					g.angular(axis[i], axis[i].dot(velTarget), -delta.getImaginaryPart()[i], drives[PxD6Drive::eSLERP], PxConstraintSolveHint::eSLERP_SPRING);
+			}
+			else 
+			{
+				if(driving & (1<<PxD6Drive::eTWIST))
+					g.angular(bX, v.x, -2.0f * delta.x, drives[PxD6Drive::eTWIST]); 
+
+				if(driving & (1<<PxD6Drive::eSWING))
+				{
+					PxVec3 err = delta.rotate(PxVec3(1.f,0,0));
+
+					if(!(locked & SWING1_FLAG))
+						g.angular(cB2w_m[1], v.y, err.z, drives[PxD6Drive::eSWING]);
+
+					if(!(locked & SWING2_FLAG))
+						g.angular(cB2w_m[2], v.z, -err.y, drives[PxD6Drive::eSWING]);
+				}
+			}
+		}
+
+		if (limited & ANGULAR_MASK)
+		{
+			PxQuat swing, twist;
+			Ps::separateSwingTwist(cB2cA.q,swing,twist);
+
+			// swing limits: if just one is limited: if the other is free, we support 
+			// (-pi/2, +pi/2) limit, using tan of the half-angle as the error measure parameter. 
+			// If the other is locked, we support (-pi, +pi) limits using the tan of the quarter-angle
+			// Notation: th == Ps::tanHalf, tq = tanQuarter
+
+			if(limited & SWING1_FLAG && limited & SWING2_FLAG)
+			{
+				Cm::ConeLimitHelper coneHelper(data.tqSwingZ, data.tqSwingY, data.tqSwingPad);
+
+				PxVec3 axis;
+				PxReal error;
+				if(coneHelper.getLimit(swing, axis, error))
+					g.angularLimit(cA2w.rotate(axis),error,data.swingLimit);
+			}
+			else
+			{
+				const PxJointLimitCone &limit = data.swingLimit;
+
+				PxReal tqPad = data.tqSwingPad, thPad = data.thSwingPad;
+
+				if(limited & SWING1_FLAG)
+				{
+					if(locked & SWING2_FLAG)
+					{
+						g.quarterAnglePair(Ps::tanHalf(swing.y, swing.w), -data.tqSwingY, data.tqSwingY, tqPad, aY, limit);
+					}
+					else
+					{
+						PxReal dot=-aZ.dot(bX);
+						g.halfAnglePair(Ps::tanHalf(dot, 1-dot*dot), -data.thSwingY, data.thSwingY, thPad, aZ.cross(bX), limit);
+					}
+				}
+				if(limited & SWING2_FLAG)
+				{
+					if(locked & SWING1_FLAG)
+					{
+						g.quarterAnglePair(Ps::tanHalf(swing.z, swing.w), -data.tqSwingZ, data.tqSwingZ, tqPad, aZ, limit);
+					}
+					else
+					{
+						PxReal dot=aY.dot(bX);
+						g.halfAnglePair(Ps::tanHalf(dot, 1-dot*dot), -data.thSwingZ, data.thSwingZ, thPad, -aY.cross(bX), limit);
+					}
+				}
+			}
+
+			if(limited & TWIST_FLAG)
+			{
+				g.quarterAnglePair(Ps::tanHalf(twist.x, twist.w), data.tqTwistLow, data.tqTwistHigh, data.tqTwistPad,
+					cB2w_m[0], data.twistLimit);
+			}
+		}
+
+		if(limited & LINEAR_MASK)
+		{
+			PxVec3 limitDir = PxVec3(0);
+
+			for(PxU32 i = 0; i < 3; i++)
+			{
+				if(limited & (1<<(PxD6Axis::eX+i)))
+					limitDir += cA2w_m[i] * cB2cA.p[i];
+			}
+
+			PxReal distance = limitDir.magnitude();
+			if(distance > data.linearMinDist)
+				g.linearLimit(limitDir * (1.0f/distance), distance, data.linearLimit.value, data.linearLimit);
+		}
+
+		// we handle specially the case of just one swing dof locked
+
+		PxU32 angularLocked = locked & ANGULAR_MASK;
+
+		if(angularLocked == SWING1_FLAG)
+		{
+			g.angularHard(bX.cross(aZ), -bX.dot(aZ));
+			locked &= ~SWING1_FLAG;
+		}
+		else if(angularLocked == SWING2_FLAG)
+		{
+			locked &= ~SWING2_FLAG;
+			g.angularHard(bX.cross(aY), -bX.dot(aY));
+		}
+
+		g.prepareLockedAxes(cA2w.q, cB2w.q, cB2cA.p,locked&7, locked>>3);
+
+		return g.getCount();
+	}
+}//namespace
+
+}
-- 
cgit v1.2.3