Initial commit:

PhysX 3.4.0 Update @ 21294896
APEX 1.4.0 Update @ 21275617

[CL 21300167]

1 files changed, 365 insertions, 0 deletions

diff --git a/KaplaDemo/samples/sampleViewer3/MathUtils.cpp b/KaplaDemo/samples/sampleViewer3/MathUtils.cpp
new file mode 100644
index 00000000..e6a0ba1c
--- /dev/null
+++ b/KaplaDemo/samples/sampleViewer3/MathUtils.cpp
@@ -0,0 +1,365 @@
+#include "MathUtils.h"
+#include "foundation/PxMat33.h"
+#include "PhysXMacros.h"
+
+//---------------------------------------------------------------------
+
+void jacobiRotate(PxMat33 &A, PxMat33 &R, int p, int q)
+{
+	// rotates A through phi in pq-plane to set A(p,q) = 0
+	// rotation stored in R whose columns are eigenvectors of A
+	if (A(p,q) == 0.0f)
+		return;
+
+	float d = (A(p,p) - A(q,q))/(2.0f*A(p,q));
+	float t = 1.0f / (PxAbs(d) + PxSqrt(d*d + 1.0f));
+	if (d < 0.0f) t = -t;
+	float c = 1.0f/PxSqrt(t*t + 1);
+	float s = t*c;
+	A(p,p) += t*A(p,q);
+	A(q,q) -= t*A(p,q);
+	A(p,q) = A(q,p) = 0.0f;
+	// transform A
+	int k;
+	for (k = 0; k < 3; k++) {
+		if (k != p && k != q) {
+			float Akp = c*A(k,p) + s*A(k,q);
+			float Akq =-s*A(k,p) + c*A(k,q);
+			A(k,p) = A(p,k) = Akp;
+			A(k,q) = A(q,k) = Akq;
+		}
+	}
+	// store rotation in R
+	for (k = 0; k < 3; k++) {
+		float Rkp = c*R(k,p) + s*R(k,q);
+		float Rkq =-s*R(k,p) + c*R(k,q);
+		R(k,p) = Rkp;
+		R(k,q) = Rkq;
+	}
+}
+
+//---------------------------------------------------------------------
+
+void eigenDecomposition(PxMat33 &A, PxMat33 &R)
+{
+	const int numJacobiIterations = 10;
+	const float epsilon = 1e-15f;
+
+	// only for symmetric matrices!
+	R = PX_MAT33_ID;	// unit matrix
+	int iter = 0;
+	while (iter < numJacobiIterations) {	// 3 off diagonal elements
+		// find off diagonal element with maximum modulus
+		int p,q;
+		float a,max;
+		max = PxAbs(A(0,1));
+		p = 0; q = 1;
+		a   = PxAbs(A(0,2));
+		if (a > max) { p = 0; q = 2; max = a; }
+		a   = PxAbs(A(1,2));
+		if (a > max) { p = 1; q = 2; max = a; }
+		// all small enough -> done
+		if (max < epsilon) break;
+		// rotate matrix with respect to that element
+		jacobiRotate(A, R, p,q);
+		iter++;
+	}
+}
+
+//---------------------------------------------------------------------
+void polarDecomposition(const PxMat33 &A, PxMat33 &R)
+{
+	// A = SR, where S is symmetric and R is orthonormal
+	// -> S = (A A^T)^(1/2)
+
+	PxMat33 AAT;
+	AAT(0,0) = A(0,0)*A(0,0) + A(0,1)*A(0,1) + A(0,2)*A(0,2);
+	AAT(1,1) = A(1,0)*A(1,0) + A(1,1)*A(1,1) + A(1,2)*A(1,2);
+	AAT(2,2) = A(2,0)*A(2,0) + A(2,1)*A(2,1) + A(2,2)*A(2,2);
+
+	AAT(0,1) = A(0,0)*A(1,0) + A(0,1)*A(1,1) + A(0,2)*A(1,2);
+	AAT(0,2) = A(0,0)*A(2,0) + A(0,1)*A(2,1) + A(0,2)*A(2,2);
+	AAT(1,2) = A(1,0)*A(2,0) + A(1,1)*A(2,1) + A(1,2)*A(2,2);
+
+	AAT(1,0) = AAT(0,1);
+	AAT(2,0) = AAT(0,2);
+	AAT(2,1) = AAT(1,2);
+
+	PxMat33 U;
+	R = PX_MAT33_ID;
+	eigenDecomposition(AAT, U);
+
+	const float eps = 1e-15f;
+
+	float l0 = AAT(0,0); if (l0 <= eps) l0 = 0.0f; else l0 = 1.0f / sqrt(l0);
+	float l1 = AAT(1,1); if (l1 <= eps) l1 = 0.0f; else l1 = 1.0f / sqrt(l1);
+	float l2 = AAT(2,2); if (l2 <= eps) l2 = 0.0f; else l2 = 1.0f / sqrt(l2);
+
+	PxMat33 S1;
+	S1(0,0) = l0*U(0,0)*U(0,0) + l1*U(0,1)*U(0,1) + l2*U(0,2)*U(0,2);
+	S1(1,1) = l0*U(1,0)*U(1,0) + l1*U(1,1)*U(1,1) + l2*U(1,2)*U(1,2);
+	S1(2,2) = l0*U(2,0)*U(2,0) + l1*U(2,1)*U(2,1) + l2*U(2,2)*U(2,2);
+
+	S1(0,1) = l0*U(0,0)*U(1,0) + l1*U(0,1)*U(1,1) + l2*U(0,2)*U(1,2);
+	S1(0,2) = l0*U(0,0)*U(2,0) + l1*U(0,1)*U(2,1) + l2*U(0,2)*U(2,2);
+	S1(1,2) = l0*U(1,0)*U(2,0) + l1*U(1,1)*U(2,1) + l2*U(1,2)*U(2,2);
+
+	S1(1,0) = S1(0,1);
+	S1(2,0) = S1(0,2);
+	S1(2,1) = S1(1,2);
+
+	R = S1 * A;
+
+	// stabilize
+	PxVec3 c0 = R.column0;
+	PxVec3 c1 = R.column1;
+	PxVec3 c2 = R.column2;
+
+	if (c0.magnitudeSquared() < eps)
+		c0 = c1.cross(c2);
+	else if (c1.magnitudeSquared() < eps)
+		c1 = c2.cross(c0);
+	else 
+		c2 = c0.cross(c1);
+	R.column0 = c0;
+	R.column1 = c1;
+	R.column2 = c2;
+}
+
+//---------------------------------------------------------------------
+PxVec3 perpVec3(const PxVec3 &v)
+{
+	PxVec3 n;
+	if (fabs(v.x) < fabs(v.y) && fabs(v.x) < fabs(v.z))
+		n = PxVec3(1.0f, 0.0f, 0.0f);
+	else if (fabs(v.y) < fabs(v.z))
+		n = PxVec3(0.0f, 1.0f, 0.0f);
+	else
+		n = PxVec3(0.0f, 0.0f, 1.0f);
+	n = v.cross(n);
+	n.normalize();
+	return n;
+}
+
+//---------------------------------------------------------------------
+void polarDecompositionStabilized(const PxMat33 &A, PxMat33 &R)
+{
+	PxMat33 ATA;
+	ATA = A.getTranspose() * A;
+
+	PxMat33 Q;
+	eigenDecomposition(ATA, Q);
+
+	int degenerated = 0;
+
+	float l0 = ATA(0,0);
+	if (fabs(l0) <= FLT_EPSILON) {
+		l0 = 0;
+		degenerated += 1;
+	}
+	else l0 = 1.0f / PxSqrt(l0);
+
+	float l1 = ATA(1,1);
+	if (fabs(l1) <= FLT_EPSILON) {
+		l1 = 0;
+		degenerated += 2;
+	}
+	else l1 = 1.0f / PxSqrt(l1);
+ 
+	float l2 = ATA(2,2);
+	if (fabs(l2) <= FLT_EPSILON) {
+		l2 = 0;
+		degenerated += 4;
+	}
+	else l2 = 1.0f / PxSqrt(l2);
+ 
+
+	if (A.getDeterminant() < 0) //Invertierung nach Irving,Fedkiw
+	{
+		float *max = &l0;
+		if (l1 > *max) {max = &l1;}
+		if (l2 > *max) {max = &l2;}
+		*max *= -1;
+	}
+
+	PxMat33 D = PX_MAT33_ZERO;
+	D(0,0) = l0;
+	D(1,1) = l1;
+	D(2,2) = l2;
+	R = A * Q * D;
+
+	Q = Q.getTranspose();
+
+	//handle singular cases
+
+	PxVec3 r0 = R.column0;
+	PxVec3 r1 = R.column1;
+	PxVec3 r2 = R.column2;
+
+	if (degenerated == 0) {			// 000
+	}
+	else if (degenerated == 1) {	// 100
+		r0 = r1.cross(r2);
+		R.column0 = r0;
+	}
+	else if (degenerated == 2) {	// 010
+		r1 = r2.cross(r0);
+		R.column1 = r1;
+	}
+	else if (degenerated == 4) {	// 001
+		r2 = r0.cross(r1);
+		R.column2 = r2;
+	}
+	else if (degenerated == 6) {	// 011
+		r1 = perpVec3(r0);
+		r2 = r0.cross(r1);
+		R.column1 = r1;
+		R.column2 = r2;
+	}
+	else if (degenerated == 5) {	// 101
+		r2 = perpVec3(r1);
+		r0 = r1.cross(r2);
+		R.column2 = r2;
+		R.column0 = r0;
+	}
+	else if (degenerated == 3) {	// 110
+		r0 = perpVec3(r2);
+		r1 = r2.cross(r0);
+		R.column0 = r0;
+		R.column1 = r1;
+	}
+	else							// 111
+		R = PX_MAT33_ID;
+
+	R = R*Q;
+}
+
+//---------------------------------------------------------------------
+void eigenDecomposition22(const PxMat33 &A, PxMat33 &R, PxMat33 &D)
+{
+	// only for symmetric matrices 
+
+	// decompose A such that
+	// A = R D R^T, where D is diagonal and R orthonormal (a rotation)
+
+	R = PX_MAT33_ID;
+	D = PX_MAT33_ID;
+	D(0,0) = A(0,0); D(0,1) = A(0,1);
+	D(1,0) = A(1,0); D(1,1) = A(1,1);
+
+	if (D(0,1) == 0.0f)
+		return;
+
+	float d = (D(0,0) - D(1,1))/(2.0f*D(0,1));
+	float t = 1.0f / (PxAbs(d) + PxSqrt(d*d + 1.0f));
+	if (d < 0.0f) t = -t;
+	float c = 1.0f/sqrt(t*t + 1);
+	float s = t*c;
+	D(0,0) += t*D(0,1);
+	D(1,1) -= t*D(0,1);
+	D(0,1) = D(1,0) = 0.0f;
+	// store rotation in R
+	for (int k = 0; k < 2; k++) {
+		float Rkp = c*R(k,0) + s*R(k,1);
+		float Rkq =-s*R(k,0) + c*R(k,1);
+		R(k,0) = Rkp;
+		R(k,1) = Rkq;
+	}
+}
+
+//---------------------------------------------------------------------
+PxMat33 outerProduct(const PxVec3 &v0, const PxVec3 &v1)
+{
+	PxMat33 M;
+	M.column0 = v0 * v1.x;
+	M.column1 = v0 * v1.y;
+	M.column2 = v0 * v1.z;
+	return M;
+}
+
+// From geometrictools.com
+PxQuat align (const PxVec3& v1, const PxVec3& v2) {
+	// vector U = Cross(V1,V2)/Length(Cross(V1,V2)).  The angle of rotation,
+	// A, is the angle between V1 and V2.  The quaternion for the rotation is
+	// q = cos(A/2) + sin(A/2)*(ux*i+uy*j+uz*k) where U = (ux,uy,uz).
+	//
+	// (1) Rather than extract A = acos(Dot(V1,V2)), multiply by 1/2, then
+	//     compute sin(A/2) and cos(A/2), we reduce the computational costs by
+	//     computing the bisector B = (V1+V2)/Length(V1+V2), so cos(A/2) =
+	//     Dot(V1,B).
+	//
+	// (2) The rotation axis is U = Cross(V1,B)/Length(Cross(V1,B)), but
+	//     Length(Cross(V1,B)) = Length(V1)*Length(B)*sin(A/2) = sin(A/2), in
+	//     which case sin(A/2)*(ux*i+uy*j+uz*k) = (cx*i+cy*j+cz*k) where
+	//     C = Cross(V1,B).
+	//
+	// If V1 = V2, then B = V1, cos(A/2) = 1, and U = (0,0,0).  If V1 = -V2,
+	// then B = 0.  This can happen even if V1 is approximately -V2 using
+	// floating point arithmetic, since Vector3::Normalize checks for
+	// closeness to zero and returns the zero vector accordingly.  The test
+	// for exactly zero is usually not recommend for floating point
+	// arithmetic, but the implementation of Vector3::Normalize guarantees
+	// the comparison is robust.  In this case, the A = pi and any axis
+	// perpendicular to V1 may be used as the rotation axis.
+
+	PxVec3 bisector = v1 + v2;
+	bisector.normalize();
+
+	float cosHalfAngle = v1.dot(bisector);
+	PxVec3 cross;
+
+	float mTuple[4];
+	mTuple[0] = cosHalfAngle;
+
+	if (cosHalfAngle != (float)0)
+	{
+		cross = v1.cross(bisector);
+		mTuple[1] = cross.x;
+		mTuple[2] = cross.y;
+		mTuple[3] = cross.z;
+	}
+	else
+	{
+		float invLength;
+		if (fabs(v1[0]) >= fabs(v1[1]))
+		{
+			// V1.x or V1.z is the largest magnitude component.
+			invLength = fastInvSqrt(v1[0]*v1[0] + v1[2]*v1[2]);
+			mTuple[1] = -v1[2]*invLength;
+			mTuple[2] = (float)0;
+			mTuple[3] = +v1[0]*invLength;
+		}
+		else
+		{
+			// V1.y or V1.z is the largest magnitude component.
+			invLength = fastInvSqrt(v1[1]*v1[1] + v1[2]*v1[2]);
+			mTuple[1] = (float)0;
+			mTuple[2] = +v1[2]*invLength;
+			mTuple[3] = -v1[1]*invLength;
+		}
+	}
+
+	PxQuat q(mTuple[1],mTuple[2],mTuple[3], mTuple[0]);
+	return q;
+}
+
+void decomposeTwistTimesSwing (const PxQuat& q,  const PxVec3& v1,
+												 PxQuat& twist, PxQuat& swing)
+{
+
+	PxVec3 v2 = v1;
+	q.rotate(v2);
+
+	swing = align(v1, v2);
+	twist = q*swing.getConjugate();
+}
+
+void decomposeSwingTimesTwist (const PxQuat& q, const PxVec3& v1,
+												 PxQuat& swing, PxQuat& twist)
+{
+	PxVec3 v2 = v1;
+	q.rotate(v2);
+
+	swing = align(v1, v2);
+	twist = swing.getConjugate()*q;
+}