update bullet test to compile from source

1 files changed, 3260 insertions, 0 deletions

diff --git a/tests/bullet/Extras/ConvexDecomposition/cd_hull.cpp b/tests/bullet/Extras/ConvexDecomposition/cd_hull.cpp
new file mode 100644
index 00000000..b85b6494
--- /dev/null
+++ b/tests/bullet/Extras/ConvexDecomposition/cd_hull.cpp
@@ -0,0 +1,3260 @@
+#include "float_math.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <assert.h>
+#include <math.h>
+#include <float.h>
+
+/*----------------------------------------------------------------------
+		Copyright (c) 2004 Open Dynamics Framework Group
+					www.physicstools.org
+		All rights reserved.
+
+		Redistribution and use in source and binary forms, with or without modification, are permitted provided
+		that the following conditions are met:
+
+		Redistributions of source code must retain the above copyright notice, this list of conditions
+		and the following disclaimer.
+
+		Redistributions in binary form must reproduce the above copyright notice,
+		this list of conditions and the following disclaimer in the documentation
+		and/or other materials provided with the distribution.
+
+		Neither the name of the Open Dynamics Framework Group nor the names of its contributors may
+		be used to endorse or promote products derived from this software without specific prior written permission.
+
+		THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES,
+		INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+		DISCLAIMED. IN NO EVENT SHALL THE INTEL OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+		EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+		LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
+		IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+		THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+-----------------------------------------------------------------------*/
+
+// http://codesuppository.blogspot.com
+//
+// mailto: jratcliff@infiniplex.net
+//
+// http://www.amillionpixels.us
+//
+
+#include "cd_hull.h"
+
+using namespace ConvexDecomposition;
+
+/*----------------------------------------------------------------------
+		Copyright (c) 2004 Open Dynamics Framework Group
+					www.physicstools.org
+		All rights reserved.
+
+		Redistribution and use in source and binary forms, with or without modification, are permitted provided
+		that the following conditions are met:
+
+		Redistributions of source code must retain the above copyright notice, this list of conditions
+		and the following disclaimer.
+
+		Redistributions in binary form must reproduce the above copyright notice,
+		this list of conditions and the following disclaimer in the documentation
+		and/or other materials provided with the distribution.
+
+		Neither the name of the Open Dynamics Framework Group nor the names of its contributors may
+		be used to endorse or promote products derived from this software without specific prior written permission.
+
+		THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES,
+		INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+		DISCLAIMED. IN NO EVENT SHALL THE INTEL OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+		EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+		LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
+		IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+		THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+-----------------------------------------------------------------------*/
+
+#define PI 3.14159264f
+
+//*****************************************************
+//*****************************************************
+//********* Stan Melax's vector math template needed
+//********* to use his hull building code.
+//*****************************************************
+//*****************************************************
+
+#define DEG2RAD (PI / 180.0f)
+#define RAD2DEG (180.0f / PI)
+#define SQRT_OF_2 (1.4142135f)
+#define OFFSET(Class,Member)  (((char*) (&(((Class*)NULL)-> Member )))- ((char*)NULL))
+
+namespace ConvexDecomposition
+{
+
+
+int    argmin(float a[],int n);
+float  sqr(float a);
+float  clampf(float a) ;
+float  Round(float a,float precision);
+float  Interpolate(const float &f0,const float &f1,float alpha) ;
+
+template <class T>
+void Swap(T &a,T &b)
+{
+	T tmp = a;
+	a=b;
+	b=tmp;
+}
+
+
+
+template <class T>
+T Max(const T &a,const T &b)
+{
+	return (a>b)?a:b;
+}
+
+template <class T>
+T Min(const T &a,const T &b) 
+{
+	return (a<b)?a:b;
+}
+
+//----------------------------------
+
+class int3  
+{
+public:
+	int x,y,z;
+	int3(){};
+	int3(int _x,int _y, int _z){x=_x;y=_y;z=_z;}
+	const int& operator[](int i) const {return (&x)[i];}
+	int& operator[](int i) {return (&x)[i];}
+};
+
+
+//-------- 2D --------
+
+class float2
+{
+public:
+	float x,y;
+	float2(){x=0;y=0;};
+	float2(float _x,float _y){x=_x;y=_y;}
+	float& operator[](int i) {assert(i>=0&&i<2);return ((float*)this)[i];}
+	const float& operator[](int i) const {assert(i>=0&&i<2);return ((float*)this)[i];}
+};
+inline float2 operator-( const float2& a, const float2& b ){return float2(a.x-b.x,a.y-b.y);}
+inline float2 operator+( const float2& a, const float2& b ){return float2(a.x+b.x,a.y+b.y);}
+
+//--------- 3D ---------
+
+class float3 // 3D
+{
+	public:
+	float x,y,z;
+	float3(){x=0;y=0;z=0;};
+	float3(float _x,float _y,float _z){x=_x;y=_y;z=_z;};
+	//operator float *() { return &x;};
+	float& operator[](int i) {assert(i>=0&&i<3);return ((float*)this)[i];}
+	const float& operator[](int i) const {assert(i>=0&&i<3);return ((float*)this)[i];}
+#	ifdef PLUGIN_3DSMAX
+	float3(const Point3 &p):x(p.x),y(p.y),z(p.z){}
+	operator Point3(){return *((Point3*)this);}
+#	endif
+};
+
+
+float3& operator+=( float3 &a, const float3& b );
+float3& operator-=( float3 &a ,const float3& b );
+float3& operator*=( float3 &v ,const float s );
+float3& operator/=( float3 &v, const float s );
+
+float  magnitude( const float3& v );
+float3 normalize( const float3& v );
+float3 safenormalize(const float3 &v);
+float3 vabs(const float3 &v);
+float3 operator+( const float3& a, const float3& b );
+float3 operator-( const float3& a, const float3& b );
+float3 operator-( const float3& v );
+float3 operator*( const float3& v, const float s );
+float3 operator*( const float s, const float3& v );
+float3 operator/( const float3& v, const float s );
+inline int operator==( const float3 &a, const float3 &b ) { return (a.x==b.x && a.y==b.y && a.z==b.z); }
+inline int operator!=( const float3 &a, const float3 &b ) { return (a.x!=b.x || a.y!=b.y || a.z!=b.z); }
+// due to ambiguity and inconsistent standards ther are no overloaded operators for mult such as va*vb.
+float  dot( const float3& a, const float3& b );
+float3 cmul( const float3 &a, const float3 &b);
+float3 cross( const float3& a, const float3& b );
+float3 Interpolate(const float3 &v0,const float3 &v1,float alpha);
+float3 Round(const float3& a,float precision);
+float3	VectorMax(const float3 &a, const float3 &b);
+float3	VectorMin(const float3 &a, const float3 &b);
+
+
+
+class float3x3
+{
+	public:
+	float3 x,y,z;  // the 3 rows of the Matrix
+	float3x3(){}
+	float3x3(float xx,float xy,float xz,float yx,float yy,float yz,float zx,float zy,float zz):x(xx,xy,xz),y(yx,yy,yz),z(zx,zy,zz){}
+	float3x3(float3 _x,float3 _y,float3 _z):x(_x),y(_y),z(_z){}
+	float3&       operator[](int i)       {assert(i>=0&&i<3);return (&x)[i];}
+	const float3& operator[](int i) const {assert(i>=0&&i<3);return (&x)[i];}
+	float&        operator()(int r, int c)       {assert(r>=0&&r<3&&c>=0&&c<3);return ((&x)[r])[c];}
+	const float&  operator()(int r, int c) const {assert(r>=0&&r<3&&c>=0&&c<3);return ((&x)[r])[c];}
+}; 
+float3x3 Transpose( const float3x3& m );
+float3   operator*( const float3& v  , const float3x3& m  );
+float3   operator*( const float3x3& m , const float3& v   );
+float3x3 operator*( const float3x3& m , const float& s   );
+float3x3 operator*( const float3x3& ma, const float3x3& mb );
+float3x3 operator/( const float3x3& a, const float& s ) ;
+float3x3 operator+( const float3x3& a, const float3x3& b );
+float3x3 operator-( const float3x3& a, const float3x3& b );
+float3x3 &operator+=( float3x3& a, const float3x3& b );
+float3x3 &operator-=( float3x3& a, const float3x3& b );
+float3x3 &operator*=( float3x3& a, const float& s );
+float    Determinant(const float3x3& m );
+float3x3 Inverse(const float3x3& a);  // its just 3x3 so we simply do that cofactor method
+
+
+//-------- 4D Math --------
+
+class float4
+{
+public:
+	float x,y,z,w;
+	float4(){x=0;y=0;z=0;w=0;};
+	float4(float _x,float _y,float _z,float _w){x=_x;y=_y;z=_z;w=_w;}
+	float4(const float3 &v,float _w){x=v.x;y=v.y;z=v.z;w=_w;}
+	//operator float *() { return &x;};
+	float& operator[](int i) {assert(i>=0&&i<4);return ((float*)this)[i];}
+	const float& operator[](int i) const {assert(i>=0&&i<4);return ((float*)this)[i];}
+	const float3& xyz() const { return *((float3*)this);}
+	float3&       xyz()       { return *((float3*)this);}
+};
+
+
+struct D3DXMATRIX;
+
+class float4x4
+{
+	public:
+	float4 x,y,z,w;  // the 4 rows
+	float4x4(){}
+	float4x4(const float4 &_x, const float4 &_y, const float4 &_z, const float4 &_w):x(_x),y(_y),z(_z),w(_w){}
+	float4x4(float m00, float m01, float m02, float m03,
+						float m10, float m11, float m12, float m13,
+				float m20, float m21, float m22, float m23, 
+				float m30, float m31, float m32, float m33 )
+			:x(m00,m01,m02,m03),y(m10,m11,m12,m13),z(m20,m21,m22,m23),w(m30,m31,m32,m33){}
+	float&       operator()(int r, int c)       {assert(r>=0&&r<4&&c>=0&&c<4);return ((&x)[r])[c];}
+	const float& operator()(int r, int c) const {assert(r>=0&&r<4&&c>=0&&c<4);return ((&x)[r])[c];}
+		operator       float* ()       {return &x.x;}
+		operator const float* () const {return &x.x;}
+	operator       struct D3DXMATRIX* ()       { return (struct D3DXMATRIX*) this;}
+	operator const struct D3DXMATRIX* () const { return (struct D3DXMATRIX*) this;}
+};
+
+
+int     operator==( const float4 &a, const float4 &b );
+float4 Homogenize(const float3 &v3,const float &w=1.0f); // Turns a 3D float3 4D vector4 by appending w
+float4 cmul( const float4 &a, const float4 &b);
+float4 operator*( const float4 &v, float s);
+float4 operator*( float s, const float4 &v);
+float4 operator+( const float4 &a, const float4 &b);
+float4 operator-( const float4 &a, const float4 &b);
+float4x4 operator*( const float4x4& a, const float4x4& b );
+float4 operator*( const float4& v, const float4x4& m );
+float4x4 Inverse(const float4x4 &m);
+float4x4 MatrixRigidInverse(const float4x4 &m);
+float4x4 MatrixTranspose(const float4x4 &m);
+float4x4 MatrixPerspectiveFov(float fovy, float Aspect, float zn, float zf );
+float4x4 MatrixTranslation(const float3 &t);
+float4x4 MatrixRotationZ(const float angle_radians);
+float4x4 MatrixLookAt(const float3& eye, const float3& at, const float3& up);
+int     operator==( const float4x4 &a, const float4x4 &b );
+
+
+//-------- Quaternion ------------
+
+class Quaternion :public float4
+{
+ public:
+	Quaternion() { x = y = z = 0.0f; w = 1.0f; }
+	Quaternion( float3 v, float t ) { v = normalize(v); w = cosf(t/2.0f); v = v*sinf(t/2.0f); x = v.x; y = v.y; z = v.z; }
+	Quaternion(float _x, float _y, float _z, float _w){x=_x;y=_y;z=_z;w=_w;}
+	float angle() const { return acosf(w)*2.0f; }
+	float3 axis() const { float3 a(x,y,z); if(fabsf(angle())<0.0000001f) return float3(1,0,0); return a*(1/sinf(angle()/2.0f)); }
+	float3 xdir() const { return float3( 1-2*(y*y+z*z),  2*(x*y+w*z),  2*(x*z-w*y) ); }
+	float3 ydir() const { return float3(   2*(x*y-w*z),1-2*(x*x+z*z),  2*(y*z+w*x) ); }
+	float3 zdir() const { return float3(   2*(x*z+w*y),  2*(y*z-w*x),1-2*(x*x+y*y) ); }
+	float3x3 getmatrix() const { return float3x3( xdir(), ydir(), zdir() ); }
+	operator float3x3() { return getmatrix(); }
+	void Normalize();
+};
+
+Quaternion& operator*=(Quaternion& a, float s );
+Quaternion	operator*( const Quaternion& a, float s );
+Quaternion	operator*( const Quaternion& a, const Quaternion& b);
+Quaternion	operator+( const Quaternion& a, const Quaternion& b );
+Quaternion	normalize( Quaternion a );
+float		dot( const Quaternion &a, const Quaternion &b );
+float3		operator*( const Quaternion& q, const float3& v );
+float3		operator*( const float3& v, const Quaternion& q );
+Quaternion	slerp( Quaternion a, const Quaternion& b, float interp );
+Quaternion  Interpolate(const Quaternion &q0,const Quaternion &q1,float alpha); 
+Quaternion  RotationArc(float3 v0, float3 v1 );  // returns quat q where q*v0=v1
+Quaternion  Inverse(const Quaternion &q);
+float4x4     MatrixFromQuatVec(const Quaternion &q, const float3 &v);
+
+
+//------ Euler Angle -----
+
+Quaternion YawPitchRoll( float yaw, float pitch, float roll );
+float Yaw( const Quaternion& q );
+float Pitch( const Quaternion& q );
+float Roll( Quaternion q );
+float Yaw( const float3& v );
+float Pitch( const float3& v );
+
+
+//------- Plane ----------
+
+class Plane
+{
+	public:
+	float3	normal;
+	float	dist;   // distance below origin - the D from plane equasion Ax+By+Cz+D=0
+			Plane(const float3 &n,float d):normal(n),dist(d){}
+			Plane():normal(),dist(0){}
+	void	Transform(const float3 &position, const Quaternion &orientation);
+};
+
+inline Plane PlaneFlip(const Plane &plane){return Plane(-plane.normal,-plane.dist);}
+inline int operator==( const Plane &a, const Plane &b ) { return (a.normal==b.normal && a.dist==b.dist); }
+inline int coplanar( const Plane &a, const Plane &b ) { return (a==b || a==PlaneFlip(b)); }
+
+
+//--------- Utility Functions ------
+
+float3  PlaneLineIntersection(const Plane &plane, const float3 &p0, const float3 &p1);
+float3  PlaneProject(const Plane &plane, const float3 &point);
+float3  LineProject(const float3 &p0, const float3 &p1, const float3 &a);  // projects a onto infinite line p0p1
+float   LineProjectTime(const float3 &p0, const float3 &p1, const float3 &a);
+float3  ThreePlaneIntersection(const Plane &p0,const Plane &p1, const Plane &p2);
+int     PolyHit(const float3 *vert,const int n,const float3 &v0, const float3 &v1, float3 *impact=NULL, float3 *normal=NULL);
+int     BoxInside(const float3 &p,const float3 &bmin, const float3 &bmax) ;
+int     BoxIntersect(const float3 &v0, const float3 &v1, const float3 &bmin, const float3 &bmax, float3 *impact);
+float   DistanceBetweenLines(const float3 &ustart, const float3 &udir, const float3 &vstart, const float3 &vdir, float3 *upoint=NULL, float3 *vpoint=NULL);
+float3  TriNormal(const float3 &v0, const float3 &v1, const float3 &v2);
+float3  NormalOf(const float3 *vert, const int n);
+Quaternion VirtualTrackBall(const float3 &cop, const float3 &cor, const float3 &dir0, const float3 &dir1);
+
+
+float   sqr(float a) {return a*a;}
+float   clampf(float a) {return Min(1.0f,Max(0.0f,a));}
+
+
+float Round(float a,float precision)
+{
+	return floorf(0.5f+a/precision)*precision;
+}
+
+
+float Interpolate(const float &f0,const float &f1,float alpha)
+{
+	return f0*(1-alpha) + f1*alpha;
+}
+
+
+int     argmin(float a[],int n)
+{
+	int r=0;
+	for(int i=1;i<n;i++) 
+		{
+		if(a[i]<a[r]) 
+				{
+			r = i;
+		}
+	}
+	return r;
+}
+
+
+
+//------------ float3 (3D) --------------
+
+
+
+float3 operator+( const float3& a, const float3& b ) 
+{
+	return float3(a.x+b.x, a.y+b.y, a.z+b.z); 
+}
+
+
+float3 operator-( const float3& a, const float3& b )
+{
+	return float3( a.x-b.x, a.y-b.y, a.z-b.z );
+}
+
+
+float3 operator-( const float3& v )                     
+{
+	return float3( -v.x, -v.y, -v.z );
+}
+
+
+float3 operator*( const float3& v, float s )      
+{
+	return float3( v.x*s, v.y*s, v.z*s );
+}
+
+
+float3 operator*( float s, const float3& v )      
+{
+	return float3( v.x*s, v.y*s, v.z*s ); 
+}
+
+
+float3 operator/( const float3& v, float s )
+{ 
+	return v*(1.0f/s); 
+}
+
+float  dot( const float3& a, const float3& b )    
+{
+	return a.x*b.x + a.y*b.y + a.z*b.z; 
+}
+
+float3 cmul( const float3 &v1, const float3 &v2) 
+{ 
+	return float3(v1.x*v2.x, v1.y*v2.y, v1.z*v2.z); 
+}
+
+
+float3 cross( const float3& a, const float3& b )
+{
+		return float3( a.y*b.z - a.z*b.y,
+									 a.z*b.x - a.x*b.z,
+									 a.x*b.y - a.y*b.x );
+}
+
+
+
+
+float3& operator+=( float3& a , const float3& b )
+{
+		a.x += b.x;
+		a.y += b.y;
+		a.z += b.z;
+		return a;
+}
+
+
+float3& operator-=( float3& a , const float3& b )
+{
+		a.x -= b.x;
+		a.y -= b.y;
+		a.z -= b.z;
+		return a;
+}
+
+
+float3& operator*=(float3& v , float s )
+{
+		v.x *= s;
+		v.y *= s;
+		v.z *= s;
+		return v;
+}
+
+
+float3& operator/=(float3& v , float s )
+{
+		float sinv = 1.0f / s;
+		v.x *= sinv;
+		v.y *= sinv;
+		v.z *= sinv;
+		return v;
+}
+
+float3 vabs(const float3 &v)
+{
+	return float3(fabsf(v.x),fabsf(v.y),fabsf(v.z));
+}
+
+
+float magnitude( const float3& v )
+{
+		return sqrtf(sqr(v.x) + sqr( v.y)+ sqr(v.z));
+}
+
+
+
+float3 normalize( const float3 &v )
+{
+	// this routine, normalize, is ok, provided magnitude works!!
+		float d=magnitude(v);
+		if (d==0)
+		{
+		printf("Cant normalize ZERO vector\n");
+		assert(0);// yes this could go here
+		d=0.1f;
+	}
+	d = 1/d;
+	return float3(v.x*d,v.y*d,v.z*d);
+}
+
+float3 safenormalize(const float3 &v)
+{
+	if(magnitude(v)<=0.0f)
+	{
+		return float3(1,0,0);
+	}
+	return normalize(v);
+}
+
+float3 Round(const float3 &a,float precision)
+{
+	return float3(Round(a.x,precision),Round(a.y,precision),Round(a.z,precision));
+}
+
+
+float3 Interpolate(const float3 &v0,const float3 &v1,float alpha) 
+{
+	return v0*(1-alpha) + v1*alpha;
+}
+
+float3 VectorMin(const float3 &a,const float3 &b)
+{
+	return float3(Min(a.x,b.x),Min(a.y,b.y),Min(a.z,b.z));
+}
+float3 VectorMax(const float3 &a,const float3 &b)
+{
+	return float3(Max(a.x,b.x),Max(a.y,b.y),Max(a.z,b.z));
+}
+
+// the statement v1*v2 is ambiguous since there are 3 types
+// of vector multiplication
+//  - componantwise (for example combining colors)
+//  - dot product
+//  - cross product
+// Therefore we never declare/implement this function.
+// So we will never see:  float3 operator*(float3 a,float3 b) 
+
+
+
+
+//------------ float3x3 ---------------
+float Determinant(const float3x3 &m)
+{
+	return  m.x.x*m.y.y*m.z.z + m.y.x*m.z.y*m.x.z + m.z.x*m.x.y*m.y.z
+			 -m.x.x*m.z.y*m.y.z - m.y.x*m.x.y*m.z.z - m.z.x*m.y.y*m.x.z ;
+}
+
+float3x3 Inverse(const float3x3 &a)
+{
+	float3x3 b;
+	float d=Determinant(a);
+	assert(d!=0);
+	for(int i=0;i<3;i++) 
+		{
+		for(int j=0;j<3;j++) 
+				{
+			int i1=(i+1)%3;
+			int i2=(i+2)%3;
+			int j1=(j+1)%3;
+			int j2=(j+2)%3;
+			// reverse indexs i&j to take transpose
+			b[j][i] = (a[i1][j1]*a[i2][j2]-a[i1][j2]*a[i2][j1])/d;
+		}
+	}
+	// Matrix check=a*b; // Matrix 'check' should be the identity (or close to it)
+	return b;
+}
+
+
+float3x3 Transpose( const float3x3& m )
+{
+	return float3x3( float3(m.x.x,m.y.x,m.z.x),
+					float3(m.x.y,m.y.y,m.z.y),
+					float3(m.x.z,m.y.z,m.z.z));
+}
+
+
+float3 operator*(const float3& v , const float3x3 &m ) { 
+	return float3((m.x.x*v.x + m.y.x*v.y + m.z.x*v.z), 
+					(m.x.y*v.x + m.y.y*v.y + m.z.y*v.z), 
+					(m.x.z*v.x + m.y.z*v.y + m.z.z*v.z));
+}
+float3 operator*(const float3x3 &m,const float3& v  ) {
+	return float3(dot(m.x,v),dot(m.y,v),dot(m.z,v));
+}
+
+
+float3x3 operator*( const float3x3& a, const float3x3& b )
+{ 
+	return float3x3(a.x*b,a.y*b,a.z*b); 
+}
+
+float3x3 operator*( const float3x3& a, const float& s )  
+{ 
+	return float3x3(a.x*s, a.y*s ,a.z*s); 
+}
+float3x3 operator/( const float3x3& a, const float& s )  
+{ 
+	float t=1/s;
+	return float3x3(a.x*t, a.y*t ,a.z*t); 
+}
+float3x3 operator+( const float3x3& a, const float3x3& b )
+{
+	return float3x3(a.x+b.x, a.y+b.y, a.z+b.z);
+}
+float3x3 operator-( const float3x3& a, const float3x3& b )
+{
+	return float3x3(a.x-b.x, a.y-b.y, a.z-b.z);
+}
+float3x3 &operator+=( float3x3& a, const float3x3& b )
+{
+	a.x+=b.x;
+	a.y+=b.y;
+	a.z+=b.z;
+	return a;
+}
+float3x3 &operator-=( float3x3& a, const float3x3& b )
+{
+	a.x-=b.x;
+	a.y-=b.y;
+	a.z-=b.z;
+	return a;
+}
+float3x3 &operator*=( float3x3& a, const float& s )
+{
+	a.x*=s;
+	a.y*=s;
+	a.z*=s;
+	return a;
+}
+
+
+
+float3 ThreePlaneIntersection(const Plane &p0,const Plane &p1, const Plane &p2){
+	float3x3 mp =Transpose(float3x3(p0.normal,p1.normal,p2.normal));
+	float3x3 mi = Inverse(mp);
+	float3 b(p0.dist,p1.dist,p2.dist);
+	return -b * mi;
+}
+
+
+//--------------- 4D ----------------
+
+float4   operator*( const float4&   v, const float4x4& m )
+{
+	return v.x*m.x + v.y*m.y + v.z*m.z + v.w*m.w; // yes this actually works
+}
+
+int operator==( const float4 &a, const float4 &b ) 
+{
+	return (a.x==b.x && a.y==b.y && a.z==b.z && a.w==b.w); 
+}
+
+
+//  Dont implement m*v for now, since that might confuse us
+//  All our transforms are based on multiplying the "row" vector on the left
+//float4   operator*(const float4x4& m , const float4&   v )
+//{
+//	return float4(dot(v,m.x),dot(v,m.y),dot(v,m.z),dot(v,m.w));
+//}
+
+
+
+float4 cmul( const float4 &a, const float4 &b) 
+{
+	return float4(a.x*b.x,a.y*b.y,a.z*b.z,a.w*b.w);
+}
+
+
+float4 operator*( const float4 &v, float s) 
+{
+	return float4(v.x*s,v.y*s,v.z*s,v.w*s);
+}
+
+
+float4 operator*( float s, const float4 &v) 
+{
+	return float4(v.x*s,v.y*s,v.z*s,v.w*s);
+}
+
+
+float4 operator+( const float4 &a, const float4 &b) 
+{
+	return float4(a.x+b.x,a.y+b.y,a.z+b.z,a.w+b.w);
+}
+
+
+
+float4 operator-( const float4 &a, const float4 &b) 
+{
+	return float4(a.x-b.x,a.y-b.y,a.z-b.z,a.w-b.w);
+}
+
+
+float4 Homogenize(const float3 &v3,const float &w)
+{
+	return float4(v3.x,v3.y,v3.z,w);
+}
+
+
+
+float4x4 operator*( const float4x4& a, const float4x4& b )
+{
+	return float4x4(a.x*b,a.y*b,a.z*b,a.w*b);
+}
+
+float4x4 MatrixTranspose(const float4x4 &m)
+{
+	return float4x4(
+		m.x.x, m.y.x, m.z.x, m.w.x,
+		m.x.y, m.y.y, m.z.y, m.w.y,
+		m.x.z, m.y.z, m.z.z, m.w.z,
+		m.x.w, m.y.w, m.z.w, m.w.w );
+}
+
+float4x4 MatrixRigidInverse(const float4x4 &m)
+{
+	float4x4 trans_inverse = MatrixTranslation(-m.w.xyz());
+	float4x4 rot   = m;
+	rot.w = float4(0,0,0,1);
+	return trans_inverse * MatrixTranspose(rot);
+}
+
+
+float4x4 MatrixPerspectiveFov(float fovy, float aspect, float zn, float zf )
+{
+	float h = 1.0f/tanf(fovy/2.0f); // view space height
+	float w = h / aspect ;  // view space width
+	return float4x4(
+		w, 0, 0             ,   0,
+		0, h, 0             ,   0,
+		0, 0, zf/(zn-zf)    ,  -1,
+		0, 0, zn*zf/(zn-zf) ,   0 );
+}
+
+
+
+float4x4 MatrixLookAt(const float3& eye, const float3& at, const float3& up)
+{
+	float4x4 m;
+	m.w.w = 1.0f;
+	m.w.xyz() = eye;
+	m.z.xyz() = normalize(eye-at);
+	m.x.xyz() = normalize(cross(up,m.z.xyz()));
+	m.y.xyz() = cross(m.z.xyz(),m.x.xyz());
+	return MatrixRigidInverse(m);
+}
+
+
+float4x4 MatrixTranslation(const float3 &t)
+{
+	return float4x4(
+		1,  0,  0,  0,
+		0,  1,  0,  0,
+		0,  0,  1,  0,
+		t.x,t.y,t.z,1 );
+}
+
+
+float4x4 MatrixRotationZ(const float angle_radians)
+{
+	float s =  sinf(angle_radians);
+	float c =  cosf(angle_radians);
+	return float4x4(
+		c,  s,  0,  0,
+		-s, c,  0,  0,
+		0,  0,  1,  0,
+		0,  0,  0,  1 );
+}
+
+
+
+int operator==( const float4x4 &a, const float4x4 &b )
+{
+	return (a.x==b.x && a.y==b.y && a.z==b.z && a.w==b.w);
+}
+
+
+float4x4 Inverse(const float4x4 &m)
+{
+	float4x4 d;
+	float *dst = &d.x.x;
+	float tmp[12]; /* temp array for pairs */
+	float src[16]; /* array of transpose source matrix */
+	float det; /* determinant */
+	/* transpose matrix */
+	for ( int i = 0; i < 4; i++) {
+		src[i] = m(i,0) ;
+		src[i + 4] = m(i,1);
+		src[i + 8] = m(i,2);
+		src[i + 12] = m(i,3); 
+	}
+	/* calculate pairs for first 8 elements (cofactors) */
+	tmp[0]  = src[10] * src[15];
+	tmp[1]  = src[11] * src[14];
+	tmp[2]  = src[9] * src[15];
+	tmp[3]  = src[11] * src[13];
+	tmp[4]  = src[9] * src[14];
+	tmp[5]  = src[10] * src[13];
+	tmp[6]  = src[8] * src[15];
+	tmp[7]  = src[11] * src[12];
+	tmp[8]  = src[8] * src[14];
+	tmp[9]  = src[10] * src[12];
+	tmp[10] = src[8] * src[13];
+	tmp[11] = src[9] * src[12];
+	/* calculate first 8 elements (cofactors) */
+	dst[0]  = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7];
+	dst[0] -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7];
+	dst[1]  = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7];
+	dst[1] -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7];
+	dst[2]  = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7];
+	dst[2] -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7];
+	dst[3]  = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6];
+	dst[3] -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6];
+	dst[4]  = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3];
+	dst[4] -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3];
+	dst[5]  = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3];
+	dst[5] -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3];
+	dst[6]  = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3];
+	dst[6] -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3];
+	dst[7]  = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2];
+	dst[7] -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2];
+	/* calculate pairs for second 8 elements (cofactors) */
+	tmp[0]  = src[2]*src[7];
+	tmp[1]  = src[3]*src[6];
+	tmp[2]  = src[1]*src[7];
+	tmp[3]  = src[3]*src[5];
+	tmp[4]  = src[1]*src[6];
+	tmp[5]  = src[2]*src[5];
+	tmp[6]  = src[0]*src[7];
+	tmp[7]  = src[3]*src[4];
+	tmp[8]  = src[0]*src[6];
+	tmp[9]  = src[2]*src[4];
+	tmp[10] = src[0]*src[5];
+	tmp[11] = src[1]*src[4];
+	/* calculate second 8 elements (cofactors) */
+	dst[8]  = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15];
+	dst[8] -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15];
+	dst[9]  = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15];
+	dst[9] -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15];
+	dst[10] = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15];
+	dst[10]-= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15];
+	dst[11] = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14];
+	dst[11]-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14];
+	dst[12] = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9];
+	dst[12]-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10];
+	dst[13] = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10];
+	dst[13]-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8];
+	dst[14] = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8];
+	dst[14]-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9];
+	dst[15] = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9];
+	dst[15]-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8];
+	/* calculate determinant */
+	det=src[0]*dst[0]+src[1]*dst[1]+src[2]*dst[2]+src[3]*dst[3];
+	/* calculate matrix inverse */
+	det = 1/det;
+	for ( int j = 0; j < 16; j++)
+	dst[j] *= det;
+	return d;
+}
+
+
+//--------- Quaternion --------------
+
+Quaternion operator*( const Quaternion& a, const Quaternion& b )
+{
+	Quaternion c;
+	c.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;
+	c.x = a.w*b.x + a.x*b.w + a.y*b.z - a.z*b.y; 
+	c.y = a.w*b.y - a.x*b.z + a.y*b.w + a.z*b.x; 
+	c.z = a.w*b.z + a.x*b.y - a.y*b.x + a.z*b.w; 
+	return c;
+}
+
+
+Quaternion operator*( const Quaternion& a, float b )
+{
+	return Quaternion(a.x*b, a.y*b, a.z*b ,a.w*b);
+}
+
+Quaternion  Inverse(const Quaternion &q)
+{
+	return Quaternion(-q.x,-q.y,-q.z,q.w);
+}
+
+Quaternion& operator*=( Quaternion& q, const float s )
+{
+		q.x *= s;
+		q.y *= s;
+		q.z *= s;
+		q.w *= s;
+		return q;
+}
+void Quaternion::Normalize()
+{
+	float m = sqrtf(sqr(w)+sqr(x)+sqr(y)+sqr(z));
+	if(m<0.000000001f) {
+		w=1.0f;
+		x=y=z=0.0f;
+		return;
+	}
+	(*this) *= (1.0f/m);
+}
+
+float3 operator*( const Quaternion& q, const float3& v )
+{
+	// The following is equivalent to:
+	//return (q.getmatrix() * v);
+	float qx2 = q.x*q.x;
+	float qy2 = q.y*q.y;
+	float qz2 = q.z*q.z;
+
+	float qxqy = q.x*q.y;
+	float qxqz = q.x*q.z;
+	float qxqw = q.x*q.w;
+	float qyqz = q.y*q.z;
+	float qyqw = q.y*q.w;
+	float qzqw = q.z*q.w;
+	return float3(
+		(1-2*(qy2+qz2))*v.x + (2*(qxqy-qzqw))*v.y + (2*(qxqz+qyqw))*v.z ,
+		(2*(qxqy+qzqw))*v.x + (1-2*(qx2+qz2))*v.y + (2*(qyqz-qxqw))*v.z ,
+		(2*(qxqz-qyqw))*v.x + (2*(qyqz+qxqw))*v.y + (1-2*(qx2+qy2))*v.z  );
+}
+
+float3 operator*( const float3& v, const Quaternion& q )
+{
+	assert(0);  // must multiply with the quat on the left
+	return float3(0.0f,0.0f,0.0f);
+}
+
+Quaternion operator+( const Quaternion& a, const Quaternion& b )
+{
+	return Quaternion(a.x+b.x, a.y+b.y, a.z+b.z, a.w+b.w);
+}
+
+float dot( const Quaternion &a,const Quaternion &b )
+{
+	return  (a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z);
+}
+
+Quaternion normalize( Quaternion a )
+{
+	float m = sqrtf(sqr(a.w)+sqr(a.x)+sqr(a.y)+sqr(a.z));
+	if(m<0.000000001) 
+		{    
+		a.w=1;
+		a.x=a.y=a.z=0;
+		return a;
+	}
+	return a * (1/m);
+}
+
+Quaternion slerp( Quaternion a, const Quaternion& b, float interp )
+{
+	if(dot(a,b) <0.0) 
+		{
+		a.w=-a.w;
+		a.x=-a.x;
+		a.y=-a.y;
+		a.z=-a.z;
+	}
+	float d = dot(a,b);
+	if(d>=1.0) {
+		return a;
+	}
+	float theta = acosf(d);
+	if(theta==0.0f) { return(a);}
+	return a*(sinf(theta-interp*theta)/sinf(theta)) + b*(sinf(interp*theta)/sinf(theta));
+}
+
+
+Quaternion Interpolate(const Quaternion &q0,const Quaternion &q1,float alpha) {
+	return slerp(q0,q1,alpha);
+}
+
+
+Quaternion YawPitchRoll( float yaw, float pitch, float roll ) 
+{
+	roll  *= DEG2RAD;
+	yaw   *= DEG2RAD;
+	pitch *= DEG2RAD;
+	return Quaternion(float3(0.0f,0.0f,1.0f),yaw)*Quaternion(float3(1.0f,0.0f,0.0f),pitch)*Quaternion(float3(0.0f,1.0f,0.0f),roll);
+}
+
+float Yaw( const Quaternion& q )
+{
+	float3 v;
+	v=q.ydir();
+	return (v.y==0.0&&v.x==0.0) ? 0.0f: atan2f(-v.x,v.y)*RAD2DEG;
+}
+
+float Pitch( const Quaternion& q )
+{
+	float3 v;
+	v=q.ydir();
+	return atan2f(v.z,sqrtf(sqr(v.x)+sqr(v.y)))*RAD2DEG;
+}
+
+float Roll( Quaternion q )
+{
+	q = Quaternion(float3(0.0f,0.0f,1.0f),-Yaw(q)*DEG2RAD)  *q;
+	q = Quaternion(float3(1.0f,0.0f,0.0f),-Pitch(q)*DEG2RAD)  *q;
+	return atan2f(-q.xdir().z,q.xdir().x)*RAD2DEG;
+}
+
+float Yaw( const float3& v )
+{
+	return (v.y==0.0&&v.x==0.0) ? 0.0f: atan2f(-v.x,v.y)*RAD2DEG;
+}
+
+float Pitch( const float3& v )
+{
+	return atan2f(v.z,sqrtf(sqr(v.x)+sqr(v.y)))*RAD2DEG;
+}
+
+
+//------------- Plane --------------
+
+
+void Plane::Transform(const float3 &position, const Quaternion &orientation) {
+	//   Transforms the plane to the space defined by the 
+	//   given position/orientation.
+	float3 newnormal;
+	float3 origin;
+
+	newnormal = Inverse(orientation)*normal;
+	origin = Inverse(orientation)*(-normal*dist - position);
+
+	normal = newnormal;
+	dist = -dot(newnormal, origin);
+}
+
+
+
+
+//--------- utility functions -------------
+
+//        RotationArc()
+// Given two vectors v0 and v1 this function
+// returns quaternion q where q*v0==v1.
+// Routine taken from game programming gems.
+Quaternion RotationArc(float3 v0,float3 v1){
+	Quaternion q;
+	v0 = normalize(v0);  // Comment these two lines out if you know its not needed.
+	v1 = normalize(v1);  // If vector is already unit length then why do it again?
+	float3  c = cross(v0,v1);
+	float   d = dot(v0,v1);
+	if(d<=-1.0f) { return Quaternion(1,0,0,0);} // 180 about x axis
+	float   s = sqrtf((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;
+}
+
+
+float4x4 MatrixFromQuatVec(const Quaternion &q, const float3 &v) 
+{
+	// builds a 4x4 transformation matrix based on orientation q and translation v 
+	float qx2 = q.x*q.x;
+	float qy2 = q.y*q.y;
+	float qz2 = q.z*q.z;
+
+	float qxqy = q.x*q.y;
+	float qxqz = q.x*q.z;
+	float qxqw = q.x*q.w;
+	float qyqz = q.y*q.z;
+	float qyqw = q.y*q.w;
+	float qzqw = q.z*q.w;
+
+	return float4x4(
+		1-2*(qy2+qz2),  
+		2*(qxqy+qzqw),
+		2*(