Blast 1.1.3. See docs/release_notes.txt.v1.1.3_rc1

1 files changed, 5197 insertions, 5197 deletions

diff --git a/sdk/extensions/authoring/source/VHACD/src/FloatMath.inl b/sdk/extensions/authoring/source/VHACD/src/FloatMath.inl
index 832d927..80d38cc 100644..100755
--- a/sdk/extensions/authoring/source/VHACD/src/FloatMath.inl
+++ b/sdk/extensions/authoring/source/VHACD/src/FloatMath.inl
@@ -1,5197 +1,5197 @@
-// a set of routines that let you do common 3d math
-// operations without any vector, matrix, or quaternion
-// classes or templates.
-//
-// a vector (or point) is a 'float *' to 3 floating point numbers.
-// a matrix is a 'float *' to an array of 16 floating point numbers representing a 4x4 transformation matrix compatible with D3D or OGL
-// a quaternion is a 'float *' to 4 floats representing a quaternion x,y,z,w
-//
-
-#pragma warning(disable:4996)
-
-namespace FLOAT_MATH
-{
-
-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);
-
-}
-
-REAL fm_getDeterminant(const REAL matrix[16])
-{
-  REAL tempv[3];
-  REAL p0[3];
-  REAL p1[3];
-  REAL p2[3];
-
-
-	p0[0] = matrix[0*4+0];
-	p0[1] = matrix[0*4+1];
-	p0[2] = matrix[0*4+2];
-
-	p1[0] = matrix[1*4+0];
-	p1[1] = matrix[1*4+1];
-	p1[2] = matrix[1*4+2];
-
-	p2[0] = matrix[2*4+0];
-	p2[1] = matrix[2*4+1];
-	p2[2] = matrix[2*4+2];
-
-  fm_cross(tempv,p1,p2);
-
-  return fm_dot(p0,tempv);
-
-}
-
-REAL fm_squared(REAL x) { return x*x; };
-
-void fm_decomposeTransform(const REAL local_transform[16],REAL trans[3],REAL rot[4],REAL scale[3])
-{
-
-  trans[0] = local_transform[12];
-  trans[1] = local_transform[13];
-  trans[2] = local_transform[14];
-
-  scale[0] = (REAL)sqrt(fm_squared(local_transform[0*4+0]) + fm_squared(local_transform[0*4+1]) + fm_squared(local_transform[0*4+2]));
-  scale[1] = (REAL)sqrt(fm_squared(local_transform[1*4+0]) + fm_squared(local_transform[1*4+1]) + fm_squared(local_transform[1*4+2]));
-  scale[2] = (REAL)sqrt(fm_squared(local_transform[2*4+0]) + fm_squared(local_transform[2*4+1]) + fm_squared(local_transform[2*4+2]));
-
-  REAL m[16];
-  memcpy(m,local_transform,sizeof(REAL)*16);
-
-  REAL sx = 1.0f / scale[0];
-  REAL sy = 1.0f / scale[1];
-  REAL sz = 1.0f / scale[2];
-
-  m[0*4+0]*=sx;
-  m[0*4+1]*=sx;
-  m[0*4+2]*=sx;
-
-  m[1*4+0]*=sy;
-  m[1*4+1]*=sy;
-  m[1*4+2]*=sy;
-
-  m[2*4+0]*=sz;
-  m[2*4+1]*=sz;
-  m[2*4+2]*=sz;
-
-  fm_matrixToQuat(m,rot);
-
-}
-
-void fm_getSubMatrix(int32_t ki,int32_t kj,REAL pDst[16],const REAL matrix[16])
-{
-	int32_t row, col;
-	int32_t dstCol = 0, dstRow = 0;
-
-	for ( col = 0; col < 4; col++ )
-	{
-		if ( col == kj )
-		{
-			continue;
-		}
-		for ( dstRow = 0, row = 0; row < 4; row++ )
-		{
-			if ( row == ki )
-			{
-				continue;
-			}
-			pDst[dstCol*4+dstRow] = matrix[col*4+row];
-			dstRow++;
-		}
-		dstCol++;
-	}
-}
-
-void  fm_inverseTransform(const REAL matrix[16],REAL inverse_matrix[16])
-{
-	REAL determinant = fm_getDeterminant(matrix);
-	determinant = 1.0f / determinant;
-	for (int32_t i = 0; i < 4; i++ )
-	{
-		for (int32_t j = 0; j < 4; j++ )
-		{
-			int32_t sign = 1 - ( ( i + j ) % 2 ) * 2;
-			REAL subMat[16];
-			fm_identity(subMat);
-			fm_getSubMatrix( i, j, subMat, matrix );
-			REAL subDeterminant = fm_getDeterminant(subMat);
-			inverse_matrix[i*4+j] = ( subDeterminant * sign ) * determinant;
-		}
-	}
-}
-
-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_quatToEuler(const REAL quat[4],REAL &ax,REAL &ay,REAL &az)
-{
-	REAL x = quat[0];
-	REAL y = quat[1];
-	REAL z = quat[2];
-	REAL w = quat[3];
-
-	REAL sint	     = (2.0f * w * y) - (2.0f * x * z);
-	REAL cost_temp = 1.0f - (sint * sint);
-	REAL cost	   	 = 0;
-
-	if ( (REAL)fabs(cost_temp) > 0.001f )
-	{
-		cost = (REAL)sqrt( cost_temp );
-	}
-
-	REAL sinv, cosv, sinf, cosf;
-	if ( (REAL)fabs(cost) > 0.001f )
-	{
-    cost = 1.0f / cost;
-		sinv = ((2.0f * y * z) + (2.0f * w * x)) * cost;
-		cosv = (1.0f - (2.0f * x * x) - (2.0f * y * y)) * cost;
-		sinf = ((2.0f * x * y) + (2.0f * w * z)) * cost;
-		cosf = (1.0f - (2.0f * y * y) - (2.0f * z * z)) * cost;
-	}
-	else
-	{
-		sinv = (2.0f * w * x) - (2.0f * y * z);
-		cosv = 1.0f - (2.0f * x * x) - (2.0f * z * z);
-		sinf = 0;
-		cosf = 1.0f;
-	}
-
-	// compute output rotations
-	ax	= (REAL)atan2( sinv, cosv );
-	ay	= (REAL)atan2( sint, cost );
-	az	= (REAL)atan2( sinf, cosf );
-
-}
-
-void fm_eulerToMatrix(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);
-}
-
-void fm_getAABB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *bmin,REAL *bmax)
-{
-
-  const uint8_t *source = (const uint8_t *) 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 (uint32_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];
-
-  }
-}
-
-void  fm_eulerToQuat(const REAL *euler,REAL *quat) // convert euler angles to quaternion.
-{
-  fm_eulerToQuat(euler[0],euler[1],euler[2],quat);
-}
-
-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 = (REAL)cos(roll);
-	REAL cp = (REAL)cos(pitch);
-	REAL cy = (REAL)cos(yaw);
-
-	REAL sr = (REAL)sin(roll);
-	REAL sp = (REAL)sin(pitch);
-	REAL sy = (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_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_quatRotate(const REAL *quat,const REAL *v,REAL *r) // rotate a vector directly by a quaternion.
-{
-  REAL left[4];
-
-	left[0] =   quat[3]*v[0] + quat[1]*v[2] - v[1]*quat[2];
-	left[1] =   quat[3]*v[1] + quat[2]*v[0] - v[2]*quat[0];
-	left[2] =   quat[3]*v[2] + quat[0]*v[1] - v[0]*quat[1];
-	left[3] = - quat[0]*v[0] - quat[1]*v[1] - quat[2]*v[2];
-
-	r[0] = (left[3]*-quat[0]) + (quat[3]*left[0]) + (left[1]*-quat[2]) - (-quat[1]*left[2]);
-	r[1] = (left[3]*-quat[1]) + (quat[3]*left[1]) + (left[2]*-quat[0]) - (-quat[2]*left[0]);
-	r[2] = (left[3]*-quat[2]) + (quat[3]*left[2]) + (left[0]*-quat[1]) - (-quat[0]*left[1]);
-
-}
-
-
-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_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
-		int32_t nxt[3] = {1, 2, 0};
-		REAL  qa[4];
-
-		int32_t i = 0;
-
-		if (matrix[1*4+1] > matrix[0*4+0]) i = 1;
-		if (matrix[2*4+2] > matrix[i*4+i]) i = 2;
-
-		int32_t j = nxt[i];
-		int32_t k = nxt[j];
-
-		REAL s = (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];
-	}
-//	fm_normalizeQuat(quat);
-}
-
-
-REAL fm_sphereVolume(REAL radius) // return's the volume of a sphere of this radius (4/3 PI * R cubed )
-{
-	return (4.0f / 3.0f ) * FM_PI * radius * radius * radius;
-}
-
-
-REAL fm_cylinderVolume(REAL radius,REAL h)
-{
-	return FM_PI * radius * radius *h;
-}
-
-REAL fm_capsuleVolume(REAL radius,REAL h)
-{
-	REAL volume = fm_sphereVolume(radius); // volume of the sphere portion.
-	REAL ch = h-radius*2; // this is the cylinder length
-	if ( ch > 0 )
-	{
-		volume+=fm_cylinderVolume(radius,ch);
-	}
-	return volume;
-}
-
-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_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];
-  }
-}
-
-
-REAL fm_distance(const REAL *p1,const REAL *p2)
-{
-	REAL dx = p1[0] - p2[0];
-	REAL dy = p1[1] - p2[1];
-	REAL dz = p1[2] - p2[2];
-
-	return (REAL)sqrt( dx*dx + dy*dy + dz *dz );
-}
-
-REAL fm_distanceSquared(const REAL *p1,const REAL *p2)
-{
-	REAL dx = p1[0] - p2[0];
-	REAL dy = p1[1] - p2[1];
-	REAL dz = p1[2] - p2[2];
-
-	return dx*dx + dy*dy + dz *dz;
-}
-
-
-REAL fm_distanceSquaredXZ(const REAL *p1,const REAL *p2)
-{
-	REAL dx = p1[0] - p2[0];
-	REAL dz = p1[2] - p2[2];
-
-	return dx*dx +  dz *dz;
-}
-
-
-REAL fm_computePlane(const REAL *A,const REAL *B,const REAL *C,REAL *n) // returns D
-{
-	REAL vx = (B[0] - C[0]);
-	REAL vy = (B[1] - C[1]);
-	REAL vz = (B[2] - C[2]);
-
-	REAL wx = (A[0] - B[0]);
-	REAL wy = (A[1] - B[1]);
-	REAL wz = (A[2] - B[2]);
-
-	REAL vw_x = vy * wz - vz * wy;
-	REAL vw_y = vz * wx - vx * wz;
-	REAL vw_z = vx * wy - vy * wx;
-
-	REAL mag = (REAL)sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
-
-	if ( mag < 0.000001f )
-	{
-		mag = 0;
-	}
-	else
-	{
-		mag = 1.0f/mag;
-	}
-
-	REAL x = vw_x * mag;
-	REAL y = vw_y * mag;
-	REAL z = vw_z * mag;
-
-
-	REAL D = 0.0f - ((x*A[0])+(y*A[1])+(z*A[2]));
-
-  n[0] = x;
-  n[1] = y;
-  n[2] = z;
-
-	return D;
-}
-
-REAL fm_distToPlane(const REAL *plane,const REAL *p) // computes the distance of this point from the plane.
-{
-  return p[0]*plane[0]+p[1]*plane[1]+p[2]*plane[2]+plane[3];
-}
-
-REAL fm_dot(const REAL *p1,const REAL *p2)
-{
-  return p1[0]*p2[0]+p1[1]*p2[1]+p1[2]*p2[2];
-}
-
-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];
-}
-
-void fm_computeNormalVector(REAL *n,const REAL *p1,const REAL *p2)
-{
-  n[0] = p2[0] - p1[0];
-  n[1] = p2[1] - p1[1];
-  n[2] = p2[2] - p1[2];
-  fm_normalize(n);
-}
-
-bool  fm_computeWindingOrder(const REAL *p1,const REAL *p2,const REAL *p3) // returns true if the triangle is clockwise.
-{
-  bool ret = false;
-
-  REAL v1[3];
-  REAL v2[3];
-
-  fm_computeNormalVector(v1,p1,p2); // p2-p1 (as vector) and then normalized
-  fm_computeNormalVector(v2,p1,p3); // p3-p1 (as vector) and then normalized
-
-  REAL cross[3];
-
-  fm_cross(cross, v1, v2 );
-  REAL ref[3] = { 1, 0, 0 };
-
-  REAL d = fm_dot( cross, ref );
-
-
-  if ( d <= 0 )
-    ret = false;
-  else
-    ret = true;
-
-  return ret;
-}
-
-REAL fm_normalize(REAL *n) // normalize this vector
-{
-  REAL dist = (REAL)sqrt(n[0]*n[0] + n[1]*n[1] + n[2]*n[2]);
-  if ( dist > 0.0000001f )
-  {
-    REAL mag = 1.0f / dist;
-    n[0]*=mag;
-    n[1]*=mag;
-    n[2]*=mag;
-  }
-  else
-  {
-    n[0] = 1;
-    n[1] = 0;
-    n[2] = 0;
-  }
-
-  return dist;
-}
-
-
-void  fm_matrixMultiply(const REAL *pA,const REAL *pB,REAL *pM)
-{
-#if 1
-
-  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;
-
-
-#else
-	memset(pM, 0, sizeof(REAL)*16);
-	for(int32_t i=0; i<4; i++ )
-		for(int32_t j=0; j<4; j++ )
-			for(int32_t k=0; k<4; k++ )
-				pM[4*i+j] +=  pA[4*i+k] * pB[4*k+j];
-#endif
-}
-
-
-void  fm_eulerToQuatDX(REAL x,REAL y,REAL z,REAL *quat) // convert euler angles to quaternion using the fucked up DirectX method
-{
-  REAL matrix[16];
-  fm_eulerToMatrix(x,y,z,matrix);
-  fm_matrixToQuat(matrix,quat);
-}
-
-// implementation copied from: http://blogs.msdn.com/mikepelton/archive/2004/10/29/249501.aspx
-void  fm_eulerToMatrixDX(REAL x,REAL y,REAL z,REAL *matrix) // convert euler angles to quaternion using the fucked up DirectX method.
-{
-  fm_identity(matrix);
-  matrix[0*4+0] = (REAL)(cos(z)*cos(y) + sin(z)*sin(x)*sin(y));
-  matrix[0*4+1] = (REAL)(sin(z)*cos(x));
-  matrix[0*4+2] = (REAL)(cos(z)*-sin(y) + sin(z)*sin(x)*cos(y));
-
-  matrix[1*4+0] = (REAL)(-sin(z)*cos(y)+cos(z)*sin(x)*sin(y));
-  matrix[1*4+1] = (REAL)(cos(z)*cos(x));
-  matrix[1*4+2] = (REAL)(sin(z)*sin(y) +cos(z)*sin(x)*cos(y));
-
-  matrix[2*4+0] = (REAL)(cos(x)*sin(y));
-  matrix[2*4+1] = (REAL)(-sin(x));
-  matrix[2*4+2] = (REAL)(cos(x)*cos(y));
-}
-
-
-void  fm_scale(REAL x,REAL y,REAL z,REAL *fscale) // apply scale to the matrix.
-{
-  fscale[0*4+0] = x;
-  fscale[1*4+1] = y;
-  fscale[2*4+2] = z;
-}
-
-
-void  fm_composeTransform(const REAL *position,const REAL *quat,const REAL *scale,REAL *matrix)
-{
-  fm_identity(matrix);
-  fm_quatToMatrix(quat,matrix);
-
-  if ( scale && ( scale[0] != 1 || scale[1] != 1 || scale[2] != 1 ) )
-  {
-    REAL work[16];
-    memcpy(work,matrix,sizeof(REAL)*16);
-    REAL mscale[16];
-    fm_identity(mscale);
-    fm_scale(scale[0],scale[1],scale[2],mscale);
-    fm_matrixMultiply(work,mscale,matrix);
-  }
-
-  matrix[12] = position[0];
-  matrix[13] = position[1];
-  matrix[14] = position[2];
-}
-
-
-void  fm_setTranslation(const REAL *translation,REAL *matrix)
-{
-  matrix[12] = translation[0];
-  matrix[13] = translation[1];
-  matrix[14] = translation[2];
-}
-
-static REAL enorm0_3d ( REAL x0, REAL y0, REAL z0, REAL x1, REAL y1, REAL z1 )
-
-/**********************************************************************/
-
-/*
-Purpose:
-
-ENORM0_3D computes the Euclidean norm of (P1-P0) in 3D.
-
-Modified:
-
-18 April 1999
-
-Author:
-
-John Burkardt
-
-Parameters:
-
-Input, REAL X0, Y0, Z0, X1, Y1, Z1, the coordinates of the points 
-P0 and P1.
-
-Output, REAL ENORM0_3D, the Euclidean norm of (P1-P0).
-*/
-{
-  REAL value;
-
-  value = (REAL)sqrt (
-    ( x1 - x0 ) * ( x1 - x0 ) + 
-    ( y1 - y0 ) * ( y1 - y0 ) + 
-    ( z1 - z0 ) * ( z1 - z0 ) );
-
-  return value;
-}
-
-
-static REAL triangle_area_3d ( REAL x1, REAL y1, REAL z1, REAL x2,REAL y2, REAL z2, REAL x3, REAL y3, REAL z3 )
-
-                        /**********************************************************************/
-
-                        /*
-                        Purpose:
-
-                        TRIANGLE_AREA_3D computes the area of a triangle in 3D.
-
-                        Modified:
-
-                        22 April 1999
-
-                        Author:
-
-                        John Burkardt
-
-                        Parameters:
-
-                        Input, REAL X1, Y1, Z1, X2, Y2, Z2, X3, Y3, Z3, the (X,Y,Z)
-                        coordinates of the corners of the triangle.
-
-                        Output, REAL TRIANGLE_AREA_3D, the area of the triangle.
-                        */
-{
-  REAL a;
-  REAL alpha;
-  REAL area;
-  REAL b;
-  REAL base;
-  REAL c;
-  REAL dot;
-  REAL height;
-  /*
-  Find the projection of (P3-P1) onto (P2-P1).
-  */
-  dot = 
-    ( x2 - x1 ) * ( x3 - x1 ) +
-    ( y2 - y1 ) * ( y3 - y1 ) +
-    ( z2 - z1 ) * ( z3 - z1 );
-
-  base = enorm0_3d ( x1, y1, z1, x2, y2, z2 );
-  /*
-  The height of the triangle is the length of (P3-P1) after its
-  projection onto (P2-P1) has been subtracted.
-  */
-  if ( base == 0.0 ) {
-
-    height = 0.0;
-
-  }
-  else {
-
-    alpha = dot / ( base * base );
-
-    a = x3 - x1 - alpha * ( x2 - x1 );
-    b = y3 - y1 - alpha * ( y2 - y1 );
-    c = z3 - z1 - alpha * ( z2 - z1 );
-
-    height = (REAL)sqrt ( a * a + b * b + c * c );
-
-  }
-
-  area = 0.5f * base * height;
-
-  return area;
-}
-
-
-REAL fm_computeArea(const REAL *p1,const REAL *p2,const REAL *p3)
-{
-  REAL ret = 0;
-
-  ret = triangle_area_3d(p1[0],p1[1],p1[2],p2[0],p2[1],p2[2],p3[0],p3[1],p3[2]);
-
-  return ret;
-}
-
-
-void  fm_lerp(const REAL *p1,const REAL *p2,REAL *dest,REAL lerpValue)
-{
-  dest[0] = ((p2[0] - p1[0])*lerpValue) + p1[0];
-  dest[1] = ((p2[1] - p1[1])*lerpValue) + p1[1];
-  dest[2] = ((p2[2] - p1[2])*lerpValue) + p1[2];
-}
-
-bool fm_pointTestXZ(const REAL *p,const REAL *i,const REAL *j)
-{
-  bool ret = false;
-
-  if (((( i[2] <= p[2] ) && ( p[2]  < j[2] )) || (( j[2] <= p[2] ) && ( p[2]  < i[2] ))) && ( p[0] < (j[0] - i[0]) * (p[2] - i[2]) / (j[2] - i[2]) + i[0]))
-    ret = true;
-
-  return ret;
-};
-
-
-bool  fm_insideTriangleXZ(const REAL *p,const REAL *p1,const REAL *p2,const REAL *p3)
-{
-  bool ret = false;
-
-  int32_t c = 0;
-  if ( fm_pointTestXZ(p,p1,p2) ) c = !c;
-  if ( fm_pointTestXZ(p,p2,p3) ) c = !c;
-  if ( fm_pointTestXZ(p,p3,p1) ) c = !c;
-  if ( c ) ret = true;
-
-  return ret;
-}
-
-bool  fm_insideAABB(const REAL *pos,const REAL *bmin,const REAL *bmax)
-{
-  bool ret = false;
-
-  if ( pos[0] >= bmin[0] && pos[0] <= bmax[0] &&
-       pos[1] >= bmin[1] && pos[1] <= bmax[1] &&
-       pos[2] >= bmin[2] && pos[2] <= bmax[2] )
-    ret = true;
-
-  return ret;
-}
-
-
-uint32_t fm_clipTestPoint(const REAL *bmin,const REAL *bmax,const REAL *pos)
-{
-  uint32_t ret = 0;
-
-  if ( pos[0] < bmin[0] )
-    ret|=FMCS_XMIN;
-  else if ( pos[0] > bmax[0] )
-    ret|=FMCS_XMAX;
-
-  if ( pos[1] < bmin[1] )
-    ret|=FMCS_YMIN;
-  else if ( pos[1] > bmax[1] )
-    ret|=FMCS_YMAX;
-
-  if ( pos[2] < bmin[2] )
-    ret|=FMCS_ZMIN;
-  else if ( pos[2] > bmax[2] )
-    ret|=FMCS_ZMAX;
-
-  return ret;
-}
-
-uint32_t fm_clipTestPointXZ(const REAL *bmin,const REAL *bmax,const REAL *pos) // only tests X and Z, not Y
-{
-  uint32_t ret = 0;
-
-  if ( pos[0] < bmin[0] )
-    ret|=FMCS_XMIN;
-  else if ( pos[0] > bmax[0] )
-    ret|=FMCS_XMAX;
-
-  if ( pos[2] < bmin[2] )
-    ret|=FMCS_ZMIN;
-  else if ( pos[2] > bmax[2] )
-    ret|=FMCS_ZMAX;
-
-  return ret;
-}
-
-uint32_t fm_clipTestAABB(const REAL *bmin,const REAL *bmax,const REAL *p1,const REAL *p2,const REAL *p3,uint32_t &andCode)
-{
-  uint32_t orCode = 0;
-
-  andCode = FMCS_XMIN | FMCS_XMAX | FMCS_YMIN | FMCS_YMAX | FMCS_ZMIN | FMCS_ZMAX;
-
-  uint32_t c = fm_clipTestPoint(bmin,bmax,p1);
-  orCode|=c;
-  andCode&=c;
-
-  c = fm_clipTestPoint(bmin,bmax,p2);
-  orCode|=c;
-  andCode&=c;
-
-  c = fm_clipTestPoint(bmin,bmax,p3);
-  orCode|=c;
-  andCode&=c;
-
-  return orCode;
-}
-
-bool intersect(const REAL *si,const REAL *ei,const REAL *bmin,const REAL *bmax,REAL *time)
-{
-  REAL st,et,fst = 0,fet = 1;
-
-  for (int32_t i = 0; i < 3; i++)
-  {
-    if (*si < *ei)
-    {
-      if (*si > *bmax || *ei < *bmin)
-        return false;
-      REAL di = *ei - *si;
-      st = (*si < *bmin)? (*bmin - *si) / di: 0;
-      et = (*ei > *bmax)? (*bmax - *si) / di: 1;
-    }
-    else
-    {
-      if (*ei > *bmax || *si < *bmin)
-        return false;
-      REAL di = *ei - *si;
-      st = (*si > *bmax)? (*bmax - *si) / di: 0;
-      et = (*ei < *bmin)? (*bmin - *si) / di: 1;
-    }
-
-    if (st > fst) fst = st;
-    if (et < fet) fet = et;
-    if (fet < fst)
-      return false;
-    bmin++; bmax++;
-    si++; ei++;
-  }
-
-  *time = fst;
-  return true;
-}
-
-
-
-bool fm_lineTestAABB(const REAL *p1,const REAL *p2,const REAL *bmin,const REAL *bmax,REAL &time)
-{
-  bool sect = intersect(p1,p2,bmin,bmax,&time);
-  return sect;
-}
-
-
-bool fm_lineTestAABBXZ(const REAL *p1,const REAL *p2,const REAL *bmin,const REAL *bmax,REAL &time)
-{
-  REAL _bmin[3];
-  REAL _bmax[3];
-
-  _bmin[0] = bmin[0];
-  _bmin[1] = -1e9;
-  _bmin[2] = bmin[2];
-
-  _bmax[0] = bmax[0];
-  _bmax[1] = 1e9;
-  _bmax[2] = bmax[2];
-
-  bool sect = intersect(p1,p2,_bmin,_bmax,&time);
-
-  return sect;
-}
-
-void  fm_minmax(const REAL *p,REAL *bmin,REAL *bmax) // accmulate to a min-max value
-{
-
-  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 fm_solveX(const REAL *plane,REAL y,REAL z) // solve for X given this plane equation and the other two components.
-{
-  REAL x = (y*plane[1]+z*plane[2]+plane[3]) / -plane[0];
-  return x;
-}
-
-REAL fm_solveY(const REAL *plane,REAL x,REAL z) // solve for Y given this plane equation and the other two components.
-{
-  REAL y = (x*plane[0]+z*plane[2]+plane[3]) / -plane[1];
-  return y;
-}
-
-
-REAL fm_solveZ(const REAL *plane,REAL x,REAL y) // solve for Y given this plane equation and the other two components.
-{
-  REAL z = (x*plane[0]+y*plane[1]+plane[3]) / -plane[2];
-  return z;
-}
-
-
-void  fm_getAABBCenter(const REAL *bmin,const REAL *bmax,REAL *center)
-{
-  center[0] = (bmax[0]-bmin[0])*0.5f+bmin[0];
-  center[1] = (bmax[1]-bmin[1])*0.5f+bmin[1];
-  center[2] = (bmax[2]-bmin[2])*0.5f+bmin[2];
-}
-
-FM_Axis fm_getDominantAxis(const REAL normal[3])
-{
-  FM_Axis ret = FM_XAXIS;
-
-  REAL x = (REAL)fabs(normal[0]);
-  REAL y = (REAL)fabs(normal[1]);
-  REAL z = (REAL)fabs(normal[2]);
-
-  if ( y > x && y > z )
-    ret = FM_YAXIS;
-  else if ( z > x && z > y )
-    ret = FM_ZAXIS;
-
-  return ret;
-}
-
-
-bool fm_lineSphereIntersect(const REAL *center,REAL radius,const REAL *p1,const REAL *p2,REAL *intersect)
-{
-  bool ret = false;
-
-  REAL dir[3];
-
-  dir[0] = p2[0]-p1[0];
-  dir[1] = p2[1]-p1[1];
-  dir[2] = p2[2]-p1[2];
-
-  REAL distance = (REAL)sqrt( dir[0]*dir[0]+dir[1]*dir[1]+dir[2]*dir[2]);
-
-  if ( distance > 0 )
-  {
-    REAL recip = 1.0f / distance;
-    dir[0]*=recip;
-    dir[1]*=recip;
-    dir[2]*=recip;
-    ret = fm_raySphereIntersect(center,radius,p1,dir,distance,intersect);
-  }
-  else
-  {
-    dir[0] = center[0]-p1[0];
-    dir[1] = center[1]-p1[1];
-    dir[2] = center[2]-p1[2];
-    REAL d2 = dir[0]*dir[0]+dir[1]*dir[1]+dir[2]*dir[2];
-    REAL r2 = radius*radius;
-    if ( d2 < r2 )
-    {
-      ret = true;
-      if ( intersect )
-      {
-        intersect[0] = p1[0];
-        intersect[1] = p1[1];
-        intersect[2] = p1[2];
-      }
-    }
-  }
-  return ret;
-}
-
-#define DOT(p1,p2) (p1[0]*p2[0]+p1[1]*p2[1]+p1[2]*p2[2])
-
-bool fm_raySphereIntersect(const REAL *center,REAL radius,const REAL *pos,const REAL *dir,REAL distance,REAL *intersect)
-{
-  bool ret = false;
-
-  REAL E0[3];
-
-  E0[0] = center[0] - pos[0];
-  E0[1] = center[1] - pos[1];
-  E0[2] = center[2] - pos[2];
-
-  REAL V[3];
-
-  V[0]  = dir[0];
-  V[1]  = dir[1];
-  V[2]  = dir[2];
-
-
-  REAL dist2   = E0[0]*E0[0] + E0[1]*E0[1] + E0[2] * E0[2];
-  REAL radius2 = radius*radius; // radius squared..
-
-  // Bug Fix For Gem, if origin is *inside* the sphere, invert the
-  // direction vector so that we get a valid intersection location.
-  if ( dist2 < radius2 )
-  {
-    V[0]*=-1;
-    V[1]*=-1;
-    V[2]*=-1;
-  }
-
-
-	REAL v = DOT(E0,V);
-
-	REAL disc = radius2 - (dist2 - v*v);
-
-	if (disc > 0.0f)
-	{
-		if ( intersect )
-		{
-		  REAL d = (REAL)sqrt(disc);
-      REAL diff = v-d;
-      if ( diff < distance )
-      {
-        intersect[0] = pos[0]+V[0]*diff;
-        intersect[1] = pos[1]+V[1]*diff;
-        intersect[2] = pos[2]+V[2]*diff;
-        ret = true;
-      }
-    }
-	}
-
-	return ret;
-}
-
-
-void fm_catmullRom(REAL *out_vector,const REAL *p1,const REAL *p2,const REAL *p3,const REAL *p4, const REAL s)
-{
-  REAL s_squared = s * s;
-  REAL s_cubed = s_squared * s;
-
-  REAL coefficient_p1 = -s_cubed + 2*s_squared - s;
-  REAL coefficient_p2 = 3 * s_cubed - 5 * s_squared + 2;
-  REAL coefficient_p3 = -3 * s_cubed +4 * s_squared + s;
-  REAL coefficient_p4 = s_cubed - s_squared;
-
-  out_vector[0] = (coefficient_p1 * p1[0] + coefficient_p2 * p2[0] + coefficient_p3 * p3[0] + coefficient_p4 * p4[0])*0.5f;
-  out_vector[1] = (coefficient_p1 * p1[1] + coefficient_p2 * p2[1] + coefficient_p3 * p3[1] + coefficient_p4 * p4[1])*0.5f;
-  out_vector[2] = (coefficient_p1 * p1[2] + coefficient_p2 * p2[2] + coefficient_p3 * p3[2] + coefficient_p4 * p4[2])*0.5f;
-}
-
-bool fm_intersectAABB(const REAL *bmin1,const REAL *bmax1,const REAL *bmin2,const REAL *bmax2)
-{
-  if ((bmin1[0] > bmax2[0]) || (bmin2[0] > bmax1[0])) return false;
-  if ((bmin1[1] > bmax2[1]) || (bmin2[1] > bmax1[1])) return false;
-  if ((bmin1[2] > bmax2[2]) || (bmin2[2] > bmax1[2])) return false;
-  return true;
-
-}
-
-bool  fm_insideAABB(const REAL *obmin,const REAL *obmax,const REAL *tbmin,const REAL *tbmax) // test if bounding box tbmin/tmbax is fully inside obmin/obmax
-{
-  bool ret = false;
-
-  if ( tbmax[0] <= obmax[0] &&
-       tbmax[1] <= obmax[1] &&
-       tbmax[2] <= obmax[2] &&
-       tbmin[0] >= obmin[0] &&
-       tbmin[1] >= obmin[1] &&
-       tbmin[2] >= obmin[2] ) ret = true;
-
-  return ret;
-}
-
-
-// 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);
-
-  if( d<= -0.99999f ) // 180 about x axis
-  {
-	  if ( fabsf((float)v0[0]) < 0.1f )
-	  {
-		  quat[0] = 0;
-		  quat[1] = v0[2];
-		  quat[2] = -v0[1];
-		  quat[3] = 0;
-	  }
-	  else
-	  {
-		  quat[0] = v0[1];
-		  quat[1] = -v0[0];
-		  quat[2] = 0;
-		  quat[3] = 0;
-	  }
-	  REAL magnitudeSquared = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3];
-	  REAL magnitude = sqrtf((float)magnitudeSquared);
-	  REAL recip = 1.0f / magnitude;
-	  quat[0]*=recip;
-	  quat[1]*=recip;
-	  quat[2]*=recip;
-	  quat[3]*=recip;
-  }
-  else
-  {
-	  REAL s = (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;
-  }
-}
-
-
-REAL fm_distancePointLineSegment(const REAL *Point,const REAL *LineStart,const REAL *LineEnd,REAL *intersection,LineSegmentType &type,REAL epsilon)
-{
-  REAL ret;
-
-  REAL LineMag = fm_distance( LineEnd, LineStart );
-
-  if ( LineMag > 0 )
-  {
-    REAL U = ( ( ( Point[0] - LineStart[0] ) * ( LineEnd[0] - LineStart[0] ) ) + ( ( Point[1] - LineStart[1] ) * ( LineEnd[1] - LineStart[1] ) ) + ( ( Point[2] - LineStart[2] ) * ( LineEnd[2] - LineStart[2] ) ) ) / ( LineMag * LineMag );
-    if( U < 0.0f || U > 1.0f )
-    {
-      REAL d1 = fm_distanceSquared(Point,LineStart);
-      REAL d2 = fm_distanceSquared(Point,LineEnd);
-      if ( d1 <= d2 )
-      {
-        ret = (REAL)sqrt(d1);
-        intersection[0] = LineStart[0];
-        intersection[1] = LineStart[1];
-        intersection[2] = LineStart[2];
-        type = LS_START;
-      }
-      else
-      {
-        ret = (REAL)sqrt(d2);
-        intersection[0] = LineEnd[0];
-        intersection[1] = LineEnd[1];
-        intersection[2] = LineEnd[2];
-        type = LS_END;
-      }
-    }
-    else
-    {
-      intersection[0] = LineStart[0] + U * ( LineEnd[0] - LineStart[0] );
-      intersection[1] = LineStart[1] + U * ( LineEnd[1] - LineStart[1] );
-      intersection[2] = LineStart[2] + U * ( LineEnd[2] - LineStart[2] );
-
-      ret = fm_distance(Point,intersection);
-
-      REAL d1 = fm_distanceSquared(intersection,LineStart);
-      REAL d2 = fm_distanceSquared(intersection,LineEnd);
-	  REAL mag = (epsilon*2)*(epsilon*2);
-
-      if ( d1 < mag ) // if less than 1/100th the total distance, treat is as the 'start'
-      {
-        type = LS_START;
-      }
-      else if ( d2 < mag )
-      {
-        type = LS_END;
-      }
-      else
-      {
-        type = LS_MIDDLE;
-      }
-
-    }
-  }
-  else
-  {
-    ret = LineMag;
-    intersection[0] = LineEnd[0];
-    intersection[1] = LineEnd[1];
-    intersection[2] = LineEnd[2];
-    type = LS_END;
-  }
-
-  return ret;
-}
-
-
-#ifndef BEST_FIT_PLANE_H
-
-#define BEST_FIT_PLANE_H
-
-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 = (REAL)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 int32_t iMaxIter = 32;
-
-    for (int32_t i0 = 0; i0 <3; i0++)
-    {
-      int32_t i1;
-      for (i1 = 0; i1 < iMaxIter; i1++)
-      {
-        int32_t i2;
-        for (i2 = i0; i2 <= (3-2); i2++)
-        {
-          Type fTmp = 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 = (REAL)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 (int32_t 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 = (REAL)sqrt(fCos*fCos+(Type)1.0);
-            m_afSubd[i3+1] = fF*fR;
-            fSin = ((Type)1.0)/fR;
-            fCos *= fSin;
-          }
-          else
-          {
-            fSin = fF/fG;
-            fR = (REAL)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 (int32_t 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 (int32_t i0 = 0, i1; i0 <= 3-2; i0++)
-    {
-      // locate maximum eigenvalue
-      i1 = i0;
-      Type fMax = m_afDiag[i1];
-      int32_t 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 (int32_t 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;
-};
-
-#endif
-
-bool fm_computeBestFitPlane(uint32_t vcount,
-                     const REAL *points,
-                     uint32_t vstride,
-                     const REAL *weights,
-                     uint32_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 (uint32_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 (uint32_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;
-}
-
-
-bool fm_colinear(const REAL a1[3],const REAL a2[3],const REAL b1[3],const REAL b2[3],REAL epsilon)  // true if these two line segments are co-linear.
-{
-  bool ret = false;
-
-  REAL dir1[3];
-  REAL dir2[3];
-
-  dir1[0] = (a2[0] - a1[0]);
-  dir1[1] = (a2[1] - a1[1]);
-  dir1[2] = (a2[2] - a1[2]);
-
-  dir2[0] = (b2[0]-a1[0]) - (b1[0]-a1[0]);
-  dir2[1] = (b2[1]-a1[1]) - (b1[1]-a1[1]);
-  dir2[2] = (b2[2]-a2[2]) - (b1[2]-a2[2]);
-
-  fm_normalize(dir1);
-  fm_normalize(dir2);
-
-  REAL dot = fm_dot(dir1,dir2);
-
-  if ( dot >= epsilon )
-  {
-    ret = true;
-  }
-
-
-  return ret;
-}
-
-bool fm_colinear(const REAL *p1,const REAL *p2,const REAL *p3,REAL epsilon)
-{
-  bool ret = false;
-
-  REAL dir1[3];
-  REAL dir2[3];
-
-  dir1[0] = p2[0] - p1[0];
-  dir1[1] = p2[1] - p1[1];
-  dir1[2] = p2[2] - p1[2];
-
-  dir2[0] = p3[0] - p2[0];
-  dir2[1] = p3[1] - p2[1];
-  dir2[2] = p3[2] - p2[2];
-
-  fm_normalize(dir1);
-  fm_normalize(dir2);
-
-  REAL dot = fm_dot(dir1,dir2);
-
-  if ( dot >= epsilon )
-  {
-    ret = true;
-  }
-
-
-  return ret;
-}
-
-void  fm_initMinMax(const REAL *p,REAL *bmin,REAL *bmax)
-{
-  bmax[0] = bmin[0] = p[0];
-  bmax[1] = bmin[1] = p[1];
-  bmax[2] = bmin[2] = p[2];
-}
-
-IntersectResult fm_intersectLineSegments2d(const REAL *a1,const REAL *a2,const REAL *b1,const REAL *b2,REAL *intersection)
-{
-  IntersectResult ret;
-
-  REAL denom  = ((b2[1] - b1[1])*(a2[0] - a1[0])) - ((b2[0] - b1[0])*(a2[1] - a1[1]));
-  REAL nume_a = ((b2[0] - b1[0])*(a1[1] - b1[1])) - ((b2[1] - b1[1])*(a1[0] - b1[0]));
-  REAL nume_b = ((a2[0] - a1[0])*(a1[1] - b1[1])) - ((a2[1] - a1[1])*(a1[0] - b1[0]));
-  if (denom == 0 )
-  {
-    if(nume_a == 0 && nume_b == 0)
-    {
-      ret = IR_COINCIDENT;
-    }
-    else
-    {
-      ret = IR_PARALLEL;
-    }
-  }
-  else
-  {
-
-    REAL recip = 1 / denom;
-    REAL ua = nume_a * recip;
-    REAL ub = nume_b * recip;
-
-    if(ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1 )
-    {
-      // Get the intersection point.
-      intersection[0] = a1[0] + ua*(a2[0] - a1[0]);
-      intersection[1] = a1[1] + ua*(a2[1] - a1[1]);
-      ret = IR_DO_INTERSECT;
-    }
-    else
-    {
-      ret = IR_DONT_INTERSECT;
-    }
-  }
-  return ret;
-}
-
-IntersectResult fm_intersectLineSegments2dTime(const REAL *a1,const REAL *a2,const REAL *b1,const REAL *b2,REAL &t1,REAL &t2)
-{
-  IntersectResult ret;
-
-  REAL denom  = ((b2[1] - b1[1])*(a2[0] - a1[0])) - ((b2[0] - b1[0])*(a2[1] - a1[1]));
-  REAL nume_a = ((b2[0] - b1[0])*(a1[1] - b1[1])) - ((b2[1] - b1[1])*(a1[0] - b1[0]));
-  REAL nume_b = ((a2[0] - a1[0])*(a1[1] - b1[1])) - ((a2[1] - a1[1])*(a1[0] - b1[0]));
-  if (denom == 0 )
-  {
-    if(nume_a == 0 && nume_b == 0)
-    {
-      ret = IR_COINCIDENT;
-    }
-    else
-    {
-      ret = IR_PARALLEL;
-    }
-  }
-  else
-  {
-
-    REAL recip = 1 / denom;
-    REAL ua = nume_a * recip;
-    REAL ub = nume_b * recip;
-
-    if(ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1 )
-    {
-      t1 = ua;
-      t2 = ub;
-      ret = IR_DO_INTERSECT;
-    }
-    else
-    {
-      ret = IR_DONT_INTERSECT;
-    }
-  }
-  return ret;
-}
-
-//**** Plane Triangle Intersection
-
-
-
-
-
-// assumes that the points are on opposite sides of the plane!
-void fm_intersectPointPlane(const REAL *p1,const REAL *p2,REAL *split,const REAL *plane)
-{
-
-  REAL dp1 = fm_distToPlane(plane,p1);
-
-  REAL dir[3];
-
-  dir[0] = p2[0] - p1[0];
-  dir[1] = p2[1] - p1[1];
-  dir[2] = p2[2] - p1[2];
-
-  REAL dot1 = dir[0]*plane[0] + dir[1]*plane[1] + dir[2]*plane[2];
-  REAL dot2 = dp1 - plane[3];
-
-  REAL    t = -(plane[3] + dot2 ) / dot1;
-
-  split[0] = (dir[0]*t)+p1[0];
-  split[1] = (dir[1]*t)+p1[1];
-  split[2] = (dir[2]*t)+p1[2];
-
-}
-
-PlaneTriResult fm_getSidePlane(const REAL *p,const REAL *plane,REAL epsilon)
-{
-  PlaneTriResult ret = PTR_ON_PLANE;
-
-  REAL d = fm_distToPlane(plane,p);
-
-  if ( d < -epsilon || d > epsilon )
-  {
-    if ( d > 0 )
-  		ret =  PTR_FRONT; // it is 'in front' within the provided epsilon value.
-    else
-      ret = PTR_BACK;
-  }
-
-  return ret;
-}
-
-
-
-#ifndef PLANE_TRIANGLE_INTERSECTION_H
-
-#define PLANE_TRIANGLE_INTERSECTION_H
-
-#define MAXPTS 256
-
-template <class Type> class point
-{
-public:
-
-  void set(const Type *p)
-  {
-    x = p[0];
-    y = p[1];
-    z = p[2];
-  }
-
-  Type x;
-  Type y;
-  Type z;
-};
-
-template <class Type> class plane
-{
-public:
-  plane(const Type *p)
-  {
-    normal.x = p[0];
-    normal.y = p[1];
-    normal.z = p[2];
-    D        = p[3];
-  }
-
-  Type Classify_Point(const point<Type> &p)
-  {
-    return p.x*normal.x + p.y*normal.y + p.z*normal.z + D;
-  }
-
-  point<Type> normal;
-  Type  D;
-};
-
-template <class Type> class polygon
-{
-public:
-  polygon(void)
-  {
-    mVcount = 0;
-  }
-
-  polygon(const Type *p1,const Type *p2,const Type *p3)
-  {
-    mVcount = 3;
-    mVertices[0].set(p1);
-    mVertices[1].set(p2);
-    mVertices[2].set(p3);
-  }
-
-
-  int32_t NumVertices(void) const { return mVcount; };
-
-  const point<Type>& Vertex(int32_t index)
-  {
-    if ( index < 0 ) index+=mVcount;
-    return mVertices[index];
-  };
-
-
-  void set(const point<Type> *pts,int32_t count)
-  {
-    for (int32_t i=0; i<count; i++)
-    {
-      mVertices[i] = pts[i];
-    }
-    mVcount = count;
-  }
-
-
-  void Split_Polygon(polygon<Type> *poly,plane<Type> *part, polygon<Type> &front, polygon<Type> &back)
-  {
-    int32_t   count = poly->NumVertices ();
-    int32_t   out_c = 0, in_c = 0;
-    point<Type> ptA, ptB,outpts[MAXPTS],inpts[MAXPTS];
-    Type sideA, sideB;
-    ptA = poly->Vertex (count - 1);
-    sideA = part->Classify_Point (ptA);
-    for (int32_t i = -1; ++i < count;)
-    {
-      ptB = poly->Vertex(i);
-      sideB = part->Classify_Point(ptB);
-      if (sideB > 0)
-      {
-        if (sideA < 0)
-        {
-  			  point<Type> v;
-          fm_intersectPointPlane(&ptB.x, &ptA.x, &v.x, &part->normal.x );
-          outpts[out_c++] = inpts[in_c++] = v;
-        }
-        outpts[out_c++] = ptB;
-      }
-      else if (sideB < 0)
-      {
-        if (sideA > 0)
-        {
-          point<Type> v;
-          fm_intersectPointPlane(&ptB.x, &ptA.x, &v.x, &part->normal.x );
-          outpts[out_c++] = inpts[in_c++] = v;
-        }
-        inpts[in_c++] = ptB;
-      }
-      else
-         outpts[out_c++] = inpts[in_c++] = ptB;
-      ptA = ptB;
-      sideA = sideB;
-    }
-
-    front.set(&outpts[0], out_c);
-    back.set(&inpts[0], in_c);
-  }
-
-  int32_t           mVcount;
-  point<Type>   mVertices[MAXPTS];
-};
-
-
-
-#endif
-
-static inline void add(const REAL *p,REAL *dest,uint32_t tstride,uint32_t &pcount)
-{
-  char *d = (char *) dest;
-  d = d + pcount*tstride;
-  dest = (REAL *) d;
-  dest[0] = p[0];
-  dest[1] = p[1];
-  dest[2] = p[2];
-  pcount++;
-	assert( pcount <= 4 );
-}
-
-
-PlaneTriResult fm_planeTriIntersection(const REAL *_plane,    // the plane equation in Ax+By+Cz+D format
-                                    const REAL *triangle, // the source triangle.
-                                    uint32_t tstride,  // stride in bytes of the input and output *vertices*
-                                    REAL        epsilon,  // the co-planar epsilon value.
-                                    REAL       *front,    // the triangle in front of the
-                                    uint32_t &fcount,  // number of vertices in the 'front' triangle
-                                    REAL       *back,     // the triangle in back of the plane
-                                    uint32_t &bcount) // the number of vertices in the 'back' triangle.
-{
-
-  fcount = 0;
-  bcount = 0;
-
-  const char *tsource = (const char *) triangle;
-
-  // get the three vertices of the triangle.
-  const REAL *p1     = (const REAL *) (tsource);
-  const REAL *p2     = (const REAL *) (tsource+tstride);
-  const REAL *p3     = (const REAL *) (tsource+tstride*2);
-
-
-  PlaneTriResult r1   = fm_getSidePlane(p1,_plane,epsilon); // compute the side of the plane each vertex is on
-  PlaneTriResult r2   = fm_getSidePlane(p2,_plane,epsilon);
-  PlaneTriResult r3   = fm_getSidePlane(p3,_plane,epsilon);
-
-  // If any of the points lay right *on* the plane....
-  if ( r1 == PTR_ON_PLANE || r2 == PTR_ON_PLANE || r3 == PTR_ON_PLANE )
-  {
-    // If the triangle is completely co-planar, then just treat it as 'front' and return!
-    if ( r1 == PTR_ON_PLANE && r2 == PTR_ON_PLANE && r3 == PTR_ON_PLANE )
-    {
-      add(p1,front,tstride,fcount);
-      add(p2,front,tstride,fcount);
-      add(p3,front,tstride,fcount);
-      return PTR_FRONT;
-    }
-    // Decide to place the co-planar points on the same side as the co-planar point.
-    PlaneTriResult r= PTR_ON_PLANE;
-    if ( r1 != PTR_ON_PLANE )
-      r = r1;
-    else if ( r2 != PTR_ON_PLANE )
-      r = r2;
-    else if ( r3 != PTR_ON_PLANE )
-      r = r3;
-
-    if ( r1 == PTR_ON_PLANE ) r1 = r;
-    if ( r2 == PTR_ON_PLANE ) r2 = r;
-    if ( r3 == PTR_ON_PLANE ) r3 = r;
-
-  }
-
-  if ( r1 == r2 && r1 == r3 ) // if all three vertices are on the same side of the plane.
-  {
-    if ( r1 == PTR_FRONT ) // if all three are in front of the plane, then copy to the 'front' output triangle.
-    {
-      add(p1,front,tstride,fcount);
-      add(p2,front,tstride,fcount);
-      add(p3,front,tstride,fcount);
-    }
-    else
-    {
-      add(p1,back,tstride,bcount); // if all three are in 'back' then copy to the 'back' output triangle.
-      add(p2,back,tstride,bcount);
-      add(p3,back,tstride,bcount);
-    }
-    return r1; // if all three points are on the same side of the plane return result
-  }
-
-
-  polygon<REAL> pi(p1,p2,p3);
-  polygon<REAL>  pfront,pback;
-
-  plane<REAL>    part(_plane);
-
-  pi.Split_Polygon(&pi,&part,pfront,pback);
-
-  for (int32_t i=0; i<pfront.mVcount; i++)
-  {
-    add( &pfront.mVertices[i].x, front, tstride, fcount );
-  }
-
-  for (int32_t i=0; i<pback.mVcount; i++)
-  {
-    add( &pback.mVertices[i].x, back, tstride, bcount );
-  }
-
-  PlaneTriResult ret = PTR_SPLIT;
-
-  if ( fcount < 3 ) fcount = 0;
-  if ( bcount < 3 ) bcount = 0;
-
-  if ( fcount == 0 && bcount )
-    ret = PTR_BACK;
-
-  if ( bcount == 0 && fcount )
-    ret = PTR_FRONT;
-
-
-  return ret;
-}
-
-// computes the OBB for this set of points relative to this transform matrix.
-void computeOBB(uint32_t vcount,const REAL *points,uint32_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 (uint32_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 fm_computeBestFitOBB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *sides,REAL *matrix,bool bruteForce)
-{
-  REAL plane[4];
-  fm_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];
-      }
-    }
-  }
-}
-
-void fm_computeBestFitOBB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *sides,REAL *pos,REAL *quat,bool bruteForce)
-{
-  REAL matrix[16];
-  fm_computeBestFitOBB(vcount,points,pstride,sides,matrix,bruteForce);
-  fm_getTranslation(matrix,pos);
-  fm_matrixToQuat(matrix,quat);
-}
-
-void fm_computeBestFitABB(uint32_t vcount,const REAL *points,uint32_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 (uint32_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;
-
-}
-
-
-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_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);
-}
-
-
-//**********************************************************
-//**********************************************************
-//**** Vertex Welding
-//**********************************************************
-//**********************************************************
-
-#ifndef VERTEX_INDEX_H
-
-#define VERTEX_INDEX_H
-
-namespace VERTEX_INDEX
-{
-
-class KdTreeNode;
-
-typedef std::vector< KdTreeNode * > KdTreeNodeVector;
-
-enum Axes
-{
-  X_AXIS = 0,
-  Y_AXIS = 1,
-  Z_AXIS = 2
-};
-
-class KdTreeFindNode
-{
-public:
-  KdTreeFindNode(void)
-  {
-    mNode = 0;
-    mDistance = 0;
-  }
-  KdTreeNode  *mNode;
-  double        mDistance;
-};
-
-class KdTreeInterface
-{
-public:
-  virtual const double * getPositionDouble(uint32_t index) const = 0;
-  virtual const float  * getPositionFloat(uint32_t index) const = 0;
-};
-
-class KdTreeNode
-{
-public:
-  KdTreeNode(void)
-  {
-    mIndex = 0;
-    mLeft = 0;
-    mRight = 0;
-  }
-
-  KdTreeNode(uint32_t index)
-  {
-    mIndex = index;
-    mLeft = 0;
-    mRight = 0;
-  };
-
-	~KdTreeNode(void)
-  {
-  }
-
-
-  void addDouble(KdTreeNode *node,Axes dim,const KdTreeInterface *iface)
-  {
-    const double *nodePosition = iface->getPositionDouble( node->mIndex );
-    const double *position     = iface->getPositionDouble( mIndex );
-    switch ( dim )
-    {
-      case X_AXIS:
-        if ( nodePosition[0] <= position[0] )
-        {
-          if ( mLeft )
-            mLeft->addDouble(node,Y_AXIS,iface);
-          else
-            mLeft = node;
-        }
-        else
-        {
-          if ( mRight )
-            mRight->addDouble(node,Y_AXIS,iface);
-          else
-            mRight = node;
-        }
-        break;
-      case Y_AXIS:
-        if ( nodePosition[1] <= position[1] )
-        {
-          if ( mLeft )
-            mLeft->addDouble(node,Z_AXIS,iface);
-          else
-            mLeft = node;
-        }
-        else
-        {
-          if ( mRight )
-            mRight->addDouble(node,Z_AXIS,iface);
-          else
-            mRight = node;
-        }
-        break;
-      case Z_AXIS:
-        if ( nodePosition[2] <= position[2] )
-        {
-          if ( mLeft )
-            mLeft->addDouble(node,X_AXIS,iface);
-          else
-            mLeft = node;
-        }
-        else
-        {
-          if ( mRight )
-            mRight->addDouble(node,X_AXIS,iface);
-          else
-            mRight = node;
-        }
-        break;
-    }
-
-  }
-
-
-  void addFloat(KdTreeNode *node,Axes dim,const KdTreeInterface *iface)
-  {
-    const float *nodePosition = iface->getPositionFloat( node->mIndex );
-    const float *position     = iface->getPositionFloat( mIndex );
-    switch ( dim )
-    {
-      case X_AXIS:
-        if ( nodePosition[0] <= position[0] )
-        {
-          if ( mLeft )
-            mLeft->addFloat(node,Y_AXIS,iface);
-          else
-            mLeft = node;
-        }
-        else
-        {
-          if ( mRight )
-            mRight->addFloat(node,Y_AXIS,iface);
-          else
-            mRight = node;
-        }
-        break;
-      case Y_AXIS:
-        if ( nodePosition[1] <= position[1] )
-        {
-          if ( mLeft )
-            mLeft->addFloat(node,Z_AXIS,iface);
-          else
-            mLeft = node;
-        }
-        else
-        {
-          if ( mRight )
-            mRight->addFloat(node,Z_AXIS,iface);
-          else
-            mRight = node;
-        }
-        break;
-      case Z_AXIS:
-        if ( nodePosition[2] <= position[2] )
-        {
-          if ( mLeft )
-            mLeft->addFloat(node,X_AXIS,iface);
-          else
-            mLeft = node;
-        }
-        else
-        {
-          if ( mRight )
-            mRight->addFloat(node,X_AXIS,iface);
-          else
-            mRight = node;
-        }
-        break;
-    }
-
-  }
-
-
-  uint32_t getIndex(void) const { return mIndex; };
-
-  void search(Axes axis,const double *pos,double radius,uint32_t &count,uint32_t maxObjects,KdTreeFindNode *found,const KdTreeInterface *iface)
-  {
-
-    const double *position = iface->getPositionDouble(mIndex);
-
-    double dx = pos[0] - position[0];
-    double dy = pos[1] - position[1];
-    double dz = pos[2] - position[2];
-
-    KdTreeNode *search1 = 0;
-    KdTreeNode *search2 = 0;
-
-    switch ( axis )
-    {
-      case X_AXIS:
-       if ( dx <= 0 )     // JWR  if we are to the left
-       {
-        search1 = mLeft; // JWR  then search to the left
-        if ( -dx < radius )  // JWR  if distance to the right is less than our search radius, continue on the right as well.
-          search2 = mRight;
-       }
-       else
-       {
-         search1 = mRight; // JWR  ok, we go down the left tree
-         if ( dx < radius ) // JWR  if the distance from the right is less than our search radius
-	  			search2 = mLeft;
-        }
-        axis = Y_AXIS;
-        break;
-      case Y_AXIS:
-        if ( dy <= 0 )
-        {
-          search1 = mLeft;
-          if ( -dy < radius )
-    				search2 = mRight;
-        }
-        else
-        {
-          search1 = mRight;
-          if ( dy < radius )
-    				search2 = mLeft;
-        }
-        axis = Z_AXIS;
-        break;
-      case Z_AXIS:
-        if ( dz <= 0 )
-        {
-          search1 = mLeft;
-          if ( -dz < radius )
-    				search2 = mRight;
-        }
-        else
-        {
-          search1 = mRight;
-          if ( dz < radius )
-    				search2 = mLeft;
-        }
-        axis = X_AXIS;
-        break;
-    }
-
-    double r2 = radius*radius;
-    double m  = dx*dx+dy*dy+dz*dz;
-
-    if ( m < r2 )
-    {
-      switch ( count )
-      {
-        case 0:
-          found[count].mNode = this;
-          found[count].mDistance = m;
-          break;
-        case 1:
-          if ( m < found[0].mDistance )
-          {
-            if ( maxObjects == 1 )
-            {
-              found[0].mNode = this;
-              found[0].mDistance = m;
-            }
-            else
-            {
-              found[1] = found[0];
-              found[0].mNode = this;
-              found[0].mDistance = m;
-            }
-          }
-          else if ( maxObjects > 1)
-          {
-            found[1].mNode = this;
-            found[1].mDistance = m;
-          }
-          break;
-        default:
-          {
-            bool inserted = false;
-
-            for (uint32_t i=0; i<count; i++)
-            {
-              if ( m < found[i].mDistance ) // if this one is closer than a pre-existing one...
-              {
-                // insertion sort...
-                uint32_t scan = count;
-                if ( scan >= maxObjects ) scan=maxObjects-1;
-                for (uint32_t j=scan; j>i; j--)
-                {
-                  found[j] = found[j-1];
-                }
-                found[i].mNode = this;
-                found[i].mDistance = m;
-                inserted = true;
-                break;
-              }
-            }
-
-            if ( !inserted && count < maxObjects )
-            {
-              found[count].mNode = this;
-              found[count].mDistance = m;
-            }
-          }
-          break;
-      }
-      count++;
-      if ( count > maxObjects )
-      {
-        count = maxObjects;
-      }
-    }
-
-
-    if ( search1 )
-  		search1->search( axis, pos,radius, count, maxObjects, found, iface);
-
-    if ( search2 )
-	  	search2->search( axis, pos,radius, count, maxObjects, found, iface);
-
-  }
-
-  void search(Axes axis,const float *pos,float radius,uint32_t &count,uint32_t maxObjects,KdTreeFindNode *found,const KdTreeInterface *iface)
-  {
-
-    const float *position = iface->getPositionFloat(mIndex);
-
-    float dx = pos[0] - position[0];
-    float dy = pos[1] - position[1];
-    float dz = pos[2] - position[2];
-
-    KdTreeNode *search1 = 0;
-    KdTreeNode *search2 = 0;
-
-    switch ( axis )
-    {
-      case X_AXIS:
-       if ( dx <= 0 )     // JWR  if we are to the left
-       {
-        search1 = mLeft; // JWR  then search to the left
-        if ( -dx < radius )  // JWR  if distance to the right is less than our search radius, continue on the right as well.
-          search2 = mRight;
-       }
-       else
-       {
-         search1 = mRight; // JWR  ok, we go down the left tree
-         if ( dx < radius ) // JWR  if the distance from the right is less than our search radius
-	  			search2 = mLeft;
-        }
-        axis = Y_AXIS;
-        break;
-      case Y_AXIS:
-        if ( dy <= 0 )
-        {
-          search1 = mLeft;
-          if ( -dy < radius )
-    				search2 = mRight;
-        }
-        else
-        {
-          search1 = mRight;
-          if ( dy < radius )
-    				search2 = mLeft;
-        }
-        axis = Z_AXIS;
-        break;
-      case Z_AXIS:
-        if ( dz <= 0 )
-        {
-          search1 = mLeft;
-          if ( -dz < radius )
-    				search2 = mRight;
-        }
-        else
-        {
-          search1 = mRight;
-          if ( dz < radius )
-    				search2 = mLeft;
-        }
-        axis = X_AXIS;
-        break;
-    }
-
-    float r2 = radius*radius;
-    float m  = dx*dx+dy*dy+dz*dz;
-
-    if ( m < r2 )
-    {
-      switch ( count )
-      {
-        case 0:
-          found[count].mNode = this;
-          found[count].mDistance = m;
-          break;
-        case 1:
-          if ( m < found[0].mDistance )
-          {
-            if ( maxObjects == 1 )
-            {
-              found[0].mNode = this;
-              found[0].mDistance = m;
-            }
-            else
-            {
-              found[1] = found[0];
-              found[0].mNode = this;
-              found[0].mDistance = m;
-            }
-          }
-          else if ( maxObjects > 1)
-          {
-            found[1].mNode = this;
-            found[1].mDistance = m;
-          }
-          break;
-        default:
-          {
-            bool inserted = false;
-
-            for (uint32_t i=0; i<count; i++)
-            {
-              if ( m < found[i].mDistance ) // if this one is closer than a pre-existing one...
-              {
-                // insertion sort...
-                uint32_t scan = count;
-                if ( scan >= maxObjects ) scan=maxObjects-1;
-                for (uint32_t j=scan; j>i; j--)
-                {
-                  found[j] = found[j-1];
-                }
-                found[i].mNode = this;
-                found[i].mDistance = m;
-                inserted = true;
-                break;
-              }
-            }
-
-            if ( !inserted && count < maxObjects )
-            {
-              found[count].mNode = this;
-              found[count].mDistance = m;
-            }
-          }
-          break;
-      }
-      count++;
-      if ( count > maxObjects )
-      {
-        count = maxObjects;
-      }
-    }
-
-
-    if ( search1 )
-  		search1->search( axis, pos,radius, count, maxObjects, found, iface);
-
-    if ( search2 )
-	  	search2->search( axis, pos,radius, count, maxObjects, found, iface);
-
-  }
-
-private:
-
-  void setLeft(KdTreeNode *left) { mLeft = left; };
-  void setRight(KdTreeNode *right) { mRight = right; };
-
-	KdTreeNode *getLeft(void)         { return mLeft; }
-	KdTreeNode *getRight(void)        { return mRight; }
-
-  uint32_t          mIndex;
-  KdTreeNode     *mLeft;
-  KdTreeNode     *mRight;
-};
-
-
-#define MAX_BUNDLE_SIZE 1024  // 1024 nodes at a time, to minimize memory allocation and guarantee that pointers are persistent.
-
-class KdTreeNodeBundle 
-{
-public:
-
-  KdTreeNodeBundle(void)
-  {
-    mNext = 0;
-    mIndex = 0;
-  }
-
-  bool isFull(void) const
-  {
-    return (bool)( mIndex == MAX_BUNDLE_SIZE );
-  }
-
-  KdTreeNode * getNextNode(void)
-  {
-    assert(mIndex<MAX_BUNDLE_SIZE);
-    KdTreeNode *ret = &mNodes[mIndex];
-    mIndex++;
-    return ret;
-  }
-
-  KdTreeNodeBundle  *mNext;
-  uint32_t             mIndex;
-  KdTreeNode         mNodes[MAX_BUNDLE_SIZE];
-};
-
-
-typedef std::vector< double > DoubleVector;
-typedef std::vector< float >  FloatVector;
-
-class KdTree : public KdTreeInterface
-{
-public:
-  KdTree(void)
-  {
-    mRoot = 0;
-    mBundle = 0;
-    mVcount = 0;
-    mUseDouble = false;
-  }
-
-  virtual ~KdTree(void)
-  {
-    reset();
-  }
-
-  const double * getPositionDouble(uint32_t index) const
-  {
-    assert( mUseDouble );
-    assert ( index < mVcount );
-    return  &mVerticesDouble[index*3];
-  }
-
-  const float * getPositionFloat(uint32_t index) const
-  {
-    assert( !mUseDouble );
-    assert ( index < mVcount );
-    return  &mVerticesFloat[index*3];
-  }
-
-  uint32_t search(const double *pos,double radius,uint32_t maxObjects,KdTreeFindNode *found) const
-  {
-    assert( mUseDouble );
-    if ( !mRoot )	return 0;
-    uint32_t count = 0;
-    mRoot->search(X_AXIS,pos,radius,count,maxObjects,found,this);
-    return count;
-  }
-
-  uint32_t search(const float *pos,float radius,uint32_t maxObjects,KdTreeFindNode *found) const
-  {
-    assert( !mUseDouble );
-    if ( !mRoot )	return 0;
-    uint32_t count = 0;
-    mRoot->search(X_AXIS,pos,radius,count,maxObjects,found,this);
-    return count;
-  }
-
-  void reset(void)
-  {
-    mRoot = 0;
-    mVerticesDouble.clear();
-    mVerticesFloat.clear();
-    KdTreeNodeBundle *bundle = mBundle;
-    while ( bundle )
-    {
-      KdTreeNodeBundle *next = bundle->mNext;
-      delete bundle;
-      bundle = next;
-    }
-    mBundle = 0;
-    mVcount = 0;
-  }
-
-  uint32_t add(double x,double y,double z)
-  {
-    assert(mUseDouble);
-    uint32_t ret = mVcount;
-    mVerticesDouble.push_back(x);
-    mVerticesDouble.push_back(y);
-    mVerticesDouble.push_back(z);
-    mVcount++;
-    KdTreeNode *node = getNewNode(ret);
-    if ( mRoot )
-    {
-      mRoot->addDouble(node,X_AXIS,this);
-    }
-    else
-    {
-      mRoot = node;
-    }
-    return ret;
-  }
-
-  uint32_t add(float x,float y,float z)
-  {
-    assert(!mUseDouble);
-    uint32_t ret = mVcount;
-    mVerticesFloat.push_back(x);
-    mVerticesFloat.push_back(y);
-    mVerticesFloat.push_back(z);
-    mVcount++;
-    KdTreeNode *node = getNewNode(ret);
-    if ( mRoot )
-    {
-      mRoot->addFloat(node,X_AXIS,this);
-    }
-    else
-    {
-      mRoot = node;
-    }
-    return ret;
-  }
-
-  KdTreeNode * getNewNode(uint32_t index)
-  {
-    if ( mBundle == 0 )
-    {
-      mBundle = new KdTreeNodeBundle;
-    }
-    if ( mBundle->isFull() )
-    {
-      KdTreeNodeBundle *bundle = new KdTreeNodeBundle;
-      mBundle->mNext = bundle;
-      mBundle = bundle;
-    }
-    KdTreeNode *node = mBundle->getNextNode();
-    new ( node ) KdTreeNode(index);
-    return node;
-  }
-
-  uint32_t getNearest(const double *pos,double radius,bool &_found) const // returns the nearest possible neighbor's index.
-  {
-    assert( mUseDouble );
-    uint32_t ret = 0;
-
-    _found = false;
-    KdTreeFindNode found[1];
-    uint32_t count = search(pos,radius,1,found);
-    if ( count )
-    {
-      KdTreeNode *node = found[0].mNode;
-      ret = node->getIndex();
-      _found = true;
-    }
-    return ret;
-  }
-
-  uint32_t getNearest(const float *pos,float radius,bool &_found) const // returns the nearest possible neighbor's index.
-  {
-    assert( !mUseDouble );
-    uint32_t ret = 0;
-
-    _found = false;
-    KdTreeFindNode found[1];
-    uint32_t count = search(pos,radius,1,found);
-    if ( count )
-    {
-      KdTreeNode *node = found[0].mNode;
-      ret = node->getIndex();
-      _found = true;
-    }
-    return ret;
-  }
-
-  const double * getVerticesDouble(void) const
-  {
-    assert( mUseDouble );
-    const double *ret = 0;
-    if ( !mVerticesDouble.empty() )
-    {
-      ret = &mVerticesDouble[0];
-    }
-    return ret;
-  }
-
-  const float * getVerticesFloat(void) const
-  {
-    assert( !mUseDouble );
-    const float * ret = 0;
-    if ( !mVerticesFloat.empty() )
-    {
-      ret = &mVerticesFloat[0];
-    }
-    return ret;
-  }
-
-  uint32_t getVcount(void) const { return mVcount; };
-
-  void setUseDouble(bool useDouble)
-  {
-    mUseDouble = useDouble;
-  }
-
-private:
-  bool                    mUseDouble;
-  KdTreeNode             *mRoot;
-  KdTreeNodeBundle       *mBundle;
-  uint32_t                  mVcount;
-  DoubleVector            mVerticesDouble;
-  FloatVector             mVerticesFloat;
-};
-
-}; // end of namespace VERTEX_INDEX
-
-class MyVertexIndex : public fm_VertexIndex
-{
-public:
-  MyVertexIndex(double granularity,bool snapToGrid)
-  {
-    mDoubleGranularity = granularity;
-    mFloatGranularity  = (float)granularity;
-    mSnapToGrid        = snapToGrid;
-    mUseDouble         = true;
-    mKdTree.setUseDouble(true);
-  }
-
-  MyVertexIndex(float granularity,bool snapToGrid)
-  {
-    mDoubleGranularity = granularity;
-    mFloatGranularity  = (float)granularity;
-    mSnapToGrid        = snapToGrid;
-    mUseDouble         = false;
-    mKdTree.setUseDouble(false);
-  }
-
-  virtual ~MyVertexIndex(void)
-  {
-
-  }
-
-
-  double snapToGrid(double p)
-  {
-    double m = fmod(p,mDoubleGranularity);
-    p-=m;
-    return p;
-  }
-
-  float snapToGrid(float p)
-  {
-    float m = fmodf(p,mFloatGranularity);
-    p-=m;
-    return p;
-  }
-
-  uint32_t    getIndex(const float *_p,bool &newPos)  // get index for a vector float
-  {
-    uint32_t ret;
-
-    if ( mUseDouble )
-    {
-      double p[3];
-      p[0] = _p[0];
-      p[1] = _p[1];
-      p[2] = _p[2];
-      return getIndex(p,newPos);
-    }
-
-    newPos = false;
-
-    float p[3];
-
-    if ( mSnapToGrid )
-    {
-      p[0] = snapToGrid(_p[0]);
-      p[1] = snapToGrid(_p[1]);
-      p[2] = snapToGrid(_p[2]);
-    }
-    else
-    {
-      p[0] = _p[0];
-      p[1] = _p[1];
-      p[2] = _p[2];
-    }
-
-    bool found;
-    ret = mKdTree.getNearest(p,mFloatGranularity,found);
-    if ( !found )
-    {
-      newPos = true;
-      ret = mKdTree.add(p[0],p[1],p[2]);
-    }
-
-
-    return ret;
-  }
-
-  uint32_t    getIndex(const double *_p,bool &newPos)  // get index for a vector double
-  {
-    uint32_t ret;
-
-    if ( !mUseDouble )
-    {
-      float p[3];
-      p[0] = (float)_p[0];
-      p[1] = (float)_p[1];
-      p[2] = (float)_p[2];
-      return getIndex(p,newPos);
-    }
-
-    newPos = false;
-
-    double p[3];
-
-    if ( mSnapToGrid )
-    {
-      p[0] = snapToGrid(_p[0]);
-      p[1] = snapToGrid(_p[1]);
-      p[2] = snapToGrid(_p[2]);
-    }
-    else
-    {
-      p[0] = _p[0];
-      p[1] = _p[1];
-      p[2] = _p[2];
-    }
-
-    bool found;
-    ret = mKdTree.getNearest(p,mDoubleGranularity,found);
-    if ( !found )
-    {
-      newPos = true;
-      ret = mKdTree.add(p[0],p[1],p[2]);
-    }
-
-
-    return ret;
-  }
-
-  const float *   getVerticesFloat(void) const
-  {
-    const float * ret = 0;
-
-    assert( !mUseDouble );
-
-    ret = mKdTree.getVerticesFloat();
-
-    return ret;
-  }
-
-  const double *  getVerticesDouble(void) const
-  {
-    const double * ret = 0;
-
-    assert( mUseDouble );
-
-    ret = mKdTree.getVerticesDouble();
-
-    return ret;
-  }
-
-  const float *   getVertexFloat(uint32_t index) const
-  {
-    const float * ret  = 0;
-    assert( !mUseDouble );
-#ifdef _DEBUG
-    uint32_t vcount = mKdTree.getVcount();
-    assert( index < vcount );
-#endif
-    ret =  mKdTree.getVerticesFloat();
-    ret = &ret[index*3];
-    return ret;
-  }
-
-  const double *   getVertexDouble(uint32_t index) const
-  {
-    const double * ret = 0;
-    assert( mUseDouble );
-#ifdef _DEBUG
-    uint32_t vcount = mKdTree.getVcount();
-    assert( index < vcount );
-#endif
-    ret =  mKdTree.getVerticesDouble();
-    ret = &ret[index*3];
-
-    return ret;
-  }
-
-  uint32_t    getVcount(void) const
-  {
-    return mKdTree.getVcount();
-  }
-
-  bool isDouble(void) const
-  {
-    return mUseDouble;
-  }
-
-
-  bool            saveAsObj(const char *fname,uint32_t tcount,uint32_t *indices)
-  {
-    bool ret = false;
-
-
-    FILE *fph = fopen(fname,"wb");
-    if ( fph )
-    {
-      ret = true;
-
-      uint32_t vcount    = getVcount();
-      if ( mUseDouble )
-      {
-        const double *v  = getVerticesDouble();
-        for (uint32_t i=0; i<vcount; i++)
-        {
-          fprintf(fph,"v %0.9f %0.9f %0.9f\r\n", (float)v[0], (float)v[1], (float)v[2] );
-          v+=3;
-        }
-      }
-      else
-      {
-        const float *v  = getVerticesFloat();
-        for (uint32_t i=0; i<vcount; i++)
-        {
-          fprintf(fph,"v %0.9f %0.9f %0.9f\r\n", v[0], v[1], v[2] );
-          v+=3;
-        }
-      }
-
-      for (uint32_t i=0; i<tcount; i++)
-      {
-        uint32_t i1 = *indices++;
-        uint32_t i2 = *indices++;
-        uint32_t i3 = *indices++;
-        fprintf(fph,"f %d %d %d\r\n", i1+1, i2+1, i3+1 );
-      }
-      fclose(fph);
-    }
-
-    return ret;
-  }
-
-private:
-  bool    mUseDouble:1;
-  bool    mSnapToGrid:1;
-  double  mDoubleGranularity;
-  float   mFloatGranularity;
-  VERTEX_INDEX::KdTree  mKdTree;
-};
-
-fm_VertexIndex * fm_createVertexIndex(double granularity,bool snapToGrid) // create an indexed vertex system for doubles
-{
-  MyVertexIndex *ret = new MyVertexIndex(granularity,snapToGrid);
-  return static_cast< fm_VertexIndex *>(ret);
-}
-
-fm_VertexIndex * fm_createVertexIndex(float granularity,bool snapToGrid)  // create an indexed vertext system for floats
-{
-  MyVertexIndex *ret = new MyVertexIndex(granularity,snapToGrid);
-  return static_cast< fm_VertexIndex *>(ret);
-}
-
-void          fm_releaseVertexIndex(fm_VertexIndex *vindex)
-{
-  MyVertexIndex *m = static_cast< MyVertexIndex *>(vindex);
-  delete m;
-}
-
-#endif   // END OF VERTEX WELDING CODE
-
-
-REAL fm_computeBestFitAABB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *bmin,REAL *bmax) // returns the diagonal distance
-{
-
-  const uint8_t *source = (const uint8_t *) 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 (uint32_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 );
-
-}
-
-
-
-/* a = b - c */
-#define vector(a,b,c) \
-	(a)[0] = (b)[0] - (c)[0];	\
-	(a)[1] = (b)[1] - (c)[1];	\
-	(a)[2] = (b)[2] - (c)[2];
-
-
-
-#define innerProduct(v,q) \
-		((v)[0] * (q)[0] + \
-		(v)[1] * (q)[1] + \
-		(v)[2] * (q)[2])
-
-#define crossProduct(a,b,c) \
-	(a)[0] = (b)[1] * (c)[2] - (c)[1] * (b)[2]; \
-	(a)[1] = (b)[2] * (c)[0] - (c)[2] * (b)[0]; \
-	(a)[2] = (b)[0] * (c)[1] - (c)[0] * (b)[1];
-
-
-bool fm_lineIntersectsTriangle(const REAL *rayStart,const REAL *rayEnd,const REAL *p1,const REAL *p2,const REAL *p3,REAL *sect)
-{
-	REAL dir[3];
-
-  dir[0] = rayEnd[0] - rayStart[0];
-  dir[1] = rayEnd[1] - rayStart[1];
-  dir[2] = rayEnd[2] - rayStart[2];
-
-  REAL d = (REAL)sqrt(dir[0]*dir[0] + dir[1]*dir[1] + dir[2]*dir[2]);
-  REAL r = 1.0f / d;
-
-  dir[0]*=r;
-  dir[1]*=r;
-  dir[2]*=r;
-
-
-  REAL t;
-
-	bool ret = fm_rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t );
-
-	if ( ret )
-	{
-		if ( t > d )
-		{
-			sect[0] = rayStart[0] + dir[0]*t;
-			sect[1] = rayStart[1] + dir[1]*t;
-			sect[2] = rayStart[2] + dir[2]*t;
-		}
-		else
-		{
-			ret = false;
-		}
-	}
-
-  return ret;
-}
-
-
-
-bool fm_rayIntersectsTriangle(const REAL *p,const REAL *d,const REAL *v0,const REAL *v1,const REAL *v2,REAL &t)
-{
-	REAL e1[3],e2[3],h[3],s[3],q[3];
-	REAL a,f,u,v;
-
-	vector(e1,v1,v0);
-	vector(e2,v2,v0);
-	crossProduct(h,d,e2);
-	a = innerProduct(e1,h);
-
-	if (a > -0.00001 && a < 0.00001)
-		return(false);
-
-	f = 1/a;
-	vector(s,p,v0);
-	u = f * (innerProduct(s,h));
-
-	if (u < 0.0 || u > 1.0)
-		return(false);
-
-	crossProduct(q,s,e1);
-	v = f * innerProduct(d,q);
-	if (v < 0.0 || u + v > 1.0)
-		return(false);
-	// at this stage we can compute t to find out where
-	// the intersection point is on the line
-	t = f * innerProduct(e2,q);
-	if (t > 0) // ray intersection
-		return(true);
-	else // this means that there is a line intersection
-		 // but not a ray intersection
-		 return (false);
-}
-
-
-inline REAL det(const REAL *p1,const REAL *p2,const REAL *p3)
-{
-  return  p1[0]*p2[1]*p3[2] + p2[0]*p3[1]*p1[2] + p3[0]*p1[1]*p2[2] -p1[0]*p3[1]*p2[2] - p2[0]*p1[1]*p3[2] - p3[0]*p2[1]*p1[2];
-}
-
-
-REAL  fm_computeMeshVolume(const REAL *vertices,uint32_t tcount,const uint32_t *indices)
-{
-	REAL volume = 0;
-
-	for (uint32_t i=0; i<tcount; i++,indices+=3)
-	{
-  	const REAL *p1 = &vertices[ indices[0]*3 ];
-		const REAL *p2 = &vertices[ indices[1]*3 ];
-		const REAL *p3 = &vertices[ indices[2]*3 ];
-		volume+=det(p1,p2,p3); // compute the volume of the tetrahedran relative to the origin.
-	}
-
-	volume*=(1.0f/6.0f);
-	if ( volume < 0 )
-		volume*=-1;
-	return volume;
-}
-
-
-const REAL * fm_getPoint(const REAL *points,uint32_t pstride,uint32_t index)
-{
-  const uint8_t *scan = (const uint8_t *)points;
-  scan+=(index*pstride);
-  return (REAL *)scan;
-}
-
-
-bool fm_insideTriangle(REAL Ax, REAL Ay,
-                      REAL Bx, REAL By,
-                      REAL Cx, REAL Cy,
-                      REAL Px, REAL Py)
-
-{
-  REAL ax, ay, bx, by, cx, cy, apx, apy, bpx, bpy, cpx, cpy;
-  REAL cCROSSap, bCROSScp, aCROSSbp;
-
-  ax = Cx - Bx;  ay = Cy - By;
-  bx = Ax - Cx;  by = Ay - Cy;
-  cx = Bx - Ax;  cy = By - Ay;
-  apx= Px - Ax;  apy= Py - Ay;
-  bpx= Px - Bx;  bpy= Py - By;
-  cpx= Px - Cx;  cpy= Py - Cy;
-
-  aCROSSbp = ax*bpy - ay*bpx;
-  cCROSSap = cx*apy - cy*apx;
-  bCROSScp = bx*cpy - by*cpx;
-
-  return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f));
-}
-
-
-REAL fm_areaPolygon2d(uint32_t pcount,const REAL *points,uint32_t pstride)
-{
-  int32_t n = (int32_t)pcount;
-
-  REAL A=0.0f;
-  for(int32_t p=n-1,q=0; q<n; p=q++)
-  {
-    const REAL *p1 = fm_getPoint(points,pstride,p);
-    const REAL *p2 = fm_getPoint(points,pstride,q);
-    A+= p1[0]*p2[1] - p2[0]*p1[1];
-  }
-  return A*0.5f;
-}
-
-
-bool  fm_pointInsidePolygon2d(uint32_t pcount,const REAL *points,uint32_t pstride,const REAL *point,uint32_t xindex,uint32_t yindex)
-{
-  uint32_t j = pcount-1;
-  int32_t oddNodes = 0;
-
-  REAL x = point[xindex];
-  REAL y = point[yindex];
-
-  for (uint32_t i=0; i<pcount; i++)
-  {
-    const REAL *p1 = fm_getPoint(points,pstride,i);
-    const REAL *p2 = fm_getPoint(points,pstride,j);
-
-    REAL x1 = p1[xindex];
-    REAL y1 = p1[yindex];
-
-    REAL x2 = p2[xindex];
-    REAL y2 = p2[yindex];
-
-    if ( y1 < y && y2 >= y ||  y2 < y && y1 >= y )
-    {
-      if (x1+(y-y1)/(y2-y1)*(x2-x1)<x)
-      {
-        oddNodes = 1-oddNodes;
-      }
-    }
-    j = i;
-  }
-
-  return oddNodes ? true : false;
-}
-
-
-uint32_t fm_consolidatePolygon(uint32_t pcount,const REAL *points,uint32_t pstride,REAL *_dest,REAL epsilon) // collapses co-linear edges.
-{
-  uint32_t ret = 0;
-
-
-  if ( pcount >= 3 )
-  {
-    const REAL *prev = fm_getPoint(points,pstride,pcount-1);
-    const REAL *current = points;
-    const REAL *next    = fm_getPoint(points,pstride,1);
-    REAL *dest = _dest;
-
-    for (uint32_t i=0; i<pcount; i++)
-    {
-
-      next = (i+1)==pcount ? points : next;
-
-      if ( !fm_colinear(prev,current,next,epsilon) )
-      {
-        dest[0] = current[0];
-        dest[1] = current[1];
-        dest[2] = current[2];
-
-        dest+=3;
-        ret++;
-      }
-
-      prev = current;
-      current+=3;
-      next+=3;
-
-    }
-  }
-
-  return ret;
-}
-
-
-#ifndef RECT3D_TEMPLATE
-
-#define RECT3D_TEMPLATE
-
-template <class T> class Rect3d
-{
-public:
-  Rect3d(void) { };
-
-  Rect3d(const T *bmin,const T *bmax)
-  {
-
-    mMin[0] = bmin[0];
-    mMin[1] = bmin[1];
-    mMin[2] = bmin[2];
-
-    mMax[0] = bmax[0];
-    mMax[1] = bmax[1];
-    mMax[2] = bmax[2];
-
-  }
-
-  void SetMin(const T *bmin)
-  {
-    mMin[0] = bmin[0];
-    mMin[1] = bmin[1];
-    mMin[2] = bmin[2];
-  }
-
-  void SetMax(const T *bmax)
-  {
-    mMax[0] = bmax[0];
-    mMax[1] = bmax[1];
-    mMax[2] = bmax[2];
-  }
-
-	void SetMin(T x,T y,T z)
-	{
-		mMin[0] = x;
-		mMin[1] = y;
-		mMin[2] = z;
-	}
-
-	void SetMax(T x,T y,T z)
-	{
-		mMax[0] = x;
-		mMax[1] = y;
-		mMax[2] = z;
-	}
-
-  T mMin[3];
-  T mMax[3];
-};
-
-#endif
-
-void splitRect(uint32_t axis,
-						   const Rect3d<REAL> &source,
-							 Rect3d<REAL> &b1,
-							 Rect3d<REAL> &b2,
-							 const REAL *midpoint)
-{
-	switch ( axis )
-	{
-		case 0:
-			b1.SetMin(source.mMin);
-			b1.SetMax( midpoint[0], source.mMax[1], source.mMax[2] );
-
-			b2.SetMin( midpoint[0], source.mMin[1], source.mMin[2] );
-			b2.SetMax(source.mMax);
-
-			break;
-		case 1:
-			b1.SetMin(source.mMin);
-			b1.SetMax( source.mMax[0], midpoint[1], source.mMax[2] );
-
-			b2.SetMin( source.mMin[0], midpoint[1], source.mMin[2] );
-			b2.SetMax(source.mMax);
-
-			break;
-		case 2:
-			b1.SetMin(source.mMin);
-			b1.SetMax( source.mMax[0], source.mMax[1], midpoint[2] );
-
-			b2.SetMin( source.mMin[0], source.mMin[1], midpoint[2] );
-			b2.SetMax(source.mMax);
-
-			break;
-	}
-}
-
-bool fm_computeSplitPlane(uint32_t vcount,
-                          const REAL *vertices,
-                          uint32_t /* tcount */,
-                          const uint32_t * /* indices */,
-                          REAL *plane)
-{
-
-  REAL sides[3];
-  REAL matrix[16];
-
-  fm_computeBestFitOBB( vcount, vertices, sizeof(REAL)*3, sides, matrix );
-
-  REAL bmax[3];
-  REAL bmin[3];
-
-  bmax[0] = sides[0]*0.5f;
-  bmax[1] = sides[1]*0.5f;
-  bmax[2] = sides[2]*0.5f;
-
-  bmin[0] = -bmax[0];
-  bmin[1] = -bmax[1];
-  bmin[2] = -bmax[2];
-
-
-  REAL dx = sides[0];
-  REAL dy = sides[1];
-  REAL dz = sides[2];
-
-
-	uint32_t axis = 0;
-
-	if ( dy > dx )
-	{
-		axis = 1;
-	}
-
-	if ( dz > dx && dz > dy )
-	{
-		axis = 2;
-	}
-
-  REAL p1[3];
-  REAL p2[3];
-  REAL p3[3];
-
-  p3[0] = p2[0] = p1[0] = bmin[0] + dx*0.5f;
-  p3[1] = p2[1] = p1[1] = bmin[1] + dy*0.5f;
-  p3[2] = p2[2] = p1[2] = bmin[2] + dz*0.5f;
-
-  Rect3d<REAL> b(bmin,bmax);
-
-  Rect3d<REAL> b1,b2;
-
-  splitRect(axis,b,b1,b2,p1);
-
-
-  switch ( axis )
-  {
-    case 0:
-      p2[1] = bmin[1];
-      p2[2] = bmin[2];
-
-      if ( dz > dy )
-      {
-        p3[1] = bmax[1];
-        p3[2] = bmin[2];
-      }
-      else
-      {
-        p3[1] = bmin[1];
-        p3[2] = bmax[2];
-      }
-
-      break;
-    case 1:
-      p2[0] = bmin[0];
-      p2[2] = bmin[2];
-
-      if ( dx > dz )
-      {
-        p3[0] = bmax[0];
-        p3[2] = bmin[2];
-      }
-      else
-      {
-        p3[0] = bmin[0];
-        p3[2] = bmax[2];
-      }
-
-      break;
-    case 2:
-      p2[0] = bmin[0];
-      p2[1] = bmin[1];
-
-      if ( dx > dy )
-      {
-        p3[0] = bmax[0];
-        p3[1] = bmin[1];
-      }
-      else
-      {
-        p3[0] = bmin[0];
-        p3[1] = bmax[1];
-      }
-
-      break;
-  }
-
-  REAL tp1[3];
-  REAL tp2[3];
-  REAL tp3[3];
-
-  fm_transform(matrix,p1,tp1);
-  fm_transform(matrix,p2,tp2);
-  fm_transform(matrix,p3,tp3);
-
-	plane[3] = fm_computePlane(tp1,tp2,tp3,plane);
-
-  return true;
-
-}
-
-#pragma warning(disable:4100)
-
-void fm_nearestPointInTriangle(const REAL * /*nearestPoint*/,const REAL * /*p1*/,const REAL * /*p2*/,const REAL * /*p3*/,REAL * /*nearest*/)
-{
-
-}
-
-static REAL Partial(const REAL *a,const REAL *p) 
-{
-	return (a[0]*p[1]) - (p[0]*a[1]);
-}
-
-REAL  fm_areaTriangle(const REAL *p0,const REAL *p1,const REAL *p2)
-{
-  REAL A = Partial(p0,p1);
-	A+= Partial(p1,p2);
-	A+= Partial(p2,p0);
-	return A*0.5f;
-}
-
-void fm_subtract(const REAL *A,const REAL *B,REAL *diff) // compute A-B and store the result in 'diff'
-{
-  diff[0] = A[0]-B[0];
-  diff[1] = A[1]-B[1];
-  diff[2] = A[2]-B[2];
-}
-
-
-void  fm_multiplyTransform(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_multiply(REAL *A,REAL scaler)
-{
-  A[0]*=scaler;
-  A[1]*=scaler;
-  A[2]*=scaler;
-}
-
-void fm_add(const REAL *A,const REAL *B,REAL *sum)
-{
-  sum[0] = A[0]+B[0];
-  sum[1] = A[1]+B[1];
-  sum[2] = A[2]+B[2];
-}
-
-void fm_copy3(const REAL *source,REAL *dest)
-{
-  dest[0] = source[0];
-  dest[1] = source[1];
-  dest[2] = source[2];
-}
-
-
-uint32_t  fm_copyUniqueVertices(uint32_t vcount,const REAL *input_vertices,REAL *output_vertices,uint32_t tcount,const uint32_t *input_indices,uint32_t *output_indices)
-{
-  uint32_t ret = 0;
-
-  REAL *vertices = (REAL *)malloc(sizeof(REAL)*vcount*3);
-  memcpy(vertices,input_vertices,sizeof(REAL)*vcount*3);
-  REAL *dest = output_vertices;
-
-  uint32_t *reindex = (uint32_t *)malloc(sizeof(uint32_t)*vcount);
-  memset(reindex,0xFF,sizeof(uint32_t)*vcount);
-
-  uint32_t icount = tcount*3;
-
-  for (uint32_t i=0; i<icount; i++)
-  {
-    uint32_t index = *input_indices++;
-
-    assert( index < vcount );
-
-    if ( reindex[index] == 0xFFFFFFFF )
-    {
-      *output_indices++ = ret;
-      reindex[index] = ret;
-      const REAL *pos = &vertices[index*3];
-      dest[0] = pos[0];
-      dest[1] = pos[1];
-      dest[2] = pos[2];
-      dest+=3;
-      ret++;
-    }
-    else
-    {
-      *output_indices++ = reindex[index];
-    }
-  }
-  free(vertices);
-  free(reindex);
-  return ret;
-}
-
-bool    fm_isMeshCoplanar(uint32_t tcount,const uint32_t *indices,const REAL *vertices,bool doubleSided) // returns true if this collection of indexed triangles are co-planar!
-{
-  bool ret = true;
-
-  if ( tcount > 0 )
-  {
-    uint32_t i1 = indices[0];
-    uint32_t i2 = indices[1];
-    uint32_t i3 = indices[2];
-    const REAL *p1 = &vertices[i1*3];
-    const REAL *p2 = &vertices[i2*3];
-    const REAL *p3 = &vertices[i3*3];
-    REAL plane[4];
-    plane[3] = fm_computePlane(p1,p2,p3,plane);
-    const uint32_t *scan = &indices[3];
-    for (uint32_t i=1; i<tcount; i++)
-    {
-      i1 = *scan++;
-      i2 = *scan++;
-      i3 = *scan++;
-      p1 = &vertices[i1*3];
-      p2 = &vertices[i2*3];
-      p3 = &vertices[i3*3];
-      REAL _plane[4];
-      _plane[3] = fm_computePlane(p1,p2,p3,_plane);
-      if ( !fm_samePlane(plane,_plane,0.01f,0.001f,doubleSided) )
-      {
-        ret = false;
-        break;
-      }
-    }
-  }
-  return ret;
-}
-
-
-bool fm_samePlane(const REAL p1[4],const REAL p2[4],REAL normalEpsilon,REAL dEpsilon,bool doubleSided)
-{
-  bool ret = false;
-
-  REAL diff = (REAL) fabs(p1[3]-p2[3]);
-  if ( diff < dEpsilon ) // if the plane -d  co-efficient is within our epsilon
-  {
-    REAL dot = fm_dot(p1,p2); // compute the dot-product of the vector normals.
-    if ( doubleSided ) dot = (REAL)fabs(dot);
-    REAL dmin = 1 - normalEpsilon;
-    REAL dmax = 1 + normalEpsilon;
-    if ( dot >= dmin && dot <= dmax )
-    {
-      ret = true; // then the plane equation is for practical purposes identical.
-    }
-  }
-
-  return ret;
-}
-
-
-void  fm_initMinMax(REAL bmin[3],REAL bmax[3])
-{
-  bmin[0] = FLT_MAX;
-  bmin[1] = FLT_MAX;
-  bmin[2] = FLT_MAX;
-
-  bmax[0] = -FLT_MAX;
-  bmax[1] = -FLT_MAX;
-  bmax[2] = -FLT_MAX;
-}
-
-void fm_inflateMinMax(REAL bmin[3], REAL bmax[3], REAL ratio)
-{
-	REAL inflate = fm_distance(bmin, bmax)*0.5f*ratio;
-
-	bmin[0] -= inflate;
-	bmin[1] -= inflate;
-	bmin[2] -= inflate;
-
-	bmax[0] += inflate;
-	bmax[1] += inflate;
-	bmax[2] += inflate;
-}
-
-#ifndef TESSELATE_H
-
-#define TESSELATE_H
-
-typedef std::vector< uint32_t > UintVector;
-
-class Myfm_Tesselate : public fm_Tesselate
-{
-public:
-  virtual ~Myfm_Tesselate(void)
-  {
-
-  }
-
-  const uint32_t * tesselate(fm_VertexIndex *vindex,uint32_t tcount,const uint32_t *indices,float longEdge,uint32_t maxDepth,uint32_t &outcount)
-  {
-    const uint32_t *ret = 0;
-
-    mMaxDepth = maxDepth;
-    mLongEdge  = longEdge*longEdge;
-    mLongEdgeD = mLongEdge;
-    mVertices = vindex;
-
-    if ( mVertices->isDouble() )
-    {
-      uint32_t vcount = mVertices->getVcount();
-      double *vertices = (double *)malloc(sizeof(double)*vcount*3);
-      memcpy(vertices,mVertices->getVerticesDouble(),sizeof(double)*vcount*3);
-
-      for (uint32_t i=0; i<tcount; i++)
-      {
-        uint32_t i1 = *indices++;
-        uint32_t i2 = *indices++;
-        uint32_t i3 = *indices++;
-
-        const double *p1 = &vertices[i1*3];
-        const double *p2 = &vertices[i2*3];
-        const double *p3 = &vertices[i3*3];
-
-        tesselate(p1,p2,p3,0);
-
-      }
-      free(vertices);
-    }
-    else
-    {
-      uint32_t vcount = mVertices->getVcount();
-      float *vertices = (float *)malloc(sizeof(float)*vcount*3);
-      memcpy(vertices,mVertices->getVerticesFloat(),sizeof(float)*vcount*3);
-
-
-      for (uint32_t i=0; i<tcount; i++)
-      {
-        uint32_t i1 = *indices++;
-        uint32_t i2 = *indices++;
-        uint32_t i3 = *indices++;
-
-        const float *p1 = &vertices[i1*3];
-        const float *p2 = &vertices[i2*3];
-        const float *p3 = &vertices[i3*3];
-
-        tesselate(p1,p2,p3,0);
-
-      }
-      free(vertices);
-    }
-
-    outcount = (uint32_t)(mIndices.size()/3);
-    ret = &mIndices[0];
-
-
-    return ret;
-  }
-
-  void tesselate(const float *p1,const float *p2,const float *p3,uint32_t recurse)
-  {
-  	bool split = false;
-  	float l1,l2,l3;
-
-    l1 = l2 = l3 = 0;
-
-  	if ( recurse < mMaxDepth )
-  	{
-  	  l1 = fm_distanceSquared(p1,p2);
-    	l2 = fm_distanceSquared(p2,p3);
-    	l3 = fm_distanceSquared(p3,p1);
-
-  	  if (  l1 > mLongEdge || l2 > mLongEdge || l3 > mLongEdge )
-  	  	split = true;
-
-    }
-
-    if ( split )
-  	{
-  		uint32_t edge;
-
-  		if ( l1 >= l2 && l1 >= l3 )
-  			edge = 0;
-  		else if ( l2 >= l1 && l2 >= l3 )
-  			edge = 1;
-  		else
-  			edge = 2;
-
-			float splits[3];
-
-  		switch ( edge )
-  		{
-  			case 0:
-  				{
-            fm_lerp(p1,p2,splits,0.5f);
-            tesselate(p1,splits,p3, recurse+1 );
-            tesselate(splits,p2,p3, recurse+1 );
-  				}
-  				break;
-  			case 1:
-  				{
-            fm_lerp(p2,p3,splits,0.5f);
-            tesselate(p1,p2,splits, recurse+1 );
-            tesselate(p1,splits,p3, recurse+1 );
-  				}
-  				break;
-  			case 2:
-  				{
-  					fm_lerp(p3,p1,splits,0.5f);
-            tesselate(p1,p2,splits, recurse+1 );
-            tesselate(splits,p2,p3, recurse+1 );
-  				}
-  				break;
-  		}
-  	}
-  	else
-  	{
-      bool newp;
-
-      uint32_t i1 = mVertices->getIndex(p1,newp);
-      uint32_t i2 = mVertices->getIndex(p2,newp);
-      uint32_t i3 = mVertices->getIndex(p3,newp);
-
-      mIndices.push_back(i1);
-      mIndices.push_back(i2);
-      mIndices.push_back(i3);
-    }
-
-  }
-
-  void tesselate(const double *p1,const double *p2,const double *p3,uint32_t recurse)
-  {
-  	bool split = false;
-  	double l1,l2,l3;
-
-    l1 = l2 = l3 = 0;
-
-  	if ( recurse < mMaxDepth )
-  	{
-  	  l1 = fm_distanceSquared(p1,p2);
-    	l2 = fm_distanceSquared(p2,p3);
-    	l3 = fm_distanceSquared(p3,p1);
-
-  	  if (  l1 > mLongEdgeD || l2 > mLongEdgeD || l3 > mLongEdgeD )
-  	  	split = true;
-
-    }
-
-    if ( split )
-  	{
-  		uint32_t edge;
-
-  		if ( l1 >= l2 && l1 >= l3 )
-  			edge = 0;
-  		else if ( l2 >= l1 && l2 >= l3 )
-  			edge = 1;
-  		else
-  			edge = 2;
-
-			double splits[3];
-
-  		switch ( edge )
-  		{
-  			case 0:
-  				{
-            fm_lerp(p1,p2,splits,0.5);
-            tesselate(p1,splits,p3, recurse+1 );
-            tesselate(splits,p2,p3, recurse+1 );
-  				}
-  				break;
-  			case 1:
-  				{
-            fm_lerp(p2,p3,splits,0.5);
-            tesselate(p1,p2,splits, recurse+1 );
-            tesselate(p1,splits,p3, recurse+1 );
-  				}
-  				break;
-  			case 2:
-  				{
-  					fm_lerp(p3,p1,splits,0.5);
-            tesselate(p1,p2,splits, recurse+1 );
-            tesselate(splits,p2,p3, recurse+1 );
-  				}
-  				break;
-  		}
-  	}
-  	else
-  	{
-      bool newp;
-
-      uint32_t i1 = mVertices->getIndex(p1,newp);
-      uint32_t i2 = mVertices->getIndex(p2,newp);
-      uint32_t i3 = mVertices->getIndex(p3,newp);
-
-      mIndices.push_back(i1);
-      mIndices.push_back(i2);
-      mIndices.push_back(i3);
-    }
-
-  }
-
-private:
-  float           mLongEdge;
-  double          mLongEdgeD;
-  fm_VertexIndex *mVertices;
-  UintVector    mIndices;
-  uint32_t          mMaxDepth;
-};
-
-fm_Tesselate * fm_createTesselate(void)
-{
-  Myfm_Tesselate *m = new Myfm_Tesselate;
-  return static_cast< fm_Tesselate * >(m);
-}
-
-void           fm_releaseTesselate(fm_Tesselate *t)
-{
-  Myfm_Tesselate *m = static_cast< Myfm_Tesselate *>(t);
-  delete m;
-}
-
-#endif
-
-
-#ifndef RAY_ABB_INTERSECT
-
-#define RAY_ABB_INTERSECT
-
-//! Integer representation of a floating-point value.
-#define IR(x)	((uint32_t&)x)
-
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-/**
-*	A method to compute a ray-AABB intersection.
-*	Original code by Andrew Woo, from "Graphics Gems", Academic Press, 1990
-*	Optimized code by Pierre Terdiman, 2000 (~20-30% faster on my Celeron 500)
-*	Epsilon value added by Klaus Hartmann. (discarding it saves a few cycles only)
-*
-*	Hence this version is faster as well as more robust than the original one.
-*
-*	Should work provided:
-*	1) the integer representation of 0.0f is 0x00000000
-*	2) the sign bit of the float is the most significant one
-*
-*	Report bugs: [email protected]
-*
-*	\param		aabb		[in] the axis-aligned bounding box
-*	\param		origin		[in] ray origin
-*	\param		dir			[in] ray direction
-*	\param		coord		[out] impact coordinates
-*	\return		true if ray intersects AABB
-*/
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-#define RAYAABB_EPSILON 0.00001f
-bool fm_intersectRayAABB(const float MinB[3],const float MaxB[3],const float origin[3],const float dir[3],float coord[3])
-{
-  bool Inside = true;
-  float MaxT[3];
-  MaxT[0]=MaxT[1]=MaxT[2]=-1.0f;
-
-  // Find candidate planes.
-  for(uint32_t i=0;i<3;i++)
-  {
-    if(origin[i] < MinB[i])
-    {
-      coord[i]	= MinB[i];
-      Inside		= false;
-
-      // Calculate T distances to candidate planes
-      if(IR(dir[i]))	MaxT[i] = (MinB[i] - origin[i]) / dir[i];
-    }
-    else if(origin[i] > MaxB[i])
-    {
-      coord[i]	= MaxB[i];
-      Inside		= false;
-
-      // Calculate T distances to candidate planes
-      if(IR(dir[i]))	MaxT[i] = (MaxB[i] - origin[i]) / dir[i];
-    }
-  }
-
-  // Ray origin inside bounding box
-  if(Inside)
-  {
-    coord[0] = origin[0];
-    coord[1] = origin[1];
-    coord[2] = origin[2];
-    return true;
-  }
-
-  // Get largest of the maxT's for final choice of intersection
-  uint32_t WhichPlane = 0;
-  if(MaxT[1] > MaxT[WhichPlane])	WhichPlane = 1;
-  if(MaxT[2] > MaxT[WhichPlane])	WhichPlane = 2;
-
-  // Check final candidate actually inside box
-  if(IR(MaxT[WhichPlane])&0x80000000) return false;
-
-  for(uint32_t i=0;i<3;i++)
-  {
-    if(i!=WhichPlane)
-    {
-      coord[i] = origin[i] + MaxT[WhichPlane] * dir[i];
-#ifdef RAYAABB_EPSILON
-      if(coord[i] < MinB[i] - RAYAABB_EPSILON || coord[i] > MaxB[i] + RAYAABB_EPSILON)	return false;
-#else
-      if(coord[i] < MinB[i] || coord[i] > MaxB[i])	return false;
-#endif
-    }
-  }
-  return true;	// ray hits box
-}
-
-bool fm_intersectLineSegmentAABB(const float bmin[3],const float bmax[3],const float p1[3],const float p2[3],float intersect[3])
-{
-  bool ret = false;
-
-  float dir[3];
-  dir[0] = p2[0] - p1[0];
-  dir[1] = p2[1] - p1[1];
-  dir[2] = p2[2] - p1[2];
-  float dist = fm_normalize(dir);
-  if ( dist > RAYAABB_EPSILON )
-  {
-    ret = fm_intersectRayAABB(bmin,bmax,p1,dir,intersect);
-    if ( ret )
-    {
-      float d = fm_distanceSquared(p1,intersect);
-      if ( d  > (dist*dist) )
-      {
-        ret = false;
-      }
-    }
-  }
-  return ret;
-}
-
-#endif
-
-#ifndef OBB_TO_AABB
-
-#define OBB_TO_AABB
-
-#pragma warning(disable:4100)
-
-void    fm_OBBtoAABB(const float /*obmin*/[3],const float /*obmax*/[3],const float /*matrix*/[16],float /*abmin*/[3],float /*abmax*/[3])
-{
-  assert(0); // not yet implemented.
-}
-
-
-const REAL * computePos(uint32_t index,const REAL *vertices,uint32_t vstride)
-{
-  const char *tmp = (const char *)vertices;
-  tmp+=(index*vstride);
-  return (const REAL*)tmp;
-}
-
-void computeNormal(uint32_t index,REAL *normals,uint32_t nstride,const REAL *normal)
-{
-  char *tmp = (char *)normals;
-  tmp+=(index*nstride);
-  REAL *dest = (REAL *)tmp;
-  dest[0]+=normal[0];
-  dest[1]+=normal[1];
-  dest[2]+=normal[2];
-}
-
-void fm_computeMeanNormals(uint32_t vcount,       // the number of vertices
-                           const REAL *vertices,     // the base address of the vertex position data.
-                           uint32_t vstride,      // the stride between position data.
-                           REAL *normals,            // the base address  of the destination for mean vector normals
-                           uint32_t nstride,      // the stride between normals
-                           uint32_t tcount,       // the number of triangles
-                           const uint32_t *indices)     // the triangle indices
-{
-
-  // Step #1 : Zero out the vertex normals
-  char *dest = (char *)normals;
-  for (uint32_t i=0; i<vcount; i++)
-  {
-    REAL *n = (REAL *)dest;
-    n[0] = 0;
-    n[1] = 0;
-    n[2] = 0;
-    dest+=nstride;
-  }
-
-  // Step #2 : Compute the face normals and accumulate them
-  const uint32_t *scan = indices;
-  for (uint32_t i=0; i<tcount; i++)
-  {
-
-    uint32_t i1 = *scan++;
-    uint32_t i2 = *scan++;
-    uint32_t i3 = *scan++;
-
-    const REAL *p1 = computePos(i1,vertices,vstride);
-    const REAL *p2 = computePos(i2,vertices,vstride);
-    const REAL *p3 = computePos(i3,vertices,vstride);
-
-    REAL normal[3];
-    fm_computePlane(p3,p2,p1,normal);
-
-    computeNormal(i1,normals,nstride,normal);
-    computeNormal(i2,normals,nstride,normal);
-    computeNormal(i3,normals,nstride,normal);
-  }
-
-
-  // Normalize the accumulated normals
-  dest = (char *)normals;
-  for (uint32_t i=0; i<vcount; i++)
-  {
-    REAL *n = (REAL *)dest;
-    fm_normalize(n);
-    dest+=nstride;
-  }
-
-}
-
-#endif
-
-
-#define BIGNUMBER 100000000.0  		/* hundred million */
-
-static inline void Set(REAL *n,REAL x,REAL y,REAL z)
-{
-	n[0] = x;
-	n[1] = y;
-	n[2] = z;
-};
-
-static inline void Copy(REAL *dest,const REAL *source)
-{
-	dest[0] = source[0];
-	dest[1] = source[1];
-	dest[2] = source[2];
-}
-
-
-REAL  fm_computeBestFitSphere(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *center)
-{
-	REAL radius;
-	REAL radius2;
-
-	REAL xmin[3];
-	REAL xmax[3];
-	REAL ymin[3];
-	REAL ymax[3];
-	REAL zmin[3];
-	REAL zmax[3];
-	REAL dia1[3];
-	REAL dia2[3];
-
-	/* FIRST PASS: find 6 minima/maxima points */
-	Set(xmin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
-	Set(xmax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
-	Set(ymin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
-	Set(ymax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
-	Set(zmin,BIGNUMBER,BIGNUMBER,BIGNUMBER);
-	Set(zmax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);
-
-	{
-		const char *scan = (const char *)points;
-		for (uint32_t i=0; i<vcount; i++)
-		{
-			const REAL *caller_p = (const REAL *)scan;
-			if (caller_p[0]<xmin[0])
-				Copy(xmin,caller_p); /* New xminimum point */
-			if (caller_p[0]>xmax[0])
-				Copy(xmax,caller_p);
-			if (caller_p[1]<ymin[1])
-				Copy(ymin,caller_p);
-			if (caller_p[1]>ymax[1])
-				Copy(ymax,caller_p);
-			if (caller_p[2]<zmin[2])
-				Copy(zmin,caller_p);
-			if (caller_p[2]>zmax[2])
-				Copy(zmax,caller_p);
-			scan+=pstride;
-		}
-	}
-
-	/* Set xspan = distance between the 2 points xmin & xmax (squared) */
-	REAL dx = xmax[0] - xmin[0];
-	REAL dy = xmax[1] - xmin[1];
-	REAL dz = xmax[2] - xmin[2];
-	REAL xspan = dx*dx + dy*dy + dz*dz;
-
-/* Same for y & z spans */
-	dx = ymax[0] - ymin[0];
-	dy = ymax[1] - ymin[1];
-	dz = ymax[2] - ymin[2];
-	REAL yspan = dx*dx + dy*dy + dz*dz;
-
-	dx = zmax[0] - zmin[0];
-	dy = zmax[1] - zmin[1];
-	dz = zmax[2] - zmin[2];
-	REAL zspan = dx*dx + dy*dy + dz*dz;
-
-	/* Set points dia1 & dia2 to the maximally separated pair */
-	Copy(dia1,xmin);
-	Copy(dia2,xmax); /* assume xspan biggest */
-	REAL maxspan = xspan;
-
-	if (yspan>maxspan)
-	{
-		maxspan = yspan;
-		Copy(dia1,ymin);
-		Copy(dia2,ymax);
-	}
-
-	if (zspan>maxspan)
-	{
-		maxspan = zspan;
-		Copy(dia1,zmin);
-		Copy(dia2,zmax);
-	}
-
-
-	/* dia1,dia2 is a diameter of initial sphere */
-	/* calc initial center */
-	center[0] = (dia1[0]+dia2[0])*0.5f;
-	center[1] = (dia1[1]+dia2[1])*0.5f;
-	center[2] = (dia1[2]+dia2[2])*0.5f;
-
-	/* calculate initial radius**2 and radius */
-
-	dx = dia2[0]-center[0]; /* x component of radius vector */
-	dy = dia2[1]-center[1]; /* y component of radius vector */
-	dz = dia2[2]-center[2]; /* z component of radius vector */
-
-	radius2 = dx*dx + dy*dy + dz*dz;
-	radius = REAL(sqrt(radius2));
-
-	/* SECOND PASS: increment current sphere */
-	{
-		const char *scan = (const char *)points;
-		for (uint32_t i=0; i<vcount; i++)
-		{
-			const REAL *caller_p = (const REAL *)scan;
-			dx = caller_p[0]-center[0];
-			dy = caller_p[1]-center[1];
-			dz = caller_p[2]-center[2];
-			REAL old_to_p_sq = dx*dx + dy*dy + dz*dz;
-			if (old_to_p_sq > radius2) 	/* do r**2 test first */
-			{ 	/* this point is outside of current sphere */
-				REAL old_to_p = REAL(sqrt(old_to_p_sq));
-				/* calc radius of new sphere */
-				radius = (radius + old_to_p) * 0.5f;
-				radius2 = radius*radius; 	/* for next r**2 compare */
-				REAL old_to_new = old_to_p - radius;
-				/* calc center of new sphere */
-				REAL recip = 1.0f /old_to_p;
-				REAL cx = (radius*center[0] + old_to_new*caller_p[0]) * recip;
-				REAL cy = (radius*center[1] + old_to_new*caller_p[1]) * recip;
-				REAL cz = (radius*center[2] + old_to_new*caller_p[2]) * recip;
-				Set(center,cx,cy,cz);
-				scan+=pstride;
-			}
-		}
-	}
-	return radius;
-}
-
-
-void fm_computeBestFitCapsule(uint32_t vcount,const REAL *points,uint32_t pstride,REAL &radius,REAL &height,REAL matrix[16],bool bruteForce)
-{
-  REAL sides[3];
-  REAL omatrix[16];
-  fm_computeBestFitOBB(vcount,points,pstride,sides,omatrix,bruteForce);
-
-  int32_t 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 uint8_t *scan = (const uint8_t *)points;
-        for (uint32_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 uint8_t *scan = (const uint8_t *)points;
-        for (uint32_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 uint8_t *scan = (const uint8_t *)points;
-        for (uint32_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);
-}
-
-
-//************* Triangulation
-
-#ifndef TRIANGULATE_H
-
-#define TRIANGULATE_H
-
-typedef uint32_t TU32;
-
-class TVec
-{
-public:
-	TVec(double _x,double _y,double _z) { x = _x; y = _y; z = _z; };
-	TVec(void) { };
-
-  double x;
-  double y;
-  double z;
-};
-
-typedef std::vector< TVec >  TVecVector;
-typedef std::vector< TU32 >  TU32Vector;
-
-class CTriangulator
-{
-public:
-    ///     Default constructor
-    CTriangulator();
-
-    ///     Default destructor
-    virtual ~CTriangulator();
-
-    ///     Triangulates the contour
-    void triangulate(TU32Vector &indices);
-
-    ///     Returns the given point in the triangulator array
-    inline TVec get(const TU32 id) { return mPoints[id]; }
-
-    virtual void reset(void)
-    {
-        mInputPoints.clear();
-        mPoints.clear();
-        mIndices.clear();
-    }
-
-    virtual void addPoint(double x,double y,double z)
-    {
-        TVec v(x,y,z);
-        // update bounding box...
-        if ( mInputPoints.empty() )
-        {
-            mMin = v;
-            mMax = v;
-        }
-        else
-        {
-            if ( x < mMin.x ) mMin.x = x;
-            if ( y < mMin.y ) mMin.y = y;
-            if ( z < mMin.z ) mMin.z = z;
-
-            if ( x > mMax.x ) mMax.x = x;
-            if ( y > mMax.y ) mMax.y = y;
-            if ( z > mMax.z ) mMax.z = z;
-        }
-        mInputPoints.push_back(v);
-    }
-
-    // Triangulation happens in 2d.  We could inverse transform the polygon around the normal direction, or we just use the two most signficant axes
-    // Here we find the two longest axes and use them to triangulate.  Inverse transforming them would introduce more doubleing point error and isn't worth it.
-    virtual uint32_t * triangulate(uint32_t &tcount,double epsilon)
-    {
-        uint32_t *ret = 0;
-        tcount = 0;
-        mEpsilon = epsilon;
-
-        if ( !mInputPoints.empty() )
-        {
-            mPoints.clear();
-
-          double dx = mMax.x - mMin.x; // locate the first, second and third longest edges and store them in i1, i2, i3
-          double dy = mMax.y - mMin.y;
-          double dz = mMax.z - mMin.z;
-
-          uint32_t i1,i2,i3;
-
-          if ( dx > dy && dx > dz )
-          {
-              i1 = 0;
-              if ( dy > dz )
-              {
-                  i2 = 1;
-                  i3 = 2;
-              }
-              else
-              {
-                  i2 = 2;
-                  i3 = 1;
-              }
-          }
-          else if ( dy > dx && dy > dz )
-          {
-              i1 = 1;
-              if ( dx > dz )
-              {
-                  i2 = 0;
-                  i3 = 2;
-              }
-              else
-              {
-                  i2 = 2;
-                  i3 = 0;
-              }
-          }
-          else
-          {
-              i1 = 2;
-              if ( dx > dy )
-              {
-                  i2 = 0;
-                  i3 = 1;
-              }
-              else
-              {
-                  i2 = 1;
-                  i3 = 0;
-              }
-          }
-
-          uint32_t pcount = (uint32_t)mInputPoints.size();
-          const double *points = &mInputPoints[0].x;
-          for (uint32_t i=0; i<pcount; i++)
-          {
-            TVec v( points[i1], points[i2], points[i3] );
-            mPoints.push_back(v);
-            points+=3;
-          }
-
-          mIndices.clear();
-          triangulate(mIndices);
-          tcount = (uint32_t)mIndices.size()/3;
-          if ( tcount )
-          {
-              ret = &mIndices[0];
-          }
-        }
-        return ret;
-    }
-
-    virtual const double * getPoint(uint32_t index)
-    {
-        return &mInputPoints[index].x;
-    }
-
-
-private:
-    double                  mEpsilon;
-    TVec                   mMin;
-    TVec                   mMax;
-    TVecVector             mInputPoints;
-    TVecVector             mPoints;
-    TU32Vector             mIndices;
-
-    ///     Tests if a point is inside the given triangle
-    bool _insideTriangle(const TVec& A, const TVec& B, const TVec& C,const TVec& P);
-
-    ///     Returns the area of the contour
-    double _area();
-
-    bool _snip(int32_t u, int32_t v, int32_t w, int32_t n, int32_t *V);
-
-    ///     Processes the triangulation
-    void _process(TU32Vector &indices);
-
-};
-
-///     Default constructor
-CTriangulator::CTriangulator(void)
-{
-}
-
-///     Default destructor
-CTriangulator::~CTriangulator()
-{
-}
-
-///     Triangulates the contour
-void CTriangulator::triangulate(TU32Vector &indices)
-{
-    _process(indices);
-}
-
-///     Processes the triangulation
-void CTriangulator::_process(TU32Vector &indices)
-{
-    const int32_t n = (const int32_t)mPoints.size();
-    if (n < 3)
-        return;
-    int32_t *V = (int32_t *)malloc(sizeof(int32_t)*n);
-
-	bool flipped = false;
-
-    if (0.0f < _area())
-    {
-        for (int32_t v = 0; v < n; v++)
-            V[v] = v;
-    }
-    else
-    {
-		flipped = true;
-        for (int32_t v = 0; v < n; v++)
-            V[v] = (n - 1) - v;
-    }
-
-    int32_t nv = n;
-    int32_t count = 2 * nv;
-    for (int32_t m = 0, v = nv - 1; nv > 2;)
-    {
-        if (0 >= (count--))
-            return;
-
-        int32_t u = v;
-        if (nv <= u)
-            u = 0;
-        v = u + 1;
-        if (nv <= v)
-            v = 0;
-        int32_t w = v + 1;
-        if (nv <= w)
-            w = 0;
-
-        if (_snip(u, v, w, nv, V))
-        {
-            int32_t a, b, c, s, t;
-            a = V[u];
-            b = V[v];
-            c = V[w];
-			if ( flipped )
-			{
-				indices.push_back(a);
-				indices.push_back(b);
-				indices.push_back(c);
-			}
-			else
-			{
-				indices.push_back(c);
-				indices.push_back(b);
-				indices.push_back(a);
-			}
-            m++;
-            for (s = v, t = v + 1; t < nv; s++, t++)
-                V[s] = V[t];
-            nv--;
-            count = 2 * nv;
-        }
-    }
-
-    free(V);
-}
-
-///     Returns the area of the contour
-double CTriangulator::_area()
-{
-    int32_t n = (uint32_t)mPoints.size();
-    double A = 0.0f;
-    for (int32_t p = n - 1, q = 0; q < n; p = q++)
-    {
-        const TVec &pval = mPoints[p];
-        const TVec &qval = mPoints[q];
-        A += pval.x * qval.y - qval.x * pval.y;
-    }
-	A*=0.5f;
-    return A;
-}
-
-bool CTriangulator::_snip(int32_t u, int32_t v, int32_t w, int32_t n, int32_t *V)
-{
-    int32_t p;
-
-    const TVec &A = mPoints[ V[u] ];
-    const TVec &B = mPoints[ V[v] ];
-    const TVec &C = mPoints[ V[w] ];
-
-    if (mEpsilon > (((B.x - A.x) * (C.y - A.y)) - ((B.y - A.y) * (C.x - A.x))) )
-        return false;
-
-    for (p = 0; p < n; p++)
-    {
-        if ((p == u) || (p == v) || (p == w))
-            continue;
-        const TVec &P = mPoints[ V[p] ];
-        if (_insideTriangle(A, B, C, P))
-            return false;
-    }
-    return true;
-}
-
-///     Tests if a point is inside the given triangle
-bool CTriangulator::_insideTriangle(const TVec& A, const TVec& B, const TVec& C,const TVec& P)
-{
-    double ax, ay, bx, by, cx, cy, apx, apy, bpx, bpy, cpx, cpy;
-    double cCROSSap, bCROSScp, aCROSSbp;
-
-    ax = C.x - B.x;  ay = C.y - B.y;
-    bx = A.x - C.x;  by = A.y - C.y;
-    cx = B.x - A.x;  cy = B.y - A.y;
-    apx = P.x - A.x;  apy = P.y - A.y;
-    bpx = P.x - B.x;  bpy = P.y - B.y;
-    cpx = P.x - C.x;  cpy = P.y - C.y;
-
-    aCROSSbp = ax * bpy - ay * bpx;
-    cCROSSap = cx * apy - cy * apx;
-    bCROSScp = bx * cpy - by * cpx;
-
-    return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f));
-}
-
-class Triangulate : public fm_Triangulate
-{
-public:
-  Triangulate(void)
-  {
-    mPointsFloat = 0;
-    mPointsDouble = 0;
-  }
-
-  virtual ~Triangulate(void)
-  {
-    reset();
-  }
-  void reset(void)
-  {
-    free(mPointsFloat);
-    free(mPointsDouble);
-    mPointsFloat = 0;
-    mPointsDouble = 0;
-  }
-
-  virtual const double *       triangulate3d(uint32_t pcount,
-                                             const double *_points,
-                                             uint32_t vstride,
-                                             uint32_t &tcount,
-                                             bool consolidate,
-                                             double epsilon)
-  {
-    reset();
-
-    double *points = (double *)malloc(sizeof(double)*pcount*3);
-    if ( consolidate )
-    {
-      pcount = fm_consolidatePolygon(pcount,_points,vstride,points,1-epsilon);
-    }
-    else
-    {
-      double *dest = points;
-      for (uint32_t i=0; i<pcount; i++)
-      {
-        const double *src = fm_getPoint(_points,vstride,i);
-        dest[0] = src[0];
-        dest[1] = src[1];
-        dest[2] = src[2];
-        dest+=3;
-      }
-      vstride = sizeof(double)*3;
-    }
-
-    if ( pcount >= 3 )
-    {
-      CTriangulator ct;
-      for (uint32_t i=0; i<pcount; i++)
-      {
-        const double *src = fm_getPoint(points,vstride,i);
-        ct.addPoint( src[0], src[1], src[2] );
-      }
-      uint32_t _tcount;
-      uint32_t *indices = ct.triangulate(_tcount,epsilon);
-      if ( indices )
-      {
-        tcount = _tcount;
-        mPointsDouble = (double *)malloc(sizeof(double)*tcount*3*3);
-        double *dest = mPointsDouble;
-        for (uint32_t i=0; i<tcount; i++)
-        {
-          uint32_t i1 = indices[i*3+0];
-          uint32_t i2 = indices[i*3+1];
-          uint32_t i3 = indices[i*3+2];
-          const double *p1 = ct.getPoint(i1);
-          const double *p2 = ct.getPoint(i2);
-          const double *p3 = ct.getPoint(i3);
-
-          dest[0] = p1[0];
-          dest[1] = p1[1];
-          dest[2] = p1[2];
-
-          dest[3] = p2[0];
-          dest[4] = p2[1];
-          dest[5] = p2[2];
-
-          dest[6] = p3[0];
-          dest[7] = p3[1];
-          dest[8] = p3[2];
-          dest+=9;
-        }
-      }
-    }
-    free(points);
-
-    return mPointsDouble;
-  }
-
-  virtual const float  *       triangulate3d(uint32_t pcount,
-                                             const float  *points,
-                                             uint32_t vstride,
-                                             uint32_t &tcount,
-                                             bool consolidate,
-                                             float epsilon)
-  {
-    reset();
-
-    double *temp = (double *)malloc(sizeof(double)*pcount*3);
-    double *dest = temp;
-    for (uint32_t i=0; i<pcount; i++)
-    {
-      const float *p = fm_getPoint(points,vstride,i);
-      dest[0] = p[0];
-      dest[1] = p[1];
-      dest[2] = p[2];
-      dest+=3;
-    }
-    const double *results = triangulate3d(pcount,temp,sizeof(double)*3,tcount,consolidate,epsilon);
-    if ( results )
-    {
-      uint32_t fcount = tcount*3*3;
-      mPointsFloat = (float *)malloc(sizeof(float)*tcount*3*3);
-      for (uint32_t i=0; i<fcount; i++)
-      {
-        mPointsFloat[i] = (float) results[i];
-      }
-      free(mPointsDouble);
-      mPointsDouble = 0;
-    }
-    free(temp);
-
-    return mPointsFloat;
-  }
-
-private:
-  float *mPointsFloat;
-  double *mPointsDouble;
-};
-
-fm_Triangulate * fm_createTriangulate(void)
-{
-  Triangulate *t = new Triangulate;
-  return static_cast< fm_Triangulate *>(t);
-}
-
-void             fm_releaseTriangulate(fm_Triangulate *t)
-{
-  Triangulate *tt = static_cast< Triangulate *>(t);
-  delete tt;
-}
-
-#endif
-
-bool validDistance(const REAL *p1,const REAL *p2,REAL epsilon)
-{
-	bool ret = true;
-
-	REAL dx = p1[0] - p2[0];
-	REAL dy = p1[1] - p2[1];
-	REAL dz = p1[2] - p2[2];
-	REAL dist = dx*dx+dy*dy+dz*dz;
-	if ( dist < (epsilon*epsilon) )
-	{
-		ret = false;
-	}
-	return ret;
-}
-
-bool fm_isValidTriangle(const REAL *p1,const REAL *p2,const REAL *p3,REAL epsilon)
-{
-  bool ret = false;
-
-  if ( validDistance(p1,p2,epsilon) &&
-	   validDistance(p1,p3,epsilon) &&
-	   validDistance(p2,p3,epsilon) )
-  {
-
-	  REAL area = fm_computeArea(p1,p2,p3);
-	  if ( area > epsilon )
-	  {
-		REAL _vertices[3*3],vertices[64*3];
-
-		_vertices[0] = p1[0];
-		_vertices[1] = p1[1];
-		_vertices[2] = p1[2];
-
-		_vertices[3] = p2[0];
-		_vertices[4] = p2[1];
-		_vertices[5] = p2[2];
-
-		_vertices[6] = p3[0];
-		_vertices[7] = p3[1];
-		_vertices[8] = p3[2];
-
-		uint32_t pcount = fm_consolidatePolygon(3,_vertices,sizeof(REAL)*3,vertices,1-epsilon);
-		if ( pcount == 3 )
-		{
-		  ret = true;
-		}
-	  }
-  }
-  return ret;
-}
-
-
-void  fm_multiplyQuat(const REAL *left,const REAL *right,REAL *quat)
-{
-	REAL a,b,c,d;
-
-	a = left[3]*right[3] - left[0]*right[0] - left[1]*right[1] - left[2]*right[2];
-	b = left[3]*right[0] + right[3]*left[0] + left[1]*right[2] - right[1]*left[2];
-	c = left[3]*right[1] + right[3]*left[1] + left[2]*right[0] - right[2]*left[0];
-	d = left[3]*right[2] + right[3]*left[2] + left[0]*right[1] - right[0]*left[1];
-
-	quat[3] = a;
-	quat[0] = b;
-	quat[1] = c;
-	quat[2] = d;
-}
-
-bool  fm_computeCentroid(uint32_t vcount,     // number of input data points
-						 const REAL *points,     // starting address of points array.
-						 uint32_t vstride,    // stride between input points.
-						 REAL *center)
-
-{
-	bool ret = false;
-	if ( vcount )
-	{
-		center[0] = 0;
-		center[1] = 0;
-		center[2] = 0;
-		const char *scan = (const char *)points;
-		for (uint32_t i=0; i<vcount; i++)
-		{
-			const REAL *p = (const REAL *)scan;
-			center[0]+=p[0];
-			center[1]+=p[1];
-			center[2]+=p[2];
-			scan+=vstride;
-		}
-		REAL recip = 1.0f / (REAL)vcount;
-		center[0]*=recip;
-		center[1]*=recip;
-		center[2]*=recip;
-		ret = true;
-	}
-	return ret;
-}
-
-#ifndef TEMPLATE_VEC3
-#define TEMPLATE_VEC3
-template <class Type> class Vec3
-{
-public:
-	Vec3(void)
-	{
-
-	}
-	Vec3(Type _x,Type _y,Type _z)
-	{
-		x = _x;
-		y = _y;
-		z = _z;
-	}
-	Type x;
-	Type y;
-	Type z;
-};
-#endif
-
-void fm_transformAABB(const REAL bmin[3],const REAL bmax[3],const REAL matrix[16],REAL tbmin[3],REAL tbmax[3])
-{
-	Vec3<REAL> box[8];
-	box[0] = Vec3< REAL >( bmin[0], bmin[1], bmin[2] );
-	box[1] = Vec3< REAL >( bmax[0], bmin[1], bmin[2] );
-	box[2] = Vec3< REAL >( bmax[0], bmax[1], bmin[2] );
-	box[3] = Vec3< REAL >( bmin[0], bmax[1], bmin[2] );
-	box[4] = Vec3< REAL >( bmin[0], bmin[1], bmax[2] );
-	box[5] = Vec3< REAL >( bmax[0], bmin[1], bmax[2] );
-	box[6] = Vec3< REAL >( bmax[0], bmax[1], bmax[2] );
-	box[7] = Vec3< REAL >( bmin[0], bmax[1], bmax[2] );
-	// transform all 8 corners of the box and then recompute a new AABB
-	for (unsigned int i=0; i<8; i++)
-	{
-		Vec3< REAL > &p = box[i];
-		fm_transform(matrix,&p.x,&p.x);
-		if ( i == 0 )
-		{
-			tbmin[0] = tbmax[0] = p.x;
-			tbmin[1] = tbmax[1] = p.y;
-			tbmin[2] = tbmax[2] = p.z;
-		}
-		else
-		{
-			if ( p.x < tbmin[0] ) tbmin[0] = p.x;
-			if ( p.y < tbmin[1] ) tbmin[1] = p.y;
-			if ( p.z < tbmin[2] ) tbmin[2] = p.z;
-			if ( p.x > tbmax[0] ) tbmax[0] = p.x;
-			if ( p.y > tbmax[1] ) tbmax[1] = p.y;
-			if ( p.z > tbmax[2] ) tbmax[2] = p.z;
-		}
-	}
-}
-
-REAL  fm_normalizeQuat(REAL n[4]) // normalize this quat
-{
-	REAL dx = n[0]*n[0];
-	REAL dy = n[1]*n[1];
-	REAL dz = n[2]*n[2];
-	REAL dw = n[3]*n[3];
-
-	REAL dist = dx*dx+dy*dy+dz*dz+dw*dw;
-
-	dist = (REAL)sqrt(dist);
-
-	REAL recip = 1.0f / dist;
-
-	n[0]*=recip;
-	n[1]*=recip;
-	n[2]*=recip;
-	n[3]*=recip;
-
-	return dist;
-}
-
-
-}; // end of namespace
+// a set of routines that let you do common 3d math

+// operations without any vector, matrix, or quaternion

+// classes or templates.

+//

+// a vector (or point) is a 'float *' to 3 floating point numbers.

+// a matrix is a 'float *' to an array of 16 floating point numbers representing a 4x4 transformation matrix compatible with D3D or OGL

+// a quaternion is a 'float *' to 4 floats representing a quaternion x,y,z,w

+//

+

+#pragma warning(disable:4996)

+

+namespace FLOAT_MATH

+{

+

+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);

+

+}

+

+REAL fm_getDeterminant(const REAL matrix[16])

+{

+  REAL tempv[3];

+  REAL p0[3];

+  REAL p1[3];

+  REAL p2[3];

+

+

+	p0[0] = matrix[0*4+0];

+	p0[1] = matrix[0*4+1];

+	p0[2] = matrix[0*4+2];

+

+	p1[0] = matrix[1*4+0];

+	p1[1] = matrix[1*4+1];

+	p1[2] = matrix[1*4+2];

+

+	p2[0] = matrix[2*4+0];

+	p2[1] = matrix[2*4+1];

+	p2[2] = matrix[2*4+2];

+

+  fm_cross(tempv,p1,p2);

+

+  return fm_dot(p0,tempv);

+

+}

+

+REAL fm_squared(REAL x) { return x*x; };

+

+void fm_decomposeTransform(const REAL local_transform[16],REAL trans[3],REAL rot[4],REAL scale[3])

+{

+

+  trans[0] = local_transform[12];

+  trans[1] = local_transform[13];

+  trans[2] = local_transform[14];

+

+  scale[0] = (REAL)sqrt(fm_squared(local_transform[0*4+0]) + fm_squared(local_transform[0*4+1]) + fm_squared(local_transform[0*4+2]));

+  scale[1] = (REAL)sqrt(fm_squared(local_transform[1*4+0]) + fm_squared(local_transform[1*4+1]) + fm_squared(local_transform[1*4+2]));

+  scale[2] = (REAL)sqrt(fm_squared(local_transform[2*4+0]) + fm_squared(local_transform[2*4+1]) + fm_squared(local_transform[2*4+2]));

+

+  REAL m[16];

+  memcpy(m,local_transform,sizeof(REAL)*16);

+

+  REAL sx = 1.0f / scale[0];

+  REAL sy = 1.0f / scale[1];

+  REAL sz = 1.0f / scale[2];

+

+  m[0*4+0]*=sx;

+  m[0*4+1]*=sx;

+  m[0*4+2]*=sx;

+

+  m[1*4+0]*=sy;

+  m[1*4+1]*=sy;

+  m[1*4+2]*=sy;

+

+  m[2*4+0]*=sz;

+  m[2*4+1]*=sz;

+  m[2*4+2]*=sz;

+

+  fm_matrixToQuat(m,rot);

+

+}

+

+void fm_getSubMatrix(int32_t ki,int32_t kj,REAL pDst[16],const REAL matrix[16])

+{

+	int32_t row, col;

+	int32_t dstCol = 0, dstRow = 0;

+

+	for ( col = 0; col < 4; col++ )

+	{

+		if ( col == kj )

+		{

+			continue;

+		}

+		for ( dstRow = 0, row = 0; row < 4; row++ )

+		{

+			if ( row == ki )

+			{

+				continue;

+			}

+			pDst[dstCol*4+dstRow] = matrix[col*4+row];

+			dstRow++;

+		}

+		dstCol++;

+	}

+}

+

+void  fm_inverseTransform(const REAL matrix[16],REAL inverse_matrix[16])

+{

+	REAL determinant = fm_getDeterminant(matrix);

+	determinant = 1.0f / determinant;

+	for (int32_t i = 0; i < 4; i++ )

+	{

+		for (int32_t j = 0; j < 4; j++ )

+		{

+			int32_t sign = 1 - ( ( i + j ) % 2 ) * 2;

+			REAL subMat[16];

+			fm_identity(subMat);

+			fm_getSubMatrix( i, j, subMat, matrix );

+			REAL subDeterminant = fm_getDeterminant(subMat);

+			inverse_matrix[i*4+j] = ( subDeterminant * sign ) * determinant;

+		}

+	}

+}

+

+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_quatToEuler(const REAL quat[4],REAL &ax,REAL &ay,REAL &az)

+{

+	REAL x = quat[0];

+	REAL y = quat[1];

+	REAL z = quat[2];

+	REAL w = quat[3];

+

+	REAL sint	     = (2.0f * w * y) - (2.0f * x * z);

+	REAL cost_temp = 1.0f - (sint * sint);

+	REAL cost	   	 = 0;

+

+	if ( (REAL)fabs(cost_temp) > 0.001f )

+	{

+		cost = (REAL)sqrt( cost_temp );

+	}

+

+	REAL sinv, cosv, sinf, cosf;

+	if ( (REAL)fabs(cost) > 0.001f )

+	{

+    cost = 1.0f / cost;

+		sinv = ((2.0f * y * z) + (2.0f * w * x)) * cost;

+		cosv = (1.0f - (2.0f * x * x) - (2.0f * y * y)) * cost;

+		sinf = ((2.0f * x * y) + (2.0f * w * z)) * cost;

+		cosf = (1.0f - (2.0f * y * y) - (2.0f * z * z)) * cost;

+	}

+	else

+	{

+		sinv = (2.0f * w * x) - (2.0f * y * z);

+		cosv = 1.0f - (2.0f * x * x) - (2.0f * z * z);

+		sinf = 0;

+		cosf = 1.0f;

+	}

+

+	// compute output rotations

+	ax	= (REAL)atan2( sinv, cosv );

+	ay	= (REAL)atan2( sint, cost );

+	az	= (REAL)atan2( sinf, cosf );

+

+}

+

+void fm_eulerToMatrix(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);

+}

+

+void fm_getAABB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *bmin,REAL *bmax)

+{

+

+  const uint8_t *source = (const uint8_t *) 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 (uint32_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];

+

+  }

+}

+

+void  fm_eulerToQuat(const REAL *euler,REAL *quat) // convert euler angles to quaternion.

+{

+  fm_eulerToQuat(euler[0],euler[1],euler[2],quat);

+}

+

+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 = (REAL)cos(roll);

+	REAL cp = (REAL)cos(pitch);

+	REAL cy = (REAL)cos(yaw);

+

+	REAL sr = (REAL)sin(roll);

+	REAL sp = (REAL)sin(pitch);

+	REAL sy = (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_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_quatRotate(const REAL *quat,const REAL *v,REAL *r) // rotate a vector directly by a quaternion.

+{

+  REAL left[4];

+

+	left[0] =   quat[3]*v[0] + quat[1]*v[2] - v[1]*quat[2];

+	left[1] =   quat[3]*v[1] + quat[2]*v[0] - v[2]*quat[0];

+	left[2] =   quat[3]*v[2] + quat[0]*v[1] - v[0]*quat[1];

+	left[3] = - quat[0]*v[0] - quat[1]*v[1] - quat[2]*v[2];

+

+	r[0] = (left[3]*-quat[0]) + (quat[3]*left[0]) + (left[1]*-quat[2]) - (-quat[1]*left[2]);

+	r[1] = (left[3]*-quat[1]) + (quat[3]*left[1]) + (left[2]*-quat[0]) - (-quat[2]*left[0]);

+	r[2] = (left[3]*-quat[2]) + (quat[3]*left[2]) + (left[0]*-quat[1]) - (-quat[0]*left[1]);

+

+}

+

+

+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_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

+		int32_t nxt[3] = {1, 2, 0};

+		REAL  qa[4];

+

+		int32_t i = 0;

+

+		if (matrix[1*4+1] > matrix[0*4+0]) i = 1;

+		if (matrix[2*4+2] > matrix[i*4+i]) i = 2;

+

+		int32_t j = nxt[i];

+		int32_t k = nxt[j];

+

+		REAL s = (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];

+	}

+//	fm_normalizeQuat(quat);

+}

+

+

+REAL fm_sphereVolume(REAL radius) // return's the volume of a sphere of this radius (4/3 PI * R cubed )

+{

+	return (4.0f / 3.0f ) * FM_PI * radius * radius * radius;

+}

+

+

+REAL fm_cylinderVolume(REAL radius,REAL h)

+{

+	return FM_PI * radius * radius *h;

+}

+

+REAL fm_capsuleVolume(REAL radius,REAL h)

+{

+	REAL volume = fm_sphereVolume(radius); // volume of the sphere portion.

+	REAL ch = h-radius*2; // this is the cylinder length

+	if ( ch > 0 )

+	{

+		volume+=fm_cylinderVolume(radius,ch);

+	}

+	return volume;

+}

+

+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_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];

+  }

+}

+

+

+REAL fm_distance(const REAL *p1,const REAL *p2)

+{

+	REAL dx = p1[0] - p2[0];

+	REAL dy = p1[1] - p2[1];

+	REAL dz = p1[2] - p2[2];

+

+	return (REAL)sqrt( dx*dx + dy*dy + dz *dz );

+}

+

+REAL fm_distanceSquared(const REAL *p1,const REAL *p2)

+{

+	REAL dx = p1[0] - p2[0];

+	REAL dy = p1[1] - p2[1];

+	REAL dz = p1[2] - p2[2];

+

+	return dx*dx + dy*dy + dz *dz;

+}

+

+

+REAL fm_distanceSquaredXZ(const REAL *p1,const REAL *p2)

+{

+	REAL dx = p1[0] - p2[0];

+	REAL dz = p1[2] - p2[2];

+

+	return dx*dx +  dz *dz;

+}

+

+

+REAL fm_computePlane(const REAL *A,const REAL *B,const REAL *C,REAL *n) // returns D

+{

+	REAL vx = (B[0] - C[0]);

+	REAL vy = (B[1] - C[1]);

+	REAL vz = (B[2] - C[2]);

+

+	REAL wx = (A[0] - B[0]);

+	REAL wy = (A[1] - B[1]);

+	REAL wz = (A[2] - B[2]);

+

+	REAL vw_x = vy * wz - vz * wy;

+	REAL vw_y = vz * wx - vx * wz;

+	REAL vw_z = vx * wy - vy * wx;

+

+	REAL mag = (REAL)sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));

+

+	if ( mag < 0.000001f )

+	{

+		mag = 0;

+	}

+	else

+	{

+		mag = 1.0f/mag;

+	}

+

+	REAL x = vw_x * mag;

+	REAL y = vw_y * mag;

+	REAL z = vw_z * mag;

+

+

+	REAL D = 0.0f - ((x*A[0])+(y*A[1])+(z*A[2]));

+

+  n[0] = x;

+  n[1] = y;

+  n[2] = z;

+

+	return D;

+}

+

+REAL fm_distToPlane(const REAL *plane,const REAL *p) // computes the distance of this point from the plane.

+{

+  return p[0]*plane[0]+p[1]*plane[1]+p[2]*plane[2]+plane[3];

+}

+

+REAL fm_dot(const REAL *p1,const REAL *p2)

+{

+  return p1[0]*p2[0]+p1[1]*p2[1]+p1[2]*p2[2];

+}

+

+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];

+}

+

+void fm_computeNormalVector(REAL *n,const REAL *p1,const REAL *p2)

+{

+  n[0] = p2[0] - p1[0];

+  n[1] = p2[1] - p1[1];

+  n[2] = p2[2] - p1[2];

+  fm_normalize(n);

+}

+

+bool  fm_computeWindingOrder(const REAL *p1,const REAL *p2,const REAL *p3) // returns true if the triangle is clockwise.

+{

+  bool ret = false;

+

+  REAL v1[3];

+  REAL v2[3];

+

+  fm_computeNormalVector(v1,p1,p2); // p2-p1 (as vector) and then normalized

+  fm_computeNormalVector(v2,p1,p3); // p3-p1 (as vector) and then normalized

+

+  REAL cross[3];

+

+  fm_cross(cross, v1, v2 );

+  REAL ref[3] = { 1, 0, 0 };

+

+  REAL d = fm_dot( cross, ref );

+

+

+  if ( d <= 0 )

+    ret = false;

+  else

+    ret = true;

+

+  return ret;

+}

+

+REAL fm_normalize(REAL *n) // normalize this vector

+{

+  REAL dist = (REAL)sqrt(n[0]*n[0] + n[1]*n[1] + n[2]*n[2]);

+  if ( dist > 0.0000001f )

+  {

+    REAL mag = 1.0f / dist;

+    n[0]*=mag;

+    n[1]*=mag;

+    n[2]*=mag;

+  }

+  else

+  {

+    n[0] = 1;

+    n[1] = 0;

+    n[2] = 0;

+  }

+

+  return dist;

+}

+

+

+void  fm_matrixMultiply(const REAL *pA,const REAL *pB,REAL *pM)

+{

+#if 1

+

+  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;

+

+

+#else

+	memset(pM, 0, sizeof(REAL)*16);

+	for(int32_t i=0; i<4; i++ )

+		for(int32_t j=0; j<4; j++ )

+			for(int32_t k=0; k<4; k++ )

+				pM[4*i+j] +=  pA[4*i+k] * pB[4*k+j];

+#endif

+}

+

+

+void  fm_eulerToQuatDX(REAL x,REAL y,REAL z,REAL *quat) // convert euler angles to quaternion using the fucked up DirectX method

+{

+  REAL matrix[16];

+  fm_eulerToMatrix(x,y,z,matrix);

+  fm_matrixToQuat(matrix,quat);

+}

+

+// implementation copied from: http://blogs.msdn.com/mikepelton/archive/2004/10/29/249501.aspx

+void  fm_eulerToMatrixDX(REAL x,REAL y,REAL z,REAL *matrix) // convert euler angles to quaternion using the fucked up DirectX method.

+{

+  fm_identity(matrix);

+  matrix[0*4+0] = (REAL)(cos(z)*cos(y) + sin(z)*sin(x)*sin(y));

+  matrix[0*4+1] = (REAL)(sin(z)*cos(x));

+  matrix[0*4+2] = (REAL)(cos(z)*-sin(y) + sin(z)*sin(x)*cos(y));

+

+  matrix[1*4+0] = (REAL)(-sin(z)*cos(y)+cos(z)*sin(x)*sin(y));

+  matrix[1*4+1] = (REAL)(cos(z)*cos(x));

+  matrix[1*4+2] = (REAL)(sin(z)*sin(y) +cos(z)*sin(x)*cos(y));

+

+  matrix[2*4+0] = (REAL)(cos(x)*sin(y));

+  matrix[2*4+1] = (REAL)(-sin(x));

+  matrix[2*4+2] = (REAL)(cos(x)*cos(y));

+}

+

+

+void  fm_scale(REAL x,REAL y,REAL z,REAL *fscale) // apply scale to the matrix.

+{

+  fscale[0*4+0] = x;

+  fscale[1*4+1] = y;

+  fscale[2*4+2] = z;

+}

+

+

+void  fm_composeTransform(const REAL *position,const REAL *quat,const REAL *scale,REAL *matrix)

+{

+  fm_identity(matrix);

+  fm_quatToMatrix(quat,matrix);

+

+  if ( scale && ( scale[0] != 1 || scale[1] != 1 || scale[2] != 1 ) )

+  {

+    REAL work[16];

+    memcpy(work,matrix,sizeof(REAL)*16);

+    REAL mscale[16];

+    fm_identity(mscale);

+    fm_scale(scale[0],scale[1],scale[2],mscale);

+    fm_matrixMultiply(work,mscale,matrix);

+  }

+

+  matrix[12] = position[0];

+  matrix[13] = position[1];

+  matrix[14] = position[2];

+}

+

+

+void  fm_setTranslation(const REAL *translation,REAL *matrix)

+{

+  matrix[12] = translation[0];

+  matrix[13] = translation[1];

+  matrix[14] = translation[2];

+}

+

+static REAL enorm0_3d ( REAL x0, REAL y0, REAL z0, REAL x1, REAL y1, REAL z1 )

+

+/**********************************************************************/

+

+/*

+Purpose:

+

+ENORM0_3D computes the Euclidean norm of (P1-P0) in 3D.

+

+Modified:

+

+18 April 1999

+

+Author:

+

+John Burkardt

+

+Parameters:

+

+Input, REAL X0, Y0, Z0, X1, Y1, Z1, the coordinates of the points 

+P0 and P1.

+

+Output, REAL ENORM0_3D, the Euclidean norm of (P1-P0).

+*/

+{

+  REAL value;

+

+  value = (REAL)sqrt (

+    ( x1 - x0 ) * ( x1 - x0 ) + 

+    ( y1 - y0 ) * ( y1 - y0 ) + 

+    ( z1 - z0 ) * ( z1 - z0 ) );

+

+  return value;

+}

+

+

+static REAL triangle_area_3d ( REAL x1, REAL y1, REAL z1, REAL x2,REAL y2, REAL z2, REAL x3, REAL y3, REAL z3 )

+

+                        /**********************************************************************/

+

+                        /*

+                        Purpose:

+

+                        TRIANGLE_AREA_3D computes the area of a triangle in 3D.

+

+                        Modified:

+

+                        22 April 1999

+

+                        Author:

+

+                        John Burkardt

+

+                        Parameters:

+

+                        Input, REAL X1, Y1, Z1, X2, Y2, Z2, X3, Y3, Z3, the (X,Y,Z)

+                        coordinates of the corners of the triangle.

+

+                        Output, REAL TRIANGLE_AREA_3D, the area of the triangle.

+                        */

+{

+  REAL a;

+  REAL alpha;

+  REAL area;

+  REAL b;

+  REAL base;

+  REAL c;

+  REAL dot;

+  REAL height;

+  /*

+  Find the projection of (P3-P1) onto (P2-P1).

+  */

+  dot = 

+    ( x2 - x1 ) * ( x3 - x1 ) +

+    ( y2 - y1 ) * ( y3 - y1 ) +

+    ( z2 - z1 ) * ( z3 - z1 );

+

+  base = enorm0_3d ( x1, y1, z1, x2, y2, z2 );

+  /*

+  The height of the triangle is the length of (P3-P1) after its

+  projection onto (P2-P1) has been subtracted.

+  */

+  if ( base == 0.0 ) {

+

+    height = 0.0;

+

+  }

+  else {

+

+    alpha = dot / ( base * base );

+

+    a = x3 - x1 - alpha * ( x2 - x1 );

+    b = y3 - y1 - alpha * ( y2 - y1 );

+    c = z3 - z1 - alpha * ( z2 - z1 );

+

+    height = (REAL)sqrt ( a * a + b * b + c * c );

+

+  }

+

+  area = 0.5f * base * height;

+

+  return area;

+}

+

+

+REAL fm_computeArea(const REAL *p1,const REAL *p2,const REAL *p3)

+{

+  REAL ret = 0;

+

+  ret = triangle_area_3d(p1[0],p1[1],p1[2],p2[0],p2[1],p2[2],p3[0],p3[1],p3[2]);

+

+  return ret;

+}

+

+

+void  fm_lerp(const REAL *p1,const REAL *p2,REAL *dest,REAL lerpValue)

+{

+  dest[0] = ((p2[0] - p1[0])*lerpValue) + p1[0];

+  dest[1] = ((p2[1] - p1[1])*lerpValue) + p1[1];

+  dest[2] = ((p2[2] - p1[2])*lerpValue) + p1[2];

+}

+

+bool fm_pointTestXZ(const REAL *p,const REAL *i,const REAL *j)

+{

+  bool ret = false;

+

+  if (((( i[2] <= p[2] ) && ( p[2]  < j[2] )) || (( j[2] <= p[2] ) && ( p[2]  < i[2] ))) && ( p[0] < (j[0] - i[0]) * (p[2] - i[2]) / (j[2] - i[2]) + i[0]))

+    ret = true;

+

+  return ret;

+};

+

+

+bool  fm_insideTriangleXZ(const REAL *p,const REAL *p1,const REAL *p2,const REAL *p3)

+{

+  bool ret = false;

+

+  int32_t c = 0;

+  if ( fm_pointTestXZ(p,p1,p2) ) c = !c;

+  if ( fm_pointTestXZ(p,p2,p3) ) c = !c;

+  if ( fm_pointTestXZ(p,p3,p1) ) c = !c;

+  if ( c ) ret = true;

+

+  return ret;

+}

+

+bool  fm_insideAABB(const REAL *pos,const REAL *bmin,const REAL *bmax)

+{

+  bool ret = false;

+

+  if ( pos[0] >= bmin[0] && pos[0] <= bmax[0] &&

+       pos[1] >= bmin[1] && pos[1] <= bmax[1] &&

+       pos[2] >= bmin[2] && pos[2] <= bmax[2] )

+    ret = true;

+

+  return ret;

+}

+

+

+uint32_t fm_clipTestPoint(const REAL *bmin,const REAL *bmax,const REAL *pos)

+{

+  uint32_t ret = 0;

+

+  if ( pos[0] < bmin[0] )

+    ret|=FMCS_XMIN;

+  else if ( pos[0] > bmax[0] )

+    ret|=FMCS_XMAX;

+

+  if ( pos[1] < bmin[1] )

+    ret|=FMCS_YMIN;

+  else if ( pos[1] > bmax[1] )

+    ret|=FMCS_YMAX;

+

+  if ( pos[2] < bmin[2] )

+    ret|=FMCS_ZMIN;

+  else if ( pos[2] > bmax[2] )

+    ret|=FMCS_ZMAX;

+

+  return ret;

+}

+

+uint32_t fm_clipTestPointXZ(const REAL *bmin,const REAL *bmax,const REAL *pos) // only tests X and Z, not Y

+{

+  uint32_t ret = 0;

+

+  if ( pos[0] < bmin[0] )

+    ret|=FMCS_XMIN;

+  else if ( pos[0] > bmax[0] )

+    ret|=FMCS_XMAX;

+

+  if ( pos[2] < bmin[2] )

+    ret|=FMCS_ZMIN;

+  else if ( pos[2] > bmax[2] )

+    ret|=FMCS_ZMAX;

+

+  return ret;

+}

+

+uint32_t fm_clipTestAABB(const REAL *bmin,const REAL *bmax,const REAL *p1,const REAL *p2,const REAL *p3,uint32_t &andCode)

+{

+  uint32_t orCode = 0;

+

+  andCode = FMCS_XMIN | FMCS_XMAX | FMCS_YMIN | FMCS_YMAX | FMCS_ZMIN | FMCS_ZMAX;

+

+  uint32_t c = fm_clipTestPoint(bmin,bmax,p1);

+  orCode|=c;

+  andCode&=c;

+

+  c = fm_clipTestPoint(bmin,bmax,p2);

+  orCode|=c;

+  andCode&=c;

+

+  c = fm_clipTestPoint(bmin,bmax,p3);

+  orCode|=c;

+  andCode&=c;

+

+  return orCode;

+}

+

+bool intersect(const REAL *si,const REAL *ei,const REAL *bmin,const REAL *bmax,REAL *time)

+{

+  REAL st,et,fst = 0,fet = 1;

+

+  for (int32_t i = 0; i < 3; i++)

+  {

+    if (*si < *ei)

+    {

+      if (*si > *bmax || *ei < *bmin)

+        return false;

+      REAL di = *ei - *si;

+      st = (*si < *bmin)? (*bmin - *si) / di: 0;

+      et = (*ei > *bmax)? (*bmax - *si) / di: 1;

+    }

+    else

+    {

+      if (*ei > *bmax || *si < *bmin)

+        return false;

+      REAL di = *ei - *si;

+      st = (*si > *bmax)? (*bmax - *si) / di: 0;

+      et = (*ei < *bmin)? (*bmin - *si) / di: 1;

+    }

+

+    if (st > fst) fst = st;

+    if (et < fet) fet = et;

+    if (fet < fst)

+      return false;

+    bmin++; bmax++;

+    si++; ei++;

+  }

+

+  *time = fst;

+  return true;

+}

+

+

+

+bool fm_lineTestAABB(const REAL *p1,const REAL *p2,const REAL *bmin,const REAL *bmax,REAL &time)

+{

+  bool sect = intersect(p1,p2,bmin,bmax,&time);

+  return sect;

+}

+

+

+bool fm_lineTestAABBXZ(const REAL *p1,const REAL *p2,const REAL *bmin,const REAL *bmax,REAL &time)

+{

+  REAL _bmin[3];

+  REAL _bmax[3];

+

+  _bmin[0] = bmin[0];

+  _bmin[1] = -1e9;

+  _bmin[2] = bmin[2];

+

+  _bmax[0] = bmax[0];

+  _bmax[1] = 1e9;

+  _bmax[2] = bmax[2];

+

+  bool sect = intersect(p1,p2,_bmin,_bmax,&time);

+

+  return sect;

+}

+

+void  fm_minmax(const REAL *p,REAL *bmin,REAL *bmax) // accmulate to a min-max value

+{

+

+  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 fm_solveX(const REAL *plane,REAL y,REAL z) // solve for X given this plane equation and the other two components.

+{

+  REAL x = (y*plane[1]+z*plane[2]+plane[3]) / -plane[0];

+  return x;

+}

+

+REAL fm_solveY(const REAL *plane,REAL x,REAL z) // solve for Y given this plane equation and the other two components.

+{

+  REAL y = (x*plane[0]+z*plane[2]+plane[3]) / -plane[1];

+  return y;

+}

+

+

+REAL fm_solveZ(const REAL *plane,REAL x,REAL y) // solve for Y given this plane equation and the other two components.

+{

+  REAL z = (x*plane[0]+y*plane[1]+plane[3]) / -plane[2];

+  return z;

+}

+

+

+void  fm_getAABBCenter(const REAL *bmin,const REAL *bmax,REAL *center)

+{

+  center[0] = (bmax[0]-bmin[0])*0.5f+bmin[0];

+  center[1] = (bmax[1]-bmin[1])*0.5f+bmin[1];

+  center[2] = (bmax[2]-bmin[2])*0.5f+bmin[2];

+}

+

+FM_Axis fm_getDominantAxis(const REAL normal[3])

+{

+  FM_Axis ret = FM_XAXIS;

+

+  REAL x = (REAL)fabs(normal[0]);

+  REAL y = (REAL)fabs(normal[1]);

+  REAL z = (REAL)fabs(normal[2]);

+

+  if ( y > x && y > z )

+    ret = FM_YAXIS;

+  else if ( z > x && z > y )

+    ret = FM_ZAXIS;

+

+  return ret;

+}

+

+

+bool fm_lineSphereIntersect(const REAL *center,REAL radius,const REAL *p1,const REAL *p2,REAL *intersect)

+{

+  bool ret = false;

+

+  REAL dir[3];

+

+  dir[0] = p2[0]-p1[0];

+  dir[1] = p2[1]-p1[1];

+  dir[2] = p2[2]-p1[2];

+

+  REAL distance = (REAL)sqrt( dir[0]*dir[0]+dir[1]*dir[1]+dir[2]*dir[2]);

+

+  if ( distance > 0 )

+  {

+    REAL recip = 1.0f / distance;

+    dir[0]*=recip;

+    dir[1]*=recip;

+    dir[2]*=recip;

+    ret = fm_raySphereIntersect(center,radius,p1,dir,distance,intersect);

+  }

+  else

+  {

+    dir[0] = center[0]-p1[0];

+    dir[1] = center[1]-p1[1];

+    dir[2] = center[2]-p1[2];

+    REAL d2 = dir[0]*dir[0]+dir[1]*dir[1]+dir[2]*dir[2];

+    REAL r2 = radius*radius;

+    if ( d2 < r2 )

+    {

+      ret = true;

+      if ( intersect )

+      {

+        intersect[0] = p1[0];

+        intersect[1] = p1[1];

+        intersect[2] = p1[2];

+      }

+    }

+  }

+  return ret;

+}

+

+#define DOT(p1,p2) (p1[0]*p2[0]+p1[1]*p2[1]+p1[2]*p2[2])

+

+bool fm_raySphereIntersect(const REAL *center,REAL radius,const REAL *pos,const REAL *dir,REAL distance,REAL *intersect)

+{

+  bool ret = false;

+

+  REAL E0[3];

+

+  E0[0] = center[0] - pos[0];

+  E0[1] = center[1] - pos[1];

+  E0[2] = center[2] - pos[2];

+

+  REAL V[3];

+

+  V[0]  = dir[0];

+  V[1]  = dir[1];

+  V[2]  = dir[2];

+

+

+  REAL dist2   = E0[0]*E0[0] + E0[1]*E0[1] + E0[2] * E0[2];

+  REAL radius2 = radius*radius; // radius squared..

+

+  // Bug Fix For Gem, if origin is *inside* the sphere, invert the

+  // direction vector so that we get a valid intersection location.

+  if ( dist2 < radius2 )

+  {

+    V[0]*=-1;

+    V[1]*=-1;

+    V[2]*=-1;

+  }

+

+

+	REAL v = DOT(E0,V);

+

+	REAL disc = radius2 - (dist2 - v*v);

+

+	if (disc > 0.0f)

+	{

+		if ( intersect )

+		{

+		  REAL d = (REAL)sqrt(disc);

+      REAL diff = v-d;

+      if ( diff < distance )

+      {

+        intersect[0] = pos[0]+V[0]*diff;

+        intersect[1] = pos[1]+V[1]*diff;

+        intersect[2] = pos[2]+V[2]*diff;

+        ret = true;

+      }

+    }

+	}

+

+	return ret;

+}

+

+

+void fm_catmullRom(REAL *out_vector,const REAL *p1,const REAL *p2,const REAL *p3,const REAL *p4, const REAL s)

+{

+  REAL s_squared = s * s;

+  REAL s_cubed = s_squared * s;

+

+  REAL coefficient_p1 = -s_cubed + 2*s_squared - s;

+  REAL coefficient_p2 = 3 * s_cubed - 5 * s_squared + 2;

+  REAL coefficient_p3 = -3 * s_cubed +4 * s_squared + s;

+  REAL coefficient_p4 = s_cubed - s_squared;

+

+  out_vector[0] = (coefficient_p1 * p1[0] + coefficient_p2 * p2[0] + coefficient_p3 * p3[0] + coefficient_p4 * p4[0])*0.5f;

+  out_vector[1] = (coefficient_p1 * p1[1] + coefficient_p2 * p2[1] + coefficient_p3 * p3[1] + coefficient_p4 * p4[1])*0.5f;

+  out_vector[2] = (coefficient_p1 * p1[2] + coefficient_p2 * p2[2] + coefficient_p3 * p3[2] + coefficient_p4 * p4[2])*0.5f;

+}

+

+bool fm_intersectAABB(const REAL *bmin1,const REAL *bmax1,const REAL *bmin2,const REAL *bmax2)

+{

+  if ((bmin1[0] > bmax2[0]) || (bmin2[0] > bmax1[0])) return false;

+  if ((bmin1[1] > bmax2[1]) || (bmin2[1] > bmax1[1])) return false;

+  if ((bmin1[2] > bmax2[2]) || (bmin2[2] > bmax1[2])) return false;

+  return true;

+

+}

+

+bool  fm_insideAABB(const REAL *obmin,const REAL *obmax,const REAL *tbmin,const REAL *tbmax) // test if bounding box tbmin/tmbax is fully inside obmin/obmax

+{

+  bool ret = false;

+

+  if ( tbmax[0] <= obmax[0] &&

+       tbmax[1] <= obmax[1] &&

+       tbmax[2] <= obmax[2] &&

+       tbmin[0] >= obmin[0] &&

+       tbmin[1] >= obmin[1] &&

+       tbmin[2] >= obmin[2] ) ret = true;

+

+  return ret;

+}

+

+

+// 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);

+

+  if( d<= -0.99999f ) // 180 about x axis

+  {

+	  if ( fabsf((float)v0[0]) < 0.1f )

+	  {

+		  quat[0] = 0;

+		  quat[1] = v0[2];

+		  quat[2] = -v0[1];

+		  quat[3] = 0;

+	  }

+	  else

+	  {

+		  quat[0] = v0[1];

+		  quat[1] = -v0[0];

+		  quat[2] = 0;

+		  quat[3] = 0;

+	  }

+	  REAL magnitudeSquared = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3];

+	  REAL magnitude = sqrtf((float)magnitudeSquared);

+	  REAL recip = 1.0f / magnitude;

+	  quat[0]*=recip;

+	  quat[1]*=recip;

+	  quat[2]*=recip;

+	  quat[3]*=recip;

+  }

+  else

+  {

+	  REAL s = (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;

+  }

+}

+

+

+REAL fm_distancePointLineSegment(const REAL *Point,const REAL *LineStart,const REAL *LineEnd,REAL *intersection,LineSegmentType &type,REAL epsilon)

+{

+  REAL ret;

+

+  REAL LineMag = fm_distance( LineEnd, LineStart );

+

+  if ( LineMag > 0 )

+  {

+    REAL U = ( ( ( Point[0] - LineStart[0] ) * ( LineEnd[0] - LineStart[0] ) ) + ( ( Point[1] - LineStart[1] ) * ( LineEnd[1] - LineStart[1] ) ) + ( ( Point[2] - LineStart[2] ) * ( LineEnd[2] - LineStart[2] ) ) ) / ( LineMag * LineMag );

+    if( U < 0.0f || U > 1.0f )

+    {

+      REAL d1 = fm_distanceSquared(Point,LineStart);

+      REAL d2 = fm_distanceSquared(Point,LineEnd);

+      if ( d1 <= d2 )

+      {

+        ret = (REAL)sqrt(d1);

+        intersection[0] = LineStart[0];

+        intersection[1] = LineStart[1];

+        intersection[2] = LineStart[2];

+        type = LS_START;

+      }

+      else

+      {

+        ret = (REAL)sqrt(d2);

+        intersection[0] = LineEnd[0];

+        intersection[1] = LineEnd[1];

+        intersection[2] = LineEnd[2];

+        type = LS_END;

+      }

+    }

+    else

+    {

+      intersection[0] = LineStart[0] + U * ( LineEnd[0] - LineStart[0] );

+      intersection[1] = LineStart[1] + U * ( LineEnd[1] - LineStart[1] );

+      intersection[2] = LineStart[2] + U * ( LineEnd[2] - LineStart[2] );

+

+      ret = fm_distance(Point,intersection);

+

+      REAL d1 = fm_distanceSquared(intersection,LineStart);

+      REAL d2 = fm_distanceSquared(intersection,LineEnd);

+	  REAL mag = (epsilon*2)*(epsilon*2);

+

+      if ( d1 < mag ) // if less than 1/100th the total distance, treat is as the 'start'

+      {

+        type = LS_START;

+      }

+      else if ( d2 < mag )

+      {

+        type = LS_END;

+      }

+      else

+      {

+        type = LS_MIDDLE;

+      }

+

+    }

+  }

+  else

+  {

+    ret = LineMag;

+    intersection[0] = LineEnd[0];

+    intersection[1] = LineEnd[1];

+    intersection[2] = LineEnd[2];

+    type = LS_END;

+  }

+

+  return ret;

+}

+

+

+#ifndef BEST_FIT_PLANE_H

+

+#define BEST_FIT_PLANE_H

+

+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 = (REAL)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 int32_t iMaxIter = 32;

+

+    for (int32_t i0 = 0; i0 <3; i0++)

+    {

+      int32_t i1;

+      for (i1 = 0; i1 < iMaxIter; i1++)

+      {

+        int32_t i2;

+        for (i2 = i0; i2 <= (3-2); i2++)

+        {

+          Type fTmp = 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 = (REAL)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 (int32_t 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 = (REAL)sqrt(fCos*fCos+(Type)1.0);

+            m_afSubd[i3+1] = fF*fR;

+            fSin = ((Type)1.0)/fR;

+            fCos *= fSin;

+          }

+          else

+          {

+            fSin = fF/fG;

+            fR = (REAL)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 (int32_t 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 (int32_t i0 = 0, i1; i0 <= 3-2; i0++)

+    {

+      // locate maximum eigenvalue

+      i1 = i0;

+      Type fMax = m_afDiag[i1];

+      int32_t 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 (int32_t 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;

+};

+

+#endif

+

+bool fm_computeBestFitPlane(uint32_t vcount,

+                     const REAL *points,

+                     uint32_t vstride,

+                     const REAL *weights,

+                     uint32_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 (uint32_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 (uint32_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;

+}

+

+

+bool fm_colinear(const REAL a1[3],const REAL a2[3],const REAL b1[3],const REAL b2[3],REAL epsilon)  // true if these two line segments are co-linear.

+{

+  bool ret = false;

+

+  REAL dir1[3];

+  REAL dir2[3];

+

+  dir1[0] = (a2[0] - a1[0]);

+  dir1[1] = (a2[1] - a1[1]);

+  dir1[2] = (a2[2] - a1[2]);

+

+  dir2[0] = (b2[0]-a1[0]) - (b1[0]-a1[0]);

+  dir2[1] = (b2[1]-a1[1]) - (b1[1]-a1[1]);

+  dir2[2] = (b2[2]-a2[2]) - (b1[2]-a2[2]);

+

+  fm_normalize(dir1);

+  fm_normalize(dir2);

+

+  REAL dot = fm_dot(dir1,dir2);

+

+  if ( dot >= epsilon )

+  {

+    ret = true;

+  }

+

+

+  return ret;

+}

+

+bool fm_colinear(const REAL *p1,const REAL *p2,const REAL *p3,REAL epsilon)

+{

+  bool ret = false;

+

+  REAL dir1[3];

+  REAL dir2[3];

+

+  dir1[0] = p2[0] - p1[0];

+  dir1[1] = p2[1] - p1[1];

+  dir1[2] = p2[2] - p1[2];

+

+  dir2[0] = p3[0] - p2[0];

+  dir2[1] = p3[1] - p2[1];

+  dir2[2] = p3[2] - p2[2];

+

+  fm_normalize(dir1);

+  fm_normalize(dir2);

+

+  REAL dot = fm_dot(dir1,dir2);

+

+  if ( dot >= epsilon )

+  {

+    ret = true;

+  }

+

+

+  return ret;

+}

+

+void  fm_initMinMax(const REAL *p,REAL *bmin,REAL *bmax)

+{

+  bmax[0] = bmin[0] = p[0];

+  bmax[1] = bmin[1] = p[1];

+  bmax[2] = bmin[2] = p[2];

+}

+

+IntersectResult fm_intersectLineSegments2d(const REAL *a1,const REAL *a2,const REAL *b1,const REAL *b2,REAL *intersection)

+{

+  IntersectResult ret;

+

+  REAL denom  = ((b2[1] - b1[1])*(a2[0] - a1[0])) - ((b2[0] - b1[0])*(a2[1] - a1[1]));

+  REAL nume_a = ((b2[0] - b1[0])*(a1[1] - b1[1])) - ((b2[1] - b1[1])*(a1[0] - b1[0]));

+  REAL nume_b = ((a2[0] - a1[0])*(a1[1] - b1[1])) - ((a2[1] - a1[1])*(a1[0] - b1[0]));

+  if (denom == 0 )

+  {

+    if(nume_a == 0 && nume_b == 0)

+    {

+      ret = IR_COINCIDENT;

+    }

+    else

+    {

+      ret = IR_PARALLEL;

+    }

+  }

+  else

+  {

+

+    REAL recip = 1 / denom;

+    REAL ua = nume_a * recip;

+    REAL ub = nume_b * recip;

+

+    if(ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1 )

+    {

+      // Get the intersection point.

+      intersection[0] = a1[0] + ua*(a2[0] - a1[0]);

+      intersection[1] = a1[1] + ua*(a2[1] - a1[1]);

+      ret = IR_DO_INTERSECT;

+    }

+    else

+    {

+      ret = IR_DONT_INTERSECT;

+    }

+  }

+  return ret;

+}

+

+IntersectResult fm_intersectLineSegments2dTime(const REAL *a1,const REAL *a2,const REAL *b1,const REAL *b2,REAL &t1,REAL &t2)

+{

+  IntersectResult ret;

+

+  REAL denom  = ((b2[1] - b1[1])*(a2[0] - a1[0])) - ((b2[0] - b1[0])*(a2[1] - a1[1]));

+  REAL nume_a = ((b2[0] - b1[0])*(a1[1] - b1[1])) - ((b2[1] - b1[1])*(a1[0] - b1[0]));

+  REAL nume_b = ((a2[0] - a1[0])*(a1[1] - b1[1])) - ((a2[1] - a1[1])*(a1[0] - b1[0]));

+  if (denom == 0 )

+  {

+    if(nume_a == 0 && nume_b == 0)

+    {

+      ret = IR_COINCIDENT;

+    }

+    else

+    {

+      ret = IR_PARALLEL;

+    }

+  }

+  else

+  {

+

+    REAL recip = 1 / denom;

+    REAL ua = nume_a * recip;

+    REAL ub = nume_b * recip;

+

+    if(ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1 )

+    {

+      t1 = ua;

+      t2 = ub;

+      ret = IR_DO_INTERSECT;

+    }

+    else

+    {

+      ret = IR_DONT_INTERSECT;

+    }

+  }

+  return ret;

+}

+

+//**** Plane Triangle Intersection

+

+

+

+

+

+// assumes that the points are on opposite sides of the plane!

+void fm_intersectPointPlane(const REAL *p1,const REAL *p2,REAL *split,const REAL *plane)

+{

+

+  REAL dp1 = fm_distToPlane(plane,p1);

+

+  REAL dir[3];

+

+  dir[0] = p2[0] - p1[0];

+  dir[1] = p2[1] - p1[1];

+  dir[2] = p2[2] - p1[2];

+

+  REAL dot1 = dir[0]*plane[0] + dir[1]*plane[1] + dir[2]*plane[2];

+  REAL dot2 = dp1 - plane[3];

+

+  REAL    t = -(plane[3] + dot2 ) / dot1;

+

+  split[0] = (dir[0]*t)+p1[0];

+  split[1] = (dir[1]*t)+p1[1];

+  split[2] = (dir[2]*t)+p1[2];

+

+}

+

+PlaneTriResult fm_getSidePlane(const REAL *p,const REAL *plane,REAL epsilon)

+{

+  PlaneTriResult ret = PTR_ON_PLANE;

+

+  REAL d = fm_distToPlane(plane,p);

+

+  if ( d < -epsilon || d > epsilon )

+  {

+    if ( d > 0 )

+  		ret =  PTR_FRONT; // it is 'in front' within the provided epsilon value.

+    else

+      ret = PTR_BACK;

+  }

+

+  return ret;

+}

+

+

+

+#ifndef PLANE_TRIANGLE_INTERSECTION_H

+

+#define PLANE_TRIANGLE_INTERSECTION_H

+

+#define MAXPTS 256

+

+template <class Type> class point

+{

+public:

+

+  void set(const Type *p)

+  {

+    x = p[0];

+    y = p[1];

+    z = p[2];

+  }

+

+  Type x;

+  Type y;

+  Type z;

+};

+

+template <class Type> class plane

+{

+public:

+  plane(const Type *p)

+  {

+    normal.x = p[0];

+    normal.y = p[1];

+    normal.z = p[2];

+    D        = p[3];

+  }

+

+  Type Classify_Point(const point<Type> &p)

+  {

+    return p.x*normal.x + p.y*normal.y + p.z*normal.z + D;

+  }

+

+  point<Type> normal;

+  Type  D;

+};

+

+template <class Type> class polygon

+{

+public:

+  polygon(void)

+  {

+    mVcount = 0;

+  }

+

+  polygon(const Type *p1,const Type *p2,const Type *p3)

+  {

+    mVcount = 3;

+    mVertices[0].set(p1);

+    mVertices[1].set(p2);

+    mVertices[2].set(p3);

+  }

+

+

+  int32_t NumVertices(void) const { return mVcount; };

+

+  const point<Type>& Vertex(int32_t index)

+  {

+    if ( index < 0 ) index+=mVcount;

+    return mVertices[index];

+  };

+

+

+  void set(const point<Type> *pts,int32_t count)

+  {

+    for (int32_t i=0; i<count; i++)

+    {

+      mVertices[i] = pts[i];

+    }

+    mVcount = count;

+  }

+

+

+  void Split_Polygon(polygon<Type> *poly,plane<Type> *part, polygon<Type> &front, polygon<Type> &back)

+  {

+    int32_t   count = poly->NumVertices ();

+    int32_t   out_c = 0, in_c = 0;

+    point<Type> ptA, ptB,outpts[MAXPTS],inpts[MAXPTS];

+    Type sideA, sideB;

+    ptA = poly->Vertex (count - 1);

+    sideA = part->Classify_Point (ptA);

+    for (int32_t i = -1; ++i < count;)

+    {

+      ptB = poly->Vertex(i);

+      sideB = part->Classify_Point(ptB);

+      if (sideB > 0)

+      {

+        if (sideA < 0)

+        {

+  			  point<Type> v;

+          fm_intersectPointPlane(&ptB.x, &ptA.x, &v.x, &part->normal.x );

+          outpts[out_c++] = inpts[in_c++] = v;

+        }

+        outpts[out_c++] = ptB;

+      }

+      else if (sideB < 0)

+      {

+        if (sideA > 0)

+        {

+          point<Type> v;

+          fm_intersectPointPlane(&ptB.x, &ptA.x, &v.x, &part->normal.x );

+          outpts[out_c++] = inpts[in_c++] = v;

+        }

+        inpts[in_c++] = ptB;

+      }

+      else

+         outpts[out_c++] = inpts[in_c++] = ptB;

+      ptA = ptB;

+      sideA = sideB;

+    }

+

+    front.set(&outpts[0], out_c);

+    back.set(&inpts[0], in_c);

+  }

+

+  int32_t           mVcount;

+  point<Type>   mVertices[MAXPTS];

+};

+

+

+

+#endif

+

+static inline void add(const REAL *p,REAL *dest,uint32_t tstride,uint32_t &pcount)

+{

+  char *d = (char *) dest;

+  d = d + pcount*tstride;

+  dest = (REAL *) d;

+  dest[0] = p[0];

+  dest[1] = p[1];

+  dest[2] = p[2];

+  pcount++;

+	assert( pcount <= 4 );

+}

+

+

+PlaneTriResult fm_planeTriIntersection(const REAL *_plane,    // the plane equation in Ax+By+Cz+D format

+                                    const REAL *triangle, // the source triangle.

+                                    uint32_t tstride,  // stride in bytes of the input and output *vertices*

+                                    REAL        epsilon,  // the co-planar epsilon value.

+                                    REAL       *front,    // the triangle in front of the

+                                    uint32_t &fcount,  // number of vertices in the 'front' triangle

+                                    REAL       *back,     // the triangle in back of the plane

+                                    uint32_t &bcount) // the number of vertices in the 'back' triangle.

+{

+

+  fcount = 0;

+  bcount = 0;

+

+  const char *tsource = (const char *) triangle;

+

+  // get the three vertices of the triangle.

+  const REAL *p1     = (const REAL *) (tsource);

+  const REAL *p2     = (const REAL *) (tsource+tstride);

+  const REAL *p3     = (const REAL *) (tsource+tstride*2);

+

+

+  PlaneTriResult r1   = fm_getSidePlane(p1,_plane,epsilon); // compute the side of the plane each vertex is on

+  PlaneTriResult r2   = fm_getSidePlane(p2,_plane,epsilon);

+  PlaneTriResult r3   = fm_getSidePlane(p3,_plane,epsilon);

+

+  // If any of the points lay right *on* the plane....

+  if ( r1 == PTR_ON_PLANE || r2 == PTR_ON_PLANE || r3 == PTR_ON_PLANE )

+  {

+    // If the triangle is completely co-planar, then just treat it as 'front' and return!

+    if ( r1 == PTR_ON_PLANE && r2 == PTR_ON_PLANE && r3 == PTR_ON_PLANE )

+    {

+      add(p1,front,tstride,fcount);

+      add(p2,front,tstride,fcount);

+      add(p3,front,tstride,fcount);

+      return PTR_FRONT;

+    }

+    // Decide to place the co-planar points on the same side as the co-planar point.

+    PlaneTriResult r= PTR_ON_PLANE;

+    if ( r1 != PTR_ON_PLANE )

+      r = r1;

+    else if ( r2 != PTR_ON_PLANE )

+      r = r2;

+    else if ( r3 != PTR_ON_PLANE )

+      r = r3;

+

+    if ( r1 == PTR_ON_PLANE ) r1 = r;

+    if ( r2 == PTR_ON_PLANE ) r2 = r;

+    if ( r3 == PTR_ON_PLANE ) r3 = r;

+

+  }

+

+  if ( r1 == r2 && r1 == r3 ) // if all three vertices are on the same side of the plane.

+  {

+    if ( r1 == PTR_FRONT ) // if all three are in front of the plane, then copy to the 'front' output triangle.

+    {

+      add(p1,front,tstride,fcount);

+      add(p2,front,tstride,fcount);

+      add(p3,front,tstride,fcount);

+    }

+    else

+    {

+      add(p1,back,tstride,bcount); // if all three are in 'back' then copy to the 'back' output triangle.

+      add(p2,back,tstride,bcount);

+      add(p3,back,tstride,bcount);

+    }

+    return r1; // if all three points are on the same side of the plane return result

+  }

+

+

+  polygon<REAL> pi(p1,p2,p3);

+  polygon<REAL>  pfront,pback;

+

+  plane<REAL>    part(_plane);

+

+  pi.Split_Polygon(&pi,&part,pfront,pback);

+

+  for (int32_t i=0; i<pfront.mVcount; i++)

+  {

+    add( &pfront.mVertices[i].x, front, tstride, fcount );

+  }

+

+  for (int32_t i=0; i<pback.mVcount; i++)

+  {

+    add( &pback.mVertices[i].x, back, tstride, bcount );

+  }

+

+  PlaneTriResult ret = PTR_SPLIT;

+

+  if ( fcount < 3 ) fcount = 0;

+  if ( bcount < 3 ) bcount = 0;

+

+  if ( fcount == 0 && bcount )

+    ret = PTR_BACK;

+

+  if ( bcount == 0 && fcount )

+    ret = PTR_FRONT;

+

+

+  return ret;

+}

+

+// computes the OBB for this set of points relative to this transform matrix.

+void computeOBB(uint32_t vcount,const REAL *points,uint32_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 (uint32_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 fm_computeBestFitOBB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *sides,REAL *matrix,bool bruteForce)

+{

+  REAL plane[4];

+  fm_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];

+      }

+    }

+  }

+}

+

+void fm_computeBestFitOBB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *sides,REAL *pos,REAL *quat,bool bruteForce)

+{

+  REAL matrix[16];

+  fm_computeBestFitOBB(vcount,points,pstride,sides,matrix,bruteForce);

+  fm_getTranslation(matrix,pos);

+  fm_matrixToQuat(matrix,quat);

+}

+

+void fm_computeBestFitABB(uint32_t vcount,const REAL *points,uint32_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 (uint32_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;

+

+}

+

+

+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_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);

+}

+

+

+//**********************************************************

+//**********************************************************

+//**** Vertex Welding

+//**********************************************************

+//**********************************************************

+

+#ifndef VERTEX_INDEX_H

+

+#define VERTEX_INDEX_H

+

+namespace VERTEX_INDEX

+{

+

+class KdTreeNode;

+

+typedef std::vector< KdTreeNode * > KdTreeNodeVector;

+

+enum Axes

+{

+  X_AXIS = 0,

+  Y_AXIS = 1,

+  Z_AXIS = 2

+};

+

+class KdTreeFindNode

+{

+public:

+  KdTreeFindNode(void)

+  {

+    mNode = 0;

+    mDistance = 0;

+  }

+  KdTreeNode  *mNode;

+  double        mDistance;

+};

+

+class KdTreeInterface

+{

+public:

+  virtual const double * getPositionDouble(uint32_t index) const = 0;

+  virtual const float  * getPositionFloat(uint32_t index) const = 0;

+};

+

+class KdTreeNode

+{

+public:

+  KdTreeNode(void)

+  {

+    mIndex = 0;

+    mLeft = 0;

+    mRight = 0;

+  }

+

+  KdTreeNode(uint32_t index)

+  {

+    mIndex = index;

+    mLeft = 0;

+    mRight = 0;

+  };

+

+	~KdTreeNode(void)

+  {

+  }

+

+

+  void addDouble(KdTreeNode *node,Axes dim,const KdTreeInterface *iface)

+  {

+    const double *nodePosition = iface->getPositionDouble( node->mIndex );

+    const double *position     = iface->getPositionDouble( mIndex );

+    switch ( dim )

+    {

+      case X_AXIS:

+        if ( nodePosition[0] <= position[0] )

+        {

+          if ( mLeft )

+            mLeft->addDouble(node,Y_AXIS,iface);

+          else

+            mLeft = node;

+        }

+        else

+        {

+          if ( mRight )

+            mRight->addDouble(node,Y_AXIS,iface);

+          else

+            mRight = node;

+        }

+        break;

+      case Y_AXIS:

+        if ( nodePosition[1] <= position[1] )

+        {

+          if ( mLeft )

+            mLeft->addDouble(node,Z_AXIS,iface);

+          else

+            mLeft = node;

+        }

+        else

+        {

+          if ( mRight )

+            mRight->addDouble(node,Z_AXIS,iface);

+          else

+            mRight = node;

+        }

+        break;

+      case Z_AXIS:

+        if ( nodePosition[2] <= position[2] )

+        {

+          if ( mLeft )

+            mLeft->addDouble(node,X_AXIS,iface);

+          else

+            mLeft = node;

+        }

+        else

+        {

+          if ( mRight )

+            mRight->addDouble(node,X_AXIS,iface);

+          else

+            mRight = node;

+        }

+        break;

+    }

+

+  }

+

+

+  void addFloat(KdTreeNode *node,Axes dim,const KdTreeInterface *iface)

+  {

+    const float *nodePosition = iface->getPositionFloat( node->mIndex );

+    const float *position     = iface->getPositionFloat( mIndex );

+    switch ( dim )

+    {

+      case X_AXIS:

+        if ( nodePosition[0] <= position[0] )

+        {

+          if ( mLeft )

+            mLeft->addFloat(node,Y_AXIS,iface);

+          else

+            mLeft = node;

+        }

+        else

+        {

+          if ( mRight )

+            mRight->addFloat(node,Y_AXIS,iface);

+          else

+            mRight = node;

+        }

+        break;

+      case Y_AXIS:

+        if ( nodePosition[1] <= position[1] )

+        {

+          if ( mLeft )

+            mLeft->addFloat(node,Z_AXIS,iface);

+          else

+            mLeft = node;

+        }

+        else

+        {

+          if ( mRight )

+            mRight->addFloat(node,Z_AXIS,iface);

+          else

+            mRight = node;

+        }

+        break;

+      case Z_AXIS:

+        if ( nodePosition[2] <= position[2] )

+        {

+          if ( mLeft )

+            mLeft->addFloat(node,X_AXIS,iface);

+          else

+            mLeft = node;

+        }

+        else

+        {

+          if ( mRight )

+            mRight->addFloat(node,X_AXIS,iface);

+          else

+            mRight = node;

+        }

+        break;

+    }

+

+  }

+

+

+  uint32_t getIndex(void) const { return mIndex; };

+

+  void search(Axes axis,const double *pos,double radius,uint32_t &count,uint32_t maxObjects,KdTreeFindNode *found,const KdTreeInterface *iface)

+  {

+

+    const double *position = iface->getPositionDouble(mIndex);

+

+    double dx = pos[0] - position[0];

+    double dy = pos[1] - position[1];

+    double dz = pos[2] - position[2];

+

+    KdTreeNode *search1 = 0;

+    KdTreeNode *search2 = 0;

+

+    switch ( axis )

+    {

+      case X_AXIS:

+       if ( dx <= 0 )     // JWR  if we are to the left

+       {

+        search1 = mLeft; // JWR  then search to the left

+        if ( -dx < radius )  // JWR  if distance to the right is less than our search radius, continue on the right as well.

+          search2 = mRight;

+       }

+       else

+       {

+         search1 = mRight; // JWR  ok, we go down the left tree

+         if ( dx < radius ) // JWR  if the distance from the right is less than our search radius

+	  			search2 = mLeft;

+        }

+        axis = Y_AXIS;

+        break;

+      case Y_AXIS:

+        if ( dy <= 0 )

+        {

+          search1 = mLeft;

+          if ( -dy < radius )

+    				search2 = mRight;

+        }

+        else

+        {

+          search1 = mRight;

+          if ( dy < radius )

+    				search2 = mLeft;

+        }

+        axis = Z_AXIS;

+        break;

+      case Z_AXIS:

+        if ( dz <= 0 )

+        {

+          search1 = mLeft;

+          if ( -dz < radius )

+    				search2 = mRight;

+        }

+        else

+        {

+          search1 = mRight;

+          if ( dz < radius )

+    				search2 = mLeft;

+        }

+        axis = X_AXIS;

+        break;

+    }

+

+    double r2 = radius*radius;

+    double m  = dx*dx+dy*dy+dz*dz;

+

+    if ( m < r2 )

+    {

+      switch ( count )

+      {

+        case 0:

+          found[count].mNode = this;

+          found[count].mDistance = m;

+          break;

+        case 1:

+          if ( m < found[0].mDistance )

+          {

+            if ( maxObjects == 1 )

+            {

+              found[0].mNode = this;

+              found[0].mDistance = m;

+            }

+            else

+            {

+              found[1] = found[0];

+              found[0].mNode = this;

+              found[0].mDistance = m;

+            }

+          }

+          else if ( maxObjects > 1)

+          {

+            found[1].mNode = this;

+            found[1].mDistance = m;

+          }

+          break;

+        default:

+          {

+            bool inserted = false;

+

+            for (uint32_t i=0; i<count; i++)

+            {

+              if ( m < found[i].mDistance ) // if this one is closer than a pre-existing one...

+              {

+                // insertion sort...

+                uint32_t scan = count;

+                if ( scan >= maxObjects ) scan=maxObjects-1;

+                for (uint32_t j=scan; j>i; j--)

+                {

+                  found[j] = found[j-1];

+                }

+                found[i].mNode = this;

+                found[i].mDistance = m;

+                inserted = true;

+                break;

+              }

+            }

+

+            if ( !inserted && count < maxObjects )

+            {

+              found[count].mNode = this;

+              found[count].mDistance = m;

+            }

+          }

+          break;

+      }

+      count++;

+      if ( count > maxObjects )

+      {

+        count = maxObjects;

+      }

+    }

+

+

+    if ( search1 )

+  		search1->search( axis, pos,radius, count, maxObjects, found, iface);

+

+    if ( search2 )

+	  	search2->search( axis, pos,radius, count, maxObjects, found, iface);

+

+  }

+

+  void search(Axes axis,const float *pos,float radius,uint32_t &count,uint32_t maxObjects,KdTreeFindNode *found,const KdTreeInterface *iface)

+  {

+

+    const float *position = iface->getPositionFloat(mIndex);

+

+    float dx = pos[0] - position[0];

+    float dy = pos[1] - position[1];

+    float dz = pos[2] - position[2];

+

+    KdTreeNode *search1 = 0;

+    KdTreeNode *search2 = 0;

+

+    switch ( axis )

+    {

+      case X_AXIS:

+       if ( dx <= 0 )     // JWR  if we are to the left

+       {

+        search1 = mLeft; // JWR  then search to the left

+        if ( -dx < radius )  // JWR  if distance to the right is less than our search radius, continue on the right as well.

+          search2 = mRight;

+       }

+       else

+       {

+         search1 = mRight; // JWR  ok, we go down the left tree

+         if ( dx < radius ) // JWR  if the distance from the right is less than our search radius

+	  			search2 = mLeft;

+        }

+        axis = Y_AXIS;

+        break;

+      case Y_AXIS:

+        if ( dy <= 0 )

+        {

+          search1 = mLeft;

+          if ( -dy < radius )

+    				search2 = mRight;

+        }

+        else

+        {

+          search1 = mRight;

+          if ( dy < radius )

+    				search2 = mLeft;

+        }

+        axis = Z_AXIS;

+        break;

+      case Z_AXIS:

+        if ( dz <= 0 )

+        {

+          search1 = mLeft;

+          if ( -dz < radius )

+    				search2 = mRight;

+        }

+        else

+        {

+          search1 = mRight;

+          if ( dz < radius )

+    				search2 = mLeft;

+        }

+        axis = X_AXIS;

+        break;

+    }

+

+    float r2 = radius*radius;

+    float m  = dx*dx+dy*dy+dz*dz;

+

+    if ( m < r2 )

+    {

+      switch ( count )

+      {

+        case 0:

+          found[count].mNode = this;

+          found[count].mDistance = m;

+          break;

+        case 1:

+          if ( m < found[0].mDistance )

+          {

+            if ( maxObjects == 1 )

+            {

+              found[0].mNode = this;

+              found[0].mDistance = m;

+            }

+            else

+            {

+              found[1] = found[0];

+              found[0].mNode = this;

+              found[0].mDistance = m;

+            }

+          }

+          else if ( maxObjects > 1)

+          {

+            found[1].mNode = this;

+            found[1].mDistance = m;

+          }

+          break;

+        default:

+          {

+            bool inserted = false;

+

+            for (uint32_t i=0; i<count; i++)

+            {

+              if ( m < found[i].mDistance ) // if this one is closer than a pre-existing one...

+              {

+                // insertion sort...

+                uint32_t scan = count;

+                if ( scan >= maxObjects ) scan=maxObjects-1;

+                for (uint32_t j=scan; j>i; j--)

+                {

+                  found[j] = found[j-1];

+                }

+                found[i].mNode = this;

+                found[i].mDistance = m;

+                inserted = true;

+                break;

+              }

+            }

+

+            if ( !inserted && count < maxObjects )

+            {

+              found[count].mNode = this;

+              found[count].mDistance = m;

+            }

+          }

+          break;

+      }

+      count++;

+      if ( count > maxObjects )

+      {

+        count = maxObjects;

+      }

+    }

+

+

+    if ( search1 )

+  		search1->search( axis, pos,radius, count, maxObjects, found, iface);

+

+    if ( search2 )

+	  	search2->search( axis, pos,radius, count, maxObjects, found, iface);

+

+  }

+

+private:

+

+  void setLeft(KdTreeNode *left) { mLeft = left; };

+  void setRight(KdTreeNode *right) { mRight = right; };

+

+	KdTreeNode *getLeft(void)         { return mLeft; }

+	KdTreeNode *getRight(void)        { return mRight; }

+

+  uint32_t          mIndex;

+  KdTreeNode     *mLeft;

+  KdTreeNode     *mRight;

+};

+

+

+#define MAX_BUNDLE_SIZE 1024  // 1024 nodes at a time, to minimize memory allocation and guarantee that pointers are persistent.

+

+class KdTreeNodeBundle 

+{

+public:

+

+  KdTreeNodeBundle(void)

+  {

+    mNext = 0;

+    mIndex = 0;

+  }

+

+  bool isFull(void) const

+  {

+    return (bool)( mIndex == MAX_BUNDLE_SIZE );

+  }

+

+  KdTreeNode * getNextNode(void)

+  {

+    assert(mIndex<MAX_BUNDLE_SIZE);

+    KdTreeNode *ret = &mNodes[mIndex];

+    mIndex++;

+    return ret;

+  }

+

+  KdTreeNodeBundle  *mNext;

+  uint32_t             mIndex;

+  KdTreeNode         mNodes[MAX_BUNDLE_SIZE];

+};

+

+

+typedef std::vector< double > DoubleVector;

+typedef std::vector< float >  FloatVector;

+

+class KdTree : public KdTreeInterface

+{

+public:

+  KdTree(void)

+  {

+    mRoot = 0;

+    mBundle = 0;

+    mVcount = 0;

+    mUseDouble = false;

+  }

+

+  virtual ~KdTree(void)

+  {

+    reset();

+  }

+

+  const double * getPositionDouble(uint32_t index) const

+  {

+    assert( mUseDouble );

+    assert ( index < mVcount );

+    return  &mVerticesDouble[index*3];

+  }

+

+  const float * getPositionFloat(uint32_t index) const

+  {

+    assert( !mUseDouble );

+    assert ( index < mVcount );

+    return  &mVerticesFloat[index*3];

+  }

+

+  uint32_t search(const double *pos,double radius,uint32_t maxObjects,KdTreeFindNode *found) const

+  {

+    assert( mUseDouble );

+    if ( !mRoot )	return 0;

+    uint32_t count = 0;

+    mRoot->search(X_AXIS,pos,radius,count,maxObjects,found,this);

+    return count;

+  }

+

+  uint32_t search(const float *pos,float radius,uint32_t maxObjects,KdTreeFindNode *found) const

+  {

+    assert( !mUseDouble );

+    if ( !mRoot )	return 0;

+    uint32_t count = 0;

+    mRoot->search(X_AXIS,pos,radius,count,maxObjects,found,this);

+    return count;

+  }

+

+  void reset(void)

+  {

+    mRoot = 0;

+    mVerticesDouble.clear();

+    mVerticesFloat.clear();

+    KdTreeNodeBundle *bundle = mBundle;

+    while ( bundle )

+    {

+      KdTreeNodeBundle *next = bundle->mNext;

+      delete bundle;

+      bundle = next;

+    }

+    mBundle = 0;

+    mVcount = 0;

+  }

+

+  uint32_t add(double x,double y,double z)

+  {

+    assert(mUseDouble);

+    uint32_t ret = mVcount;

+    mVerticesDouble.push_back(x);

+    mVerticesDouble.push_back(y);

+    mVerticesDouble.push_back(z);

+    mVcount++;

+    KdTreeNode *node = getNewNode(ret);

+    if ( mRoot )

+    {

+      mRoot->addDouble(node,X_AXIS,this);

+    }

+    else

+    {

+      mRoot = node;

+    }

+    return ret;

+  }

+

+  uint32_t add(float x,float y,float z)

+  {

+    assert(!mUseDouble);

+    uint32_t ret = mVcount;

+    mVerticesFloat.push_back(x);

+    mVerticesFloat.push_back(y);

+    mVerticesFloat.push_back(z);

+    mVcount++;

+    KdTreeNode *node = getNewNode(ret);

+    if ( mRoot )

+    {

+      mRoot->addFloat(node,X_AXIS,this);

+    }

+    else

+    {

+      mRoot = node;

+    }

+    return ret;

+  }

+

+  KdTreeNode * getNewNode(uint32_t index)

+  {

+    if ( mBundle == 0 )

+    {

+      mBundle = new KdTreeNodeBundle;

+    }

+    if ( mBundle->isFull() )

+    {

+      KdTreeNodeBundle *bundle = new KdTreeNodeBundle;

+      mBundle->mNext = bundle;

+      mBundle = bundle;

+    }

+    KdTreeNode *node = mBundle->getNextNode();

+    new ( node ) KdTreeNode(index);

+    return node;

+  }

+

+  uint32_t getNearest(const double *pos,double radius,bool &_found) const // returns the nearest possible neighbor's index.

+  {

+    assert( mUseDouble );

+    uint32_t ret = 0;

+

+    _found = false;

+    KdTreeFindNode found[1];

+    uint32_t count = search(pos,radius,1,found);

+    if ( count )

+    {

+      KdTreeNode *node = found[0].mNode;

+      ret = node->getIndex();

+      _found = true;

+    }

+    return ret;

+  }

+

+  uint32_t getNearest(const float *pos,float radius,bool &_found) const // returns the nearest possible neighbor's index.

+  {

+    assert( !mUseDouble );

+    uint32_t ret = 0;

+

+    _found = false;

+    KdTreeFindNode found[1];

+    uint32_t count = search(pos,radius,1,found);

+    if ( count )

+    {

+      KdTreeNode *node = found[0].mNode;

+      ret = node->getIndex();

+      _found = true;

+    }

+    return ret;

+  }

+

+  const double * getVerticesDouble(void) const

+  {

+    assert( mUseDouble );

+    const double *ret = 0;

+    if ( !mVerticesDouble.empty() )

+    {

+      ret = &mVerticesDouble[0];

+    }

+    return ret;

+  }

+

+  const float * getVerticesFloat(void) const

+  {

+    assert( !mUseDouble );

+    const float * ret = 0;

+    if ( !mVerticesFloat.empty() )

+    {

+      ret = &mVerticesFloat[0];

+    }

+    return ret;

+  }

+

+  uint32_t getVcount(void) const { return mVcount; };

+

+  void setUseDouble(bool useDouble)

+  {

+    mUseDouble = useDouble;

+  }

+

+private:

+  bool                    mUseDouble;

+  KdTreeNode             *mRoot;

+  KdTreeNodeBundle       *mBundle;

+  uint32_t                  mVcount;

+  DoubleVector            mVerticesDouble;

+  FloatVector             mVerticesFloat;

+};

+

+}; // end of namespace VERTEX_INDEX

+

+class MyVertexIndex : public fm_VertexIndex

+{

+public:

+  MyVertexIndex(double granularity,bool snapToGrid)

+  {

+    mDoubleGranularity = granularity;

+    mFloatGranularity  = (float)granularity;

+    mSnapToGrid        = snapToGrid;

+    mUseDouble         = true;

+    mKdTree.setUseDouble(true);

+  }

+

+  MyVertexIndex(float granularity,bool snapToGrid)

+  {

+    mDoubleGranularity = granularity;

+    mFloatGranularity  = (float)granularity;

+    mSnapToGrid        = snapToGrid;

+    mUseDouble         = false;

+    mKdTree.setUseDouble(false);

+  }

+

+  virtual ~MyVertexIndex(void)

+  {

+

+  }

+

+

+  double snapToGrid(double p)

+  {

+    double m = fmod(p,mDoubleGranularity);

+    p-=m;

+    return p;

+  }

+

+  float snapToGrid(float p)

+  {

+    float m = fmodf(p,mFloatGranularity);

+    p-=m;

+    return p;

+  }

+

+  uint32_t    getIndex(const float *_p,bool &newPos)  // get index for a vector float

+  {

+    uint32_t ret;

+

+    if ( mUseDouble )

+    {

+      double p[3];

+      p[0] = _p[0];

+      p[1] = _p[1];

+      p[2] = _p[2];

+      return getIndex(p,newPos);

+    }

+

+    newPos = false;

+

+    float p[3];

+

+    if ( mSnapToGrid )

+    {

+      p[0] = snapToGrid(_p[0]);

+      p[1] = snapToGrid(_p[1]);

+      p[2] = snapToGrid(_p[2]);

+    }

+    else

+    {

+      p[0] = _p[0];

+      p[1] = _p[1];

+      p[2] = _p[2];

+    }

+

+    bool found;

+    ret = mKdTree.getNearest(p,mFloatGranularity,found);

+    if ( !found )

+    {

+      newPos = true;

+      ret = mKdTree.add(p[0],p[1],p[2]);

+    }

+

+

+    return ret;

+  }

+

+  uint32_t    getIndex(const double *_p,bool &newPos)  // get index for a vector double

+  {

+    uint32_t ret;

+

+    if ( !mUseDouble )

+    {

+      float p[3];

+      p[0] = (float)_p[0];

+      p[1] = (float)_p[1];

+      p[2] = (float)_p[2];

+      return getIndex(p,newPos);

+    }

+

+    newPos = false;

+

+    double p[3];

+

+    if ( mSnapToGrid )

+    {

+      p[0] = snapToGrid(_p[0]);

+      p[1] = snapToGrid(_p[1]);

+      p[2] = snapToGrid(_p[2]);

+    }

+    else

+    {

+      p[0] = _p[0];

+      p[1] = _p[1];

+      p[2] = _p[2];

+    }

+

+    bool found;

+    ret = mKdTree.getNearest(p,mDoubleGranularity,found);

+    if ( !found )

+    {

+      newPos = true;

+      ret = mKdTree.add(p[0],p[1],p[2]);

+    }

+

+

+    return ret;

+  }

+

+  const float *   getVerticesFloat(void) const

+  {

+    const float * ret = 0;

+

+    assert( !mUseDouble );

+

+    ret = mKdTree.getVerticesFloat();

+

+    return ret;

+  }

+

+  const double *  getVerticesDouble(void) const

+  {

+    const double * ret = 0;

+

+    assert( mUseDouble );

+

+    ret = mKdTree.getVerticesDouble();

+

+    return ret;

+  }

+

+  const float *   getVertexFloat(uint32_t index) const

+  {

+    const float * ret  = 0;

+    assert( !mUseDouble );

+#ifdef _DEBUG

+    uint32_t vcount = mKdTree.getVcount();

+    assert( index < vcount );

+#endif

+    ret =  mKdTree.getVerticesFloat();

+    ret = &ret[index*3];

+    return ret;

+  }

+

+  const double *   getVertexDouble(uint32_t index) const

+  {

+    const double * ret = 0;

+    assert( mUseDouble );

+#ifdef _DEBUG

+    uint32_t vcount = mKdTree.getVcount();

+    assert( index < vcount );

+#endif

+    ret =  mKdTree.getVerticesDouble();

+    ret = &ret[index*3];

+

+    return ret;

+  }

+

+  uint32_t    getVcount(void) const

+  {

+    return mKdTree.getVcount();

+  }

+

+  bool isDouble(void) const

+  {

+    return mUseDouble;

+  }

+

+

+  bool            saveAsObj(const char *fname,uint32_t tcount,uint32_t *indices)

+  {

+    bool ret = false;

+

+

+    FILE *fph = fopen(fname,"wb");

+    if ( fph )

+    {

+      ret = true;

+

+      uint32_t vcount    = getVcount();

+      if ( mUseDouble )

+      {

+        const double *v  = getVerticesDouble();

+        for (uint32_t i=0; i<vcount; i++)

+        {

+          fprintf(fph,"v %0.9f %0.9f %0.9f\r\n", (float)v[0], (float)v[1], (float)v[2] );

+          v+=3;

+        }

+      }

+      else

+      {

+        const float *v  = getVerticesFloat();

+        for (uint32_t i=0; i<vcount; i++)

+        {

+          fprintf(fph,"v %0.9f %0.9f %0.9f\r\n", v[0], v[1], v[2] );

+          v+=3;

+        }

+      }

+

+      for (uint32_t i=0; i<tcount; i++)

+      {

+        uint32_t i1 = *indices++;

+        uint32_t i2 = *indices++;

+        uint32_t i3 = *indices++;

+        fprintf(fph,"f %d %d %d\r\n", i1+1, i2+1, i3+1 );

+      }

+      fclose(fph);

+    }

+

+    return ret;

+  }

+

+private:

+  bool    mUseDouble:1;

+  bool    mSnapToGrid:1;

+  double  mDoubleGranularity;

+  float   mFloatGranularity;

+  VERTEX_INDEX::KdTree  mKdTree;

+};

+

+fm_VertexIndex * fm_createVertexIndex(double granularity,bool snapToGrid) // create an indexed vertex system for doubles

+{

+  MyVertexIndex *ret = new MyVertexIndex(granularity,snapToGrid);

+  return static_cast< fm_VertexIndex *>(ret);

+}

+

+fm_VertexIndex * fm_createVertexIndex(float granularity,bool snapToGrid)  // create an indexed vertext system for floats

+{

+  MyVertexIndex *ret = new MyVertexIndex(granularity,snapToGrid);

+  return static_cast< fm_VertexIndex *>(ret);

+}

+

+void          fm_releaseVertexIndex(fm_VertexIndex *vindex)

+{

+  MyVertexIndex *m = static_cast< MyVertexIndex *>(vindex);

+  delete m;

+}

+

+#endif   // END OF VERTEX WELDING CODE

+

+

+REAL fm_computeBestFitAABB(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *bmin,REAL *bmax) // returns the diagonal distance

+{

+

+  const uint8_t *source = (const uint8_t *) 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 (uint32_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 );

+

+}

+

+

+

+/* a = b - c */

+#define vector(a,b,c) \

+	(a)[0] = (b)[0] - (c)[0];	\

+	(a)[1] = (b)[1] - (c)[1];	\

+	(a)[2] = (b)[2] - (c)[2];

+

+

+

+#define innerProduct(v,q) \

+		((v)[0] * (q)[0] + \

+		(v)[1] * (q)[1] + \

+		(v)[2] * (q)[2])

+

+#define crossProduct(a,b,c) \

+	(a)[0] = (b)[1] * (c)[2] - (c)[1] * (b)[2]; \

+	(a)[1] = (b)[2] * (c)[0] - (c)[2] * (b)[0]; \

+	(a)[2] = (b)[0] * (c)[1] - (c)[0] * (b)[1];

+

+

+bool fm_lineIntersectsTriangle(const REAL *rayStart,const REAL *rayEnd,const REAL *p1,const REAL *p2,const REAL *p3,REAL *sect)

+{

+	REAL dir[3];

+

+  dir[0] = rayEnd[0] - rayStart[0];

+  dir[1] = rayEnd[1] - rayStart[1];

+  dir[2] = rayEnd[2] - rayStart[2];

+

+  REAL d = (REAL)sqrt(dir[0]*dir[0] + dir[1]*dir[1] + dir[2]*dir[2]);

+  REAL r = 1.0f / d;

+

+  dir[0]*=r;

+  dir[1]*=r;

+  dir[2]*=r;

+

+

+  REAL t;

+

+	bool ret = fm_rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t );

+

+	if ( ret )

+	{

+		if ( t > d )

+		{

+			sect[0] = rayStart[0] + dir[0]*t;

+			sect[1] = rayStart[1] + dir[1]*t;

+			sect[2] = rayStart[2] + dir[2]*t;

+		}

+		else

+		{

+			ret = false;

+		}

+	}

+

+  return ret;

+}

+

+

+

+bool fm_rayIntersectsTriangle(const REAL *p,const REAL *d,const REAL *v0,const REAL *v1,const REAL *v2,REAL &t)

+{

+	REAL e1[3],e2[3],h[3],s[3],q[3];

+	REAL a,f,u,v;

+

+	vector(e1,v1,v0);

+	vector(e2,v2,v0);

+	crossProduct(h,d,e2);

+	a = innerProduct(e1,h);

+

+	if (a > -0.00001 && a < 0.00001)

+		return(false);

+

+	f = 1/a;

+	vector(s,p,v0);

+	u = f * (innerProduct(s,h));

+

+	if (u < 0.0 || u > 1.0)

+		return(false);

+

+	crossProduct(q,s,e1);

+	v = f * innerProduct(d,q);

+	if (v < 0.0 || u + v > 1.0)

+		return(false);

+	// at this stage we can compute t to find out where

+	// the intersection point is on the line

+	t = f * innerProduct(e2,q);

+	if (t > 0) // ray intersection

+		return(true);

+	else // this means that there is a line intersection

+		 // but not a ray intersection

+		 return (false);

+}

+

+

+inline REAL det(const REAL *p1,const REAL *p2,const REAL *p3)

+{

+  return  p1[0]*p2[1]*p3[2] + p2[0]*p3[1]*p1[2] + p3[0]*p1[1]*p2[2] -p1[0]*p3[1]*p2[2] - p2[0]*p1[1]*p3[2] - p3[0]*p2[1]*p1[2];

+}

+

+

+REAL  fm_computeMeshVolume(const REAL *vertices,uint32_t tcount,const uint32_t *indices)

+{

+	REAL volume = 0;

+

+	for (uint32_t i=0; i<tcount; i++,indices+=3)

+	{

+  	const REAL *p1 = &vertices[ indices[0]*3 ];

+		const REAL *p2 = &vertices[ indices[1]*3 ];

+		const REAL *p3 = &vertices[ indices[2]*3 ];

+		volume+=det(p1,p2,p3); // compute the volume of the tetrahedran relative to the origin.

+	}

+

+	volume*=(1.0f/6.0f);

+	if ( volume < 0 )

+		volume*=-1;

+	return volume;

+}

+

+

+const REAL * fm_getPoint(const REAL *points,uint32_t pstride,uint32_t index)

+{

+  const uint8_t *scan = (const uint8_t *)points;

+  scan+=(index*pstride);

+  return (REAL *)scan;

+}

+

+

+bool fm_insideTriangle(REAL Ax, REAL Ay,

+                      REAL Bx, REAL By,

+                      REAL Cx, REAL Cy,

+                      REAL Px, REAL Py)

+

+{

+  REAL ax, ay, bx, by, cx, cy, apx, apy, bpx, bpy, cpx, cpy;

+  REAL cCROSSap, bCROSScp, aCROSSbp;

+

+  ax = Cx - Bx;  ay = Cy - By;

+  bx = Ax - Cx;  by = Ay - Cy;

+  cx = Bx - Ax;  cy = By - Ay;

+  apx= Px - Ax;  apy= Py - Ay;

+  bpx= Px - Bx;  bpy= Py - By;

+  cpx= Px - Cx;  cpy= Py - Cy;

+

+  aCROSSbp = ax*bpy - ay*bpx;

+  cCROSSap = cx*apy - cy*apx;

+  bCROSScp = bx*cpy - by*cpx;

+

+  return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f));

+}

+

+

+REAL fm_areaPolygon2d(uint32_t pcount,const REAL *points,uint32_t pstride)

+{

+  int32_t n = (int32_t)pcount;

+

+  REAL A=0.0f;

+  for(int32_t p=n-1,q=0; q<n; p=q++)

+  {

+    const REAL *p1 = fm_getPoint(points,pstride,p);

+    const REAL *p2 = fm_getPoint(points,pstride,q);

+    A+= p1[0]*p2[1] - p2[0]*p1[1];

+  }

+  return A*0.5f;

+}

+

+

+bool  fm_pointInsidePolygon2d(uint32_t pcount,const REAL *points,uint32_t pstride,const REAL *point,uint32_t xindex,uint32_t yindex)

+{

+  uint32_t j = pcount-1;

+  int32_t oddNodes = 0;

+

+  REAL x = point[xindex];

+  REAL y = point[yindex];

+

+  for (uint32_t i=0; i<pcount; i++)

+  {

+    const REAL *p1 = fm_getPoint(points,pstride,i);

+    const REAL *p2 = fm_getPoint(points,pstride,j);

+

+    REAL x1 = p1[xindex];

+    REAL y1 = p1[yindex];

+

+    REAL x2 = p2[xindex];

+    REAL y2 = p2[yindex];

+

+    if ( y1 < y && y2 >= y ||  y2 < y && y1 >= y )

+    {

+      if (x1+(y-y1)/(y2-y1)*(x2-x1)<x)

+      {

+        oddNodes = 1-oddNodes;

+      }

+    }

+    j = i;

+  }

+

+  return oddNodes ? true : false;

+}

+

+

+uint32_t fm_consolidatePolygon(uint32_t pcount,const REAL *points,uint32_t pstride,REAL *_dest,REAL epsilon) // collapses co-linear edges.

+{

+  uint32_t ret = 0;

+

+

+  if ( pcount >= 3 )

+  {

+    const REAL *prev = fm_getPoint(points,pstride,pcount-1);

+    const REAL *current = points;

+    const REAL *next    = fm_getPoint(points,pstride,1);

+    REAL *dest = _dest;

+

+    for (uint32_t i=0; i<pcount; i++)

+    {

+

+      next = (i+1)==pcount ? points : next;

+

+      if ( !fm_colinear(prev,current,next,epsilon) )

+      {

+        dest[0] = current[0];

+        dest[1] = current[1];

+        dest[2] = current[2];

+

+        dest+=3;

+        ret++;

+      }

+

+      prev = current;

+      current+=3;

+      next+=3;

+

+    }

+  }

+

+  return ret;

+}

+

+

+#ifndef RECT3D_TEMPLATE

+

+#define RECT3D_TEMPLATE

+

+template <class T> class Rect3d

+{

+public:

+  Rect3d(void) { };

+

+  Rect3d(const T *bmin,const T *bmax)

+  {

+

+    mMin[0] = bmin[0];

+    mMin[1] = bmin[1];

+    mMin[2] = bmin[2];

+

+    mMax[0] = bmax[0];

+    mMax[1] = bmax[1];

+    mMax[2] = bmax[2];

+

+  }

+

+  void SetMin(const T *bmin)

+  {

+    mMin[0] = bmin[0];

+    mMin[1] = bmin[1];

+    mMin[2] = bmin[2];

+  }

+

+  void SetMax(const T *bmax)

+  {

+    mMax[0] = bmax[0];

+    mMax[1] = bmax[1];

+    mMax[2] = bmax[2];

+  }

+

+	void SetMin(T x,T y,T z)

+	{

+		mMin[0] = x;

+		mMin[1] = y;

+		mMin[2] = z;

+	}

+

+	void SetMax(T x,T y,T z)

+	{

+		mMax[0] = x;

+		mMax[1] = y;

+		mMax[2] = z;

+	}

+

+  T mMin[3];

+  T mMax[3];

+};

+

+#endif

+

+void splitRect(uint32_t axis,

+						   const Rect3d<REAL> &source,

+							 Rect3d<REAL> &b1,

+							 Rect3d<REAL> &b2,

+							 const REAL *midpoint)

+{

+	switch ( axis )

+	{

+		case 0:

+			b1.SetMin(source.mMin);

+			b1.SetMax( midpoint[0], source.mMax[1], source.mMax[2] );

+

+			b2.SetMin( midpoint[0], source.mMin[1], source.mMin[2] );

+			b2.SetMax(source.mMax);

+

+			break;

+		case 1:

+			b1.SetMin(source.mMin);

+			b1.SetMax( source.mMax[0], midpoint[1], source.mMax[2] );

+

+			b2.SetMin( source.mMin[0], midpoint[1], source.mMin[2] );

+			b2.SetMax(source.mMax);

+

+			break;

+		case 2:

+			b1.SetMin(source.mMin);

+			b1.SetMax( source.mMax[0], source.mMax[1], midpoint[2] );

+

+			b2.SetMin( source.mMin[0], source.mMin[1], midpoint[2] );

+			b2.SetMax(source.mMax);

+

+			break;

+	}

+}

+

+bool fm_computeSplitPlane(uint32_t vcount,

+                          const REAL *vertices,

+                          uint32_t /* tcount */,

+                          const uint32_t * /* indices */,

+                          REAL *plane)

+{

+

+  REAL sides[3];

+  REAL matrix[16];

+

+  fm_computeBestFitOBB( vcount, vertices, sizeof(REAL)*3, sides, matrix );

+

+  REAL bmax[3];

+  REAL bmin[3];

+

+  bmax[0] = sides[0]*0.5f;

+  bmax[1] = sides[1]*0.5f;

+  bmax[2] = sides[2]*0.5f;

+

+  bmin[0] = -bmax[0];

+  bmin[1] = -bmax[1];

+  bmin[2] = -bmax[2];

+

+

+  REAL dx = sides[0];

+  REAL dy = sides[1];

+  REAL dz = sides[2];

+

+

+	uint32_t axis = 0;

+

+	if ( dy > dx )

+	{

+		axis = 1;

+	}

+

+	if ( dz > dx && dz > dy )

+	{

+		axis = 2;

+	}

+

+  REAL p1[3];

+  REAL p2[3];

+  REAL p3[3];

+

+  p3[0] = p2[0] = p1[0] = bmin[0] + dx*0.5f;

+  p3[1] = p2[1] = p1[1] = bmin[1] + dy*0.5f;

+  p3[2] = p2[2] = p1[2] = bmin[2] + dz*0.5f;

+

+  Rect3d<REAL> b(bmin,bmax);

+

+  Rect3d<REAL> b1,b2;

+

+  splitRect(axis,b,b1,b2,p1);

+

+

+  switch ( axis )

+  {

+    case 0:

+      p2[1] = bmin[1];

+      p2[2] = bmin[2];

+

+      if ( dz > dy )

+      {

+        p3[1] = bmax[1];

+        p3[2] = bmin[2];

+      }

+      else

+      {

+        p3[1] = bmin[1];

+        p3[2] = bmax[2];

+      }

+

+      break;

+    case 1:

+      p2[0] = bmin[0];

+      p2[2] = bmin[2];

+

+      if ( dx > dz )

+      {

+        p3[0] = bmax[0];

+        p3[2] = bmin[2];

+      }

+      else

+      {

+        p3[0] = bmin[0];

+        p3[2] = bmax[2];

+      }

+

+      break;

+    case 2:

+      p2[0] = bmin[0];

+      p2[1] = bmin[1];

+

+      if ( dx > dy )

+      {

+        p3[0] = bmax[0];

+        p3[1] = bmin[1];

+      }

+      else

+      {

+        p3[0] = bmin[0];

+        p3[1] = bmax[1];

+      }

+

+      break;

+  }

+

+  REAL tp1[3];

+  REAL tp2[3];

+  REAL tp3[3];

+

+  fm_transform(matrix,p1,tp1);

+  fm_transform(matrix,p2,tp2);

+  fm_transform(matrix,p3,tp3);

+

+	plane[3] = fm_computePlane(tp1,tp2,tp3,plane);

+

+  return true;

+

+}

+

+#pragma warning(disable:4100)

+

+void fm_nearestPointInTriangle(const REAL * /*nearestPoint*/,const REAL * /*p1*/,const REAL * /*p2*/,const REAL * /*p3*/,REAL * /*nearest*/)

+{

+

+}

+

+static REAL Partial(const REAL *a,const REAL *p) 

+{

+	return (a[0]*p[1]) - (p[0]*a[1]);

+}

+

+REAL  fm_areaTriangle(const REAL *p0,const REAL *p1,const REAL *p2)

+{

+  REAL A = Partial(p0,p1);

+	A+= Partial(p1,p2);

+	A+= Partial(p2,p0);

+	return A*0.5f;

+}

+

+void fm_subtract(const REAL *A,const REAL *B,REAL *diff) // compute A-B and store the result in 'diff'

+{

+  diff[0] = A[0]-B[0];

+  diff[1] = A[1]-B[1];

+  diff[2] = A[2]-B[2];

+}

+

+

+void  fm_multiplyTransform(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_multiply(REAL *A,REAL scaler)

+{

+  A[0]*=scaler;

+  A[1]*=scaler;

+  A[2]*=scaler;

+}

+

+void fm_add(const REAL *A,const REAL *B,REAL *sum)

+{

+  sum[0] = A[0]+B[0];

+  sum[1] = A[1]+B[1];

+  sum[2] = A[2]+B[2];

+}

+

+void fm_copy3(const REAL *source,REAL *dest)

+{

+  dest[0] = source[0];

+  dest[1] = source[1];

+  dest[2] = source[2];

+}

+

+

+uint32_t  fm_copyUniqueVertices(uint32_t vcount,const REAL *input_vertices,REAL *output_vertices,uint32_t tcount,const uint32_t *input_indices,uint32_t *output_indices)

+{

+  uint32_t ret = 0;

+

+  REAL *vertices = (REAL *)malloc(sizeof(REAL)*vcount*3);

+  memcpy(vertices,input_vertices,sizeof(REAL)*vcount*3);

+  REAL *dest = output_vertices;

+

+  uint32_t *reindex = (uint32_t *)malloc(sizeof(uint32_t)*vcount);

+  memset(reindex,0xFF,sizeof(uint32_t)*vcount);

+

+  uint32_t icount = tcount*3;

+

+  for (uint32_t i=0; i<icount; i++)

+  {

+    uint32_t index = *input_indices++;

+

+    assert( index < vcount );

+

+    if ( reindex[index] == 0xFFFFFFFF )

+    {

+      *output_indices++ = ret;

+      reindex[index] = ret;

+      const REAL *pos = &vertices[index*3];

+      dest[0] = pos[0];

+      dest[1] = pos[1];

+      dest[2] = pos[2];

+      dest+=3;

+      ret++;

+    }

+    else

+    {

+      *output_indices++ = reindex[index];

+    }

+  }

+  free(vertices);

+  free(reindex);

+  return ret;

+}

+

+bool    fm_isMeshCoplanar(uint32_t tcount,const uint32_t *indices,const REAL *vertices,bool doubleSided) // returns true if this collection of indexed triangles are co-planar!

+{

+  bool ret = true;

+

+  if ( tcount > 0 )

+  {

+    uint32_t i1 = indices[0];

+    uint32_t i2 = indices[1];

+    uint32_t i3 = indices[2];

+    const REAL *p1 = &vertices[i1*3];

+    const REAL *p2 = &vertices[i2*3];

+    const REAL *p3 = &vertices[i3*3];

+    REAL plane[4];

+    plane[3] = fm_computePlane(p1,p2,p3,plane);

+    const uint32_t *scan = &indices[3];

+    for (uint32_t i=1; i<tcount; i++)

+    {

+      i1 = *scan++;

+      i2 = *scan++;

+      i3 = *scan++;

+      p1 = &vertices[i1*3];

+      p2 = &vertices[i2*3];

+      p3 = &vertices[i3*3];

+      REAL _plane[4];

+      _plane[3] = fm_computePlane(p1,p2,p3,_plane);

+      if ( !fm_samePlane(plane,_plane,0.01f,0.001f,doubleSided) )

+      {

+        ret = false;

+        break;

+      }

+    }

+  }

+  return ret;

+}

+

+

+bool fm_samePlane(const REAL p1[4],const REAL p2[4],REAL normalEpsilon,REAL dEpsilon,bool doubleSided)

+{

+  bool ret = false;

+

+  REAL diff = (REAL) fabs(p1[3]-p2[3]);

+  if ( diff < dEpsilon ) // if the plane -d  co-efficient is within our epsilon

+  {

+    REAL dot = fm_dot(p1,p2); // compute the dot-product of the vector normals.

+    if ( doubleSided ) dot = (REAL)fabs(dot);

+    REAL dmin = 1 - normalEpsilon;

+    REAL dmax = 1 + normalEpsilon;

+    if ( dot >= dmin && dot <= dmax )

+    {

+      ret = true; // then the plane equation is for practical purposes identical.

+    }

+  }

+

+  return ret;

+}

+

+

+void  fm_initMinMax(REAL bmin[3],REAL bmax[3])

+{

+  bmin[0] = FLT_MAX;

+  bmin[1] = FLT_MAX;

+  bmin[2] = FLT_MAX;

+

+  bmax[0] = -FLT_MAX;

+  bmax[1] = -FLT_MAX;

+  bmax[2] = -FLT_MAX;

+}

+

+void fm_inflateMinMax(REAL bmin[3], REAL bmax[3], REAL ratio)

+{

+	REAL inflate = fm_distance(bmin, bmax)*0.5f*ratio;

+

+	bmin[0] -= inflate;

+	bmin[1] -= inflate;

+	bmin[2] -= inflate;

+

+	bmax[0] += inflate;

+	bmax[1] += inflate;

+	bmax[2] += inflate;

+}

+

+#ifndef TESSELATE_H

+

+#define TESSELATE_H

+

+typedef std::vector< uint32_t > UintVector;

+

+class Myfm_Tesselate : public fm_Tesselate

+{

+public:

+  virtual ~Myfm_Tesselate(void)

+  {

+

+  }

+

+  const uint32_t * tesselate(fm_VertexIndex *vindex,uint32_t tcount,const uint32_t *indices,float longEdge,uint32_t maxDepth,uint32_t &outcount)

+  {

+    const uint32_t *ret = 0;

+

+    mMaxDepth = maxDepth;

+    mLongEdge  = longEdge*longEdge;

+    mLongEdgeD = mLongEdge;

+    mVertices = vindex;

+

+    if ( mVertices->isDouble() )

+    {

+      uint32_t vcount = mVertices->getVcount();

+      double *vertices = (double *)malloc(sizeof(double)*vcount*3);

+      memcpy(vertices,mVertices->getVerticesDouble(),sizeof(double)*vcount*3);

+

+      for (uint32_t i=0; i<tcount; i++)

+      {

+        uint32_t i1 = *indices++;

+        uint32_t i2 = *indices++;

+        uint32_t i3 = *indices++;

+

+        const double *p1 = &vertices[i1*3];

+        const double *p2 = &vertices[i2*3];

+        const double *p3 = &vertices[i3*3];

+

+        tesselate(p1,p2,p3,0);

+

+      }

+      free(vertices);

+    }

+    else

+    {

+      uint32_t vcount = mVertices->getVcount();

+      float *vertices = (float *)malloc(sizeof(float)*vcount*3);

+      memcpy(vertices,mVertices->getVerticesFloat(),sizeof(float)*vcount*3);

+

+

+      for (uint32_t i=0; i<tcount; i++)

+      {

+        uint32_t i1 = *indices++;

+        uint32_t i2 = *indices++;

+        uint32_t i3 = *indices++;

+

+        const float *p1 = &vertices[i1*3];

+        const float *p2 = &vertices[i2*3];

+        const float *p3 = &vertices[i3*3];

+

+        tesselate(p1,p2,p3,0);

+

+      }

+      free(vertices);

+    }

+

+    outcount = (uint32_t)(mIndices.size()/3);

+    ret = &mIndices[0];

+

+

+    return ret;

+  }

+

+  void tesselate(const float *p1,const float *p2,const float *p3,uint32_t recurse)

+  {

+  	bool split = false;

+  	float l1,l2,l3;

+

+    l1 = l2 = l3 = 0;

+

+  	if ( recurse < mMaxDepth )

+  	{

+  	  l1 = fm_distanceSquared(p1,p2);

+    	l2 = fm_distanceSquared(p2,p3);

+    	l3 = fm_distanceSquared(p3,p1);

+

+  	  if (  l1 > mLongEdge || l2 > mLongEdge || l3 > mLongEdge )

+  	  	split = true;

+

+    }

+

+    if ( split )

+  	{

+  		uint32_t edge;

+

+  		if ( l1 >= l2 && l1 >= l3 )

+  			edge = 0;

+  		else if ( l2 >= l1 && l2 >= l3 )

+  			edge = 1;

+  		else

+  			edge = 2;

+

+			float splits[3];

+

+  		switch ( edge )

+  		{

+  			case 0:

+  				{

+            fm_lerp(p1,p2,splits,0.5f);

+            tesselate(p1,splits,p3, recurse+1 );

+            tesselate(splits,p2,p3, recurse+1 );

+  				}

+  				break;

+  			case 1:

+  				{

+            fm_lerp(p2,p3,splits,0.5f);

+            tesselate(p1,p2,splits, recurse+1 );

+            tesselate(p1,splits,p3, recurse+1 );

+  				}

+  				break;

+  			case 2:

+  				{

+  					fm_lerp(p3,p1,splits,0.5f);

+            tesselate(p1,p2,splits, recurse+1 );

+            tesselate(splits,p2,p3, recurse+1 );

+  				}

+  				break;

+  		}

+  	}

+  	else

+  	{

+      bool newp;

+

+      uint32_t i1 = mVertices->getIndex(p1,newp);

+      uint32_t i2 = mVertices->getIndex(p2,newp);

+      uint32_t i3 = mVertices->getIndex(p3,newp);

+

+      mIndices.push_back(i1);

+      mIndices.push_back(i2);

+      mIndices.push_back(i3);

+    }

+

+  }

+

+  void tesselate(const double *p1,const double *p2,const double *p3,uint32_t recurse)

+  {

+  	bool split = false;

+  	double l1,l2,l3;

+

+    l1 = l2 = l3 = 0;

+

+  	if ( recurse < mMaxDepth )

+  	{

+  	  l1 = fm_distanceSquared(p1,p2);

+    	l2 = fm_distanceSquared(p2,p3);

+    	l3 = fm_distanceSquared(p3,p1);

+

+  	  if (  l1 > mLongEdgeD || l2 > mLongEdgeD || l3 > mLongEdgeD )

+  	  	split = true;

+

+    }

+

+    if ( split )

+  	{

+  		uint32_t edge;

+

+  		if ( l1 >= l2 && l1 >= l3 )

+  			edge = 0;

+  		else if ( l2 >= l1 && l2 >= l3 )

+  			edge = 1;

+  		else

+  			edge = 2;

+

+			double splits[3];

+

+  		switch ( edge )

+  		{

+  			case 0:

+  				{

+            fm_lerp(p1,p2,splits,0.5);

+            tesselate(p1,splits,p3, recurse+1 );

+            tesselate(splits,p2,p3, recurse+1 );

+  				}

+  				break;

+  			case 1:

+  				{

+            fm_lerp(p2,p3,splits,0.5);

+            tesselate(p1,p2,splits, recurse+1 );

+            tesselate(p1,splits,p3, recurse+1 );

+  				}

+  				break;

+  			case 2:

+  				{

+  					fm_lerp(p3,p1,splits,0.5);

+            tesselate(p1,p2,splits, recurse+1 );

+            tesselate(splits,p2,p3, recurse+1 );

+  				}

+  				break;

+  		}

+  	}

+  	else

+  	{

+      bool newp;

+

+      uint32_t i1 = mVertices->getIndex(p1,newp);

+      uint32_t i2 = mVertices->getIndex(p2,newp);

+      uint32_t i3 = mVertices->getIndex(p3,newp);

+

+      mIndices.push_back(i1);

+      mIndices.push_back(i2);

+      mIndices.push_back(i3);

+    }

+

+  }

+

+private:

+  float           mLongEdge;

+  double          mLongEdgeD;

+  fm_VertexIndex *mVertices;

+  UintVector    mIndices;

+  uint32_t          mMaxDepth;

+};

+

+fm_Tesselate * fm_createTesselate(void)

+{

+  Myfm_Tesselate *m = new Myfm_Tesselate;

+  return static_cast< fm_Tesselate * >(m);

+}

+

+void           fm_releaseTesselate(fm_Tesselate *t)

+{

+  Myfm_Tesselate *m = static_cast< Myfm_Tesselate *>(t);

+  delete m;

+}

+

+#endif

+

+

+#ifndef RAY_ABB_INTERSECT

+

+#define RAY_ABB_INTERSECT

+

+//! Integer representation of a floating-point value.

+#define IR(x)	((uint32_t&)x)

+

+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

+/**

+*	A method to compute a ray-AABB intersection.

+*	Original code by Andrew Woo, from "Graphics Gems", Academic Press, 1990

+*	Optimized code by Pierre Terdiman, 2000 (~20-30% faster on my Celeron 500)

+*	Epsilon value added by Klaus Hartmann. (discarding it saves a few cycles only)

+*

+*	Hence this version is faster as well as more robust than the original one.

+*

+*	Should work provided:

+*	1) the integer representation of 0.0f is 0x00000000

+*	2) the sign bit of the float is the most significant one

+*

+*	Report bugs: [email protected]

+*

+*	\param		aabb		[in] the axis-aligned bounding box

+*	\param		origin		[in] ray origin

+*	\param		dir			[in] ray direction

+*	\param		coord		[out] impact coordinates

+*	\return		true if ray intersects AABB

+*/

+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

+#define RAYAABB_EPSILON 0.00001f

+bool fm_intersectRayAABB(const float MinB[3],const float MaxB[3],const float origin[3],const float dir[3],float coord[3])

+{

+  bool Inside = true;

+  float MaxT[3];

+  MaxT[0]=MaxT[1]=MaxT[2]=-1.0f;

+

+  // Find candidate planes.

+  for(uint32_t i=0;i<3;i++)

+  {

+    if(origin[i] < MinB[i])

+    {

+      coord[i]	= MinB[i];

+      Inside		= false;

+

+      // Calculate T distances to candidate planes

+      if(IR(dir[i]))	MaxT[i] = (MinB[i] - origin[i]) / dir[i];

+    }

+    else if(origin[i] > MaxB[i])

+    {

+      coord[i]	= MaxB[i];

+      Inside		= false;

+

+      // Calculate T distances to candidate planes

+      if(IR(dir[i]))	MaxT[i] = (MaxB[i] - origin[i]) / dir[i];

+    }

+  }

+

+  // Ray origin inside bounding box

+  if(Inside)

+  {

+    coord[0] = origin[0];

+    coord[1] = origin[1];

+    coord[2] = origin[2];

+    return true;

+  }

+

+  // Get largest of the maxT's for final choice of intersection

+  uint32_t WhichPlane = 0;

+  if(MaxT[1] > MaxT[WhichPlane])	WhichPlane = 1;

+  if(MaxT[2] > MaxT[WhichPlane])	WhichPlane = 2;

+

+  // Check final candidate actually inside box

+  if(IR(MaxT[WhichPlane])&0x80000000) return false;

+

+  for(uint32_t i=0;i<3;i++)

+  {

+    if(i!=WhichPlane)

+    {

+      coord[i] = origin[i] + MaxT[WhichPlane] * dir[i];

+#ifdef RAYAABB_EPSILON

+      if(coord[i] < MinB[i] - RAYAABB_EPSILON || coord[i] > MaxB[i] + RAYAABB_EPSILON)	return false;

+#else

+      if(coord[i] < MinB[i] || coord[i] > MaxB[i])	return false;

+#endif

+    }

+  }

+  return true;	// ray hits box

+}

+

+bool fm_intersectLineSegmentAABB(const float bmin[3],const float bmax[3],const float p1[3],const float p2[3],float intersect[3])

+{

+  bool ret = false;

+

+  float dir[3];

+  dir[0] = p2[0] - p1[0];

+  dir[1] = p2[1] - p1[1];

+  dir[2] = p2[2] - p1[2];

+  float dist = fm_normalize(dir);

+  if ( dist > RAYAABB_EPSILON )

+  {

+    ret = fm_intersectRayAABB(bmin,bmax,p1,dir,intersect);

+    if ( ret )

+    {

+      float d = fm_distanceSquared(p1,intersect);

+      if ( d  > (dist*dist) )

+      {

+        ret = false;

+      }

+    }

+  }

+  return ret;

+}

+

+#endif

+

+#ifndef OBB_TO_AABB

+

+#define OBB_TO_AABB

+

+#pragma warning(disable:4100)

+

+void    fm_OBBtoAABB(const float /*obmin*/[3],const float /*obmax*/[3],const float /*matrix*/[16],float /*abmin*/[3],float /*abmax*/[3])

+{

+  assert(0); // not yet implemented.

+}

+

+

+const REAL * computePos(uint32_t index,const REAL *vertices,uint32_t vstride)

+{

+  const char *tmp = (const char *)vertices;

+  tmp+=(index*vstride);

+  return (const REAL*)tmp;

+}

+

+void computeNormal(uint32_t index,REAL *normals,uint32_t nstride,const REAL *normal)

+{

+  char *tmp = (char *)normals;

+  tmp+=(index*nstride);

+  REAL *dest = (REAL *)tmp;

+  dest[0]+=normal[0];

+  dest[1]+=normal[1];

+  dest[2]+=normal[2];

+}

+

+void fm_computeMeanNormals(uint32_t vcount,       // the number of vertices

+                           const REAL *vertices,     // the base address of the vertex position data.

+                           uint32_t vstride,      // the stride between position data.

+                           REAL *normals,            // the base address  of the destination for mean vector normals

+                           uint32_t nstride,      // the stride between normals

+                           uint32_t tcount,       // the number of triangles

+                           const uint32_t *indices)     // the triangle indices

+{

+

+  // Step #1 : Zero out the vertex normals

+  char *dest = (char *)normals;

+  for (uint32_t i=0; i<vcount; i++)

+  {

+    REAL *n = (REAL *)dest;

+    n[0] = 0;

+    n[1] = 0;

+    n[2] = 0;

+    dest+=nstride;

+  }

+

+  // Step #2 : Compute the face normals and accumulate them

+  const uint32_t *scan = indices;

+  for (uint32_t i=0; i<tcount; i++)

+  {

+

+    uint32_t i1 = *scan++;

+    uint32_t i2 = *scan++;

+    uint32_t i3 = *scan++;

+

+    const REAL *p1 = computePos(i1,vertices,vstride);

+    const REAL *p2 = computePos(i2,vertices,vstride);

+    const REAL *p3 = computePos(i3,vertices,vstride);

+

+    REAL normal[3];

+    fm_computePlane(p3,p2,p1,normal);

+

+    computeNormal(i1,normals,nstride,normal);

+    computeNormal(i2,normals,nstride,normal);

+    computeNormal(i3,normals,nstride,normal);

+  }

+

+

+  // Normalize the accumulated normals

+  dest = (char *)normals;

+  for (uint32_t i=0; i<vcount; i++)

+  {

+    REAL *n = (REAL *)dest;

+    fm_normalize(n);

+    dest+=nstride;

+  }

+

+}

+

+#endif

+

+

+#define BIGNUMBER 100000000.0  		/* hundred million */

+

+static inline void Set(REAL *n,REAL x,REAL y,REAL z)

+{

+	n[0] = x;

+	n[1] = y;

+	n[2] = z;

+};

+

+static inline void Copy(REAL *dest,const REAL *source)

+{

+	dest[0] = source[0];

+	dest[1] = source[1];

+	dest[2] = source[2];

+}

+

+

+REAL  fm_computeBestFitSphere(uint32_t vcount,const REAL *points,uint32_t pstride,REAL *center)

+{

+	REAL radius;

+	REAL radius2;

+

+	REAL xmin[3];

+	REAL xmax[3];

+	REAL ymin[3];

+	REAL ymax[3];

+	REAL zmin[3];

+	REAL zmax[3];

+	REAL dia1[3];

+	REAL dia2[3];

+

+	/* FIRST PASS: find 6 minima/maxima points */

+	Set(xmin,BIGNUMBER,BIGNUMBER,BIGNUMBER);

+	Set(xmax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);

+	Set(ymin,BIGNUMBER,BIGNUMBER,BIGNUMBER);

+	Set(ymax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);

+	Set(zmin,BIGNUMBER,BIGNUMBER,BIGNUMBER);

+	Set(zmax,-BIGNUMBER,-BIGNUMBER,-BIGNUMBER);

+

+	{

+		const char *scan = (const char *)points;

+		for (uint32_t i=0; i<vcount; i++)

+		{

+			const REAL *caller_p = (const REAL *)scan;

+			if (caller_p[0]<xmin[0])

+				Copy(xmin,caller_p); /* New xminimum point */

+			if (caller_p[0]>xmax[0])

+				Copy(xmax,caller_p);

+			if (caller_p[1]<ymin[1])

+				Copy(ymin,caller_p);

+			if (caller_p[1]>ymax[1])

+				Copy(ymax,caller_p);

+			if (caller_p[2]<zmin[2])

+				Copy(zmin,caller_p);

+			if (caller_p[2]>zmax[2])

+				Copy(zmax,caller_p);

+			scan+=pstride;

+		}

+	}

+

+	/* Set xspan = distance between the 2 points xmin & xmax (squared) */

+	REAL dx = xmax[0] - xmin[0];

+	REAL dy = xmax[1] - xmin[1];

+	REAL dz = xmax[2] - xmin[2];

+	REAL xspan = dx*dx + dy*dy + dz*dz;

+

+/* Same for y & z spans */

+	dx = ymax[0] - ymin[0];

+	dy = ymax[1] - ymin[1];

+	dz = ymax[2] - ymin[2];

+	REAL yspan = dx*dx + dy*dy + dz*dz;

+

+	dx = zmax[0] - zmin[0];

+	dy = zmax[1] - zmin[1];

+	dz = zmax[2] - zmin[2];

+	REAL zspan = dx*dx + dy*dy + dz*dz;

+

+	/* Set points dia1 & dia2 to the maximally separated pair */

+	Copy(dia1,xmin);

+	Copy(dia2,xmax); /* assume xspan biggest */

+	REAL maxspan = xspan;

+

+	if (yspan>maxspan)

+	{

+		maxspan = yspan;

+		Copy(dia1,ymin);

+		Copy(dia2,ymax);

+	}

+

+	if (zspan>maxspan)

+	{

+		maxspan = zspan;

+		Copy(dia1,zmin);

+		Copy(dia2,zmax);

+	}

+

+

+	/* dia1,dia2 is a diameter of initial sphere */

+	/* calc initial center */

+	center[0] = (dia1[0]+dia2[0])*0.5f;

+	center[1] = (dia1[1]+dia2[1])*0.5f;

+	center[2] = (dia1[2]+dia2[2])*0.5f;

+

+	/* calculate initial radius**2 and radius */

+

+	dx = dia2[0]-center[0]; /* x component of radius vector */

+	dy = dia2[1]-center[1]; /* y component of radius vector */

+	dz = dia2[2]-center[2]; /* z component of radius vector */

+

+	radius2 = dx*dx + dy*dy + dz*dz;

+	radius = REAL(sqrt(radius2));

+

+	/* SECOND PASS: increment current sphere */

+	{

+		const char *scan = (const char *)points;

+		for (uint32_t i=0; i<vcount; i++)

+		{

+			const REAL *caller_p = (const REAL *)scan;

+			dx = caller_p[0]-center[0];

+			dy = caller_p[1]-center[1];

+			dz = caller_p[2]-center[2];

+			REAL old_to_p_sq = dx*dx + dy*dy + dz*dz;

+			if (old_to_p_sq > radius2) 	/* do r**2 test first */

+			{ 	/* this point is outside of current sphere */

+				REAL old_to_p = REAL(sqrt(old_to_p_sq));

+				/* calc radius of new sphere */

+				radius = (radius + old_to_p) * 0.5f;

+				radius2 = radius*radius; 	/* for next r**2 compare */

+				REAL old_to_new = old_to_p - radius;

+				/* calc center of new sphere */

+				REAL recip = 1.0f /old_to_p;

+				REAL cx = (radius*center[0] + old_to_new*caller_p[0]) * recip;

+				REAL cy = (radius*center[1] + old_to_new*caller_p[1]) * recip;

+				REAL cz = (radius*center[2] + old_to_new*caller_p[2]) * recip;

+				Set(center,cx,cy,cz);

+				scan+=pstride;

+			}

+		}

+	}

+	return radius;

+}

+

+

+void fm_computeBestFitCapsule(uint32_t vcount,const REAL *points,uint32_t pstride,REAL &radius,REAL &height,REAL matrix[16],bool bruteForce)

+{

+  REAL sides[3];

+  REAL omatrix[16];

+  fm_computeBestFitOBB(vcount,points,pstride,sides,omatrix,bruteForce);

+

+  int32_t 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 uint8_t *scan = (const uint8_t *)points;

+        for (uint32_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 uint8_t *scan = (const uint8_t *)points;

+        for (uint32_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 uint8_t *scan = (const uint8_t *)points;

+        for (uint32_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);

+}

+

+

+//************* Triangulation

+

+#ifndef TRIANGULATE_H

+

+#define TRIANGULATE_H

+

+typedef uint32_t TU32;

+

+class TVec

+{

+public:

+	TVec(double _x,double _y,double _z) { x = _x; y = _y; z = _z; };

+	TVec(void) { };

+

+  double x;

+  double y;

+  double z;

+};

+

+typedef std::vector< TVec >  TVecVector;

+typedef std::vector< TU32 >  TU32Vector;

+

+class CTriangulator

+{

+public:

+    ///     Default constructor

+    CTriangulator();

+

+    ///     Default destructor

+    virtual ~CTriangulator();

+

+    ///     Triangulates the contour

+    void triangulate(TU32Vector &indices);

+

+    ///     Returns the given point in the triangulator array

+    inline TVec get(const TU32 id) { return mPoints[id]; }

+

+    virtual void reset(void)

+    {

+        mInputPoints.clear();

+        mPoints.clear();

+        mIndices.clear();

+    }

+

+    virtual void addPoint(double x,double y,double z)

+    {

+        TVec v(x,y,z);

+        // update bounding box...

+        if ( mInputPoints.empty() )

+        {

+            mMin = v;

+            mMax = v;

+        }

+        else

+        {

+            if ( x < mMin.x ) mMin.x = x;

+            if ( y < mMin.y ) mMin.y = y;

+            if ( z < mMin.z ) mMin.z = z;

+

+            if ( x > mMax.x ) mMax.x = x;

+            if ( y > mMax.y ) mMax.y = y;

+            if ( z > mMax.z ) mMax.z = z;

+        }

+        mInputPoints.push_back(v);

+    }

+

+    // Triangulation happens in 2d.  We could inverse transform the polygon around the normal direction, or we just use the two most signficant axes

+    // Here we find the two longest axes and use them to triangulate.  Inverse transforming them would introduce more doubleing point error and isn't worth it.

+    virtual uint32_t * triangulate(uint32_t &tcount,double epsilon)

+    {

+        uint32_t *ret = 0;

+        tcount = 0;

+        mEpsilon = epsilon;

+

+        if ( !mInputPoints.empty() )

+        {

+            mPoints.clear();

+

+          double dx = mMax.x - mMin.x; // locate the first, second and third longest edges and store them in i1, i2, i3

+          double dy = mMax.y - mMin.y;

+          double dz = mMax.z - mMin.z;

+

+          uint32_t i1,i2,i3;

+

+          if ( dx > dy && dx > dz )

+          {

+              i1 = 0;

+              if ( dy > dz )

+              {

+                  i2 = 1;

+                  i3 = 2;

+              }

+              else

+              {

+                  i2 = 2;

+                  i3 = 1;

+              }

+          }

+          else if ( dy > dx && dy > dz )

+          {

+              i1 = 1;

+              if ( dx > dz )

+              {

+                  i2 = 0;

+                  i3 = 2;

+              }

+              else

+              {

+                  i2 = 2;

+                  i3 = 0;

+              }

+          }

+          else

+          {

+              i1 = 2;

+              if ( dx > dy )

+              {

+                  i2 = 0;

+                  i3 = 1;

+              }

+              else

+              {

+                  i2 = 1;

+                  i3 = 0;

+              }

+          }

+

+          uint32_t pcount = (uint32_t)mInputPoints.size();

+          const double *points = &mInputPoints[0].x;

+          for (uint32_t i=0; i<pcount; i++)

+          {

+            TVec v( points[i1], points[i2], points[i3] );

+            mPoints.push_back(v);

+            points+=3;

+          }

+

+          mIndices.clear();

+          triangulate(mIndices);

+          tcount = (uint32_t)mIndices.size()/3;

+          if ( tcount )

+          {

+              ret = &mIndices[0];

+          }

+        }

+        return ret;

+    }

+

+    virtual const double * getPoint(uint32_t index)

+    {

+        return &mInputPoints[index].x;

+    }

+

+

+private:

+    double                  mEpsilon;

+    TVec                   mMin;

+    TVec                   mMax;

+    TVecVector             mInputPoints;

+    TVecVector             mPoints;

+    TU32Vector             mIndices;

+

+    ///     Tests if a point is inside the given triangle

+    bool _insideTriangle(const TVec& A, const TVec& B, const TVec& C,const TVec& P);

+

+    ///     Returns the area of the contour

+    double _area();

+

+    bool _snip(int32_t u, int32_t v, int32_t w, int32_t n, int32_t *V);

+

+    ///     Processes the triangulation

+    void _process(TU32Vector &indices);

+

+};

+

+///     Default constructor

+CTriangulator::CTriangulator(void)

+{

+}

+

+///     Default destructor

+CTriangulator::~CTriangulator()

+{

+}

+

+///     Triangulates the contour

+void CTriangulator::triangulate(TU32Vector &indices)

+{

+    _process(indices);

+}

+

+///     Processes the triangulation

+void CTriangulator::_process(TU32Vector &indices)

+{

+    const int32_t n = (const int32_t)mPoints.size();

+    if (n < 3)

+        return;

+    int32_t *V = (int32_t *)malloc(sizeof(int32_t)*n);

+

+	bool flipped = false;

+

+    if (0.0f < _area())

+    {

+        for (int32_t v = 0; v < n; v++)

+            V[v] = v;

+    }

+    else

+    {

+		flipped = true;

+        for (int32_t v = 0; v < n; v++)

+            V[v] = (n - 1) - v;

+    }

+

+    int32_t nv = n;

+    int32_t count = 2 * nv;

+    for (int32_t m = 0, v = nv - 1; nv > 2;)

+    {

+        if (0 >= (count--))

+            return;

+

+        int32_t u = v;

+        if (nv <= u)

+            u = 0;

+        v = u + 1;

+        if (nv <= v)

+            v = 0;

+        int32_t w = v + 1;

+        if (nv <= w)

+            w = 0;

+

+        if (_snip(u, v, w, nv, V))

+        {

+            int32_t a, b, c, s, t;

+            a = V[u];

+            b = V[v];

+            c = V[w];

+			if ( flipped )

+			{

+				indices.push_back(a);

+				indices.push_back(b);

+				indices.push_back(c);

+			}

+			else

+			{

+				indices.push_back(c);

+				indices.push_back(b);

+				indices.push_back(a);

+			}

+            m++;

+            for (s = v, t = v + 1; t < nv; s++, t++)

+                V[s] = V[t];

+            nv--;

+            count = 2 * nv;

+        }

+    }

+

+    free(V);

+}

+

+///     Returns the area of the contour

+double CTriangulator::_area()

+{

+    int32_t n = (uint32_t)mPoints.size();

+    double A = 0.0f;

+    for (int32_t p = n - 1, q = 0; q < n; p = q++)

+    {

+        const TVec &pval = mPoints[p];

+        const TVec &qval = mPoints[q];

+        A += pval.x * qval.y - qval.x * pval.y;

+    }

+	A*=0.5f;

+    return A;

+}

+

+bool CTriangulator::_snip(int32_t u, int32_t v, int32_t w, int32_t n, int32_t *V)

+{

+    int32_t p;

+

+    const TVec &A = mPoints[ V[u] ];

+    const TVec &B = mPoints[ V[v] ];

+    const TVec &C = mPoints[ V[w] ];

+

+    if (mEpsilon > (((B.x - A.x) * (C.y - A.y)) - ((B.y - A.y) * (C.x - A.x))) )

+        return false;

+

+    for (p = 0; p < n; p++)

+    {

+        if ((p == u) || (p == v) || (p == w))

+            continue;

+        const TVec &P = mPoints[ V[p] ];

+        if (_insideTriangle(A, B, C, P))

+            return false;

+    }

+    return true;

+}

+

+///     Tests if a point is inside the given triangle

+bool CTriangulator::_insideTriangle(const TVec& A, const TVec& B, const TVec& C,const TVec& P)

+{

+    double ax, ay, bx, by, cx, cy, apx, apy, bpx, bpy, cpx, cpy;

+    double cCROSSap, bCROSScp, aCROSSbp;

+

+    ax = C.x - B.x;  ay = C.y - B.y;

+    bx = A.x - C.x;  by = A.y - C.y;

+    cx = B.x - A.x;  cy = B.y - A.y;

+    apx = P.x - A.x;  apy = P.y - A.y;

+    bpx = P.x - B.x;  bpy = P.y - B.y;

+    cpx = P.x - C.x;  cpy = P.y - C.y;

+

+    aCROSSbp = ax * bpy - ay * bpx;

+    cCROSSap = cx * apy - cy * apx;

+    bCROSScp = bx * cpy - by * cpx;

+

+    return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f));

+}

+

+class Triangulate : public fm_Triangulate

+{

+public:

+  Triangulate(void)

+  {

+    mPointsFloat = 0;

+    mPointsDouble = 0;

+  }

+

+  virtual ~Triangulate(void)

+  {

+    reset();

+  }

+  void reset(void)

+  {

+    free(mPointsFloat);

+    free(mPointsDouble);

+    mPointsFloat = 0;

+    mPointsDouble = 0;

+  }

+

+  virtual const double *       triangulate3d(uint32_t pcount,

+                                             const double *_points,

+                                             uint32_t vstride,

+                                             uint32_t &tcount,

+                                             bool consolidate,

+                                             double epsilon)

+  {

+    reset();

+

+    double *points = (double *)malloc(sizeof(double)*pcount*3);

+    if ( consolidate )

+    {

+      pcount = fm_consolidatePolygon(pcount,_points,vstride,points,1-epsilon);

+    }

+    else

+    {

+      double *dest = points;

+      for (uint32_t i=0; i<pcount; i++)

+      {

+        const double *src = fm_getPoint(_points,vstride,i);

+        dest[0] = src[0];

+        dest[1] = src[1];

+        dest[2] = src[2];

+        dest+=3;

+      }

+      vstride = sizeof(double)*3;

+    }

+

+    if ( pcount >= 3 )

+    {

+      CTriangulator ct;

+      for (uint32_t i=0; i<pcount; i++)

+      {

+        const double *src = fm_getPoint(points,vstride,i);

+        ct.addPoint( src[0], src[1], src[2] );

+      }

+      uint32_t _tcount;

+      uint32_t *indices = ct.triangulate(_tcount,epsilon);

+      if ( indices )

+      {

+        tcount = _tcount;

+        mPointsDouble = (double *)malloc(sizeof(double)*tcount*3*3);

+        double *dest = mPointsDouble;

+        for (uint32_t i=0; i<tcount; i++)

+        {

+          uint32_t i1 = indices[i*3+0];

+          uint32_t i2 = indices[i*3+1];

+          uint32_t i3 = indices[i*3+2];

+          const double *p1 = ct.getPoint(i1);

+          const double *p2 = ct.getPoint(i2);

+          const double *p3 = ct.getPoint(i3);

+

+          dest[0] = p1[0];

+          dest[1] = p1[1];

+          dest[2] = p1[2];

+

+          dest[3] = p2[0];

+          dest[4] = p2[1];

+          dest[5] = p2[2];

+

+          dest[6] = p3[0];

+          dest[7] = p3[1];

+          dest[8] = p3[2];

+          dest+=9;

+        }

+      }

+    }

+    free(points);

+

+    return mPointsDouble;

+  }

+

+  virtual const float  *       triangulate3d(uint32_t pcount,

+                                             const float  *points,

+                                             uint32_t vstride,

+                                             uint32_t &tcount,

+                                             bool consolidate,

+                                             float epsilon)

+  {

+    reset();

+

+    double *temp = (double *)malloc(sizeof(double)*pcount*3);

+    double *dest = temp;

+    for (uint32_t i=0; i<pcount; i++)

+    {

+      const float *p = fm_getPoint(points,vstride,i);

+      dest[0] = p[0];

+      dest[1] = p[1];

+      dest[2] = p[2];

+      dest+=3;

+    }

+    const double *results = triangulate3d(pcount,temp,sizeof(double)*3,tcount,consolidate,epsilon);

+    if ( results )

+    {

+      uint32_t fcount = tcount*3*3;

+      mPointsFloat = (float *)malloc(sizeof(float)*tcount*3*3);

+      for (uint32_t i=0; i<fcount; i++)

+      {

+        mPointsFloat[i] = (float) results[i];

+      }

+      free(mPointsDouble);

+      mPointsDouble = 0;

+    }

+    free(temp);

+

+    return mPointsFloat;

+  }

+

+private:

+  float *mPointsFloat;

+  double *mPointsDouble;

+};

+

+fm_Triangulate * fm_createTriangulate(void)

+{

+  Triangulate *t = new Triangulate;

+  return static_cast< fm_Triangulate *>(t);

+}

+

+void             fm_releaseTriangulate(fm_Triangulate *t)

+{

+  Triangulate *tt = static_cast< Triangulate *>(t);

+  delete tt;

+}

+

+#endif

+

+bool validDistance(const REAL *p1,const REAL *p2,REAL epsilon)

+{

+	bool ret = true;

+

+	REAL dx = p1[0] - p2[0];

+	REAL dy = p1[1] - p2[1];

+	REAL dz = p1[2] - p2[2];

+	REAL dist = dx*dx+dy*dy+dz*dz;

+	if ( dist < (epsilon*epsilon) )

+	{

+		ret = false;

+	}

+	return ret;

+}

+

+bool fm_isValidTriangle(const REAL *p1,const REAL *p2,const REAL *p3,REAL epsilon)

+{

+  bool ret = false;

+

+  if ( validDistance(p1,p2,epsilon) &&

+	   validDistance(p1,p3,epsilon) &&

+	   validDistance(p2,p3,epsilon) )

+  {

+

+	  REAL area = fm_computeArea(p1,p2,p3);

+	  if ( area > epsilon )

+	  {

+		REAL _vertices[3*3],vertices[64*3];

+

+		_vertices[0] = p1[0];

+		_vertices[1] = p1[1];

+		_vertices[2] = p1[2];

+

+		_vertices[3] = p2[0];

+		_vertices[4] = p2[1];

+		_vertices[5] = p2[2];

+

+		_vertices[6] = p3[0];

+		_vertices[7] = p3[1];

+		_vertices[8] = p3[2];

+

+		uint32_t pcount = fm_consolidatePolygon(3,_vertices,sizeof(REAL)*3,vertices,1-epsilon);

+		if ( pcount == 3 )

+		{

+		  ret = true;

+		}

+	  }

+  }

+  return ret;

+}

+

+

+void  fm_multiplyQuat(const REAL *left,const REAL *right,REAL *quat)

+{

+	REAL a,b,c,d;

+

+	a = left[3]*right[3] - left[0]*right[0] - left[1]*right[1] - left[2]*right[2];

+	b = left[3]*right[0] + right[3]*left[0] + left[1]*right[2] - right[1]*left[2];

+	c = left[3]*right[1] + right[3]*left[1] + left[2]*right[0] - right[2]*left[0];

+	d = left[3]*right[2] + right[3]*left[2] + left[0]*right[1] - right[0]*left[1];

+

+	quat[3] = a;

+	quat[0] = b;

+	quat[1] = c;

+	quat[2] = d;

+}

+

+bool  fm_computeCentroid(uint32_t vcount,     // number of input data points

+						 const REAL *points,     // starting address of points array.

+						 uint32_t vstride,    // stride between input points.

+						 REAL *center)

+

+{

+	bool ret = false;

+	if ( vcount )

+	{

+		center[0] = 0;

+		center[1] = 0;

+		center[2] = 0;

+		const char *scan = (const char *)points;

+		for (uint32_t i=0; i<vcount; i++)

+		{

+			const REAL *p = (const REAL *)scan;

+			center[0]+=p[0];

+			center[1]+=p[1];

+			center[2]+=p[2];

+			scan+=vstride;

+		}

+		REAL recip = 1.0f / (REAL)vcount;

+		center[0]*=recip;

+		center[1]*=recip;

+		center[2]*=recip;

+		ret = true;

+	}

+	return ret;

+}

+

+#ifndef TEMPLATE_VEC3

+#define TEMPLATE_VEC3

+template <class Type> class Vec3

+{

+public:

+	Vec3(void)

+	{

+

+	}

+	Vec3(Type _x,Type _y,Type _z)

+	{

+		x = _x;

+		y = _y;

+		z = _z;

+	}

+	Type x;

+	Type y;

+	Type z;

+};

+#endif

+

+void fm_transformAABB(const REAL bmin[3],const REAL bmax[3],const REAL matrix[16],REAL tbmin[3],REAL tbmax[3])

+{

+	Vec3<REAL> box[8];

+	box[0] = Vec3< REAL >( bmin[0], bmin[1], bmin[2] );

+	box[1] = Vec3< REAL >( bmax[0], bmin[1], bmin[2] );

+	box[2] = Vec3< REAL >( bmax[0], bmax[1], bmin[2] );

+	box[3] = Vec3< REAL >( bmin[0], bmax[1], bmin[2] );

+	box[4] = Vec3< REAL >( bmin[0], bmin[1], bmax[2] );

+	box[5] = Vec3< REAL >( bmax[0], bmin[1], bmax[2] );

+	box[6] = Vec3< REAL >( bmax[0], bmax[1], bmax[2] );

+	box[7] = Vec3< REAL >( bmin[0], bmax[1], bmax[2] );

+	// transform all 8 corners of the box and then recompute a new AABB

+	for (unsigned int i=0; i<8; i++)

+	{

+		Vec3< REAL > &p = box[i];

+		fm_transform(matrix,&p.x,&p.x);

+		if ( i == 0 )

+		{

+			tbmin[0] = tbmax[0] = p.x;

+			tbmin[1] = tbmax[1] = p.y;

+			tbmin[2] = tbmax[2] = p.z;

+		}

+		else

+		{

+			if ( p.x < tbmin[0] ) tbmin[0] = p.x;

+			if ( p.y < tbmin[1] ) tbmin[1] = p.y;

+			if ( p.z < tbmin[2] ) tbmin[2] = p.z;

+			if ( p.x > tbmax[0] ) tbmax[0] = p.x;

+			if ( p.y > tbmax[1] ) tbmax[1] = p.y;

+			if ( p.z > tbmax[2] ) tbmax[2] = p.z;

+		}

+	}

+}

+

+REAL  fm_normalizeQuat(REAL n[4]) // normalize this quat

+{

+	REAL dx = n[0]*n[0];

+	REAL dy = n[1]*n[1];

+	REAL dz = n[2]*n[2];

+	REAL dw = n[3]*n[3];

+

+	REAL dist = dx*dx+dy*dy+dz*dz+dw*dw;

+

+	dist = (REAL)sqrt(dist);

+

+	REAL recip = 1.0f / dist;

+

+	n[0]*=recip;

+	n[1]*=recip;

+	n[2]*=recip;

+	n[3]*=recip;

+

+	return dist;

+}

+

+

+}; // end of namespace