Initial commit:

PhysX 3.4.0 Update @ 21294896
APEX 1.4.0 Update @ 21275617

[CL 21300167]

1 files changed, 574 insertions, 0 deletions

diff --git a/APEX_1.4/shared/general/HACD/src/dgMatrix.cpp b/APEX_1.4/shared/general/HACD/src/dgMatrix.cpp
new file mode 100644
index 00000000..77a5faf0
--- /dev/null
+++ b/APEX_1.4/shared/general/HACD/src/dgMatrix.cpp
@@ -0,0 +1,574 @@
+/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
+* 
+* This software is provided 'as-is', without any express or implied
+* warranty. In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* 
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "dgTypes.h"
+#include "dgMatrix.h"
+#include "dgQuaternion.h"
+
+
+static dgMatrix zeroMatrix (dgVector (float(0.0f), float(0.0f), float(0.0f), float(0.0f)),
+							dgVector (float(0.0f), float(0.0f), float(0.0f), float(0.0f)),
+							dgVector (float(0.0f), float(0.0f), float(0.0f), float(0.0f)),
+							dgVector (float(0.0f), float(0.0f), float(0.0f), float(0.0f)));
+
+static dgMatrix identityMatrix (dgVector (float(1.0f), float(0.0f), float(0.0f), float(0.0f)),
+								dgVector (float(0.0f), float(1.0f), float(0.0f), float(0.0f)),
+								dgVector (float(0.0f), float(0.0f), float(1.0f), float(0.0f)),
+								dgVector (float(0.0f), float(0.0f), float(0.0f), float(1.0f)));
+
+const dgMatrix& dgGetIdentityMatrix()
+{
+	return identityMatrix;
+}
+
+const dgMatrix& dgGetZeroMatrix ()
+{
+	return zeroMatrix;
+}
+
+
+dgMatrix::dgMatrix (const dgQuaternion &rotation, const dgVector &position)
+{
+	float x2 = float (2.0f) * rotation.m_q1 * rotation.m_q1;
+	float y2 = float (2.0f) * rotation.m_q2 * rotation.m_q2;
+	float z2 = float (2.0f) * rotation.m_q3 * rotation.m_q3;
+
+	float xy = float (2.0f) * rotation.m_q1 * rotation.m_q2;
+	float xz = float (2.0f) * rotation.m_q1 * rotation.m_q3;
+	float xw = float (2.0f) * rotation.m_q1 * rotation.m_q0;
+	float yz = float (2.0f) * rotation.m_q2 * rotation.m_q3;
+	float yw = float (2.0f) * rotation.m_q2 * rotation.m_q0;
+	float zw = float (2.0f) * rotation.m_q3 * rotation.m_q0;
+
+	m_front = dgVector (float(1.0f) - y2 - z2, xy + zw,                   xz - yw				    , float(0.0f));
+	m_up    = dgVector (xy - zw,                   float(1.0f) - x2 - z2, yz + xw					, float(0.0f));
+	m_right = dgVector (xz + yw,                   yz - xw,                   float(1.0f) - x2 - y2 , float(0.0f));
+
+
+	m_posit.m_x = position.m_x;
+	m_posit.m_y = position.m_y;
+	m_posit.m_z = position.m_z;
+	m_posit.m_w = float(1.0f);
+}
+
+dgMatrix dgMatrix::operator* (const dgMatrix &B) const
+{
+	const dgMatrix& A = *this;
+	return dgMatrix (dgVector (A[0][0] * B[0][0] + A[0][1] * B[1][0] + A[0][2] * B[2][0] + A[0][3] * B[3][0],
+							   A[0][0] * B[0][1] + A[0][1] * B[1][1] + A[0][2] * B[2][1] + A[0][3] * B[3][1],
+							   A[0][0] * B[0][2] + A[0][1] * B[1][2] + A[0][2] * B[2][2] + A[0][3] * B[3][2],
+							   A[0][0] * B[0][3] + A[0][1] * B[1][3] + A[0][2] * B[2][3] + A[0][3] * B[3][3]),
+					 dgVector (A[1][0] * B[0][0] + A[1][1] * B[1][0] + A[1][2] * B[2][0] + A[1][3] * B[3][0],
+							   A[1][0] * B[0][1] + A[1][1] * B[1][1] + A[1][2] * B[2][1] + A[1][3] * B[3][1],
+							   A[1][0] * B[0][2] + A[1][1] * B[1][2] + A[1][2] * B[2][2] + A[1][3] * B[3][2],
+							   A[1][0] * B[0][3] + A[1][1] * B[1][3] + A[1][2] * B[2][3] + A[1][3] * B[3][3]),
+					 dgVector (A[2][0] * B[0][0] + A[2][1] * B[1][0] + A[2][2] * B[2][0] + A[2][3] * B[3][0],
+							   A[2][0] * B[0][1] + A[2][1] * B[1][1] + A[2][2] * B[2][1] + A[2][3] * B[3][1],
+							   A[2][0] * B[0][2] + A[2][1] * B[1][2] + A[2][2] * B[2][2] + A[2][3] * B[3][2],
+							   A[2][0] * B[0][3] + A[2][1] * B[1][3] + A[2][2] * B[2][3] + A[2][3] * B[3][3]),
+					 dgVector (A[3][0] * B[0][0] + A[3][1] * B[1][0] + A[3][2] * B[2][0] + A[3][3] * B[3][0],
+							   A[3][0] * B[0][1] + A[3][1] * B[1][1] + A[3][2] * B[2][1] + A[3][3] * B[3][1],
+							   A[3][0] * B[0][2] + A[3][1] * B[1][2] + A[3][2] * B[2][2] + A[3][3] * B[3][2],
+							   A[3][0] * B[0][3] + A[3][1] * B[1][3] + A[3][2] * B[2][3] + A[3][3] * B[3][3]));
+}
+
+
+
+void dgMatrix::TransformTriplex (float* const dst, int32_t dstStrideInBytes, const float* const src, int32_t srcStrideInBytes, int32_t count) const
+{
+	int32_t dstStride = dstStrideInBytes / (int32_t)sizeof (float);
+	int32_t srcStride = srcStrideInBytes / (int32_t)sizeof (float);
+
+	int32_t dstIndex = 0;
+	int32_t srcIndex = 0;
+	for (int32_t i = 0 ; i < count; i ++ ) {
+		float x = src[srcIndex + 0];
+		float y = src[srcIndex + 1];
+		float z = src[srcIndex + 2];
+		srcIndex += srcStride;
+		dst[dstIndex + 0] = x * m_front.m_x + y * m_up.m_x + z * m_right.m_x + m_posit.m_x;
+		dst[dstIndex + 1] = x * m_front.m_y + y * m_up.m_y + z * m_right.m_y + m_posit.m_y;
+		dst[dstIndex + 2] = x * m_front.m_z + y * m_up.m_z + z * m_right.m_z + m_posit.m_z;
+		dstIndex += dstStride;
+	}
+}
+
+void dgMatrix::TransformTriplex (double* const dst, int32_t dstStrideInBytes, const double* const src, int32_t srcStrideInBytes, int32_t count) const
+{
+	int32_t dstStride = dstStrideInBytes / (int32_t)sizeof (double);
+	int32_t srcStride = srcStrideInBytes / (int32_t)sizeof (double);
+
+	int32_t dstIndex = 0;
+	int32_t srcIndex = 0;
+	for (int32_t i = 0 ; i < count; i ++ ) {
+		double x = src[srcIndex + 0];
+		double y = src[srcIndex + 1];
+		double z = src[srcIndex + 2];
+		srcIndex += srcStride;
+		dst[dstIndex + 0] = x * m_front.m_x + y * m_up.m_x + z * m_right.m_x + m_posit.m_x;
+		dst[dstIndex + 1] = x * m_front.m_y + y * m_up.m_y + z * m_right.m_y + m_posit.m_y;
+		dst[dstIndex + 2] = x * m_front.m_z + y * m_up.m_z + z * m_right.m_z + m_posit.m_z;
+		dstIndex += dstStride;
+	}
+}
+
+
+void dgMatrix::TransformTriplex (double* const dst, int32_t dstStrideInBytes, const float* const src, int32_t srcStrideInBytes, int32_t count) const
+{
+	int32_t dstStride = dstStrideInBytes / (int32_t)sizeof (double);
+	int32_t srcStride = srcStrideInBytes / (int32_t)sizeof (float);
+
+	int32_t dstIndex = 0;
+	int32_t srcIndex = 0;
+	for (int32_t i = 0 ; i < count; i ++ ) {
+		double x = src[srcIndex + 0];
+		double y = src[srcIndex + 1];
+		double z = src[srcIndex + 2];
+		srcIndex += srcStride;
+		dst[dstIndex + 0] = x * m_front.m_x + y * m_up.m_x + z * m_right.m_x + m_posit.m_x;
+		dst[dstIndex + 1] = x * m_front.m_y + y * m_up.m_y + z * m_right.m_y + m_posit.m_y;
+		dst[dstIndex + 2] = x * m_front.m_z + y * m_up.m_z + z * m_right.m_z + m_posit.m_z;
+		dstIndex += dstStride;
+	}
+}
+
+
+void dgMatrix::TransformBBox (const dgVector& p0local, const dgVector& p1local, dgVector& p0, dgVector& p1) const
+{
+	dgVector box[8];
+
+	box[0][0] = p0local[0];
+	box[0][1] = p0local[1];
+	box[0][2] = p0local[2];
+	box[0][3] = float(1.0f);
+
+	box[1][0] = p0local[0];
+	box[1][1] = p0local[1];
+	box[1][2] = p1local[2];
+	box[1][3] = float(1.0f);
+
+	box[2][0] = p0local[0];
+	box[2][1] = p1local[1];
+	box[2][2] = p0local[2];
+	box[2][3] = float(1.0f);
+
+	box[3][0] = p0local[0];
+	box[3][1] = p1local[1];
+	box[3][2] = p1local[2];
+	box[3][3] = float(1.0f);
+
+	box[4][0] = p1local[0];
+	box[4][1] = p0local[1];
+	box[4][2] = p0local[2];
+	box[4][3] = float(1.0f);
+
+	box[5][0] = p1local[0];
+	box[5][1] = p0local[1];
+	box[5][2] = p1local[2];
+	box[1][3] = float(1.0f);
+
+	box[6][0] = p1local[0];
+	box[6][1] = p1local[1];
+	box[6][2] = p0local[2];
+	box[6][3] = float(1.0f);
+
+	box[7][0] = p1local[0];
+	box[7][1] = p1local[1];
+	box[7][2] = p1local[2];
+	box[7][3] = float(1.0f);
+
+	TransformTriplex (&box[0].m_x, sizeof (dgVector), &box[0].m_x, sizeof (dgVector), 8);
+
+	p0 = box[0];
+	p1 = box[0];
+	for (int32_t i = 1; i < 8; i ++) {
+		p0.m_x = GetMin (p0.m_x, box[i].m_x);
+		p0.m_y = GetMin (p0.m_y, box[i].m_y);
+		p0.m_z = GetMin (p0.m_z, box[i].m_z);
+
+		p1.m_x = GetMax (p1.m_x, box[i].m_x);
+		p1.m_y = GetMax (p1.m_y, box[i].m_y);
+		p1.m_z = GetMax (p1.m_z, box[i].m_z);
+	}
+}
+
+
+
+dgMatrix dgMatrix::Symetric3by3Inverse () const
+{
+	const dgMatrix& mat = *this;
+	double det = mat[0][0] * mat[1][1] * mat[2][2] + 
+			mat[0][1] * mat[1][2] * mat[0][2] * float (2.0f) -
+			mat[0][2] * mat[1][1] * mat[0][2] -
+			mat[0][1] * mat[0][1] * mat[2][2] -
+			mat[0][0] * mat[1][2] * mat[1][2];
+
+	det = float (1.0f) / det;
+
+	float x11 = (float)(det * (mat[1][1] * mat[2][2] - mat[1][2] * mat[1][2]));  
+	float x22 = (float)(det * (mat[0][0] * mat[2][2] - mat[0][2] * mat[0][2]));  
+	float x33 = (float)(det * (mat[0][0] * mat[1][1] - mat[0][1] * mat[0][1]));  
+
+	float x12 = (float)(det * (mat[1][2] * mat[2][0] - mat[1][0] * mat[2][2]));  
+	float x13 = (float)(det * (mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]));  
+	float x23 = (float)(det * (mat[0][1] * mat[2][0] - mat[0][0] * mat[2][1]));  
+
+
+#ifdef _DEBUG
+	dgMatrix matInv (dgVector (x11, x12, x13, float(0.0f)),
+				     dgVector (x12, x22, x23, float(0.0f)),
+					 dgVector (x13, x23, x33, float(0.0f)),
+					 dgVector (float(0.0f), float(0.0f), float(0.0f), float(1.0f)));
+
+	dgMatrix test (matInv * mat);
+	HACD_ASSERT (dgAbsf (test[0][0] - float(1.0f)) < float(0.01f));
+	HACD_ASSERT (dgAbsf (test[1][1] - float(1.0f)) < float(0.01f));
+	HACD_ASSERT (dgAbsf (test[2][2] - float(1.0f)) < float(0.01f));
+#endif
+
+	return dgMatrix (dgVector (x11, x12, x13, float(0.0f)),
+					 dgVector (x12, x22, x23, float(0.0f)),
+					 dgVector (x13, x23, x33, float(0.0f)),
+					 dgVector (float(0.0f), float(0.0f), float(0.0f), float(1.0f)));
+}
+
+
+
+
+
+dgVector dgMatrix::CalcPitchYawRoll () const
+{
+	const float minSin = float(0.99995f);
+
+	const dgMatrix& matrix = *this;
+
+	float roll = float(0.0f);
+	float pitch  = float(0.0f);
+	float yaw = dgAsin (-ClampValue (matrix[0][2], float(-0.999999f), float(0.999999f)));
+
+	HACD_ASSERT (HACD_ISFINITE (yaw));
+	if (matrix[0][2] < minSin) {
+		if (matrix[0][2] > (-minSin)) {
+			roll = dgAtan2 (matrix[0][1], matrix[0][0]);
+			pitch = dgAtan2 (matrix[1][2], matrix[2][2]);
+		} else {
+			pitch = dgAtan2 (matrix[1][0], matrix[1][1]);
+		}
+	} else {
+		pitch = -dgAtan2 (matrix[1][0], matrix[1][1]);
+	}
+
+	return dgVector (pitch, yaw, roll, float(0.0f));
+}
+
+
+/*
+static inline void ROT(dgMatrix &a, int32_t i, int32_t j, int32_t k, int32_t l, float s, float tau) 
+{
+	float g = a[i][j]; 
+	float h = a[k][l]; 
+	a[i][j] = g - s * (h + g * tau); 
+	a[k][l] = h + s * (g - h * tau);
+}
+
+// from numerical recipes in c
+// Jacobian method for computing the eigenvectors of a symmetric matrix
+void dgMatrix::EigenVectors (dgVector &eigenValues, const dgMatrix& initialGuess)
+{
+	float b[3];
+	float z[3];
+	float d[3];
+
+	//	dgMatrix eigenVectors (initialGuess.Transpose4X4());
+	//	dgMatrix &mat = *this;
+
+	dgMatrix& mat = *this;
+	dgMatrix eigenVectors (initialGuess.Transpose4X4());
+	mat = initialGuess * mat * eigenVectors;
+
+
+	b[0] = mat[0][0]; 
+	b[1] = mat[1][1];
+	b[2] = mat[2][2];
+
+	d[0] = mat[0][0]; 
+	d[1] = mat[1][1]; 
+	d[2] = mat[2][2]; 
+
+	z[0] = float (0.0f);
+	z[1] = float (0.0f);
+	z[2] = float (0.0f);
+
+	int32_t nrot = 0;
+	for (int32_t i = 0; i < 50; i++) {
+		float sm = dgAbsf(mat[0][1]) + dgAbsf(mat[0][2]) + dgAbsf(mat[1][2]);
+
+		if (sm < float (1.0e-6f)) {
+			HACD_ASSERT (dgAbsf((eigenVectors.m_front % eigenVectors.m_front) - float(1.0f)) < dgEPSILON);
+			HACD_ASSERT (dgAbsf((eigenVectors.m_up % eigenVectors.m_up) - float(1.0f)) < dgEPSILON);
+			HACD_ASSERT (dgAbsf((eigenVectors.m_right % eigenVectors.m_right) - float(1.0f)) < dgEPSILON);
+
+			// order the eigenvalue vectors	
+			dgVector tmp (eigenVectors.m_front * eigenVectors.m_up);
+			if (tmp % eigenVectors.m_right < float(0.0f)) {
+				eigenVectors.m_right = eigenVectors.m_right.Scale (-float(1.0f));
+			}
+
+			eigenValues = dgVector (d[0], d[1], d[2], float (0.0f));
+			*this = eigenVectors.Inverse();
+			return;
+		}
+
+		float thresh = float (0.0f);
+		if (i < 3) {
+			thresh = (float)(0.2f / 9.0f) * sm;
+		}
+
+
+		// First row
+		float g = float (100.0f) * dgAbsf(mat[0][1]);
+		if ((i > 3) && (dgAbsf(d[0]) + g == dgAbsf(d[0])) && (dgAbsf(d[1]) + g == dgAbsf(d[1]))) {
+			mat[0][1] = float (0.0f);
+		} else if (dgAbsf(mat[0][1]) > thresh) {
+			float h = d[1] - d[0];
+			float t;
+			if (dgAbsf(h) + g == dgAbsf(h)) {
+				t = mat[0][1] / h;
+			} else {
+				float theta = float (0.5f) * h / mat[0][1];
+				t = float(1.0f) / (dgAbsf(theta) + dgSqrt(float(1.0f) + theta * theta));
+				if (theta < float (0.0f)) {
+					t = -t;
+				}
+			}
+			float c = float(1.0f) / dgSqrt (float (1.0f) + t * t); 
+			float s = t * c; 
+			float tau = s / (float(1.0f) + c); 
+			h = t * mat[0][1];
+			z[0] -= h; 
+			z[1] += h; 
+			d[0] -= h; 
+			d[1] += h;
+			mat[0][1] = float(0.0f);
+			ROT (mat, 0, 2, 1, 2, s, tau); 
+			ROT (eigenVectors, 0, 0, 0, 1, s, tau); 
+			ROT (eigenVectors, 1, 0, 1, 1, s, tau); 
+			ROT (eigenVectors, 2, 0, 2, 1, s, tau); 
+
+			nrot++;
+		}
+
+
+		// second row
+		g = float (100.0f) * dgAbsf(mat[0][2]);
+		if ((i > 3) && (dgAbsf(d[0]) + g == dgAbsf(d[0])) && (dgAbsf(d[2]) + g == dgAbsf(d[2]))) {
+			mat[0][2] = float (0.0f);
+		} else if (dgAbsf(mat[0][2]) > thresh) {
+			float h = d[2] - d[0];
+			float t;
+			if (dgAbsf(h) + g == dgAbsf(h)) {
+				t = (mat[0][2]) / h;
+			}	else {
+				float theta = float (0.5f) * h / mat[0][2];
+				t = float(1.0f) / (dgAbsf(theta) + dgSqrt(float(1.0f) + theta * theta));
+				if (theta < float (0.0f)) {
+					t = -t;
+				}
+			}
+			float c = float(1.0f) / dgSqrt(float (1.0f) + t * t); 
+			float s = t * c; 
+			float tau = s / (float(1.0f) + c); 
+			h = t * mat[0][2];
+			z[0] -= h; 
+			z[2] += h; 
+			d[0] -= h; 
+			d[2] += h;
+			mat[0][2]=float (0.0f);
+			ROT (mat, 0, 1, 1, 2, s, tau); 
+			ROT (eigenVectors, 0, 0, 0, 2, s, tau); 
+			ROT (eigenVectors, 1, 0, 1, 2, s, tau); 
+			ROT (eigenVectors, 2, 0, 2, 2, s, tau); 
+		}
+
+		// third row
+		g = float (100.0f) * dgAbsf(mat[1][2]);
+		if ((i > 3) && (dgAbsf(d[1]) + g == dgAbsf(d[1])) && (dgAbsf(d[2]) + g == dgAbsf(d[2]))) {
+			mat[1][2] = float (0.0f);
+		} else if (dgAbsf(mat[1][2]) > thresh) {
+			float h = d[2] - d[1];
+			float t;
+			if (dgAbsf(h) + g == dgAbsf(h)) {
+				t = mat[1][2] / h;
+			}	else {
+				float theta = float (0.5f) * h / mat[1][2];
+				t = float(1.0f) / (dgAbsf(theta) + dgSqrt(float(1.0f) + theta * theta));
+				if (theta < float (0.0f)) {
+					t = -t;
+				}
+			}
+			float c = float(1.0f) / dgSqrt(float (1.0f) + t*t); 
+			float s = t * c; 
+			float tau = s / (float(1.0f) + c); 
+
+			h = t * mat[1][2];
+			z[1] -= h; 
+			z[2] += h; 
+			d[1] -= h; 
+			d[2] += h;
+			mat[1][2] = float (0.0f);
+			ROT (mat, 0, 1, 0, 2, s, tau); 
+			ROT (eigenVectors, 0, 1, 0, 2, s, tau); 
+			ROT (eigenVectors, 1, 1, 1, 2, s, tau); 
+			ROT (eigenVectors, 2, 1, 2, 2, s, tau); 
+			nrot++;
+		}
+
+		b[0] += z[0]; d[0] = b[0]; z[0] = float (0.0f);
+		b[1] += z[1]; d[1] = b[1]; z[1] = float (0.0f);
+		b[2] += z[2]; d[2] = b[2]; z[2] = float (0.0f);
+	}
+
+	eigenValues = dgVector (d[0], d[1], d[2], float (0.0f));
+	*this = dgGetIdentityMatrix();
+} 	
+*/
+
+void dgMatrix::EigenVectors (dgVector &eigenValues, const dgMatrix& initialGuess)
+{
+	float b[3];
+	float z[3];
+	float d[3];
+
+	dgMatrix& mat = *this;
+	dgMatrix eigenVectors (initialGuess.Transpose4X4());
+	mat = initialGuess * mat * eigenVectors;
+
+	b[0] = mat[0][0]; 
+	b[1] = mat[1][1];
+	b[2] = mat[2][2];
+
+	d[0] = mat[0][0]; 
+	d[1] = mat[1][1]; 
+	d[2] = mat[2][2]; 
+
+	z[0] = float (0.0f);
+	z[1] = float (0.0f);
+	z[2] = float (0.0f);
+
+	for (int32_t i = 0; i < 50; i++) {
+		float sm = dgAbsf(mat[0][1]) + dgAbsf(mat[0][2]) + dgAbsf(mat[1][2]);
+
+		if (sm < float (1.0e-6f)) {
+			HACD_ASSERT (dgAbsf((eigenVectors.m_front % eigenVectors.m_front) - float(1.0f)) < dgEPSILON);
+			HACD_ASSERT (dgAbsf((eigenVectors.m_up % eigenVectors.m_up) - float(1.0f)) < dgEPSILON);
+			HACD_ASSERT (dgAbsf((eigenVectors.m_right % eigenVectors.m_right) - float(1.0f)) < dgEPSILON);
+
+			// order the eigenvalue vectors	
+			dgVector tmp (eigenVectors.m_front * eigenVectors.m_up);
+			if (tmp % eigenVectors.m_right < float(0.0f)) {
+				eigenVectors.m_right = eigenVectors.m_right.Scale (-float(1.0f));
+			}
+
+			eigenValues = dgVector (d[0], d[1], d[2], float (0.0f));
+			*this = eigenVectors.Inverse();
+			return;
+		}
+
+		float thresh = float (0.0f);
+		if (i < 3) {
+			thresh = (float)(0.2f / 9.0f) * sm;
+		}
+
+		for (int32_t ip = 0; ip < 2; ip ++) {
+			for (int32_t iq = ip + 1; iq < 3; iq ++) {
+				float g = float (100.0f) * dgAbsf(mat[ip][iq]);
+				//if ((i > 3) && (dgAbsf(d[0]) + g == dgAbsf(d[ip])) && (dgAbsf(d[1]) + g == dgAbsf(d[1]))) {
+				if ((i > 3) && ((dgAbsf(d[ip]) + g) == dgAbsf(d[ip])) && ((dgAbsf(d[iq]) + g) == dgAbsf(d[iq]))) {
+					mat[ip][iq] = float (0.0f);
+				} else if (dgAbsf(mat[ip][iq]) > thresh) {
+
+					float t;
+					float h = d[iq] - d[ip];
+					if (dgAbsf(h) + g == dgAbsf(h)) {
+						t = mat[ip][iq] / h;
+					} else {
+						float theta = float (0.5f) * h / mat[ip][iq];
+						t = float(1.0f) / (dgAbsf(theta) + dgSqrt(float(1.0f) + theta * theta));
+						if (theta < float (0.0f)) {
+							t = -t;
+						}
+					}
+					float c = float(1.0f) / dgSqrt (float (1.0f) + t * t); 
+					float s = t * c; 
+					float tau = s / (float(1.0f) + c); 
+					h = t * mat[ip][iq];
+					z[ip] -= h; 
+					z[iq] += h; 
+					d[ip] -= h; 
+					d[iq] += h;
+					mat[ip][iq] = float(0.0f);
+
+					for (int32_t j = 0; j <= ip - 1; j ++) {
+						//ROT (mat, j, ip, j, iq, s, tau); 
+						//ROT(dgMatrix &a, int32_t i, int32_t j, int32_t k, int32_t l, float s, float tau) 
+						float g = mat[j][ip]; 
+						float h = mat[j][iq]; 
+						mat[j][ip] = g - s * (h + g * tau); 
+						mat[j][iq] = h + s * (g - h * tau);
+
+					}
+					for (int32_t j = ip + 1; j <= iq - 1; j ++) {
+						//ROT (mat, ip, j, j, iq, s, tau); 
+						//ROT(dgMatrix &a, int32_t i, int32_t j, int32_t k, int32_t l, float s, float tau) 
+						float g = mat[ip][j]; 
+						float h = mat[j][iq]; 
+						mat[ip][j] = g - s * (h + g * tau); 
+						mat[j][iq] = h + s * (g - h * tau);
+					}
+					for (int32_t j = iq + 1; j < 3; j ++) {
+						//ROT (mat, ip, j, iq, j, s, tau); 
+						//ROT(dgMatrix &a, int32_t i, int32_t j, int32_t k, int32_t l, float s, float tau) 
+						float g = mat[ip][j]; 
+						float h = mat[iq][j]; 
+						mat[ip][j] = g - s * (h + g * tau); 
+						mat[iq][j] = h + s * (g - h * tau);
+					}
+
+					for (int32_t j = 0; j < 3; j ++) {
+						//ROT (eigenVectors, j, ip, j, iq, s, tau); 
+						//ROT(dgMatrix &a, int32_t i, int32_t j, int32_t k, int32_t l, float s, float tau) 
+						float g = eigenVectors[j][ip]; 
+						float h = eigenVectors[j][iq]; 
+						eigenVectors[j][ip] = g - s * (h + g * tau); 
+						eigenVectors[j][iq] = h + s * (g - h * tau);
+					}
+				}
+			}
+		}
+		b[0] += z[0]; d[0] = b[0]; z[0] = float (0.0f);
+		b[1] += z[1]; d[1] = b[1]; z[1] = float (0.0f);
+		b[2] += z[2]; d[2] = b[2]; z[2] = float (0.0f);
+	}
+
+	eigenValues = dgVector (d[0], d[1], d[2], float (0.0f));
+	*this = dgGetIdentityMatrix();
+}
+