Initial commit:

PhysX 3.4.0 Update @ 21294896
APEX 1.4.0 Update @ 21275617

[CL 21300167]

1 files changed, 1063 insertions, 0 deletions

diff --git a/APEX_1.4/shared/external/src/BestFit.cpp b/APEX_1.4/shared/external/src/BestFit.cpp
new file mode 100644
index 00000000..5aefcb41
--- /dev/null
+++ b/APEX_1.4/shared/external/src/BestFit.cpp
@@ -0,0 +1,1063 @@
+/*
+ * Copyright (c) 2008-2015, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * NVIDIA CORPORATION and its licensors retain all intellectual property
+ * and proprietary rights in and to this software, 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.
+ */
+
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <assert.h>
+#include <math.h>
+#include <float.h>
+
+#include "BestFit.h"
+
+#define REAL float
+
+namespace SharedTools
+{
+
+const float FM_PI = 3.1415926535897932384626433832795028841971693993751f;
+const float FM_DEG_TO_RAD = ((2.0f * FM_PI) / 360.0f);
+//const float FM_RAD_TO_DEG = (360.0f / (2.0f * FM_PI));
+
+void fm_identity(REAL matrix[16]) // set 4x4 matrix to identity.
+{
+	matrix[0 * 4 + 0] = 1;
+	matrix[1 * 4 + 1] = 1;
+	matrix[2 * 4 + 2] = 1;
+	matrix[3 * 4 + 3] = 1;
+
+	matrix[1 * 4 + 0] = 0;
+	matrix[2 * 4 + 0] = 0;
+	matrix[3 * 4 + 0] = 0;
+
+	matrix[0 * 4 + 1] = 0;
+	matrix[2 * 4 + 1] = 0;
+	matrix[3 * 4 + 1] = 0;
+
+	matrix[0 * 4 + 2] = 0;
+	matrix[1 * 4 + 2] = 0;
+	matrix[3 * 4 + 2] = 0;
+
+	matrix[0 * 4 + 3] = 0;
+	matrix[1 * 4 + 3] = 0;
+	matrix[2 * 4 + 3] = 0;
+
+}
+
+
+
+void  fm_matrixMultiply(const REAL* pA, const REAL* pB, REAL* pM)
+{
+	REAL a = pA[0 * 4 + 0] * pB[0 * 4 + 0] + pA[0 * 4 + 1] * pB[1 * 4 + 0] + pA[0 * 4 + 2] * pB[2 * 4 + 0] + pA[0 * 4 + 3] * pB[3 * 4 + 0];
+	REAL b = pA[0 * 4 + 0] * pB[0 * 4 + 1] + pA[0 * 4 + 1] * pB[1 * 4 + 1] + pA[0 * 4 + 2] * pB[2 * 4 + 1] + pA[0 * 4 + 3] * pB[3 * 4 + 1];
+	REAL c = pA[0 * 4 + 0] * pB[0 * 4 + 2] + pA[0 * 4 + 1] * pB[1 * 4 + 2] + pA[0 * 4 + 2] * pB[2 * 4 + 2] + pA[0 * 4 + 3] * pB[3 * 4 + 2];
+	REAL d = pA[0 * 4 + 0] * pB[0 * 4 + 3] + pA[0 * 4 + 1] * pB[1 * 4 + 3] + pA[0 * 4 + 2] * pB[2 * 4 + 3] + pA[0 * 4 + 3] * pB[3 * 4 + 3];
+
+	REAL e = pA[1 * 4 + 0] * pB[0 * 4 + 0] + pA[1 * 4 + 1] * pB[1 * 4 + 0] + pA[1 * 4 + 2] * pB[2 * 4 + 0] + pA[1 * 4 + 3] * pB[3 * 4 + 0];
+	REAL f = pA[1 * 4 + 0] * pB[0 * 4 + 1] + pA[1 * 4 + 1] * pB[1 * 4 + 1] + pA[1 * 4 + 2] * pB[2 * 4 + 1] + pA[1 * 4 + 3] * pB[3 * 4 + 1];
+	REAL g = pA[1 * 4 + 0] * pB[0 * 4 + 2] + pA[1 * 4 + 1] * pB[1 * 4 + 2] + pA[1 * 4 + 2] * pB[2 * 4 + 2] + pA[1 * 4 + 3] * pB[3 * 4 + 2];
+	REAL h = pA[1 * 4 + 0] * pB[0 * 4 + 3] + pA[1 * 4 + 1] * pB[1 * 4 + 3] + pA[1 * 4 + 2] * pB[2 * 4 + 3] + pA[1 * 4 + 3] * pB[3 * 4 + 3];
+
+	REAL i = pA[2 * 4 + 0] * pB[0 * 4 + 0] + pA[2 * 4 + 1] * pB[1 * 4 + 0] + pA[2 * 4 + 2] * pB[2 * 4 + 0] + pA[2 * 4 + 3] * pB[3 * 4 + 0];
+	REAL j = pA[2 * 4 + 0] * pB[0 * 4 + 1] + pA[2 * 4 + 1] * pB[1 * 4 + 1] + pA[2 * 4 + 2] * pB[2 * 4 + 1] + pA[2 * 4 + 3] * pB[3 * 4 + 1];
+	REAL k = pA[2 * 4 + 0] * pB[0 * 4 + 2] + pA[2 * 4 + 1] * pB[1 * 4 + 2] + pA[2 * 4 + 2] * pB[2 * 4 + 2] + pA[2 * 4 + 3] * pB[3 * 4 + 2];
+	REAL l = pA[2 * 4 + 0] * pB[0 * 4 + 3] + pA[2 * 4 + 1] * pB[1 * 4 + 3] + pA[2 * 4 + 2] * pB[2 * 4 + 3] + pA[2 * 4 + 3] * pB[3 * 4 + 3];
+
+	REAL m = pA[3 * 4 + 0] * pB[0 * 4 + 0] + pA[3 * 4 + 1] * pB[1 * 4 + 0] + pA[3 * 4 + 2] * pB[2 * 4 + 0] + pA[3 * 4 + 3] * pB[3 * 4 + 0];
+	REAL n = pA[3 * 4 + 0] * pB[0 * 4 + 1] + pA[3 * 4 + 1] * pB[1 * 4 + 1] + pA[3 * 4 + 2] * pB[2 * 4 + 1] + pA[3 * 4 + 3] * pB[3 * 4 + 1];
+	REAL o = pA[3 * 4 + 0] * pB[0 * 4 + 2] + pA[3 * 4 + 1] * pB[1 * 4 + 2] + pA[3 * 4 + 2] * pB[2 * 4 + 2] + pA[3 * 4 + 3] * pB[3 * 4 + 2];
+	REAL p = pA[3 * 4 + 0] * pB[0 * 4 + 3] + pA[3 * 4 + 1] * pB[1 * 4 + 3] + pA[3 * 4 + 2] * pB[2 * 4 + 3] + pA[3 * 4 + 3] * pB[3 * 4 + 3];
+
+	pM[0] = a;
+	pM[1] = b;
+	pM[2] = c;
+	pM[3] = d;
+
+	pM[4] = e;
+	pM[5] = f;
+	pM[6] = g;
+	pM[7] = h;
+
+	pM[8] = i;
+	pM[9] = j;
+	pM[10] = k;
+	pM[11] = l;
+
+	pM[12] = m;
+	pM[13] = n;
+	pM[14] = o;
+	pM[15] = p;
+
+}
+
+void fm_matrixToQuat(const REAL* matrix, REAL* quat) // convert the 3x3 portion of a 4x4 matrix into a quaterion as x,y,z,w
+{
+	REAL tr = matrix[0 * 4 + 0] + matrix[1 * 4 + 1] + matrix[2 * 4 + 2];
+
+	// check the diagonal
+
+	if (tr > 0.0f)
+	{
+		REAL s = (REAL) sqrt((double)(tr + 1.0f));
+		quat[3] = s * 0.5f;
+		s = 0.5f / s;
+		quat[0] = (matrix[1 * 4 + 2] - matrix[2 * 4 + 1]) * s;
+		quat[1] = (matrix[2 * 4 + 0] - matrix[0 * 4 + 2]) * s;
+		quat[2] = (matrix[0 * 4 + 1] - matrix[1 * 4 + 0]) * s;
+
+	}
+	else
+	{
+		// diagonal is negative
+		int nxt[3] = {1, 2, 0};
+		REAL  qa[4];
+
+		int i = 0;
+
+		if (matrix[1 * 4 + 1] > matrix[0 * 4 + 0])
+		{
+			i = 1;
+		}
+		if (matrix[2 * 4 + 2] > matrix[i * 4 + i])
+		{
+			i = 2;
+		}
+
+		int j = nxt[i];
+		int k = nxt[j];
+
+		REAL s = static_cast<REAL>(sqrt(((matrix[i * 4 + i] - (matrix[j * 4 + j] + matrix[k * 4 + k])) + 1.0f)));
+
+		qa[i] = s * 0.5f;
+
+		if (s != 0.0f)
+		{
+			s = 0.5f / s;
+		}
+
+		qa[3] = (matrix[j * 4 + k] - matrix[k * 4 + j]) * s;
+		qa[j] = (matrix[i * 4 + j] + matrix[j * 4 + i]) * s;
+		qa[k] = (matrix[i * 4 + k] + matrix[k * 4 + i]) * s;
+
+		quat[0] = qa[0];
+		quat[1] = qa[1];
+		quat[2] = qa[2];
+		quat[3] = qa[3];
+	}
+}
+
+
+void fm_getTranslation(const REAL* matrix, REAL* t)
+{
+	t[0] = matrix[3 * 4 + 0];
+	t[1] = matrix[3 * 4 + 1];
+	t[2] = matrix[3 * 4 + 2];
+}
+
+
+void  fm_rotate(const REAL matrix[16], const REAL v[3], REAL t[3]) // rotate and translate this point
+{
+	if (matrix)
+	{
+		REAL tx = (matrix[0 * 4 + 0] * v[0]) + (matrix[1 * 4 + 0] * v[1]) + (matrix[2 * 4 + 0] * v[2]);
+		REAL ty = (matrix[0 * 4 + 1] * v[0]) + (matrix[1 * 4 + 1] * v[1]) + (matrix[2 * 4 + 1] * v[2]);
+		REAL tz = (matrix[0 * 4 + 2] * v[0]) + (matrix[1 * 4 + 2] * v[1]) + (matrix[2 * 4 + 2] * v[2]);
+		t[0] = tx;
+		t[1] = ty;
+		t[2] = tz;
+	}
+	else
+	{
+		t[0] = v[0];
+		t[1] = v[1];
+		t[2] = v[2];
+	}
+}
+
+void fm_inverseRT(const REAL matrix[16], const REAL pos[3], REAL t[3]) // inverse rotate translate the point.
+{
+
+	REAL _x = pos[0] - matrix[3 * 4 + 0];
+	REAL _y = pos[1] - matrix[3 * 4 + 1];
+	REAL _z = pos[2] - matrix[3 * 4 + 2];
+
+	// Multiply inverse-translated source vector by inverted rotation transform
+
+	t[0] = (matrix[0 * 4 + 0] * _x) + (matrix[0 * 4 + 1] * _y) + (matrix[0 * 4 + 2] * _z);
+	t[1] = (matrix[1 * 4 + 0] * _x) + (matrix[1 * 4 + 1] * _y) + (matrix[1 * 4 + 2] * _z);
+	t[2] = (matrix[2 * 4 + 0] * _x) + (matrix[2 * 4 + 1] * _y) + (matrix[2 * 4 + 2] * _z);
+
+}
+
+void  fm_setTranslation(const REAL* translation, REAL* matrix)
+{
+	matrix[12] = translation[0];
+	matrix[13] = translation[1];
+	matrix[14] = translation[2];
+}
+
+void  fm_transform(const REAL matrix[16], const REAL v[3], REAL t[3]) // rotate and translate this point
+{
+	if (matrix)
+	{
+		REAL tx = (matrix[0 * 4 + 0] * v[0]) + (matrix[1 * 4 + 0] * v[1]) + (matrix[2 * 4 + 0] * v[2]) + matrix[3 * 4 + 0];
+		REAL ty = (matrix[0 * 4 + 1] * v[0]) + (matrix[1 * 4 + 1] * v[1]) + (matrix[2 * 4 + 1] * v[2]) + matrix[3 * 4 + 1];
+		REAL tz = (matrix[0 * 4 + 2] * v[0]) + (matrix[1 * 4 + 2] * v[1]) + (matrix[2 * 4 + 2] * v[2]) + matrix[3 * 4 + 2];
+		t[0] = tx;
+		t[1] = ty;
+		t[2] = tz;
+	}
+	else
+	{
+		t[0] = v[0];
+		t[1] = v[1];
+		t[2] = v[2];
+	}
+}
+
+void fm_quatToMatrix(const REAL* quat, REAL* matrix) // convert quaterinion rotation to matrix, zeros out the translation component.
+{
+
+	REAL xx = quat[0] * quat[0];
+	REAL yy = quat[1] * quat[1];
+	REAL zz = quat[2] * quat[2];
+	REAL xy = quat[0] * quat[1];
+	REAL xz = quat[0] * quat[2];
+	REAL yz = quat[1] * quat[2];
+	REAL wx = quat[3] * quat[0];
+	REAL wy = quat[3] * quat[1];
+	REAL wz = quat[3] * quat[2];
+
+	matrix[0 * 4 + 0] = 1 - 2 * (yy + zz);
+	matrix[1 * 4 + 0] =     2 * (xy - wz);
+	matrix[2 * 4 + 0] =     2 * (xz + wy);
+
+	matrix[0 * 4 + 1] =     2 * (xy + wz);
+	matrix[1 * 4 + 1] = 1 - 2 * (xx + zz);
+	matrix[2 * 4 + 1] =     2 * (yz - wx);
+
+	matrix[0 * 4 + 2] =     2 * (xz - wy);
+	matrix[1 * 4 + 2] =     2 * (yz + wx);
+	matrix[2 * 4 + 2] = 1 - 2 * (xx + yy);
+
+	matrix[3 * 4 + 0] = matrix[3 * 4 + 1] = matrix[3 * 4 + 2] = (REAL) 0.0f;
+	matrix[0 * 4 + 3] = matrix[1 * 4 + 3] = matrix[2 * 4 + 3] = (REAL) 0.0f;
+	matrix[3 * 4 + 3] = (REAL) 1.0f;
+
+}
+
+void fm_cross(REAL* cross, const REAL* a, const REAL* b)
+{
+	cross[0] = a[1] * b[2] - a[2] * b[1];
+	cross[1] = a[2] * b[0] - a[0] * b[2];
+	cross[2] = a[0] * b[1] - a[1] * b[0];
+}
+
+
+REAL fm_dot(const REAL* p1, const REAL* p2)
+{
+	return p1[0] * p2[0] + p1[1] * p2[1] + p1[2] * p2[2];
+}
+
+// Reference, from Stan Melax in Game Gems I
+//  Quaternion q;
+//  vector3 c = CrossProduct(v0,v1);
+//  REAL   d = DotProduct(v0,v1);
+//  REAL   s = (REAL)sqrt((1+d)*2);
+//  q.x = c.x / s;
+//  q.y = c.y / s;
+//  q.z = c.z / s;
+//  q.w = s /2.0f;
+//  return q;
+void fm_rotationArc(const REAL* v0, const REAL* v1, REAL* quat)
+{
+	REAL cross[3];
+
+	fm_cross(cross, v0, v1);
+	REAL d = fm_dot(v0, v1);
+	REAL s = static_cast<REAL>(sqrt((1 + d) * 2));
+	REAL recip = 1.0f / s;
+
+	quat[0] = cross[0] * recip;
+	quat[1] = cross[1] * recip;
+	quat[2] = cross[2] * recip;
+	quat[3] = s * 0.5f;
+
+}
+
+void fm_planeToMatrix(const REAL* plane, REAL* matrix) // convert a plane equation to a 4x4 rotation matrix
+{
+	REAL ref[3] = { 0, 1, 0 };
+	REAL quat[4];
+	fm_rotationArc(ref, plane, quat);
+	fm_quatToMatrix(quat, matrix);
+	REAL origin[3] = { 0, -plane[3], 0 };
+	REAL center[3];
+	fm_transform(matrix, origin, center);
+	fm_setTranslation(center, matrix);
+}
+
+void fm_planeToQuat(const REAL* plane, REAL* quat, REAL* pos) // convert a plane equation to a quaternion and translation
+{
+	REAL ref[3] = { 0, 1, 0 };
+	REAL matrix[16];
+	fm_rotationArc(ref, plane, quat);
+	fm_quatToMatrix(quat, matrix);
+	REAL origin[3] = { 0, plane[3], 0 };
+	fm_transform(matrix, origin, pos);
+}
+
+void fm_eulerToQuat(REAL roll, REAL pitch, REAL yaw, REAL* quat) // convert euler angles to quaternion.
+{
+	roll  *= 0.5f;
+	pitch *= 0.5f;
+	yaw   *= 0.5f;
+
+	REAL cr = static_cast<REAL>(cos(roll));
+	REAL cp = static_cast<REAL>(cos(pitch));
+	REAL cy = static_cast<REAL>(cos(yaw));
+
+	REAL sr = static_cast<REAL>(sin(roll));
+	REAL sp = static_cast<REAL>(sin(pitch));
+	REAL sy = static_cast<REAL>(sin(yaw));
+
+	REAL cpcy = cp * cy;
+	REAL spsy = sp * sy;
+	REAL spcy = sp * cy;
+	REAL cpsy = cp * sy;
+
+	quat[0]   = (sr * cpcy - cr * spsy);
+	quat[1]   = (cr * spcy + sr * cpsy);
+	quat[2]   = (cr * cpsy - sr * spcy);
+	quat[3]   = cr * cpcy + sr * spsy;
+}
+
+void  fm_eulerToQuat(const REAL* euler, REAL* quat) // convert euler angles to quaternion.
+{
+	fm_eulerToQuat(euler[0], euler[1], euler[2], quat);
+}
+
+
+void fm_eulerMatrix(REAL ax, REAL ay, REAL az, REAL* matrix) // convert euler (in radians) to a dest 4x4 matrix (translation set to zero)
+{
+	REAL quat[4];
+	fm_eulerToQuat(ax, ay, az, quat);
+	fm_quatToMatrix(quat, matrix);
+}
+
+template <class Type> class Eigen
+{
+public:
+
+
+	void DecrSortEigenStuff(void)
+	{
+		Tridiagonal(); //diagonalize the matrix.
+		QLAlgorithm(); //
+		DecreasingSort();
+		GuaranteeRotation();
+	}
+
+	void Tridiagonal(void)
+	{
+		Type fM00 = mElement[0][0];
+		Type fM01 = mElement[0][1];
+		Type fM02 = mElement[0][2];
+		Type fM11 = mElement[1][1];
+		Type fM12 = mElement[1][2];
+		Type fM22 = mElement[2][2];
+
+		m_afDiag[0] = fM00;
+		m_afSubd[2] = 0;
+		if (fM02 != (Type)0.0)
+		{
+			Type fLength = static_cast<Type>(sqrt(fM01 * fM01 + fM02 * fM02));
+			Type fInvLength = ((Type)1.0) / fLength;
+			fM01 *= fInvLength;
+			fM02 *= fInvLength;
+			Type fQ = ((Type)2.0) * fM01 * fM12 + fM02 * (fM22 - fM11);
+			m_afDiag[1] = fM11 + fM02 * fQ;
+			m_afDiag[2] = fM22 - fM02 * fQ;
+			m_afSubd[0] = fLength;
+			m_afSubd[1] = fM12 - fM01 * fQ;
+			mElement[0][0] = (Type)1.0;
+			mElement[0][1] = (Type)0.0;
+			mElement[0][2] = (Type)0.0;
+			mElement[1][0] = (Type)0.0;
+			mElement[1][1] = fM01;
+			mElement[1][2] = fM02;
+			mElement[2][0] = (Type)0.0;
+			mElement[2][1] = fM02;
+			mElement[2][2] = -fM01;
+			m_bIsRotation = false;
+		}
+		else
+		{
+			m_afDiag[1] = fM11;
+			m_afDiag[2] = fM22;
+			m_afSubd[0] = fM01;
+			m_afSubd[1] = fM12;
+			mElement[0][0] = (Type)1.0;
+			mElement[0][1] = (Type)0.0;
+			mElement[0][2] = (Type)0.0;
+			mElement[1][0] = (Type)0.0;
+			mElement[1][1] = (Type)1.0;
+			mElement[1][2] = (Type)0.0;
+			mElement[2][0] = (Type)0.0;
+			mElement[2][1] = (Type)0.0;
+			mElement[2][2] = (Type)1.0;
+			m_bIsRotation = true;
+		}
+	}
+
+	bool QLAlgorithm(void)
+	{
+		const int iMaxIter = 32;
+
+		for (int i0 = 0; i0 < 3; i0++)
+		{
+			int i1;
+			for (i1 = 0; i1 < iMaxIter; i1++)
+			{
+				int i2;
+				for (i2 = i0; i2 <= (3 - 2); i2++)
+				{
+					Type fTmp = static_cast<Type>(fabs(m_afDiag[i2]) + fabs(m_afDiag[i2 + 1]));
+					if (fabs(m_afSubd[i2]) + fTmp == fTmp)
+					{
+						break;
+					}
+				}
+				if (i2 == i0)
+				{
+					break;
+				}
+
+				Type fG = (m_afDiag[i0 + 1] - m_afDiag[i0]) / (((Type)2.0) * m_afSubd[i0]);
+				Type fR = static_cast<Type>(sqrt(fG * fG + (Type)1.0));
+				if (fG < (Type)0.0)
+				{
+					fG = m_afDiag[i2] - m_afDiag[i0] + m_afSubd[i0] / (fG - fR);
+				}
+				else
+				{
+					fG = m_afDiag[i2] - m_afDiag[i0] + m_afSubd[i0] / (fG + fR);
+				}
+				Type fSin = (Type)1.0, fCos = (Type)1.0, fP = (Type)0.0;
+				for (int i3 = i2 - 1; i3 >= i0; i3--)
+				{
+					Type fF = fSin * m_afSubd[i3];
+					Type fB = fCos * m_afSubd[i3];
+					if (fabs(fF) >= fabs(fG))
+					{
+						fCos = fG / fF;
+						fR = static_cast<Type>(sqrt(fCos * fCos + (Type)1.0));
+						m_afSubd[i3 + 1] = fF * fR;
+						fSin = ((Type)1.0) / fR;
+						fCos *= fSin;
+					}
+					else
+					{
+						fSin = fF / fG;
+						fR = static_cast<Type>(sqrt(fSin * fSin + (Type)1.0));
+						m_afSubd[i3 + 1] = fG * fR;
+						fCos = ((Type)1.0) / fR;
+						fSin *= fCos;
+					}
+					fG = m_afDiag[i3 + 1] - fP;
+					fR = (m_afDiag[i3] - fG) * fSin + ((Type)2.0) * fB * fCos;
+					fP = fSin * fR;
+					m_afDiag[i3 + 1] = fG + fP;
+					fG = fCos * fR - fB;
+					for (int i4 = 0; i4 < 3; i4++)
+					{
+						fF = mElement[i4][i3 + 1];
+						mElement[i4][i3 + 1] = fSin * mElement[i4][i3] + fCos * fF;
+						mElement[i4][i3] = fCos * mElement[i4][i3] - fSin * fF;
+					}
+				}
+				m_afDiag[i0] -= fP;
+				m_afSubd[i0] = fG;
+				m_afSubd[i2] = (Type)0.0;
+			}
+			if (i1 == iMaxIter)
+			{
+				return false;
+			}
+		}
+		return true;
+	}
+
+	void DecreasingSort(void)
+	{
+		//sort eigenvalues in decreasing order, e[0] >= ... >= e[iSize-1]
+		for (int i0 = 0, i1; i0 <= 3 - 2; i0++)
+		{
+			// locate maximum eigenvalue
+			i1 = i0;
+			Type fMax = m_afDiag[i1];
+			int i2;
+			for (i2 = i0 + 1; i2 < 3; i2++)
+			{
+				if (m_afDiag[i2] > fMax)
+				{
+					i1 = i2;
+					fMax = m_afDiag[i1];
+				}
+			}
+
+			if (i1 != i0)
+			{
+				// swap eigenvalues
+				m_afDiag[i1] = m_afDiag[i0];
+				m_afDiag[i0] = fMax;
+				// swap eigenvectors
+				for (i2 = 0; i2 < 3; i2++)
+				{
+					Type fTmp = mElement[i2][i0];
+					mElement[i2][i0] = mElement[i2][i1];
+					mElement[i2][i1] = fTmp;
+					m_bIsRotation = !m_bIsRotation;
+				}
+			}
+		}
+	}
+
+
+	void GuaranteeRotation(void)
+	{
+		if (!m_bIsRotation)
+		{
+			// change sign on the first column
+			for (int iRow = 0; iRow < 3; iRow++)
+			{
+				mElement[iRow][0] = -mElement[iRow][0];
+			}
+		}
+	}
+
+	Type mElement[3][3];
+	Type m_afDiag[3];
+	Type m_afSubd[3];
+	bool m_bIsRotation;
+};
+
+bool computeBestFitPlane(size_t vcount,
+                         const REAL* points,
+                         size_t vstride,
+                         const REAL* weights,
+                         size_t wstride,
+                         REAL* plane)
+{
+	bool ret = false;
+
+	REAL kOrigin[3] = { 0, 0, 0 };
+
+	REAL wtotal = 0;
+
+	{
+		const char* source  = (const char*) points;
+		const char* wsource = (const char*) weights;
+
+		for (size_t i = 0; i < vcount; i++)
+		{
+
+			const REAL* p = (const REAL*) source;
+
+			REAL w = 1;
+
+			if (wsource)
+			{
+				const REAL* ws = (const REAL*) wsource;
+				w = *ws; //
+				wsource += wstride;
+			}
+
+			kOrigin[0] += p[0] * w;
+			kOrigin[1] += p[1] * w;
+			kOrigin[2] += p[2] * w;
+
+			wtotal += w;
+
+			source += vstride;
+		}
+	}
+
+	REAL recip = 1.0f / wtotal; // reciprocol of total weighting
+
+	kOrigin[0] *= recip;
+	kOrigin[1] *= recip;
+	kOrigin[2] *= recip;
+
+
+	REAL fSumXX = 0;
+	REAL fSumXY = 0;
+	REAL fSumXZ = 0;
+
+	REAL fSumYY = 0;
+	REAL fSumYZ = 0;
+	REAL fSumZZ = 0;
+
+
+	{
+		const char* source  = (const char*) points;
+		const char* wsource = (const char*) weights;
+
+		for (size_t i = 0; i < vcount; i++)
+		{
+
+			const REAL* p = (const REAL*) source;
+
+			REAL w = 1;
+
+			if (wsource)
+			{
+				const REAL* ws = (const REAL*) wsource;
+				w = *ws; //
+				wsource += wstride;
+			}
+
+			REAL kDiff[3];
+
+			kDiff[0] = w * (p[0] - kOrigin[0]); // apply vertex weighting!
+			kDiff[1] = w * (p[1] - kOrigin[1]);
+			kDiff[2] = w * (p[2] - kOrigin[2]);
+
+			fSumXX += kDiff[0] * kDiff[0]; // sume of the squares of the differences.
+			fSumXY += kDiff[0] * kDiff[1]; // sume of the squares of the differences.
+			fSumXZ += kDiff[0] * kDiff[2]; // sume of the squares of the differences.
+
+			fSumYY += kDiff[1] * kDiff[1];
+			fSumYZ += kDiff[1] * kDiff[2];
+			fSumZZ += kDiff[2] * kDiff[2];
+
+
+			source += vstride;
+		}
+	}
+
+	fSumXX *= recip;
+	fSumXY *= recip;
+	fSumXZ *= recip;
+	fSumYY *= recip;
+	fSumYZ *= recip;
+	fSumZZ *= recip;
+
+	// setup the eigensolver
+	Eigen<REAL> kES;
+
+	kES.mElement[0][0] = fSumXX;
+	kES.mElement[0][1] = fSumXY;
+	kES.mElement[0][2] = fSumXZ;
+
+	kES.mElement[1][0] = fSumXY;
+	kES.mElement[1][1] = fSumYY;
+	kES.mElement[1][2] = fSumYZ;
+
+	kES.mElement[2][0] = fSumXZ;
+	kES.mElement[2][1] = fSumYZ;
+	kES.mElement[2][2] = fSumZZ;
+
+	// compute eigenstuff, smallest eigenvalue is in last position
+	kES.DecrSortEigenStuff();
+
+	REAL kNormal[3];
+
+	kNormal[0] = kES.mElement[0][2];
+	kNormal[1] = kES.mElement[1][2];
+	kNormal[2] = kES.mElement[2][2];
+
+	// the minimum energy
+	plane[0] = kNormal[0];
+	plane[1] = kNormal[1];
+	plane[2] = kNormal[2];
+
+	plane[3] = 0 - fm_dot(kNormal, kOrigin);
+
+	ret = true;
+
+	return ret;
+}
+
+// computes the OBB for this set of points relative to this transform matrix.
+void computeOBB(size_t vcount, const REAL* points, size_t pstride, REAL* sides, REAL* matrix)
+{
+	const char* src = (const char*) points;
+
+	REAL bmin[3] = { 1e9, 1e9, 1e9 };
+	REAL bmax[3] = { -1e9, -1e9, -1e9 };
+
+	for (size_t i = 0; i < vcount; i++)
+	{
+		const REAL* p = (const REAL*) src;
+		REAL t[3];
+
+		fm_inverseRT(matrix, p, t);  // inverse rotate translate
+
+		if (t[0] < bmin[0])
+		{
+			bmin[0] = t[0];
+		}
+		if (t[1] < bmin[1])
+		{
+			bmin[1] = t[1];
+		}
+		if (t[2] < bmin[2])
+		{
+			bmin[2] = t[2];
+		}
+
+		if (t[0] > bmax[0])
+		{
+			bmax[0] = t[0];
+		}
+		if (t[1] > bmax[1])
+		{
+			bmax[1] = t[1];
+		}
+		if (t[2] > bmax[2])
+		{
+			bmax[2] = t[2];
+		}
+
+		src += pstride;
+	}
+
+	REAL center[3];
+
+	sides[0] = bmax[0] - bmin[0];
+	sides[1] = bmax[1] - bmin[1];
+	sides[2] = bmax[2] - bmin[2];
+
+	center[0] = sides[0] * 0.5f + bmin[0];
+	center[1] = sides[1] * 0.5f + bmin[1];
+	center[2] = sides[2] * 0.5f + bmin[2];
+
+	REAL ocenter[3];
+
+	fm_rotate(matrix, center, ocenter);
+
+	matrix[12] += ocenter[0];
+	matrix[13] += ocenter[1];
+	matrix[14] += ocenter[2];
+
+}
+
+
+
+void computeBestFitOBB(size_t vcount, const REAL* points, size_t pstride, REAL* sides, REAL* matrix, bool bruteForce)
+{
+	fm_identity(matrix);
+	REAL bmin[3];
+	REAL bmax[3];
+	computeBestFitAABB(vcount, points, pstride, bmin, bmax);
+
+	REAL avolume = (bmax[0] - bmin[0]) * (bmax[1] - bmin[1]) * (bmax[2] - bmin[2]);
+
+	REAL plane[4];
+	computeBestFitPlane(vcount, points, pstride, 0, 0, plane);
+	fm_planeToMatrix(plane, matrix);
+	computeOBB(vcount, points, pstride, sides, matrix);
+
+	REAL refmatrix[16];
+	memcpy(refmatrix, matrix, 16 * sizeof(REAL));
+
+	REAL volume = sides[0] * sides[1] * sides[2];
+	if (bruteForce)
+	{
+		for (REAL a = 10; a < 180; a += 10)
+		{
+			REAL quat[4];
+			fm_eulerToQuat(0, a * FM_DEG_TO_RAD, 0, quat);
+			REAL temp[16];
+			REAL pmatrix[16];
+			fm_quatToMatrix(quat, temp);
+			fm_matrixMultiply(temp, refmatrix, pmatrix);
+			REAL psides[3];
+			computeOBB(vcount, points, pstride, psides, pmatrix);
+			REAL v = psides[0] * psides[1] * psides[2];
+			if (v < volume)
+			{
+				volume = v;
+				memcpy(matrix, pmatrix, sizeof(REAL) * 16);
+				sides[0] = psides[0];
+				sides[1] = psides[1];
+				sides[2] = psides[2];
+			}
+		}
+	}
+	if (avolume < volume)
+	{
+		fm_identity(matrix);
+		matrix[12] = (bmin[0] + bmax[0]) * 0.5f;
+		matrix[13] = (bmin[1] + bmax[1]) * 0.5f;
+		matrix[14] = (bmin[2] + bmax[2]) * 0.5f;
+		sides[0] = bmax[0] - bmin[0];
+		sides[1] = bmax[1] - bmin[1];
+		sides[2] = bmax[2] - bmin[2];
+	}
+}
+
+void computeBestFitOBB(size_t vcount, const REAL* points, size_t pstride, REAL* sides, REAL* pos, REAL* quat, bool bruteForce)
+{
+	REAL matrix[16];
+	computeBestFitOBB(vcount, points, pstride, sides, matrix, bruteForce);
+	fm_getTranslation(matrix, pos);
+	fm_matrixToQuat(matrix, quat);
+}
+
+void computeBestFitABB(size_t vcount, const REAL* points, size_t pstride, REAL* sides, REAL* pos)
+{
+	REAL bmin[3];
+	REAL bmax[3];
+
+	bmin[0] = points[0];
+	bmin[1] = points[1];
+	bmin[2] = points[2];
+
+	bmax[0] = points[0];
+	bmax[1] = points[1];
+	bmax[2] = points[2];
+
+	const char* cp = (const char*) points;
+	for (size_t i = 0; i < vcount; i++)
+	{
+		const REAL* p = (const REAL*) cp;
+
+		if (p[0] < bmin[0])
+		{
+			bmin[0] = p[0];
+		}
+		if (p[1] < bmin[1])
+		{
+			bmin[1] = p[1];
+		}
+		if (p[2] < bmin[2])
+		{
+			bmin[2] = p[2];
+		}
+
+		if (p[0] > bmax[0])
+		{
+			bmax[0] = p[0];
+		}
+		if (p[1] > bmax[1])
+		{
+			bmax[1] = p[1];
+		}
+		if (p[2] > bmax[2])
+		{
+			bmax[2] = p[2];
+		}
+
+		cp += pstride;
+	}
+
+
+	sides[0] = bmax[0] - bmin[0];
+	sides[1] = bmax[1] - bmin[1];
+	sides[2] = bmax[2] - bmin[2];
+
+	pos[0] = bmin[0] + sides[0] * 0.5f;
+	pos[1] = bmin[1] + sides[1] * 0.5f;
+	pos[2] = bmin[2] + sides[2] * 0.5f;
+
+}
+
+
+REAL  computeBestFitSphere(size_t vcount, const REAL* points, size_t pstride, REAL* center)
+{
+	REAL sides[3];
+	REAL omatrix[16];
+	computeBestFitOBB(vcount, points, pstride, sides, omatrix, true);
+	center[0] = omatrix[12];
+	center[1] = omatrix[13];
+	center[2] = omatrix[14];
+	REAL radius = static_cast<REAL>(sqrt(sides[0] * sides[0] + sides[1] * sides[1] + sides[2] * sides[2]));
+	return radius * 0.5f;
+}
+
+void computeBestFitCapsule(size_t vcount, const REAL* points, size_t pstride, REAL& radius, REAL& height, REAL matrix[16], bool bruteForce)
+{
+	REAL sides[3];
+	REAL omatrix[16];
+	computeBestFitOBB(vcount, points, pstride, sides, omatrix, bruteForce);
+
+	int axis = 0;
+	if (sides[0] > sides[1] && sides[0] > sides[2])
+	{
+		axis = 0;
+	}
+	else if (sides[1] > sides[0] && sides[1] > sides[2])
+	{
+		axis = 1;
+	}
+	else
+	{
+		axis = 2;
+	}
+
+	REAL localTransform[16];
+
+	REAL maxDist = 0;
+	REAL maxLen = 0;
+
+	switch (axis)
+	{
+	case 0:
+	{
+		fm_eulerMatrix(0, 0, FM_PI / 2, localTransform);
+		fm_matrixMultiply(localTransform, omatrix, matrix);
+
+		const unsigned char* scan = (const unsigned char*)points;
+		for (size_t i = 0; i < vcount; i++)
+		{
+			const REAL* p = (const REAL*)scan;
+			REAL t[3];
+			fm_inverseRT(omatrix, p, t);
+			REAL dist = t[1] * t[1] + t[2] * t[2];
+			if (dist > maxDist)
+			{
+				maxDist = dist;
+			}
+			REAL l = (REAL) fabs(t[0]);
+			if (l > maxLen)
+			{
+				maxLen = l;
+			}
+			scan += pstride;
+		}
+	}
+	height = sides[0];
+	break;
+	case 1:
+	{
+		fm_eulerMatrix(0, FM_PI / 2, 0, localTransform);
+		fm_matrixMultiply(localTransform, omatrix, matrix);
+
+		const unsigned char* scan = (const unsigned char*)points;
+		for (size_t i = 0; i < vcount; i++)
+		{
+			const REAL* p = (const REAL*)scan;
+			REAL t[3];
+			fm_inverseRT(omatrix, p, t);
+			REAL dist = t[0] * t[0] + t[2] * t[2];
+			if (dist > maxDist)
+			{
+				maxDist = dist;
+			}
+			REAL l = (REAL) fabs(t[1]);
+			if (l > maxLen)
+			{
+				maxLen = l;
+			}
+			scan += pstride;
+		}
+	}
+	height = sides[1];
+	break;
+	case 2:
+	{
+		fm_eulerMatrix(FM_PI / 2, 0, 0, localTransform);
+		fm_matrixMultiply(localTransform, omatrix, matrix);
+
+		const unsigned char* scan = (const unsigned char*)points;
+		for (size_t i = 0; i < vcount; i++)
+		{
+			const REAL* p = (const REAL*)scan;
+			REAL t[3];
+			fm_inverseRT(omatrix, p, t);
+			REAL dist = t[0] * t[0] + t[1] * t[1];
+			if (dist > maxDist)
+			{
+				maxDist = dist;
+			}
+			REAL l = (REAL) fabs(t[2]);
+			if (l > maxLen)
+			{
+				maxLen = l;
+			}
+			scan += pstride;
+		}
+	}
+	height = sides[2];
+	break;
+	}
+	radius = (REAL)sqrt(maxDist);
+	height = (maxLen * 2) - (radius * 2);
+}
+
+
+void computeBestFitCapsule(size_t vcount, const float* points, size_t pstride, float& radius, float& height, float pos[3], float quat[4], bool bruteForce)
+{
+	REAL matrix[16];
+	computeBestFitCapsule(vcount, points, pstride, radius, height, matrix, bruteForce);
+	fm_getTranslation(matrix, pos);
+	fm_matrixToQuat(matrix, quat);
+}
+
+REAL computeBestFitAABB(size_t vcount, const REAL* points, size_t pstride, REAL* bmin, REAL* bmax) // returns the diagonal distance
+{
+
+	const unsigned char* source = (const unsigned char*) points;
+
+	bmin[0] = points[0];
+	bmin[1] = points[1];
+	bmin[2] = points[2];
+
+	bmax[0] = points[0];
+	bmax[1] = points[1];
+	bmax[2] = points[2];
+
+
+	for (size_t i = 1; i < vcount; i++)
+	{
+		source += pstride;
+		const REAL* p = (const REAL*) source;
+
+		if (p[0] < bmin[0])
+		{
+			bmin[0] = p[0];
+		}
+		if (p[1] < bmin[1])
+		{
+			bmin[1] = p[1];
+		}
+		if (p[2] < bmin[2])
+		{
+			bmin[2] = p[2];
+		}
+
+		if (p[0] > bmax[0])
+		{
+			bmax[0] = p[0];
+		}
+		if (p[1] > bmax[1])
+		{
+			bmax[1] = p[1];
+		}
+		if (p[2] > bmax[2])
+		{
+			bmax[2] = p[2];
+		}
+
+	}
+
+	REAL dx = bmax[0] - bmin[0];
+	REAL dy = bmax[1] - bmin[1];
+	REAL dz = bmax[2] - bmin[2];
+
+	return (REAL) sqrt(dx * dx + dy * dy + dz * dz);
+
+}
+
+
+}; // namespace SharedTools