moved files around

LinearMath/AabbUtil2.h

@@ -0,0 +1,57 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef AABB_UTIL2
+#define AABB_UTIL2
+
+#include "SimdVector3.h"
+
+#define SimdMin(a,b) ((a < b ? a : b))
+#define SimdMax(a,b) ((a > b ? a : b))
+
+
+/// conservative test for overlap between two aabbs
+SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const SimdVector3 &aabbMin1, const SimdVector3 &aabbMax1,
+								const SimdVector3 &aabbMin2, const SimdVector3 &aabbMax2)
+{
+	bool overlap = true;
+	overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
+	overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
+	overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
+	return overlap;
+}
+
+/// conservative test for overlap between triangle and aabb
+SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const SimdVector3 *vertices,
+									const SimdVector3 &aabbMin, const SimdVector3 &aabbMax)
+{
+	const SimdVector3 &p1 = vertices[0];
+	const SimdVector3 &p2 = vertices[1];
+	const SimdVector3 &p3 = vertices[2];
+
+	if (SimdMin(SimdMin(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false;
+	if (SimdMax(SimdMax(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false;
+
+	if (SimdMin(SimdMin(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false;
+	if (SimdMax(SimdMax(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false;
+  
+	if (SimdMin(SimdMin(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false;
+	if (SimdMax(SimdMax(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false;
+	return true;
+}
+
+#endif
+

LinearMath/GEN_List.h

@@ -0,0 +1,73 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef GEN_LIST_H
+#define GEN_LIST_H
+
+class GEN_Link {
+public:
+    GEN_Link() : m_next(0), m_prev(0) {}
+    GEN_Link(GEN_Link *next, GEN_Link *prev) : m_next(next), m_prev(prev) {}
+    
+    GEN_Link *getNext() const { return m_next; }  
+    GEN_Link *getPrev() const { return m_prev; }  
+
+    bool isHead() const { return m_prev == 0; }
+    bool isTail() const { return m_next == 0; }
+
+    void insertBefore(GEN_Link *link) {
+        m_next         = link;
+        m_prev         = link->m_prev;
+        m_next->m_prev = this;
+        m_prev->m_next = this;
+    } 
+
+    void insertAfter(GEN_Link *link) {
+        m_next         = link->m_next;
+        m_prev         = link;
+        m_next->m_prev = this;
+        m_prev->m_next = this;
+    } 
+
+    void remove() { 
+        m_next->m_prev = m_prev; 
+        m_prev->m_next = m_next;
+    }
+
+private:  
+    GEN_Link  *m_next;
+    GEN_Link  *m_prev;
+};
+
+class GEN_List {
+public:
+    GEN_List() : m_head(&m_tail, 0), m_tail(0, &m_head) {}
+
+    GEN_Link *getHead() const { return m_head.getNext(); } 
+    GEN_Link *getTail() const { return m_tail.getPrev(); } 
+
+    void addHead(GEN_Link *link) { link->insertAfter(&m_head); }
+    void addTail(GEN_Link *link) { link->insertBefore(&m_tail); }
+    
+private:
+    GEN_Link m_head;
+    GEN_Link m_tail;
+};
+
+#endif
+
+
+

LinearMath/GEN_MinMax.h

@@ -0,0 +1,69 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef GEN_MINMAX_H
+#define GEN_MINMAX_H
+
+template <class T>
+SIMD_FORCE_INLINE const T& GEN_min(const T& a, const T& b) 
+{
+  return b < a ? b : a;
+}
+
+template <class T>
+SIMD_FORCE_INLINE const T& GEN_max(const T& a, const T& b) 
+{
+  return  a < b ? b : a;
+}
+
+template <class T>
+SIMD_FORCE_INLINE const T& GEN_clamped(const T& a, const T& lb, const T& ub) 
+{
+	return a < lb ? lb : (ub < a ? ub : a); 
+}
+
+template <class T>
+SIMD_FORCE_INLINE void GEN_set_min(T& a, const T& b) 
+{
+    if (b < a) 
+	{
+		a = b;
+	}
+}
+
+template <class T>
+SIMD_FORCE_INLINE void GEN_set_max(T& a, const T& b) 
+{
+    if (a < b) 
+	{
+		a = b;
+	}
+}
+
+template <class T>
+SIMD_FORCE_INLINE void GEN_clamp(T& a, const T& lb, const T& ub) 
+{
+	if (a < lb) 
+	{
+		a = lb; 
+	}
+	else if (ub < a) 
+	{
+		a = ub;
+	}
+}
+
+#endif

LinearMath/GEN_random.h

@@ -0,0 +1,42 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef GEN_RANDOM_H
+#define GEN_RANDOM_H
+
+#ifdef MT19937
+
+#include <limits.h>
+#include <mt19937.h>
+
+#define GEN_RAND_MAX UINT_MAX
+
+SIMD_FORCE_INLINE void         GEN_srand(unsigned int seed) { init_genrand(seed); }
+SIMD_FORCE_INLINE unsigned int GEN_rand()                   { return genrand_int32(); }
+
+#else
+
+#include <stdlib.h>
+
+#define GEN_RAND_MAX RAND_MAX
+
+SIMD_FORCE_INLINE void         GEN_srand(unsigned int seed) { srand(seed); } 
+SIMD_FORCE_INLINE unsigned int GEN_rand()                   { return rand(); }
+
+#endif
+
+#endif
+

LinearMath/Geometry.cpp

@@ -0,0 +1,229 @@
+// Bullet Continuous Collision Detection and Physics Library
+// Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+//
+// Geometry.cpp
+//
+// Copyright (c) 2006 Simon Hobbs
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+// 3. This notice may not be removed or altered from any source distribution.
+
+
+///for now this is windows only, Intel SSE SIMD intrinsics
+#ifdef WIN32
+#if _MSC_VER >= 1310
+
+
+#include "Geometry.h"
+#include "Maths.h"
+#include <assert.h>
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Line
+
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Ray
+
+
+// returns false if the lines are parallel
+// t1 and t2 are set to the times of the nearest points on each line
+bool Intersect(const Line& la, const Line& lb, float& ta, float& tb)
+{
+	Vector3 ea = la.m_end - la.m_start;
+	Vector3 eb = lb.m_end - lb.m_start;
+	Vector3 u = la.m_start - lb.m_start;
+
+	float a = Dot(ea, ea);
+	float b = Dot(ea, eb);
+	float c = Dot(eb, eb);
+	float d = Dot(ea, u);
+	float e = Dot(eb, u);
+
+	float det = (a * c - b * b);
+
+	if (Abs(det) < 0.001f)
+		return false;
+
+	float invDet = RcpNr(det);
+	ta = (b * e - c * d) * invDet;
+	tb = (a * e - b * d) * invDet;
+
+	return true;
+}
+
+bool IntersectSegments(const Line& la, const Line& lb, float& ta, float& tb)
+{
+	Vector3 ea = la.m_end - la.m_start;
+	Vector3 eb = lb.m_end - lb.m_start;
+	Vector3 u = la.m_start - lb.m_start;
+
+	float a = Dot(ea, ea);
+	float b = Dot(ea, eb);
+	float c = Dot(eb, eb);
+	float d = Dot(ea, u);
+	float e = Dot(eb, u);
+
+	float det = (a * c - b * b);
+
+	if (Abs(det) < 0.001f)
+		return false;
+
+	float numa = (b * e - c * d);
+	float numb = (a * e - b * d);
+
+	// clip a
+	float dena = det, denb = det;
+	if (numa < 0.0f)
+	{
+		numa = 0.0f;
+		numb = e;
+		denb = c;
+	}
+	else if (numa > det)
+	{
+		numa = det;
+		numb = e + b;
+		denb = c;
+	}
+	else
+		denb = det;
+
+	// clip b
+	if (numb < 0.0f)
+	{
+		numb = 0.0f;
+		if (-d < 0.0f)
+		{
+			numa = 0.0f;
+		}
+		else if (-d > a)
+		{
+			numa = dena;
+		}
+		else
+		{
+			numa = -d;
+			dena = a;
+		}
+	}
+	else if (numb > denb)
+	{
+		numb = denb;
+		if ((-d + b) < 0.0f)
+		{
+			numa = 0.0f;
+		}
+		else if ((-d + b) > a)
+		{
+			numa = dena;
+		}
+		else
+		{
+			numa = -d + b;
+			dena = a;
+		}
+	}
+
+	// compute the times
+	ta = numa / dena;
+	tb = numb / denb;
+
+	return true;
+}
+
+// returns intersection of 2 rays or nearest point to it
+// t1 and t2 are set to the times of the nearest points on each ray (not clamped to ray though)
+// asserts if rays are parallel
+bool Intersect(const Ray& ra, const Ray& rb, float& ta, float& tb)
+{
+	Vector3 u = ra.m_start - rb.m_start;
+
+	Scalar a = Dot(ra.m_dir, ra.m_dir);
+	Scalar b = Dot(ra.m_dir, rb.m_dir);
+	Scalar c = Dot(rb.m_dir, rb.m_dir);
+	Scalar d = Dot(ra.m_dir, u);
+	Scalar e = Dot(rb.m_dir, u);
+
+	Scalar det = (a * c - b * b);
+
+	if (Abs(det) < 0.001f)
+		return false;
+
+	Scalar invDet = RcpNr(det);
+	ta = (b * e - c * d) * invDet;
+	tb = (a * e - b * d) * invDet;
+
+	return true;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Plane
+bool Plane::IsFinite() const
+{
+	if (IsNan(GetX()) || IsNan(GetY()) || IsNan(GetZ()) || IsNan(GetW()))
+		return false;
+	return true;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Bounds3 - axis aligned bounding box
+
+Bounds3::OriginTag Bounds3::Origin;
+Bounds3::EmptyTag Bounds3::Empty;
+
+bool Bounds3::Intersect(const Ray& ray, float& tnear, float& tfar) const
+{
+	Vector3 rcpDir = RcpNr(ray.m_dir);
+
+	Vector3 v1 = (m_min - ray.m_start) * rcpDir;
+	Vector3 v2 = (m_max - ray.m_start) * rcpDir;
+
+	Vector3 vmin = Min(v1, v2);
+	Vector3 vmax = Max(v1, v2);
+
+	Scalar snear = MaxComp(vmin);
+
+	// handle ray being parallel to any axis
+	// (most rays don't need this)
+	if (IsNan(snear))
+	{
+		int inside = (ray.m_start >= m_min) & (ray.m_start <= m_max);
+
+		for (int i = 0; i < 3; i++)
+		{
+			if (IsNan(rcpDir.Get(i)))
+			{
+				if ((inside & (1 << i)) == 0)
+					return false;
+				vmin.Set(i, Scalar::Consts::MinValue);
+				vmax.Set(i, Scalar::Consts::MaxValue);
+			}
+		}
+
+		snear = MaxComp(vmin);
+	}
+
+	tnear = snear;
+	tfar = MinComp(vmax);
+
+	if (tnear > tfar)
+		return false;
+
+	if (tfar < 0.0f)
+		return false;
+
+	return true;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// OrientedBounds3 - oriented bounding box
+
+#endif
+#endif //WIN32

LinearMath/Geometry.h

@@ -0,0 +1,195 @@
+// Bullet Continuous Collision Detection and Physics Library
+// Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+//
+// Geometry.h
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+
+#ifndef BULLET_MATH_GEOMETRY_H
+#define BULLET_MATH_GEOMETRY_H
+
+
+#ifdef WIN32
+
+#include "Vector.h"
+#include "Matrix.h"
+
+class Matrix44;
+class Transform;
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Line
+class Line
+{
+public:
+	Point3 m_start;
+	Point3 m_end;
+
+	Line();
+	Line(const Point3& start, const Point3& end);
+
+	// returns false if the lines are parallel
+	friend bool Intersect(const Line& la, const Line& lb, float& ta, float& tb);
+	friend bool IntersectSegments(const Line& la, const Line& lb, float& ta, float& tb);
+
+	// get projection vector between a point and a line
+	// (i.e. if you add the vector to the point, then the new point will lie on the line)
+	friend Vector3 GetProjectionVector(const Line& ln, const Point3& pt);
+
+	// get distance from point to line (and time along line)
+	friend float Distance(const Line& ln, const Point3& pt, float& t);
+};
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Ray
+class Ray
+{
+public:
+	Point3 m_start;
+	Vector3 m_dir;
+
+	Ray();
+	Ray(const Point3& start, const Vector3& dir);
+
+	explicit Ray(const Line& line);
+
+	// returns false if the rays are parallel
+	friend bool Intersect(const Ray& ra, const Ray& rb, float& ta, float& tb);
+};
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Plane
+class Plane : public Vector4Base
+{
+public:
+	// constructors
+	Plane();
+	Plane(const Plane& p);
+	Plane(const Scalar& x, const Scalar& y, const Scalar& z, const Scalar& w);
+	Plane(const Vector3& xyz, const Scalar& w);
+	Plane(const Point3& a, const Point3& b, const Point3& c);
+	Plane(const Vector3& normal, const Point3& pt);
+
+	// construction to constant
+	Plane(const Maths::ZeroTag&);
+	Plane(const Maths::UnitXTag&);
+	Plane(const Maths::UnitYTag&);
+	Plane(const Maths::UnitZTag&);
+	Plane(const Maths::UnitNegXTag&);
+	Plane(const Maths::UnitNegYTag&);
+	Plane(const Maths::UnitNegZTag&);
+
+	// explicit constructors
+	explicit Plane(const __m128 b);
+	explicit Plane(const Vector3& v);
+	explicit Plane(const Vector4& v);
+	explicit Plane(const float* p);
+
+	// assignment
+	const Plane& operator=(const Plane& v);
+	const Plane& operator=(const Maths::ZeroTag&);
+	const Plane& operator=(const Maths::UnitXTag&);
+	const Plane& operator=(const Maths::UnitYTag&);
+	const Plane& operator=(const Maths::UnitZTag&);
+	const Plane& operator=(const Maths::UnitNegXTag&);
+	const Plane& operator=(const Maths::UnitNegYTag&);
+	const Plane& operator=(const Maths::UnitNegZTag&);
+
+	// element access
+	const Vector3 GetNormal() const;
+	const Scalar GetDistance() const;
+
+	// transformations
+	friend const Plane operator-(const Plane& p);
+	friend const Plane operator*(const Plane& p, const Transform& m);
+
+	// operations
+	friend const Scalar Dot(const Plane& p, const Point3& v);
+	friend const Scalar Dot(const Point3& v, const Plane& p);
+
+	friend const Scalar Dot(const Plane& p, const Vector3& v);
+	friend const Scalar Dot(const Vector3& v, const Plane& p);
+
+	friend const Scalar Dot(const Plane& p, const Vector4& v);
+	friend const Scalar Dot(const Vector4& v, const Plane& p);
+
+	friend const Scalar Intersect(const Plane& p, const Ray& ray);
+	friend const Scalar Intersect(const Plane& p, const Line& line);
+
+	// validation
+	bool IsFinite() const;
+};
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Bounds3 - axis aligned bounding box
+class Bounds3
+{
+public:
+	Point3 m_min, m_max;
+
+	static const enum OriginTag { } Origin;
+	static const enum EmptyTag { } Empty;
+
+	// constructors
+	Bounds3();
+	Bounds3(const Bounds3& aabb);
+	Bounds3(const Point3& min, const Point3& max);
+
+	// construction to constant
+	Bounds3(const OriginTag&);
+	Bounds3(const EmptyTag&);
+
+	// explicit constructors
+	explicit Bounds3(const Point3& minMax);
+
+	// assignment
+	const Bounds3& operator=(const Bounds3& aabb);
+	const Bounds3& operator=(const Point3& pt);
+
+	const Bounds3& operator=(const OriginTag&);
+	const Bounds3& operator=(const EmptyTag&);
+	
+	// in place operations
+	void operator+=(const Point3& pt);
+	void operator+=(const Bounds3& aabb);
+
+	// operations
+	friend Bounds3 operator+(const Bounds3& aabb, const Point3& pt);
+	friend Bounds3 operator+(const Point3& pt, const Bounds3& aabb);
+	friend Bounds3 operator+(const Bounds3& aabb, const Bounds3& aabb2);
+
+	bool Contains(const Point3& pt) const;
+	bool Contains(const Bounds3& aabb) const;
+	bool Touches(const Bounds3& aabb) const;
+
+	bool Intersect(const Ray& ray, float& tnear, float& tfar) const;
+	bool Intersect(const Line& line, float& tnear, float& tfar) const;
+
+	Point3 GetCenter() const;
+	Vector3 GetExtent() const;
+	Vector3 GetSize() const;
+
+	// validation
+	bool IsFinite() const;
+	bool HasVolume() const;
+};
+
+
+#include "Geometry.inl"
+
+#endif //WIN32
+#endif //BULLET_MATH_GEOMETRY_H

LinearMath/Geometry.inl

@@ -0,0 +1,436 @@
+// Bullet Continuous Collision Detection and Physics Library
+// Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+//
+// Geometry.inl
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+#pragma once
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Line
+
+inline Line::Line()
+{
+}
+
+inline Line::Line(const Point3& start, const Point3& end)
+{
+	m_start = start;
+	m_end = end;
+}
+
+inline Vector3 GetProjectionVector(const Line& ln, const Point3& pt)
+{
+	Vector3 d = Normalize(ln.m_end - ln.m_start);
+	Vector3 v = pt - ln.m_start;
+	Scalar s = Dot(v, d);
+	return -(v - d * s);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Ray
+
+inline Ray::Ray()
+{
+}
+
+inline Ray::Ray(const Point3& start, const Vector3& dir)
+{
+	m_start = start;
+	m_dir = dir;
+}
+
+inline Ray::Ray(const Line& line)
+{
+	m_start = line.m_start;
+	m_dir = Normalize(line.m_end - line.m_start);
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Plane
+
+inline Plane::Plane()
+{
+}
+
+inline Plane::Plane(const Plane& p)
+{
+	base = p.base;
+}
+
+inline Plane::Plane(const Scalar& x, const Scalar& y, const Scalar& z, const Scalar& w)
+{
+	Set(x.base, y.base, z.base, w.base);
+}
+
+inline Plane::Plane(const Vector3& xyz, const Scalar& w)
+{
+	Set(xyz.base, w.base);
+}
+
+inline Plane::Plane(const Point3& a, const Point3& b, const Point3& c)
+{
+	Vector3 normal = Normalize(Cross(c - a, b - a));
+	*this = Plane(normal, -Dot(normal, Vector3(a)));
+}
+
+inline Plane::Plane(const Vector3& normal, const Point3& pt)
+{
+	*this = Plane(normal, -Dot(normal, Vector3(pt)));
+}
+
+inline Plane::Plane(const Maths::ZeroTag&)
+{
+	base = _mm_setzero_ps();
+}
+
+inline Plane::Plane(const Maths::UnitXTag&)
+{
+	base = Vector4Base::Consts::k1000;
+}
+
+inline Plane::Plane(const Maths::UnitYTag&)
+{
+	base = Vector4Base::Consts::k0100;
+}
+
+inline Plane::Plane(const Maths::UnitZTag&)
+{
+	base = Vector4Base::Consts::k0010;
+}
+
+inline Plane::Plane(const Maths::UnitNegXTag&)
+{
+	base = Vector4Base::Consts::kNeg1000;
+}
+
+inline Plane::Plane(const Maths::UnitNegYTag&)
+{
+	base = Vector4Base::Consts::kNeg0100;
+}
+
+inline Plane::Plane(const Maths::UnitNegZTag&)
+{
+	base = Vector4Base::Consts::kNeg0010;
+}
+
+inline Plane::Plane(const __m128 b)
+{
+	base = b;
+}
+
+inline Plane::Plane(const Vector3& v)
+{
+	base = v.base;
+}
+
+inline Plane::Plane(const Vector4& v)
+{
+	base = v.base;
+}
+
+inline Plane::Plane(const float* p)
+{
+	base = _mm_load_ps(p);
+}
+
+inline const Plane& Plane::operator=(const Plane& v)
+{
+	base = v.base;
+	return *this;
+}
+
+inline const Plane& Plane::operator=(const Maths::ZeroTag&)
+{
+	base = _mm_setzero_ps();
+	return *this;
+}
+
+inline const Plane& Plane::operator=(const Maths::UnitXTag&)
+{
+	base = Vector4Base::Consts::k1000;
+	return *this;
+}
+
+inline const Plane& Plane::operator=(const Maths::UnitYTag&)
+{
+	base = Vector4Base::Consts::k0100;
+	return *this;
+}
+
+inline const Plane& Plane::operator=(const Maths::UnitZTag&)
+{
+	base = Vector4Base::Consts::k0010;
+	return *this;
+}
+
+inline const Plane& Plane::operator=(const Maths::UnitNegXTag&)
+{
+	base = Vector4Base::Consts::kNeg1000;
+	return *this;
+}
+
+inline const Plane& Plane::operator=(const Maths::UnitNegYTag&)
+{
+	base = Vector4Base::Consts::kNeg0100;
+	return *this;
+}
+
+inline const Plane& Plane::operator=(const Maths::UnitNegZTag&)
+{
+	base = Vector4Base::Consts::kNeg0010;
+	return *this;
+}
+
+inline const Vector3 Plane::GetNormal() const
+{
+	return Vector3(base);
+}
+
+inline const Scalar Plane::GetDistance() const
+{
+	return GetW();
+}
+
+inline const Plane operator-(const Plane& p)
+{
+	return Plane(_mm_sub_ps(_mm_setzero_ps(), p.base));
+}
+
+inline const Plane operator*(const Plane& p, const Transform& m)
+{
+	Vector3 np = Vector3(p) * m;
+	return Plane(np, p.GetW() - Dot(Vector3(np), Vector3(m.GetTranslation())));
+}
+
+inline const Scalar Dot(const Plane& p, const Point3& v)
+{
+	return Scalar(Vector4Base::Dot3(p.base, v.base)) + p.GetW();
+}
+
+inline const Scalar Dot(const Point3& v, const Plane& p)
+{
+	return Scalar(Vector4Base::Dot3(p.base, v.base)) + p.GetW();
+}
+
+inline const Scalar Dot(const Plane& p, const Vector3& v)
+{
+	return Scalar(Vector4Base::Dot3(p.base, v.base));
+}
+
+inline const Scalar Dot(const Vector3& v, const Plane& p)
+{
+	return Scalar(Vector4Base::Dot3(p.base, v.base));
+}
+
+inline const Scalar Dot(const Plane& p, const Vector4& v)
+{
+	return Scalar(Vector4Base::Dot4(p.base, v.base));
+}
+
+inline const Scalar Dot(const Vector4& v, const Plane& p)
+{
+	return Scalar(Vector4Base::Dot4(p.base, v.base));
+}
+
+// returns NaN if ray is perpendicular to ray
+inline const Scalar Intersect(const Plane& p, const Ray& ray)
+{
+	Scalar ds = Dot(p, ray.m_start);
+	Scalar dd = Dot(p, ray.m_dir);
+	return -ds * RcpNr(dd);
+}
+
+// returns NaN if line is perpendicular to ray
+inline const Scalar Intersect(const Plane& p, const Line& line)
+{
+	Scalar ds = Dot(p, line.m_start);
+	Scalar de = Dot(p, line.m_end);
+	return ds * RcpNr(ds - de);
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Bounds3 - axis aligned bounding box
+
+inline Bounds3::Bounds3()
+{
+}
+
+inline Bounds3::Bounds3(const Bounds3& aabb)
+{
+	*this = aabb;
+}
+
+inline Bounds3::Bounds3(const Point3& min, const Point3& max)
+{
+	m_min = min;
+	m_max = max;
+}
+
+inline Bounds3::Bounds3(const OriginTag&)
+{
+	m_min = m_max = Maths::Zero;
+}
+
+inline Bounds3::Bounds3(const EmptyTag&)
+{
+	// max maximal inverted aabb - ready to have points accumulated into it
+	m_min = Point3(Scalar::Consts::MaxValue);
+	m_max = Point3(Scalar::Consts::MinValue);
+}
+
+inline Bounds3::Bounds3(const Point3& minMax)
+{
+	m_min = m_max = minMax;
+}
+
+inline const Bounds3& Bounds3::operator=(const Bounds3& aabb)
+{
+	m_min = aabb.m_min;
+	m_max = aabb.m_max;
+	return *this;
+}
+
+inline const Bounds3& Bounds3::operator=(const Point3& pt)
+{
+	m_min = m_max = pt;
+	return *this;
+}
+
+inline const Bounds3& Bounds3::operator=(const OriginTag&)
+{
+	m_min = m_max = Maths::Zero;
+}
+
+inline const Bounds3& Bounds3::operator=(const EmptyTag&)
+{
+	// max maximal inverted aabb - ready to have points accumulated into it
+	m_min = Point3(Scalar::Consts::MaxValue);
+	m_max = Point3(Scalar::Consts::MinValue);
+	return *this;
+}
+
+inline void Bounds3::operator+=(const Point3& pt)
+{
+	m_min = Min(m_min, pt);
+	m_max = Max(m_max, pt);
+}
+
+inline void Bounds3::operator+=(const Bounds3& aabb)
+{
+	m_min = Min(m_min, aabb.m_min);
+	m_max = Max(m_max, aabb.m_max);
+}
+
+inline Bounds3 operator+(const Bounds3& aabb, const Point3& pt)
+{
+	return Bounds3(Min(aabb.m_min, pt), Max(aabb.m_max, pt));
+}
+
+inline Bounds3 operator+(const Point3& pt, const Bounds3& aabb)
+{
+	return Bounds3(Min(aabb.m_min, pt), Max(aabb.m_max, pt));
+}
+
+inline Bounds3 operator+(const Bounds3& aabb, const Bounds3& aabb2)
+{
+	return Bounds3(Min(aabb.m_min, aabb2.m_min), Max(aabb.m_max, aabb2.m_max));
+}
+
+inline bool Bounds3::Contains(const Point3& pt) const
+{
+	return ((pt >= m_min) & (pt <= m_max)) == 7;
+}
+
+inline bool Bounds3::Contains(const Bounds3& aabb) const
+{
+	return ((aabb.m_min >= m_min) & (aabb.m_max <= m_max)) == 7;
+}
+
+inline bool Bounds3::Touches(const Bounds3& aabb) const
+{
+	return ((aabb.m_max >= m_min) & (aabb.m_min <= m_max)) == 7;
+}
+
+// returns intersection of 2 lines or nearest point to it
+// t1 and t2 are set to the times of the nearest points on each line
+// asserts if rays are parallel
+inline const Point3 IntersectPrev(const Line& la, const Line& lb, float& ta, float& tb)
+{
+	Vector3 ea = la.m_end - la.m_start;
+	Vector3 eb = lb.m_end - lb.m_start;
+	Vector3 u = la.m_start - lb.m_start;
+
+	Scalar a = Dot(ea, ea);
+	Scalar b = Dot(ea, eb);
+	Scalar c = Dot(eb, eb);
+	Scalar d = Dot(ea, u);
+	Scalar e = Dot(eb, u);
+
+	Scalar det = (a * c - b * b);
+
+	// assert if rays are parallel
+	assert(Abs(det) > Scalar(0.0001f));
+
+	Scalar invDet = RcpNr(det);
+	ta = (b * e - c * d) * invDet;
+	tb = (a * e - b * d) * invDet;
+
+	return la.m_start + ea * ta;
+}
+
+inline const Point3 IntersectPrev(const Line& a, const Line& b)
+{
+	float ta, tb;
+	return IntersectPrev(a, b, ta, tb);
+}
+
+inline bool Bounds3::Intersect(const Line& line, float& tnear, float& tfar) const
+{
+	return Intersect(Ray(line), tnear, tfar);
+}
+
+inline Point3 Bounds3::GetCenter() const
+{
+	return Lerp(m_min, m_max, Scalar::Consts::Half);
+}
+
+inline Vector3 Bounds3::GetExtent() const
+{
+	return (m_max - m_min) * Scalar::Consts::Half;
+}
+
+inline Vector3 Bounds3::GetSize() const
+{
+	return m_max - m_min;
+}
+
+inline bool Bounds3::IsFinite() const
+{
+	return m_min.IsFinite() && m_max.IsFinite();
+}
+
+inline bool Bounds3::HasVolume() const
+{
+	return (((m_min <= m_max) & 7) == 7);
+}
+
+
+
+////////////////////////////////////////////////////////////////////////////////
+// OrientedBounds3 - oriented bounding box
+
+

LinearMath/IDebugDraw.h

@@ -0,0 +1,69 @@
+/*
+Copyright (c) 2005 Gino van den Bergen / Erwin Coumans http://continuousphysics.com
+
+Permission is hereby granted, free of charge, to any person or organization
+obtaining a copy of the software and accompanying documentation covered by
+this license (the "Software") to use, reproduce, display, distribute,
+execute, and transmit the Software, and to prepare derivative works of the
+Software, and to permit third-parties to whom the Software is furnished to
+do so, all subject to the following:
+
+The copyright notices in the Software and this entire statement, including
+the above license grant, this restriction and the following disclaimer,
+must be included in all copies of the Software, in whole or in part, and
+all derivative works of the Software, unless such copies or derivative
+works are solely in the form of machine-executable object code generated by
+a source language processor.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
+SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
+FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
+ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+DEALINGS IN THE SOFTWARE.
+*/
+
+
+#ifndef IDEBUG_DRAW__H
+#define IDEBUG_DRAW__H
+
+#include "SimdVector3.h"
+
+
+class	IDebugDraw
+{
+	public:
+
+	enum	DebugDrawModes
+	{
+		DBG_NoDebug=0,
+		DBG_DrawAabb=1,
+		DBG_DrawText=2,
+		DBG_DrawFeaturesText=4,
+		DBG_DrawContactPoints=8,
+		DBG_NoDeactivation=16,
+		DBG_NoHelpText = 32,
+		DBG_DrawWireframe = 64,
+		DBG_ProfileTimings = 128,
+		DBG_EnableSatComparison = 256,
+		DBG_DisableBulletLCP = 512,
+		DBG_EnableCCD = 1024,
+		DBG_MAX_DEBUG_DRAW_MODE
+	};
+
+	virtual ~IDebugDraw() {};
+
+	virtual void	DrawLine(const SimdVector3& from,const SimdVector3& to,const SimdVector3& color)=0;
+
+	virtual void	DrawContactPoint(const SimdVector3& PointOnB,const SimdVector3& normalOnB,float distance,int lifeTime,const SimdVector3& color)=0;
+
+	virtual void	SetDebugMode(int debugMode) =0;
+	
+	virtual int		GetDebugMode() const = 0;
+
+
+};
+
+#endif //IDEBUG_DRAW__H
+

LinearMath/Jamfile

@@ -0,0 +1,9 @@
+SubDir TOP LinearMath ;
+
+Description bulletmath : "Bullet Math Library" ;
+
+Library bulletmath :
+[ Wildcard *.h *.cpp ]
+	;
+
+InstallHeader [ Wildcard *.h ] ;

LinearMath/Maths.h

@@ -0,0 +1,175 @@
+// Bullet Continuous Collision Detection and Physics Library
+// Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+//
+// Maths.h
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+#ifndef BULLET_MATH_H
+#define BULLET_MATH_H
+
+#ifdef WIN32
+
+#include <math.h>
+#include <stdlib.h>
+#include <float.h>
+
+// intrinsics headers
+#include <mmintrin.h>
+#include <xmmintrin.h>
+
+// vector maths classes require aligned alloc
+#include "Memory2.h"
+
+// constants
+#define PI				3.141592654f
+
+#define Angle5			0.087266462f
+#define Angle10			0.174532925f
+#define Angle15			0.261799388f
+#define Angle30			0.523598776f
+#define Angle45			0.785398163f
+#define Angle60			0.523598776f
+#define Angle90			1.570796327f
+#define Angle180		PI
+#define Angle270		4.71238898f
+#define Angle360		6.283185307f
+
+#define Deg2RadF		0.01745329251994329547f
+#define Rad2DegF		57.29577951308232286465f
+
+#define MinValueF		-3.402823466e+38f
+#define MaxValueF		3.402823466e+38F
+
+#define DefaultEpsilon	0.001f
+
+
+// float functions
+
+inline float Sin(const float f)
+{
+	return sinf(f);
+}
+
+inline float Cos(const float f)
+{
+	return cosf(f);
+}
+
+inline float Tan(const float f)
+{
+	return tanf(f);
+}
+
+inline float Asin(const float f)
+{
+	return asinf(f);
+}
+
+inline float Acos(const float f)
+{
+	return acosf(f);
+}
+
+inline float Atan(const float f)
+{
+	return atanf(f);
+}
+
+inline float Atan2(const float y, const float x)
+{
+	return atan2f(y, x);
+}
+
+inline float Pow(const float x, const float y)
+{
+	return powf(x, y);
+}
+
+inline float Abs(const float x)
+{
+	return fabsf(x);
+}
+
+inline float Min(const float x, const float y)
+{
+	return (x < y) ? x : y;
+}
+
+inline float Max(const float x, const float y)
+{
+	return (x > y) ? x : y;
+}
+
+inline float Clamp(const float x, const float min, const float max)
+{
+	return (x >= max) ? max : Max(x, min);
+}
+
+inline float Sgn(const float f)
+{
+	// TODO: replace this with something that doesn't branch
+	if (f < 0.0f)
+		return -1.0f;
+	if (f > 0.0f)
+		return 1.0f;
+	return 0.0f;
+}
+
+inline float Floor(const float f)
+{
+	return floorf(f);
+}
+
+inline float Ceil(const float f)
+{
+	return ceilf(f);
+}
+
+inline float Mod(const float x, const float y)
+{
+	float n = Floor(x / y);
+	return x - n * y;
+}
+
+inline float Sqrt(const float f)
+{
+	return sqrtf(f);
+}
+
+inline bool Equal(const float x, const float y, const float epsilon = DefaultEpsilon)
+{
+	return Abs(x-y) <= epsilon;
+}
+
+inline float Lerp(const float x, const float y, const float t)
+{
+	return x + (y - x) * t;
+}
+
+inline int Min(const int x, const int y)
+{
+	return (x < y) ? x : y;
+}
+
+inline int Max(const int x, const int y)
+{
+	return (x > y) ? x : y;
+}
+
+#include "Vector.h"
+#include "Matrix.h"
+#include "Quat.h"
+#include "Geometry.h"
+
+#endif //WIN32
+#endif //BULLET_MATH_H

LinearMath/Matrix.cpp

@@ -0,0 +1,203 @@
+// Bullet Continuous Collision Detection and Physics Library
+// Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+//
+// Matrix.cpp
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+
+#ifdef WIN32
+#if _MSC_VER >= 1310
+
+#include "matrix.h"
+#include <assert.h>
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Matrix33
+Matrix33::Matrix33(const Quat& q)
+{
+	float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
+	float *pIn = (float*)&q;
+	float *pOut = (float*)this;
+
+	float x = pIn[0], y = pIn[1], z = pIn[2], w = pIn[3];
+
+	x2 = x + x; y2 = y + y;	z2 = z + z;
+	xx = x * x2; yy = y * y2; zz = z * z2;
+	wx = w * x2; wy = w * y2; wz = w * z2;
+	xy = x * y2; xz = x * z2;
+	yz = y * z2;
+
+	pOut[0] = 1.0f - (yy + zz);
+	pOut[1] = xy + wz;
+	pOut[2] = xz - wy;
+	pOut[3] = 0.0f;
+
+	pOut[4] = xy - wz;
+	pOut[5] = 1.0f - (xx + zz);
+	pOut[6] = yz + wx;
+	pOut[7] = 0.0f;
+
+	pOut[8] = xz + wy;
+	pOut[9] = yz - wx;
+	pOut[10] = 1.0f - (xx + yy);
+	pOut[11] = 0.0f;
+}
+
+const Matrix33 Inv(const Matrix33& m)
+{
+	// TODO: do this efficiently - for now we just use the Matrix44 version
+	Matrix44 m44 = Inv(Matrix44(Vector4(m[0]), Vector4(m[1]), Vector4(m[2]), Vector4(Maths::UnitW)));
+	return Matrix33(Vector3(m44[0]), Vector3(m44[1]), Vector3(m44[2]));
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Matrix44
+
+// calculate the inverse of a general 4x4 matrix
+//
+//     -1
+//     A  = ___1__ adjoint A
+//         det A
+//
+const Matrix44 Inv(const Matrix44& src)
+{
+	__m128	minor0, minor1, minor2, minor3;
+	__m128	row0,   row1 = _mm_set_ps1(0.0f),   row2,   row3 = row1;
+	__m128	det,    tmp1 = row1,  tmp2;
+
+	Matrix44 tmp(src);
+	float	*m = (float*)&tmp;
+
+	tmp1	= _mm_loadh_pi( _mm_loadl_pi( tmp1, (__m64*)(m  ) ), (__m64*)(m+ 4) );
+	row1	= _mm_loadh_pi( _mm_loadl_pi( row1, (__m64*)(m+8) ), (__m64*)(m+12) );
+
+	row0	= _mm_shuffle_ps( tmp1, row1, 0x88 );
+	row1	= _mm_shuffle_ps( row1, tmp1, 0xDD );
+
+	tmp1	= _mm_loadh_pi( _mm_loadl_pi( tmp1, (__m64*)(m+ 2) ), (__m64*)(m+ 6) );
+	row3	= _mm_loadh_pi( _mm_loadl_pi( row3, (__m64*)(m+10) ), (__m64*)(m+14) );
+
+	row2	= _mm_shuffle_ps( tmp1, row3, 0x88 );
+	row3	= _mm_shuffle_ps( row3, tmp1, 0xDD );
+//	-----------------------------------------------
+	tmp2	= _mm_mul_ps( row2, row3 );
+	tmp1	= _mm_shuffle_ps( tmp2, tmp2, 0xB1);
+
+	minor0	= _mm_mul_ps( row1, tmp1 );
+	minor1	= _mm_mul_ps( row0, tmp1 );
+
+	tmp1	= _mm_shuffle_ps( tmp1, tmp1, 0x4E );
+
+	minor0	= _mm_sub_ps( _mm_mul_ps( row1, tmp1 ), minor0 );
+	minor1	= _mm_sub_ps( _mm_mul_ps( row0, tmp1 ), minor1 );
+	minor1	= _mm_shuffle_ps( minor1, minor1, 0x4E );
+//	-----------------------------------------------
+	tmp2	= _mm_mul_ps( row1, row2);
+	tmp1	= _mm_shuffle_ps( tmp2, tmp2, 0xB1 );
+
+	minor0	= _mm_add_ps( _mm_mul_ps( row3, tmp1 ), minor0  );
+	minor3	= _mm_mul_ps( row0, tmp1 );
+
+	tmp1	= _mm_shuffle_ps( tmp1, tmp1, 0x4E );
+
+	minor0	= _mm_sub_ps( minor0, _mm_mul_ps( row3, tmp1 ) );
+	minor3	= _mm_sub_ps( _mm_mul_ps( row0, tmp1 ), minor3 );
+	minor3	= _mm_shuffle_ps( minor3, minor3, 0x4E );
+//	-----------------------------------------------
+	tmp2	= _mm_mul_ps( _mm_shuffle_ps( row1, row1, 0x4E ), row3 );
+	tmp1	= _mm_shuffle_ps( tmp2, tmp2, 0xB1 );
+	row2	= _mm_shuffle_ps( row2, row2, 0x4E );
+
+	minor0	= _mm_add_ps( _mm_mul_ps( row2, tmp1 ), minor0 );
+	minor2	= _mm_mul_ps( row0, tmp1 );
+
+	tmp1	= _mm_shuffle_ps( tmp1, tmp1, 0x4E );
+
+	minor0	= _mm_sub_ps( minor0, _mm_mul_ps( row2, tmp1 ) );
+	minor2	= _mm_sub_ps( _mm_mul_ps( row0, tmp1 ), minor2 );
+	minor2	= _mm_shuffle_ps( minor2, minor2, 0x4E );
+//	-----------------------------------------------
+	tmp2	= _mm_mul_ps( row0, row1);
+	tmp1	= _mm_shuffle_ps( tmp2, tmp2, 0xB1 );	
+
+	minor2	= _mm_add_ps( _mm_mul_ps( row3, tmp1 ), minor2 );
+	minor3	= _mm_sub_ps( _mm_mul_ps( row2, tmp1 ), minor3 );
+	
+	tmp1	= _mm_shuffle_ps( tmp1, tmp1, 0x4E );
+	
+	minor2	= _mm_sub_ps( _mm_mul_ps( row3, tmp1 ), minor2 );
+	minor3	= _mm_sub_ps( minor3, _mm_mul_ps( row2, tmp1 ) );
+//	-----------------------------------------------
+	tmp2	= _mm_mul_ps( row0, row3);
+	tmp1	= _mm_shuffle_ps( tmp2, tmp2, 0xB1 );
+	
+	minor1	= _mm_sub_ps( minor1, _mm_mul_ps( row2, tmp1 ) );
+	minor2	= _mm_add_ps( _mm_mul_ps( row1, tmp1 ), minor2 );	
+	
+	tmp1	= _mm_shuffle_ps( tmp1, tmp1, 0x4E );
+	
+	minor1	= _mm_add_ps( _mm_mul_ps( row2, tmp1 ), minor1 );
+	minor2	= _mm_sub_ps( minor2, _mm_mul_ps( row1, tmp1 ) );	
+//	-----------------------------------------------
+	tmp2	= _mm_mul_ps( row0, row2);
+	tmp1	= _mm_shuffle_ps( tmp2, tmp2, 0xB1 );
+	
+	minor1	= _mm_add_ps( _mm_mul_ps( row3, tmp1 ), minor1 );
+	minor3	= _mm_sub_ps( minor3, _mm_mul_ps( row1, tmp1 ) );	
+	
+	tmp1	= _mm_shuffle_ps( tmp1, tmp1, 0x4E );
+	
+	minor1	= _mm_sub_ps( minor1, _mm_mul_ps( row3, tmp1 ) );
+	minor3	= _mm_add_ps( _mm_mul_ps( row1, tmp1 ), minor3 );	
+//	-----------------------------------------------
+	det		= _mm_mul_ps( row0, minor0 );
+	det		= _mm_add_ps( _mm_shuffle_ps( det, det, 0x4E ), det );
+	det		= _mm_add_ss( _mm_shuffle_ps( det, det, 0xB1 ), det );
+	tmp1	= _mm_rcp_ss( det );
+
+	det		= _mm_sub_ss( _mm_add_ss( tmp1, tmp1 ), _mm_mul_ss( det, _mm_mul_ss( tmp1, tmp1 ) ) );
+	det		= _mm_shuffle_ps( det, det, 0x00 );
+
+	minor0	= _mm_mul_ps( det, minor0 );
+	_mm_storel_pi( (__m64*)( m ), minor0 );
+	_mm_storeh_pi( (__m64*)(m+2), minor0 );
+
+	minor1	= _mm_mul_ps( det, minor1 );
+	_mm_storel_pi( (__m64*)(m+4), minor1 );
+	_mm_storeh_pi( (__m64*)(m+6), minor1 );
+
+	minor2	= _mm_mul_ps( det, minor2 );
+	_mm_storel_pi( (__m64*)(m+ 8), minor2 );
+	_mm_storeh_pi( (__m64*)(m+10), minor2 );
+
+	minor3	= _mm_mul_ps( det, minor3 );
+	_mm_storel_pi( (__m64*)(m+12), minor3 );
+	_mm_storeh_pi( (__m64*)(m+14), minor3 );
+
+	return tmp;
+}
+
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Transform
+
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Matrix66
+
+#endif
+#endif //#ifdef WIN32

LinearMath/Matrix.h

@@ -0,0 +1,208 @@
+// Bullet Continuous Collision Detection and Physics Library
+// Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+//
+// Matrix.h
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+#ifndef BULLET_MATRIX_H
+#define BULLET_MATRIX_H
+
+#ifdef WIN32
+
+#include "Vector.h"
+#include "Memory2.h"
+
+class Quat;
+
+#include <malloc.h>
+
+
+////////////////////////////////////////////////////////////////////////////////
+/// Matrix33
+__declspec(align(16)) class Matrix33
+{
+private:
+	Vector3 m_rows[3];
+
+public:
+
+	BULLET_ALIGNED_NEW_AND_DELETE
+
+	
+	// constructors
+	Matrix33();
+	Matrix33(const Vector3& x, const Vector3& y, const Vector3& z);
+
+	// explicit constructors
+	explicit Matrix33(const Quat& q);
+
+	explicit Matrix33(const Maths::ZeroTag&);
+	explicit Matrix33(const Maths::IdentityTag&);
+	explicit Matrix33(const Maths::RotateXTag&, float radians);
+	explicit Matrix33(const Maths::RotateYTag&, float radians);
+	explicit Matrix33(const Maths::RotateZTag&, float radians);
+	explicit Matrix33(const Maths::ScaleTag&, const Vector3& scale);
+	explicit Matrix33(const Maths::SkewTag&, const Vector3& v);
+
+	// assignment
+	const Matrix33& operator=(const Matrix33& m);
+
+	// assignment to constant
+	const Matrix33& operator=(const Maths::ZeroTag&);
+	const Matrix33& operator=(const Maths::IdentityTag&);
+
+	// element access
+	Vector3& operator[](int row);
+	const Vector3& operator[](int row) const;
+
+	Vector3& GetAxisX();
+	const Vector3& GetAxisX() const;
+	void SetAxisX(const Vector3& v);
+
+	Vector3& GetAxisY();
+	const Vector3& GetAxisY() const;
+	void SetAxisY(const Vector3& v);
+
+	Vector3& GetAxisZ();
+	const Vector3& GetAxisZ() const;
+	void SetAxisZ(const Vector3& v);
+
+	// operations
+	void operator*=(const Matrix33& a);
+	void operator*=(const Scalar& s);
+	void operator+=(const Matrix33& a);
+	void operator-=(const Matrix33& a);
+
+	friend const Vector3 operator*(const Vector3& v, const Matrix33& m);
+	friend const Vector3 operator*(const Matrix33& m, const Vector3& vT);
+	friend const Matrix33 operator*(const Matrix33& a, const Matrix33& b);
+	friend const Matrix33 operator*(const Matrix33& m, const Scalar& s);
+	friend const Matrix33 operator+(const Matrix33& a, const Matrix33& b);
+	friend const Matrix33 operator-(const Matrix33& a, const Matrix33& b);
+
+	friend const Matrix33 Transpose(const Matrix33& m);
+	friend const Matrix33 Inv(const Matrix33& m);
+	friend const Scalar Det(const Matrix33& m);
+};
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Matrix44
+
+__declspec(align(16)) class Matrix44
+{
+private:
+	Vector4 m_rows[4];
+
+public:
+	// constructors
+	Matrix44();
+	Matrix44(const Vector4& x, const Vector4& y, const Vector4& z, const Vector4& w);
+
+	// explicit constructors
+	explicit Matrix44(const Maths::ZeroTag&);
+	explicit Matrix44(const Maths::IdentityTag&);
+	explicit Matrix44(const Maths::RotateXTag&, float radians);
+	explicit Matrix44(const Maths::RotateYTag&, float radians);
+	explicit Matrix44(const Maths::RotateZTag&, float radians);
+
+	// assignment
+	const Matrix44& operator=(const Matrix44& m);
+
+	// assignment to constant
+	const Matrix44& operator=(const Maths::ZeroTag&);
+	const Matrix44& operator=(const Maths::IdentityTag&);
+
+	// element access
+	Vector4& operator[](int row);
+	const Vector4& operator[](int row) const;
+
+	// operations
+	void operator*=(const Matrix44& a);
+	void operator*=(const Scalar& s);
+	void operator+=(const Matrix44& a);
+	void operator-=(const Matrix44& a);
+
+	friend const Vector3 operator*(const Vector3& v, const Matrix44& m);
+	friend const Point3 operator*(const Point3& v, const Matrix44& m);
+	friend const Vector4 operator*(const Vector4& v, const Matrix44& m);
+
+	friend const Matrix44 operator*(const Matrix44& a, const Matrix44& b);
+	friend const Matrix44 operator*(const Scalar& s, const Matrix44& m);
+	friend const Matrix44 operator*(const Matrix44& m, const Scalar& s);
+	friend const Matrix44 operator+(const Matrix44& a, const Matrix44& b);
+	friend const Matrix44 operator-(const Matrix44& a, const Matrix44& b);
+
+	friend const Matrix44 Transpose(const Matrix44& m);
+	friend const Matrix44 Inv(const Matrix44& m);
+};
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Transform
+
+__declspec(align(16)) class Transform
+{
+private:
+	Matrix33 m_rotation;
+	Point3 m_translation;
+
+public:
+	// constructors
+	Transform();
+	Transform(const Matrix33& xyz, const Point3& w);
+	Transform(const Vector3& x, const Vector3& y, const Vector3& z, const Point3& w);
+
+	// explicit constructors
+	explicit Transform(const Maths::IdentityTag&);
+
+	// assignment
+	const Transform& operator=(const Transform& m);
+
+	// assignment to constant
+	const Transform& operator=(const Maths::IdentityTag&);
+
+	// element access
+	Matrix33& GetRotation();
+	const Matrix33& GetRotation() const;
+	void SetRotation(const Matrix33& m);
+	void SetRotation(const Quat& q);
+
+	Point3& GetTranslation();
+	const Point3& GetTranslation() const;
+	void SetTranslation(const Point3& p);
+
+	Vector3& GetAxisX();
+	const Vector3& GetAxisX() const;
+
+	Vector3& GetAxisY();
+	const Vector3& GetAxisY() const;
+
+	Vector3& GetAxisZ();
+	const Vector3& GetAxisZ() const;
+
+	// operations
+	friend const Vector3 operator*(const Vector3& v, const Transform& m);
+	friend const Point3 operator*(const Point3& v, const Transform& m);
+
+	friend const Transform operator*(const Transform& v, const Transform& m);
+
+	friend const Transform Inv(const Transform& m);
+};
+
+
+
+#include "matrix.inl"
+
+#endif //#ifdef WIN32
+#endif //BULLET_MATRIX_H

LinearMath/Matrix.inl

@@ -0,0 +1,609 @@
+// Bullet Continuous Collision Detection and Physics Library
+// Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+//
+//
+// Matrix.inl
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+#pragma once
+
+#include <assert.h>
+#include "math.h"
+
+////////////////////////////////////////////////////////////////////////////////
+// Matrix33
+
+inline Matrix33::Matrix33()
+{
+}
+
+inline Matrix33::Matrix33(const Vector3& x, const Vector3& y, const Vector3& z)
+{
+	m_rows[0] = x;
+	m_rows[1] = y;
+	m_rows[2] = z;
+}
+
+inline Matrix33::Matrix33(const Maths::ZeroTag&)
+{
+	m_rows[0] = Maths::Zero;
+	m_rows[1] = Maths::Zero;
+	m_rows[2] = Maths::Zero;
+}
+
+inline Matrix33::Matrix33(const Maths::IdentityTag&)
+{
+	m_rows[0] = Maths::UnitX;
+	m_rows[1] = Maths::UnitY;
+	m_rows[2] = Maths::UnitZ;
+}
+
+inline Matrix33::Matrix33(const Maths::RotateXTag&, float radians)
+{
+	float c = cosf(radians);
+	float s = sinf(radians);
+
+	m_rows[0] = Maths::UnitX;
+	m_rows[1] = Vector3(0.0f, c, s);
+	m_rows[2] = Vector3(0.0f, -s, c);
+}
+
+inline Matrix33::Matrix33(const Maths::RotateYTag&, float radians)
+{
+	float c = cosf(radians);
+	float s = sinf(radians);
+
+	m_rows[0] = Vector3(c, 0.0f, -s);
+	m_rows[1] = Maths::UnitY;
+	m_rows[2] = Vector3(s, 0.0f, c);
+}
+
+inline Matrix33::Matrix33(const Maths::RotateZTag&, float radians)
+{
+	float c = cosf(radians);
+	float s = sinf(radians);
+
+	m_rows[0] = Vector3(c, s, 0.0f);
+	m_rows[1] = Vector3(-s, c, 0.0f);
+	m_rows[2] = Maths::UnitZ;
+}
+
+inline Matrix33::Matrix33(const Maths::ScaleTag&, const Vector3& scale)
+{
+	m_rows[0] = scale * Vector3(Maths::UnitX);
+	m_rows[1] = scale * Vector3(Maths::UnitY);
+	m_rows[2] = scale * Vector3(Maths::UnitZ);
+}
+
+inline Matrix33::Matrix33(const Maths::SkewTag&, const Vector3& v)
+{
+	m_rows[0] = Vector3(0.0f, v[2], -v[1]);
+	m_rows[1] = Vector3(-v[2], 0.0f, v[0]);
+	m_rows[2] = Vector3(v[1], -v[0], 0.0f);
+}
+
+inline const Matrix33& Matrix33::operator=(const Matrix33& m)
+{
+	m_rows[0] = m.m_rows[0];
+	m_rows[1] = m.m_rows[1];
+	m_rows[2] = m.m_rows[2];
+	return *this;
+}
+
+inline const Matrix33& Matrix33::operator=(const Maths::ZeroTag&)
+{
+	m_rows[0] = m_rows[1] = m_rows[2] = Maths::Zero;
+	return *this;
+}
+
+inline const Matrix33& Matrix33::operator=(const Maths::IdentityTag&)
+{
+	m_rows[0] = Maths::UnitX;
+	m_rows[1] = Maths::UnitY;
+	m_rows[2] = Maths::UnitZ;
+	return *this;
+}
+
+inline Vector3& Matrix33::operator[](int row)
+{
+	return m_rows[row];
+}
+
+inline const Vector3& Matrix33::operator[](int row) const
+{
+	return m_rows[row];
+}
+
+inline Vector3& Matrix33::GetAxisX()
+{
+	return m_rows[0];
+}
+
+inline const Vector3& Matrix33::GetAxisX() const
+{
+	return m_rows[0];
+}
+
+inline void Matrix33::SetAxisX(const Vector3& v)
+{
+	m_rows[0] = v;
+}
+
+inline Vector3& Matrix33::GetAxisY()
+{
+	return m_rows[1];
+}
+
+inline const Vector3& Matrix33::GetAxisY() const
+{
+	return m_rows[1];
+}
+
+inline void Matrix33::SetAxisY(const Vector3& v)
+{
+	m_rows[1] = v;
+}
+
+inline Vector3& Matrix33::GetAxisZ()
+{
+	return m_rows[2];
+}
+
+inline const Vector3& Matrix33::GetAxisZ() const
+{
+	return m_rows[2];
+}
+
+inline void Matrix33::SetAxisZ(const Vector3& v)
+{
+	m_rows[2] = v;
+}
+
+inline const Vector3 operator*(const Vector3& v, const Matrix33& m)
+{
+	Scalar xxxx = v.GetX();
+	Scalar yyyy = v.GetY();
+	Scalar zzzz = v.GetZ();
+
+	return xxxx * m[0] + yyyy * m[1] + zzzz * m[2];
+}
+
+inline const Vector3 operator*(const Matrix33& m, const Vector3& vT)
+{
+	return Vector3(Dot(m[0], vT), Dot(m[1], vT), Dot(m[2], vT));
+}
+
+inline void Matrix33::operator*=(const Matrix33& a)
+{
+	*this = *this * a;
+}
+
+inline void Matrix33::operator*=(const Scalar& s)
+{
+	m_rows[0] *= s;
+	m_rows[1] *= s;
+	m_rows[2] *= s;
+}
+
+inline void Matrix33::operator+=(const Matrix33& a)
+{
+	m_rows[0] += a[0];
+	m_rows[1] += a[1];
+	m_rows[2] += a[2];
+}
+
+inline void Matrix33::operator-=(const Matrix33& a)
+{
+	m_rows[0] -= a[0];
+	m_rows[1] -= a[1];
+	m_rows[2] -= a[2];
+}
+
+inline const Matrix33 operator*(const Scalar& s, const Matrix33& m)
+{
+	Vector3 scale(s);
+	return Matrix33(scale * m[0], scale * m[1], scale * m[2]);
+}
+
+inline const Matrix33 operator*(const Matrix33& m, const Scalar& s)
+{
+	Vector3 scale(s);
+	return Matrix33(scale * m[0], scale * m[1], scale * m[2]);
+}
+
+inline const Matrix33 operator*(const Matrix33& a, const Matrix33& b)
+{
+	return Matrix33(a[0] * b, a[1] * b, a[2] * b);
+}
+
+inline const Matrix33 operator+(const Matrix33& a, const Matrix33& b)
+{
+	return Matrix33(a[0] + b[0], a[1] + b[1], a[2] + b[2]);
+}
+
+inline const Matrix33 operator-(const Matrix33& a, const Matrix33& b)
+{
+	return Matrix33(a[0] - b[0], a[1] - b[1], a[2] - b[2]);
+}
+
+inline const Matrix33 Transpose(const Matrix33& m)
+{
+	// easiest way is to actually do a 4 * 4 transpose with an implied zero bottom row:
+
+	// a b c d			a e i 0
+	// e f g h	 --->	b f j 0
+	// i j k l			c g k 0
+	// 0 0 0 0
+
+	// shuffle the rows to make 4 quarters
+	__m128 abef = _mm_shuffle_ps(m[0].base, m[1].base, _MM_SHUFFLE(1, 0, 1, 0));
+	__m128 cdgh = _mm_shuffle_ps(m[0].base, m[1].base, _MM_SHUFFLE(3, 2, 3, 2));
+	__m128 ij00 = _mm_shuffle_ps(m[2].base, _mm_setzero_ps(), _MM_SHUFFLE(1, 0, 1, 0));
+	__m128 kl00 = _mm_shuffle_ps(m[2].base, _mm_setzero_ps(), _MM_SHUFFLE(3, 2, 3, 2));
+
+	// shuffle the quarters to make new rows
+	__m128 aei0 = _mm_shuffle_ps(abef, ij00, _MM_SHUFFLE(2, 0, 2, 0));
+	__m128 bfj0 = _mm_shuffle_ps(abef, ij00, _MM_SHUFFLE(3, 1, 3, 1));
+	__m128 cgk0 = _mm_shuffle_ps(cdgh, kl00, _MM_SHUFFLE(2, 0, 2, 0));
+
+	return Matrix33(Vector3(aei0), Vector3(bfj0), Vector3(cgk0));
+}
+
+inline const Scalar Det(const Matrix33& m)
+{
+	return Dot(Cross(m.GetAxisX(), m.GetAxisY()),m.GetAxisZ());
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Matrix44
+
+inline Matrix44::Matrix44()
+{
+}
+
+inline Matrix44::Matrix44(const Vector4& x, const Vector4& y, const Vector4& z, const Vector4& w)
+{
+	m_rows[0] = x;
+	m_rows[1] = y;
+	m_rows[2] = z;
+	m_rows[3] = w;
+}
+
+inline Matrix44::Matrix44(const Maths::ZeroTag&)
+{
+	m_rows[0] = Maths::Zero;
+	m_rows[1] = Maths::Zero;
+	m_rows[2] = Maths::Zero;
+	m_rows[3] = Maths::Zero;
+}
+
+inline Matrix44::Matrix44(const Maths::IdentityTag&)
+{
+	m_rows[0] = Maths::UnitX;
+	m_rows[1] = Maths::UnitY;
+	m_rows[2] = Maths::UnitZ;
+	m_rows[3] = Maths::UnitW;
+}
+
+inline Matrix44::Matrix44(const Maths::RotateXTag&, float radians)
+{
+	float c = cosf(radians);
+	float s = sinf(radians);
+
+	m_rows[0] = Maths::UnitX;
+	m_rows[1] = Vector4(0.0f, c, s, 0.0f);
+	m_rows[2] = Vector4(0.0f, -s, c, 0.0f);
+	m_rows[3] = Maths::UnitW;
+}
+
+inline Matrix44::Matrix44(const Maths::RotateYTag&, float radians)
+{
+	float c = cosf(radians);
+	float s = sinf(radians);
+
+	m_rows[0] = Vector4(c, 0.0f, -s, 0.0f);
+	m_rows[1] = Maths::UnitY;
+	m_rows[2] = Vector4(s, 0.0f, c, 0.0f);
+	m_rows[3] = Maths::UnitW;
+}
+
+inline Matrix44::Matrix44(const Maths::RotateZTag&, float radians)
+{
+	float c = cosf(radians);
+	float s = sinf(radians);
+
+	m_rows[0] = Vector4(c, s, 0.0f, 0.0f);
+	m_rows[1] = Vector4(-s, c, 0.0f, 0.0f);
+	m_rows[2] = Maths::UnitZ;
+	m_rows[3] = Maths::UnitW;
+}
+
+inline const Matrix44& Matrix44::operator=(const Matrix44& m)
+{
+	m_rows[0] = m.m_rows[0];
+	m_rows[1] = m.m_rows[1];
+	m_rows[2] = m.m_rows[2];
+	m_rows[3] = m.m_rows[3];
+	return *this;
+}
+
+inline const Matrix44& Matrix44::operator=(const Maths::ZeroTag&)
+{
+	m_rows[0] = m_rows[1] = m_rows[2] = m_rows[3] = Maths::Zero;
+	return *this;
+}
+
+inline const Matrix44& Matrix44::operator=(const Maths::IdentityTag&)
+{
+	m_rows[0] = Maths::UnitX;
+	m_rows[1] = Maths::UnitY;
+	m_rows[2] = Maths::UnitZ;
+	m_rows[3] = Maths::UnitW;
+	return *this;
+}
+
+inline Vector4& Matrix44::operator[](int row)
+{
+	return m_rows[row];
+}
+
+inline const Vector4& Matrix44::operator[](int row) const
+{
+	return m_rows[row];
+}
+
+inline void Matrix44::operator*=(const Matrix44& a)
+{
+	*this = *this * a;
+}
+
+inline void Matrix44::operator*=(const Scalar& s)
+{
+	m_rows[0] *= s;
+	m_rows[1] *= s;
+	m_rows[2] *= s;
+	m_rows[3] *= s;
+}
+
+inline void Matrix44::operator+=(const Matrix44& a)
+{
+	m_rows[0] += a[0];
+	m_rows[1] += a[1];
+	m_rows[2] += a[2];
+	m_rows[3] += a[3];
+}
+
+inline void Matrix44::operator-=(const Matrix44& a)
+{
+	m_rows[0] -= a[0];
+	m_rows[1] -= a[1];
+	m_rows[2] -= a[2];
+	m_rows[3] -= a[3];
+}
+
+inline const Vector3 operator*(const Vector3& v, const Matrix44& m)
+{
+	Scalar xxxx = v.GetX();
+	Scalar yyyy = v.GetY();
+	Scalar zzzz = v.GetZ();
+
+	return xxxx * Vector3(m[0]) + yyyy * Vector3(m[1]) + zzzz * Vector3(m[2]);
+}
+
+inline const Point3 operator*(const Point3& v, const Matrix44& m)
+{
+	Scalar xxxx = v.GetX();
+	Scalar yyyy = v.GetY();
+	Scalar zzzz = v.GetZ();
+
+	return Point3(xxxx * m[0] + yyyy * m[1] + zzzz * m[2] + m[3]);
+}
+
+inline const Vector4 operator*(const Vector4& v, const Matrix44& m)
+{
+	Scalar xxxx = v.GetX();
+	Scalar yyyy = v.GetY();
+	Scalar zzzz = v.GetZ();
+	Scalar wwww = v.GetW();
+
+	return xxxx * m[0] + yyyy * m[1] + zzzz * m[2] + wwww * m[3];
+}
+
+inline const Matrix44 operator*(const Matrix44& a, const Matrix44& b)
+{
+	return Matrix44(a[0] * b, a[1] * b, a[2] * b, a[3] * b);
+}
+
+inline const Matrix44 operator*(const Matrix44& m, const Scalar& s)
+{
+	Vector4 scale(s);
+	return Matrix44(scale * m[0], scale * m[1], scale * m[2], scale * m[3]);
+}
+
+inline const Matrix44 operator*(const Scalar& s, const Matrix44& m)
+{
+	Vector4 scale(s);
+	return Matrix44(scale * m[0], scale * m[1], scale * m[2], scale * m[3]);
+}
+
+inline const Matrix44 operator+(const Matrix44& a, const Matrix44& b)
+{
+	return Matrix44(a[0] + b[0], a[1] + b[1], a[2] + b[2], a[3] + b[3]);
+}
+
+inline const Matrix44 operator-(const Matrix44& a, const Matrix44& b)
+{
+	return Matrix44(a[0] - b[0], a[1] - b[1], a[2] - b[2], a[3] - b[3]);
+}
+
+inline const Matrix44 Transpose(const Matrix44& m)
+{
+	// a b c d			a e i m
+	// e f g h	 --->	b f j n
+	// i j k l			c g k o
+	// m n o p			d h l p
+
+	// shuffle the rows to make 4 quarters
+	__m128 abef = _mm_shuffle_ps(m[0].base, m[1].base, _MM_SHUFFLE(1, 0, 1, 0));
+	__m128 cdgh = _mm_shuffle_ps(m[0].base, m[1].base, _MM_SHUFFLE(3, 2, 3, 2));
+	__m128 ijmn = _mm_shuffle_ps(m[2].base, m[3].base, _MM_SHUFFLE(1, 0, 1, 0));
+	__m128 klop = _mm_shuffle_ps(m[2].base, m[3].base, _MM_SHUFFLE(3, 2, 3, 2));
+
+	// shuffle the quarters to make new rows
+	__m128 aeim = _mm_shuffle_ps(abef, ijmn, _MM_SHUFFLE(2, 0, 2, 0));
+	__m128 bfjn = _mm_shuffle_ps(abef, ijmn, _MM_SHUFFLE(3, 1, 3, 1));
+	__m128 cgko = _mm_shuffle_ps(cdgh, klop, _MM_SHUFFLE(2, 0, 2, 0));
+	__m128 dhlp = _mm_shuffle_ps(cdgh, klop, _MM_SHUFFLE(3, 1, 3, 1));
+
+	return Matrix44(Vector4(aeim), Vector4(bfjn), Vector4(cgko), Vector4(dhlp));
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Transform
+
+inline Transform::Transform()
+{
+}
+
+inline Transform::Transform(const Matrix33& xyz, const Point3& w)
+{
+	m_rotation = xyz;
+	m_translation = w;
+}
+
+inline Transform::Transform(const Vector3& x, const Vector3& y, const Vector3& z, const Point3& w)
+{
+	m_rotation[0] = x;
+	m_rotation[1] = y;
+	m_rotation[2] = z;
+	m_translation = w;
+}
+
+inline Transform::Transform(const Maths::IdentityTag&)
+{
+	m_rotation = Maths::Identity;
+	m_translation = Maths::Zero;
+}
+
+inline const Transform& Transform::operator=(const Transform& m)
+{
+	m_rotation = m.m_rotation;
+	m_translation = m.m_translation;
+	return *this;
+}
+
+inline const Transform& Transform::operator=(const Maths::IdentityTag&)
+{
+	m_rotation = Maths::Identity;
+	m_translation = Maths::Zero;
+	return *this;
+}
+
+inline Matrix33& Transform::GetRotation()
+{
+	return m_rotation;
+}
+
+inline const Matrix33& Transform::GetRotation() const
+{
+	return m_rotation;
+}
+
+inline void Transform::SetRotation(const Matrix33& m)
+{
+	m_rotation = m;
+}
+
+inline void Transform::SetRotation(const Quat& q)
+{
+	m_rotation = Matrix33(q);
+}
+
+inline Point3& Transform::GetTranslation()
+{
+	return m_translation;
+}
+
+inline const Point3& Transform::GetTranslation() const
+{
+	return m_translation;
+}
+
+inline void Transform::SetTranslation(const Point3& t)
+{
+	m_translation = t;
+}
+
+inline Vector3& Transform::GetAxisX()
+{
+	return m_rotation[0];
+}
+
+inline const Vector3& Transform::GetAxisX() const
+{
+	return m_rotation[0];
+}
+
+inline Vector3& Transform::GetAxisY()
+{
+	return m_rotation[1];
+}
+
+inline const Vector3& Transform::GetAxisY() const
+{
+	return m_rotation[1];
+}
+
+inline Vector3& Transform::GetAxisZ()
+{
+	return m_rotation[2];
+}
+
+inline const Vector3& Transform::GetAxisZ() const
+{
+	return m_rotation[2];
+}
+
+inline const Vector3 operator*(const Vector3& v, const Transform& m)
+{
+	Scalar xxxx = v.GetX();
+	Scalar yyyy = v.GetY();
+	Scalar zzzz = v.GetZ();
+
+	return xxxx * m.GetAxisX() + yyyy * m.GetAxisY() + zzzz * m.GetAxisZ();
+}
+
+inline const Point3 operator*(const Point3& v, const Transform& m)
+{
+	Scalar xxxx = v.GetX();
+	Scalar yyyy = v.GetY();
+	Scalar zzzz = v.GetZ();
+
+	return xxxx * m.GetAxisX() + yyyy * m.GetAxisY() + zzzz * m.GetAxisZ() + m.GetTranslation();
+}
+
+inline const Transform operator*(const Transform& a, const Transform& b)
+{
+	return Transform(a.GetAxisX() * b, a.GetAxisY() * b, a.GetAxisZ() * b, a.GetTranslation() * b);
+}
+
+inline const Transform Inv(const Transform& m)
+{
+	Matrix33 r(Transpose(m.GetRotation()));
+	Point3 t = m.GetTranslation();
+	t = Point3(-(t.GetX() * r.GetAxisX() + t.GetY() * r.GetAxisY() + t.GetZ() * r.GetAxisZ()));
+
+	return Transform(r, t); 
+}

LinearMath/Memory2.h

@@ -0,0 +1,61 @@
+// Bullet Continuous Collision Detection and Physics Library
+// Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+//
+// Memory.h
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+
+#ifndef BULLET_MEMORY2_H
+#define BULLET_MEMORY2_H
+
+#ifdef WIN32
+
+// default new and delete overrides that guarantee 16 byte alignment and zero allocated memory
+void* operator new(size_t sz) throw();
+void* operator new[](size_t sz) throw();
+void operator delete(void* m) throw();
+void operator delete[](void* m) throw();
+
+#include <malloc.h>
+
+#define BULLET_ALIGNED_NEW_AND_DELETE \
+\
+inline void* operator new(size_t sz) throw()	\
+{												\
+	void* mem = _aligned_malloc(sz + 64, 16);	\
+	return mem;									\
+}												\
+												\
+inline void* operator new[](size_t sz) throw()	\
+{												\
+	void* mem = _aligned_malloc(sz + 64, 16);	\
+	return mem;									\
+}												\
+												\
+inline void operator delete(void* m) throw()	\
+{												\
+	if (m == 0)									\
+		return;									\
+	_aligned_free(m);							\
+}												\
+												\
+inline void operator delete[](void* m) throw()	\
+{												\
+	_aligned_free(m);							\
+}												\
+
+
+
+#endif //WIN32	
+
+#endif //MEMORY2_H

LinearMath/Quat.cpp

@@ -0,0 +1,108 @@
+// Bullet Continuous Collision Detection and Physics Library
+// Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+//
+// Quat.cpp
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+
+#ifdef WIN32
+#if _MSC_VER >= 1310
+
+#include "Quat.h"
+#include "Maths.h"
+
+bool Quat::IsFinite() const
+{
+	if (_finite(GetX()) && _finite(GetY()) && _finite(GetZ()) && _finite(GetW()))
+		return true;
+	return false;
+}
+
+Quat::Quat(const Matrix33& m)
+{
+	float tr, s, q[4];
+	int i, j, k;
+	int nxt[3] = {1, 2, 0};
+	float x, y, z, w;
+
+	// Check the diagonal
+	tr = m[0][0] + m[1][1] + m[2][2];
+	if (tr >= 0.0f)
+	{
+		// Diagonal is positive
+		s = ::Sqrt(tr + 1.0f);
+		w = s * 0.5f;
+		s = 0.5f / s;
+		x = (m[1][2] - m[2][1]) * s;
+		y = (m[2][0] - m[0][2]) * s;
+		z = (m[0][1] - m[1][0]) * s;
+	}
+	else
+	{
+		// Diagonal is negative
+		i = 0;
+		if (m[1][1] > m[0][0]) i = 1;
+		if (m[2][2] > m[i][i]) i = 2;
+		j = nxt[i];
+		k = nxt[j];
+
+		s = ::Sqrt((m[i][i] - (m[j][j] + m[k][k])) + 1.0f);
+		q[i] = s * 0.5f;
+		if (s != 0.0f) s = 0.5f / s;
+
+		q[3] = (m[j][k] - m[k][j]) * s;
+		q[j] = (m[i][j] + m[j][i]) * s;
+		q[k] = (m[i][k] + m[k][i]) * s;
+
+		x = q[0];
+		y = q[1];
+		z = q[2];
+		w = q[3];
+	}
+
+	*this = Quat(x, y, z, w);
+}
+
+const Quat Slerp(const Quat& a, const Quat& b, const Scalar& t)
+{
+	Quat e;
+	Scalar cosom, t0, t1;
+	
+	cosom = Dot(a, b);
+
+	if (cosom < Scalar::Consts::Zero)
+	{
+		cosom = -cosom;
+		e = -b;
+	}
+	else
+		e = b;
+
+	if (cosom < 0.9999f)
+	{
+		float omega = ::Acos(cosom);
+		Scalar rcpSinom = Rcp(Scalar(::Sin(omega)));
+		t0 = Scalar(::Sin((1.0f - (float)t) * omega)) * rcpSinom;
+		t1 = Scalar(::Sin((float)t * omega)) * rcpSinom;
+	}
+	else
+	{
+		t0 = Scalar(1.0f) - t;
+		t1 = t;
+	}
+
+	return a * t0 + e * t;
+}
+
+#endif 
+#endif //#ifdef WIN32

LinearMath/Quat.h

@@ -0,0 +1,87 @@
+// Bullet Continuous Collision Detection and Physics Library
+// Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+//
+// Quat.h
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+#ifndef BULLET_QUAT_H
+#define BULLET_QUAT_H
+
+#include "Vector.h"
+
+class Matrix33;
+
+
+class Quat : public Vector4Base
+{
+public:
+	BULLET_ALIGNED_NEW_AND_DELETE
+
+	// constructors
+	Quat();
+	Quat(const Scalar& x, const Scalar& y, const Scalar& z, const Scalar& w);
+	Quat(const Vector3& axis, const Scalar& angle);
+
+	// construction to constant
+	Quat(const Maths::IdentityTag&);
+
+	// explicit constructors
+	explicit Quat(const __m128 b);
+	explicit Quat(const Vector3& v);
+	explicit Quat(const Vector4& v);
+	explicit Quat(const Matrix33& m);
+	explicit Quat(const float* p);
+
+	// assignment
+	const Quat& operator=(const Quat& v);
+	const Quat& operator=(const Maths::IdentityTag&);
+
+	// in place operations
+	void operator+=(const Quat& b);
+	void operator-=(const Quat& b);
+	void operator*=(const Quat& b);
+	void operator*=(const Scalar& s);
+
+	// operations
+	friend const Quat operator-(const Quat& a);
+	friend const Quat operator+(const Quat& a, const Quat& b);
+	friend const Quat operator-(const Quat& a, const Quat& b);
+	friend const Quat operator*(const Quat& a, const Quat& b);
+	friend const Quat operator*(const Quat& a, const Scalar& s);
+	friend const Quat operator*(const Scalar& s, const Quat& a);
+
+	friend const Quat Inv(const Quat& a);
+
+	friend const Scalar Dot(const Quat& a, const Quat& b);
+
+	friend const Scalar Length(const Quat& a);
+	friend const Quat Normalize(const Quat& a);
+
+	friend const Scalar LengthFast(const Quat& a);
+	friend const Quat NormalizeFast(const Quat& a);
+
+	friend const Quat Slerp(const Quat& a, const Quat& b, const Scalar& t);
+	friend const Quat Lerp(const Quat& a, const Quat& b, const Scalar& t);
+
+	const Vector3 Rotate(const Vector3& v) const;
+	void GetAngleAxis(Vector3& axis, Scalar& angle) const;
+
+	// validation
+	bool IsFinite() const;
+};
+
+
+#include "Quat.inl"
+
+
+#endif //BULLET_QUAT_H

LinearMath/Quat.inl

@@ -0,0 +1,189 @@
+// Bullet Continuous Collision Detection and Physics Library
+// Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+//
+// Quat.inl
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+#pragma once
+
+#include <assert.h>
+#include "math.h"
+
+inline Quat::Quat()
+{
+}
+
+inline  Quat::Quat(const Scalar& x, const Scalar& y, const Scalar& z, const Scalar& w)
+{
+	Set(x.base, y.base, z.base, w.base);
+}
+
+inline Quat::Quat(const Vector3& axis, const Scalar& angle)
+{
+	assert(axis.IsFinite());
+	Set((sinf(0.5f * (float)angle) * axis).base, Scalar(cosf(0.5f * (float)angle)).base);
+}
+
+inline Quat::Quat(const Maths::IdentityTag&)
+{
+	base = Vector4Base::Consts::k0001;
+}
+
+inline Quat::Quat(const Vector3& v)
+{
+	base = v.base;
+}
+
+inline Quat::Quat(const Vector4& v)
+{
+	base = v.base;
+}
+
+inline Quat::Quat(const __m128 b)
+{
+	base = b;
+}
+
+inline Quat::Quat(const float* p)
+{
+	base = _mm_load_ps(p);
+}
+
+inline const Quat& Quat::operator=(const Quat &q)
+{		
+	base = q.base;
+	return *this;
+}
+
+inline const Quat& Quat::operator=(const Maths::IdentityTag&)
+{
+	base = Vector4Base::Consts::k0001;
+	return *this;
+}
+
+inline void Quat::operator+=(const Quat& b)
+{
+	base = _mm_add_ps(base, b.base);
+}
+
+inline void Quat::operator-=(const Quat& b)
+{
+	base = _mm_sub_ps(base, b.base);
+}
+
+inline void Quat::operator*=(const Quat& b)
+{
+	*this = (*this * b);
+}
+
+inline void Quat::operator*=(const Scalar& s)
+{
+	base = _mm_mul_ps(base, _mm_shuffle_ps(s.base, s.base, _MM_SHUFFLE(0, 0, 0, 0)));
+}
+
+inline const Quat operator-(const Quat& a)
+{
+	return Quat(_mm_sub_ps(_mm_setzero_ps(), a.base));
+}
+
+inline const Quat operator+(const Quat& a, const Quat& b)
+{
+	return Quat(_mm_add_ps(a.base, b.base));
+}
+
+inline const Quat operator-(const Quat& a, const Quat& b)
+{
+	return Quat(_mm_sub_ps(a.base, b.base));
+}
+
+inline const Quat operator*(const Quat& a, const Quat& b)
+{
+	// TODO: not happy with this
+	Vector3 va(a.base);
+	Vector3 vb(b.base);
+	Scalar wa(va.GetW());
+	Scalar wb(vb.GetW());
+	return Quat(Vector4(Cross(va, vb) + (va * wb) + (vb * wa), (wa * wb) - Dot(va, vb)));
+}
+
+inline const Quat operator*(const Quat& a, const Scalar& s)
+{
+	return Quat(_mm_mul_ps(a.base, _mm_shuffle_ps(s.base, s.base, _MM_SHUFFLE(0, 0, 0, 0))));
+}
+
+inline const Quat operator*(const Scalar& s, const Quat& a)
+{
+	return Quat(_mm_mul_ps(a.base, _mm_shuffle_ps(s.base, s.base, _MM_SHUFFLE(0, 0, 0, 0))));
+}
+
+inline const Quat Inv(const Quat& a)
+{
+	return Quat(_mm_mul_ps(a.base, Vector4Base::Consts::kNeg111_1));
+}
+
+inline const Scalar Dot(const Quat& a, const Quat& b)
+{
+	return Scalar(Vector4Base::Dot4(a.base, b.base));
+}
+
+inline const Scalar Length(const Quat& a)
+{
+	return RsqrtNr(Dot(a, a));
+}
+
+inline const Quat Normalize(const Quat& a)
+{
+	return a * RsqrtNr(Dot(a, a)); 
+}
+
+inline const Scalar LengthFast(const Quat& a)
+{
+	return Rsqrt(Dot(a, a));
+}
+
+inline const Quat NormalizeFast(const Quat& a)
+{
+	return a * Rsqrt(Dot(a, a)); 
+}
+
+inline const Quat Lerp(const Quat& a, const Quat& b, const Scalar& t)
+{
+	Quat e;
+
+	// go the shortest route
+	if (IsNegative(Dot(a, b)))
+		e = -b;
+	else
+		e = b;
+
+	return Normalize(a + (e - a) * t);
+}
+
+inline const Vector3 Quat::Rotate(const Vector3& v) const
+{
+	return Vector3(*this * Quat(v) * Inv(*this));
+}
+
+inline void Quat::GetAngleAxis(Vector3& axis, Scalar& angle) const
+{
+	float cosa = GetW();
+
+	angle = acosf(cosa);
+	angle += angle;		// * 2;
+
+	float sina = sqrtf(1.0f - cosa * cosa);
+
+	//    if ( fabs( sina ) < 0.0005 ) sina = 1;
+
+	axis = RcpNr(sina) * Vector3(*this);
+}

LinearMath/Scalar.cpp

@@ -0,0 +1,38 @@
+// Bullet Continuous Collision Detection and Physics Library
+// Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+//
+// Scalar.cpp
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+#ifdef WIN32
+#if _MSC_VER >= 1310
+
+#include "Scalar.h"
+
+const Scalar Scalar::Consts::MinusOne(-1.0f);
+const Scalar Scalar::Consts::Zero(0.0f);
+const Scalar Scalar::Consts::Half(0.5f);
+const Scalar Scalar::Consts::One(1.0f);
+const Scalar Scalar::Consts::Three(3.0f);
+
+const Scalar Scalar::Consts::MinValue(-3.402823466e+38f);
+const Scalar Scalar::Consts::MaxValue(3.402823466e+38f);
+const Scalar Scalar::Consts::Epsilon(1.192092896e-07f);
+
+const Scalar Scalar::Consts::PosInfinity(0x7f800000, true);
+const Scalar Scalar::Consts::NegInfinity(0xff800000, true);
+
+const Scalar Scalar::Consts::AbsMask(0x7fffffff, true);
+
+#endif
+#endif //WIN32

LinearMath/Scalar.h

@@ -0,0 +1,106 @@
+// Bullet Continuous Collision Detection and Physics Library
+// Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+//
+// Scalar.h
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+#ifndef BULLET_SCALAR_H
+#define BULLET_SCALAR_H
+
+
+#include <xmmintrin.h>
+#include "Memory2.h"
+
+// resolved overload found with Koenig lookup
+#pragma warning (disable : 4675)
+
+
+__declspec(align(16)) class Scalar
+{
+public:
+	BULLET_ALIGNED_NEW_AND_DELETE
+
+	__m128 base;
+
+	// constants
+	struct Consts{
+		static const Scalar
+			MinusOne, 
+			Zero,
+			Half,
+			One,
+			Three,
+			MinValue,
+			MaxValue,
+			Epsilon,
+			NegInfinity,
+			PosInfinity,
+			AbsMask;
+	};
+
+	// constructors
+	Scalar();
+	Scalar(float f);
+	Scalar(int i, bool forceNoConvert);
+
+	// explicit constructors
+	explicit Scalar(__m128 s);
+	explicit Scalar(int i);
+
+	// assignment
+	const Scalar& operator=(const Scalar& a);
+
+	// conversion
+	operator const float() const;
+	operator const float();
+
+	// in place operations
+	void operator+=(const Scalar& b);
+	void operator-=(const Scalar& b);
+	void operator*=(const Scalar& b);
+	void operator/=(const Scalar& b);
+
+	// operations
+	friend const Scalar operator-(const Scalar& a);
+
+	friend const Scalar operator+(const Scalar& a, const Scalar& b);
+	friend const Scalar operator-(const Scalar& a, const Scalar& b);
+	friend const Scalar operator*(const Scalar& a, const Scalar& b);
+	friend const Scalar operator/(const Scalar& a, const Scalar& b);
+
+	friend const Scalar Abs(const Scalar& a);
+	friend const Scalar Rcp(const Scalar& a);
+	friend const Scalar Rsqrt(const Scalar& a);
+	friend const Scalar Sqrt(const Scalar& a);
+	friend const Scalar RcpNr(const Scalar& a);
+	friend const Scalar RsqrtNr(const Scalar& a);
+
+	friend const Scalar Min(const Scalar& a, const Scalar& b);
+	friend const Scalar Max(const Scalar& a, const Scalar& b);
+	friend const Scalar Clamp(const Scalar& a, const Scalar& min, const Scalar& max);
+
+	friend const Scalar Lerp(const Scalar& a, const Scalar& b, const Scalar& t);
+
+	// comparison
+	friend const int IsNegative(const Scalar& a);
+
+	friend const int IsNan(const Scalar& a);
+	friend const int IsInfinity(const Scalar& a);
+	friend const int IsPosInfinity(const Scalar& a);
+	friend const int IsNegInfinity(const Scalar& a);
+};
+
+
+#include "Scalar.inl"
+
+#endif //BULLET_SCALAR_H

LinearMath/Scalar.inl

@@ -0,0 +1,196 @@
+// Bullet Continuous Collision Detection and Physics Library
+// Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+//
+//
+// Scalar.inl
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+#pragma once
+
+
+inline Scalar::Scalar()
+{
+}
+
+inline Scalar::Scalar(float f)
+{
+	base = _mm_set1_ps(f);
+}
+
+
+inline Scalar::Scalar(int i, bool forceNoConvert)
+{
+	__declspec(align(16)) int iv[4] = {i, i, i, i};
+	*(Scalar*)&base = *(Scalar*)&iv;
+}
+
+inline Scalar::Scalar(__m128 s)
+{
+	base = s;
+}
+
+inline Scalar::Scalar(int i)
+{
+	base = _mm_cvtsi32_ss(base, i);
+	base = _mm_shuffle_ps(base, base, _MM_SHUFFLE(0, 0, 0, 0));
+}
+
+inline const Scalar& Scalar::operator=(const Scalar &a)
+{
+	base = a.base;
+	return *this;
+}
+
+inline Scalar::operator const float() const
+{
+	float f;
+	_mm_store_ss(&f, base);
+	return f;
+}
+
+inline Scalar::operator const float()
+{
+	float f;
+	_mm_store_ss(&f, base);
+	return f;
+}
+
+inline void Scalar::operator+=(const Scalar& b)
+{
+	base = _mm_add_ps(base, b.base);
+}
+
+inline void Scalar::operator-=(const Scalar& b)
+{
+	base = _mm_sub_ps(base, b.base);
+}
+
+inline void Scalar::operator*=(const Scalar& b)
+{
+	base = _mm_mul_ps(base, b.base);
+}
+
+inline void Scalar::operator/=(const Scalar& b)
+{
+	base = _mm_div_ps(base, b.base);
+}
+
+inline const Scalar operator-(const Scalar& a)
+{
+	return Scalar(_mm_sub_ps(_mm_setzero_ps(), a.base));
+}
+
+inline const Scalar Abs(const Scalar& a)
+{
+	return Scalar(_mm_and_ps(a.base, Scalar::Consts::AbsMask.base));
+}
+
+inline const Scalar Rcp(const Scalar& a)
+{
+	return Scalar(_mm_rcp_ps(a.base));
+}
+
+inline const Scalar Rsqrt(const Scalar& a)
+{
+	return Scalar(_mm_rsqrt_ps(a.base));
+}
+
+inline const Scalar Sqrt(const Scalar& a)
+{
+	return Scalar(_mm_sqrt_ps(a.base));
+}
+
+// Newton Raphson Reciprocal
+// (2 * Rcp(x)) - (x * Rcp(x) * Rcp(x))]
+inline const Scalar RcpNr(const Scalar& a)
+{
+	Scalar rcp = Rcp(a);
+	return (rcp + rcp) - (a * rcp * rcp);
+}
+
+// Newton Raphson Reciprocal Square Root
+// 0.5 * Rsqrt * (3 - x * Rsqrt(x) * Rsqrt(x))
+inline const Scalar RsqrtNr(const Scalar& a)
+{
+	Scalar rcp = Rsqrt(a);
+	return (Scalar::Consts::Half * rcp) * (Scalar::Consts::Three - (a * rcp) * rcp);
+}
+
+// binary
+inline const Scalar operator+(const Scalar& a, const Scalar& b)
+{
+	return Scalar(_mm_add_ps(a.base, b.base));
+}
+
+inline const Scalar operator-(const Scalar& a, const Scalar& b)
+{
+	return Scalar(_mm_sub_ps(a.base, b.base));
+}
+
+inline const Scalar operator*(const Scalar& a, const Scalar& b)
+{
+	return Scalar(_mm_mul_ps(a.base, b.base));
+}
+
+inline const Scalar operator/(const Scalar& a, const Scalar& b)
+{
+	return Scalar(_mm_div_ps(a.base, b.base));
+}
+
+inline const Scalar Min(const Scalar& a, const Scalar& b)
+{
+	return Scalar(_mm_min_ps(a.base, b.base));
+}
+
+inline const Scalar Max(const Scalar& a, const Scalar& b)
+{
+	return Scalar(_mm_max_ps(a.base, b.base));
+}
+
+inline const Scalar Clamp(const Scalar& a, const Scalar& min, const Scalar& max)
+{
+	return Scalar(_mm_min_ps(max.base, _mm_max_ps(min.base, a.base)));
+}
+
+inline const Scalar Lerp(const Scalar& a, const Scalar& b, const Scalar& t)
+{
+	return Scalar(a + (b - a) * t);
+}
+
+inline const int IsNegative(const Scalar& a)
+{
+	return _mm_movemask_ps(a.base) & 1;
+}
+
+// warning. this only checks for quiet nan
+inline const int IsNan(const Scalar& a)
+{
+	int aInt = *(int*)&a;
+	return ((aInt & 0x7fc00000) == 0x7fc00000);
+}
+
+inline const int IsInfinity(const Scalar& a)
+{
+	return (a == Scalar::Consts::PosInfinity || a == Scalar::Consts::NegInfinity);
+}
+
+inline const int IsPosInfinity(const Scalar& a)
+{
+	return (a == Scalar::Consts::PosInfinity);
+}
+
+inline const int IsNegInfinity(const Scalar& a)
+{
+	return (a == Scalar::Consts::NegInfinity);
+}
+

LinearMath/SimdMatrix3x3.h

@@ -0,0 +1,395 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef SimdMatrix3x3_H
+#define SimdMatrix3x3_H
+
+#include "SimdScalar.h"
+
+#include "SimdVector3.h"
+#include "SimdQuaternion.h"
+
+
+class SimdMatrix3x3 {
+	public:
+		SimdMatrix3x3 () {}
+		
+//		explicit SimdMatrix3x3(const SimdScalar *m) { setFromOpenGLSubMatrix(m); }
+		
+		explicit SimdMatrix3x3(const SimdQuaternion& q) { setRotation(q); }
+		/*
+		template <typename SimdScalar>
+		Matrix3x3(const SimdScalar& yaw, const SimdScalar& pitch, const SimdScalar& roll)
+		{ 
+			setEulerYPR(yaw, pitch, roll);
+		}
+		*/
+		SimdMatrix3x3(const SimdScalar& xx, const SimdScalar& xy, const SimdScalar& xz,
+				  const SimdScalar& yx, const SimdScalar& yy, const SimdScalar& yz,
+				  const SimdScalar& zx, const SimdScalar& zy, const SimdScalar& zz)
+		{ 
+			setValue(xx, xy, xz, 
+					 yx, yy, yz, 
+					 zx, zy, zz);
+		}
+		
+		SimdVector3 getColumn(int i) const
+		{
+			return SimdVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
+		}
+
+		const SimdVector3& getRow(int i) const
+		{
+			return m_el[i];
+		}
+
+
+		SIMD_FORCE_INLINE SimdVector3&  operator[](int i)
+		{ 
+			assert(0 <= i && i < 3);
+			return m_el[i]; 
+		}
+		
+		const SimdVector3& operator[](int i) const
+		{
+			assert(0 <= i && i < 3);
+			return m_el[i]; 
+		}
+		
+		SimdMatrix3x3& operator*=(const SimdMatrix3x3& m); 
+		
+	
+	void setFromOpenGLSubMatrix(const SimdScalar *m)
+		{
+			m_el[0][0] = (m[0]); 
+			m_el[1][0] = (m[1]); 
+			m_el[2][0] = (m[2]);
+			m_el[0][1] = (m[4]); 
+			m_el[1][1] = (m[5]); 
+			m_el[2][1] = (m[6]);
+			m_el[0][2] = (m[8]); 
+			m_el[1][2] = (m[9]); 
+			m_el[2][2] = (m[10]);
+		}
+
+		void setValue(const SimdScalar& xx, const SimdScalar& xy, const SimdScalar& xz, 
+					  const SimdScalar& yx, const SimdScalar& yy, const SimdScalar& yz, 
+					  const SimdScalar& zx, const SimdScalar& zy, const SimdScalar& zz)
+		{
+			m_el[0][0] = SimdScalar(xx); 
+			m_el[0][1] = SimdScalar(xy); 
+			m_el[0][2] = SimdScalar(xz);
+			m_el[1][0] = SimdScalar(yx); 
+			m_el[1][1] = SimdScalar(yy); 
+			m_el[1][2] = SimdScalar(yz);
+			m_el[2][0] = SimdScalar(zx); 
+			m_el[2][1] = SimdScalar(zy); 
+			m_el[2][2] = SimdScalar(zz);
+		}
+  
+		void setRotation(const SimdQuaternion& q) 
+		{
+			SimdScalar d = q.length2();
+			assert(d != SimdScalar(0.0));
+			SimdScalar s = SimdScalar(2.0) / d;
+			SimdScalar xs = q[0] * s,   ys = q[1] * s,   zs = q[2] * s;
+			SimdScalar wx = q[3] * xs,  wy = q[3] * ys,  wz = q[3] * zs;
+			SimdScalar xx = q[0] * xs,  xy = q[0] * ys,  xz = q[0] * zs;
+			SimdScalar yy = q[1] * ys,  yz = q[1] * zs,  zz = q[2] * zs;
+			setValue(SimdScalar(1.0) - (yy + zz), xy - wz, xz + wy,
+					 xy + wz, SimdScalar(1.0) - (xx + zz), yz - wx,
+					 xz - wy, yz + wx, SimdScalar(1.0) - (xx + yy));
+		}
+		
+
+
+		void setEulerYPR(const SimdScalar& yaw, const SimdScalar& pitch, const SimdScalar& roll) 
+		{
+
+			SimdScalar cy(SimdCos(yaw)); 
+			SimdScalar  sy(SimdSin(yaw)); 
+			SimdScalar  cp(SimdCos(pitch)); 
+			SimdScalar  sp(SimdSin(pitch)); 
+			SimdScalar  cr(SimdCos(roll));
+			SimdScalar  sr(SimdSin(roll));
+			SimdScalar  cc = cy * cr; 
+			SimdScalar  cs = cy * sr; 
+			SimdScalar  sc = sy * cr; 
+			SimdScalar  ss = sy * sr;
+			setValue(cc - sp * ss, -cs - sp * sc, -sy * cp,
+                     cp * sr,       cp * cr,      -sp,
+					 sc + sp * cs, -ss + sp * cc,  cy * cp);
+		
+		}
+
+	/**
+	 * setEulerZYX
+	 * @param euler a const reference to a SimdVector3 of euler angles
+	 * These angles are used to produce a rotation matrix. The euler
+	 * angles are applied in ZYX order. I.e a vector is first rotated 
+	 * about X then Y and then Z
+	 **/
+	
+	void setEulerZYX(SimdScalar eulerX,SimdScalar eulerY,SimdScalar eulerZ) {
+		SimdScalar ci ( SimdCos(eulerX)); 
+		SimdScalar cj ( SimdCos(eulerY)); 
+		SimdScalar ch ( SimdCos(eulerZ)); 
+		SimdScalar si ( SimdSin(eulerX)); 
+		SimdScalar sj ( SimdSin(eulerY)); 
+		SimdScalar sh ( SimdSin(eulerZ)); 
+		SimdScalar cc = ci * ch; 
+		SimdScalar cs = ci * sh; 
+		SimdScalar sc = si * ch; 
+		SimdScalar ss = si * sh;
+		
+		setValue(cj * ch, sj * sc - cs, sj * cc + ss,
+				 cj * sh, sj * ss + cc, sj * cs - sc, 
+	       			 -sj,      cj * si,      cj * ci);
+	}
+
+		void setIdentity()
+		{ 
+			setValue(SimdScalar(1.0), SimdScalar(0.0), SimdScalar(0.0), 
+					 SimdScalar(0.0), SimdScalar(1.0), SimdScalar(0.0), 
+					 SimdScalar(0.0), SimdScalar(0.0), SimdScalar(1.0)); 
+		}
+    
+		void getOpenGLSubMatrix(SimdScalar *m) const 
+		{
+			m[0]  = SimdScalar(m_el[0][0]); 
+			m[1]  = SimdScalar(m_el[1][0]);
+			m[2]  = SimdScalar(m_el[2][0]);
+			m[3]  = SimdScalar(0.0); 
+			m[4]  = SimdScalar(m_el[0][1]);
+			m[5]  = SimdScalar(m_el[1][1]);
+			m[6]  = SimdScalar(m_el[2][1]);
+			m[7]  = SimdScalar(0.0); 
+			m[8]  = SimdScalar(m_el[0][2]); 
+			m[9]  = SimdScalar(m_el[1][2]);
+			m[10] = SimdScalar(m_el[2][2]);
+			m[11] = SimdScalar(0.0); 
+		}
+
+		void getRotation(SimdQuaternion& q) const
+		{
+			SimdScalar trace = m_el[0][0] + m_el[1][1] + m_el[2][2];
+			
+			if (trace > SimdScalar(0.0)) 
+			{
+				SimdScalar s = SimdSqrt(trace + SimdScalar(1.0));
+				q[3] = s * SimdScalar(0.5);
+				s = SimdScalar(0.5) / s;
+				
+				q[0] = (m_el[2][1] - m_el[1][2]) * s;
+				q[1] = (m_el[0][2] - m_el[2][0]) * s;
+				q[2] = (m_el[1][0] - m_el[0][1]) * s;
+			} 
+			else 
+			{
+				int i = m_el[0][0] < m_el[1][1] ? 
+					(m_el[1][1] < m_el[2][2] ? 2 : 1) :
+					(m_el[0][0] < m_el[2][2] ? 2 : 0); 
+				int j = (i + 1) % 3;  
+				int k = (i + 2) % 3;
+				
+				SimdScalar s = SimdSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + SimdScalar(1.0));
+				q[i] = s * SimdScalar(0.5);
+				s = SimdScalar(0.5) / s;
+				
+				q[3] = (m_el[k][j] - m_el[j][k]) * s;
+				q[j] = (m_el[j][i] + m_el[i][j]) * s;
+				q[k] = (m_el[k][i] + m_el[i][k]) * s;
+			}
+		}
+	
+
+		
+		void getEuler(SimdScalar& yaw, SimdScalar& pitch, SimdScalar& roll) const
+		{
+			pitch = SimdScalar(SimdAsin(-m_el[2][0]));
+			if (pitch < SIMD_2_PI)
+			{
+				if (pitch > SIMD_2_PI)
+				{
+					yaw = SimdScalar(SimdAtan2(m_el[1][0], m_el[0][0]));
+					roll = SimdScalar(SimdAtan2(m_el[2][1], m_el[2][2]));
+				}
+				else 
+				{
+					yaw = SimdScalar(-SimdAtan2(-m_el[0][1], m_el[0][2]));
+					roll = SimdScalar(0.0);
+				}
+			}
+			else
+			{
+				yaw = SimdScalar(SimdAtan2(-m_el[0][1], m_el[0][2]));
+				roll = SimdScalar(0.0);
+			}
+		}
+
+		SimdVector3 getScaling() const
+		{
+			return SimdVector3(m_el[0][0] * m_el[0][0] + m_el[1][0] * m_el[1][0] + m_el[2][0] * m_el[2][0],
+								   m_el[0][1] * m_el[0][1] + m_el[1][1] * m_el[1][1] + m_el[2][1] * m_el[2][1],
+								   m_el[0][2] * m_el[0][2] + m_el[1][2] * m_el[1][2] + m_el[2][2] * m_el[2][2]);
+		}
+		
+		
+		SimdMatrix3x3 scaled(const SimdVector3& s) const
+		{
+			return SimdMatrix3x3(m_el[0][0] * s[0], m_el[0][1] * s[1], m_el[0][2] * s[2],
+									 m_el[1][0] * s[0], m_el[1][1] * s[1], m_el[1][2] * s[2],
+									 m_el[2][0] * s[0], m_el[2][1] * s[1], m_el[2][2] * s[2]);
+		}
+
+		SimdScalar            determinant() const;
+		SimdMatrix3x3 adjoint() const;
+		SimdMatrix3x3 absolute() const;
+		SimdMatrix3x3 transpose() const;
+		SimdMatrix3x3 inverse() const; 
+		
+		SimdMatrix3x3 transposeTimes(const SimdMatrix3x3& m) const;
+		SimdMatrix3x3 timesTranspose(const SimdMatrix3x3& m) const;
+		
+		SimdScalar tdot(int c, const SimdVector3& v) const 
+		{
+			return m_el[0][c] * v[0] + m_el[1][c] * v[1] + m_el[2][c] * v[2];
+		}
+		
+	protected:
+		SimdScalar cofac(int r1, int c1, int r2, int c2) const 
+		{
+			return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
+		}
+
+		SimdVector3 m_el[3];
+	};
+	
+	SIMD_FORCE_INLINE SimdMatrix3x3& 
+	SimdMatrix3x3::operator*=(const SimdMatrix3x3& m)
+	{
+		setValue(m.tdot(0, m_el[0]), m.tdot(1, m_el[0]), m.tdot(2, m_el[0]),
+				 m.tdot(0, m_el[1]), m.tdot(1, m_el[1]), m.tdot(2, m_el[1]),
+				 m.tdot(0, m_el[2]), m.tdot(1, m_el[2]), m.tdot(2, m_el[2]));
+		return *this;
+	}
+	
+	SIMD_FORCE_INLINE SimdScalar 
+	SimdMatrix3x3::determinant() const
+	{ 
+		return triple((*this)[0], (*this)[1], (*this)[2]);
+	}
+	
+
+	SIMD_FORCE_INLINE SimdMatrix3x3 
+	SimdMatrix3x3::absolute() const
+	{
+		return SimdMatrix3x3(
+			SimdFabs(m_el[0][0]), SimdFabs(m_el[0][1]), SimdFabs(m_el[0][2]),
+			SimdFabs(m_el[1][0]), SimdFabs(m_el[1][1]), SimdFabs(m_el[1][2]),
+			SimdFabs(m_el[2][0]), SimdFabs(m_el[2][1]), SimdFabs(m_el[2][2]));
+	}
+
+	SIMD_FORCE_INLINE SimdMatrix3x3 
+	SimdMatrix3x3::transpose() const 
+	{
+		return SimdMatrix3x3(m_el[0][0], m_el[1][0], m_el[2][0],
+								 m_el[0][1], m_el[1][1], m_el[2][1],
+								 m_el[0][2], m_el[1][2], m_el[2][2]);
+	}
+	
+	SIMD_FORCE_INLINE SimdMatrix3x3 
+	SimdMatrix3x3::adjoint() const 
+	{
+		return SimdMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
+								 cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
+								 cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
+	}
+	
+	SIMD_FORCE_INLINE SimdMatrix3x3 
+	SimdMatrix3x3::inverse() const
+	{
+		SimdVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
+		SimdScalar det = (*this)[0].dot(co);
+		assert(det != SimdScalar(0.0f));
+		SimdScalar s = SimdScalar(1.0f) / det;
+		return SimdMatrix3x3(co[0] * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
+								 co[1] * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
+								 co[2] * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
+	}
+	
+	SIMD_FORCE_INLINE SimdMatrix3x3 
+	SimdMatrix3x3::transposeTimes(const SimdMatrix3x3& m) const
+	{
+		return SimdMatrix3x3(
+			m_el[0][0] * m[0][0] + m_el[1][0] * m[1][0] + m_el[2][0] * m[2][0],
+			m_el[0][0] * m[0][1] + m_el[1][0] * m[1][1] + m_el[2][0] * m[2][1],
+			m_el[0][0] * m[0][2] + m_el[1][0] * m[1][2] + m_el[2][0] * m[2][2],
+			m_el[0][1] * m[0][0] + m_el[1][1] * m[1][0] + m_el[2][1] * m[2][0],
+			m_el[0][1] * m[0][1] + m_el[1][1] * m[1][1] + m_el[2][1] * m[2][1],
+			m_el[0][1] * m[0][2] + m_el[1][1] * m[1][2] + m_el[2][1] * m[2][2],
+			m_el[0][2] * m[0][0] + m_el[1][2] * m[1][0] + m_el[2][2] * m[2][0],
+			m_el[0][2] * m[0][1] + m_el[1][2] * m[1][1] + m_el[2][2] * m[2][1],
+			m_el[0][2] * m[0][2] + m_el[1][2] * m[1][2] + m_el[2][2] * m[2][2]);
+	}
+	
+	SIMD_FORCE_INLINE SimdMatrix3x3 
+	SimdMatrix3x3::timesTranspose(const SimdMatrix3x3& m) const
+	{
+		return SimdMatrix3x3(
+			m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
+			m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
+			m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
+		
+	}
+
+	SIMD_FORCE_INLINE SimdVector3 
+	operator*(const SimdMatrix3x3& m, const SimdVector3& v) 
+	{
+		return SimdVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
+	}
+	
+
+	SIMD_FORCE_INLINE SimdVector3
+	operator*(const SimdVector3& v, const SimdMatrix3x3& m)
+	{
+		return SimdVector3(m.tdot(0, v), m.tdot(1, v), m.tdot(2, v));
+	}
+
+	SIMD_FORCE_INLINE SimdMatrix3x3 
+	operator*(const SimdMatrix3x3& m1, const SimdMatrix3x3& m2)
+	{
+		return SimdMatrix3x3(
+			m2.tdot(0, m1[0]), m2.tdot(1, m1[0]), m2.tdot(2, m1[0]),
+			m2.tdot(0, m1[1]), m2.tdot(1, m1[1]), m2.tdot(2, m1[1]),
+			m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2]));
+	}
+
+
+	SIMD_FORCE_INLINE SimdMatrix3x3 SimdMultTransposeLeft(const SimdMatrix3x3& m1, const SimdMatrix3x3& m2) {
+    return SimdMatrix3x3(
+        m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
+        m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
+        m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
+        m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
+        m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
+        m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
+        m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
+        m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
+        m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
+}
+
+
+#endif

LinearMath/SimdMinMax.h

@@ -0,0 +1,40 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef SIMD_MINMAX_H
+#define SIMD_MINMAX_H
+
+template <class T>
+SIMD_FORCE_INLINE const T& SimdMin(const T& a, const T& b) {
+  return b < a ? b : a;
+}
+
+template <class T>
+SIMD_FORCE_INLINE const T& SimdMax(const T& a, const T& b) {
+  return  a < b ? b : a;
+}
+
+template <class T>
+SIMD_FORCE_INLINE void SimdSetMin(T& a, const T& b) {
+    if (a > b) a = b;
+}
+
+template <class T>
+SIMD_FORCE_INLINE void SimdSetMax(T& a, const T& b) {
+    if (a < b) a = b;
+}
+
+#endif

LinearMath/SimdPoint3.h

@@ -0,0 +1,24 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef SimdPoint3_H
+#define SimdPoint3_H
+
+#include "SimdVector3.h"
+
+typedef SimdVector3 SimdPoint3;
+
+#endif

LinearMath/SimdQuadWord.h

@@ -0,0 +1,134 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef SIMD_QUADWORD_H
+#define SIMD_QUADWORD_H
+
+#include "SimdScalar.h"
+
+
+
+
+
+class	SimdQuadWord
+{
+	protected:
+		ATTRIBUTE_ALIGNED16	(SimdScalar	m_x);
+		SimdScalar	m_y;
+		SimdScalar	m_z;
+		SimdScalar	m_unusedW;
+
+	public:
+	
+		SIMD_FORCE_INLINE SimdScalar&       operator[](int i)       { return (&m_x)[i];	}      
+		SIMD_FORCE_INLINE const SimdScalar& operator[](int i) const { return (&m_x)[i]; }
+
+		SIMD_FORCE_INLINE const SimdScalar& getX() const { return m_x; }
+
+		SIMD_FORCE_INLINE const SimdScalar& getY() const { return m_y; }
+
+		SIMD_FORCE_INLINE const SimdScalar& getZ() const { return m_z; }
+
+		SIMD_FORCE_INLINE void	setX(float x) { m_x = x;};
+
+		SIMD_FORCE_INLINE void	setY(float y) { m_y = y;};
+
+		SIMD_FORCE_INLINE void	setZ(float z) { m_z = z;};
+
+		SIMD_FORCE_INLINE const SimdScalar& x() const { return m_x; }
+
+		SIMD_FORCE_INLINE const SimdScalar& y() const { return m_y; }
+
+		SIMD_FORCE_INLINE const SimdScalar& z() const { return m_z; }
+
+
+		operator       SimdScalar *()       { return &m_x; }
+		operator const SimdScalar *() const { return &m_x; }
+
+		SIMD_FORCE_INLINE void 	setValue(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z)
+		{
+			m_x=x;
+			m_y=y;
+			m_z=z;
+		}
+
+/*		void getValue(SimdScalar *m) const 
+		{
+			m[0] = m_x;
+			m[1] = m_y;
+			m[2] = m_z;
+		}
+*/
+		SIMD_FORCE_INLINE void	setValue(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z,const SimdScalar& w)
+		{
+			m_x=x;
+			m_y=y;
+			m_z=z;
+			m_unusedW=w;
+		}
+
+		SIMD_FORCE_INLINE SimdQuadWord() :
+		m_x(0.f),m_y(0.f),m_z(0.f),m_unusedW(0.f)
+		{
+		}
+
+		SIMD_FORCE_INLINE SimdQuadWord(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z) 
+		:m_x(x),m_y(y),m_z(z)
+		//todo, remove this in release/simd ?
+		,m_unusedW(0.f)
+		{
+		}
+
+		SIMD_FORCE_INLINE SimdQuadWord(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z,const SimdScalar& w) 
+			:m_x(x),m_y(y),m_z(z),m_unusedW(w)
+		{
+		}
+
+
+		SIMD_FORCE_INLINE void	setMax(const SimdQuadWord& other)
+		{
+			if (other.m_x > m_x)
+				m_x = other.m_x;
+
+			if (other.m_y > m_y)
+				m_y = other.m_y;
+
+			if (other.m_z > m_z)
+				m_z = other.m_z;
+
+			if (other.m_unusedW > m_unusedW)
+				m_unusedW = other.m_unusedW;
+		}
+
+		SIMD_FORCE_INLINE void	setMin(const SimdQuadWord& other)
+		{
+			if (other.m_x < m_x)
+				m_x = other.m_x;
+
+			if (other.m_y < m_y)
+				m_y = other.m_y;
+
+			if (other.m_z < m_z)
+				m_z = other.m_z;
+
+			if (other.m_unusedW < m_unusedW)
+				m_unusedW = other.m_unusedW;
+		}
+
+
+
+};
+
+#endif //SIMD_QUADWORD_H

LinearMath/SimdQuaternion.h

@@ -0,0 +1,290 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef SIMD__QUATERNION_H_
+#define SIMD__QUATERNION_H_
+
+#include "SimdVector3.h"
+
+class SimdQuaternion : public SimdQuadWord {
+public:
+	SimdQuaternion() {}
+
+	//		template <typename SimdScalar>
+	//		explicit Quaternion(const SimdScalar *v) : Tuple4<SimdScalar>(v) {}
+
+	SimdQuaternion(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z, const SimdScalar& w) 
+		: SimdQuadWord(x, y, z, w) 
+	{}
+
+	SimdQuaternion(const SimdVector3& axis, const SimdScalar& angle) 
+	{ 
+		setRotation(axis, angle); 
+	}
+
+	SimdQuaternion(const SimdScalar& yaw, const SimdScalar& pitch, const SimdScalar& roll)
+	{ 
+		setEuler(yaw, pitch, roll); 
+	}
+
+	void setRotation(const SimdVector3& axis, const SimdScalar& angle)
+	{
+		SimdScalar d = axis.length();
+		assert(d != SimdScalar(0.0));
+		SimdScalar s = SimdSin(angle * SimdScalar(0.5)) / d;
+		setValue(axis.x() * s, axis.y() * s, axis.z() * s, 
+			SimdCos(angle * SimdScalar(0.5)));
+	}
+
+	void setEuler(const SimdScalar& yaw, const SimdScalar& pitch, const SimdScalar& roll)
+	{
+		SimdScalar halfYaw = SimdScalar(yaw) * SimdScalar(0.5);  
+		SimdScalar halfPitch = SimdScalar(pitch) * SimdScalar(0.5);  
+		SimdScalar halfRoll = SimdScalar(roll) * SimdScalar(0.5);  
+		SimdScalar cosYaw = SimdCos(halfYaw);
+		SimdScalar sinYaw = SimdSin(halfYaw);
+		SimdScalar cosPitch = SimdCos(halfPitch);
+		SimdScalar sinPitch = SimdSin(halfPitch);
+		SimdScalar cosRoll = SimdCos(halfRoll);
+		SimdScalar sinRoll = SimdSin(halfRoll);
+		setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
+			cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
+			sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
+			cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
+	}
+
+	SimdQuaternion& operator+=(const SimdQuaternion& q)
+	{
+		m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q[3];
+		return *this;
+	}
+
+	SimdQuaternion& operator-=(const SimdQuaternion& q) 
+	{
+		m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q[3];
+		return *this;
+	}
+
+	SimdQuaternion& operator*=(const SimdScalar& s)
+	{
+		m_x *= s; m_y *= s; m_z *= s; m_unusedW *= s;
+		return *this;
+	}
+
+
+	SimdQuaternion& operator*=(const SimdQuaternion& q)
+	{
+		setValue(m_unusedW * q.x() + m_x * q[3] + m_y * q.z() - m_z * q.y(),
+			m_unusedW * q.y() + m_y * q[3] + m_z * q.x() - m_x * q.z(),
+			m_unusedW * q.z() + m_z * q[3] + m_x * q.y() - m_y * q.x(),
+			m_unusedW * q[3] - m_x * q.x() - m_y * q.y() - m_z * q.z());
+		return *this;
+	}
+
+	SimdScalar dot(const SimdQuaternion& q) const
+	{
+		return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q[3];
+	}
+
+	SimdScalar length2() const
+	{
+		return dot(*this);
+	}
+
+	SimdScalar length() const
+	{
+		return SimdSqrt(length2());
+	}
+
+	SimdQuaternion& normalize() 
+	{
+		return *this /= length();
+	}
+
+	SIMD_FORCE_INLINE SimdQuaternion
+	operator*(const SimdScalar& s) const
+	{
+		return SimdQuaternion(x() * s, y() * s, z() * s, m_unusedW * s);
+	}
+
+
+
+	SimdQuaternion operator/(const SimdScalar& s) const
+	{
+		assert(s != SimdScalar(0.0));
+		return *this * (SimdScalar(1.0) / s);
+	}
+
+
+	SimdQuaternion& operator/=(const SimdScalar& s) 
+	{
+		assert(s != SimdScalar(0.0));
+		return *this *= SimdScalar(1.0) / s;
+	}
+
+
+	SimdQuaternion normalized() const 
+	{
+		return *this / length();
+	} 
+
+	SimdScalar angle(const SimdQuaternion& q) const 
+	{
+		SimdScalar s = SimdSqrt(length2() * q.length2());
+		assert(s != SimdScalar(0.0));
+		return SimdAcos(dot(q) / s);
+	}
+
+	SimdScalar getAngle() const 
+	{
+		SimdScalar s = 2.f * SimdAcos(m_unusedW);
+		return s;
+	}
+
+
+
+	SimdQuaternion inverse() const
+	{
+		return SimdQuaternion(m_x, m_y, m_z, -m_unusedW);
+	}
+
+	SIMD_FORCE_INLINE SimdQuaternion
+	operator+(const SimdQuaternion& q2) const
+	{
+		const SimdQuaternion& q1 = *this;
+		return SimdQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1[3] + q2[3]);
+	}
+
+	SIMD_FORCE_INLINE SimdQuaternion
+	operator-(const SimdQuaternion& q2) const
+	{
+		const SimdQuaternion& q1 = *this;
+		return SimdQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1[3] - q2[3]);
+	}
+
+	SIMD_FORCE_INLINE SimdQuaternion operator-() const
+	{
+		const SimdQuaternion& q2 = *this;
+		return SimdQuaternion( - q2.x(), - q2.y(),  - q2.z(),  - q2[3]);
+	}
+
+	SIMD_FORCE_INLINE SimdQuaternion farthest( const SimdQuaternion& qd) const 
+	{
+		SimdQuaternion diff,sum;
+		diff = *this - qd;
+		sum = *this + qd;
+		if( diff.dot(diff) > sum.dot(sum) )
+			return qd;
+		return (-qd);
+	}
+
+	SimdQuaternion slerp(const SimdQuaternion& q, const SimdScalar& t) const
+	{
+		SimdScalar theta = angle(q);
+		if (theta != SimdScalar(0.0))
+		{
+			SimdScalar d = SimdScalar(1.0) / SimdSin(theta);
+			SimdScalar s0 = SimdSin((SimdScalar(1.0) - t) * theta);
+			SimdScalar s1 = SimdSin(t * theta);   
+			return SimdQuaternion((m_x * s0 + q.x() * s1) * d,
+				(m_y * s0 + q.y() * s1) * d,
+				(m_z * s0 + q.z() * s1) * d,
+				(m_unusedW * s0 + q[3] * s1) * d);
+		}
+		else
+		{
+			return *this;
+		}
+	}
+
+	
+
+};
+
+
+
+SIMD_FORCE_INLINE SimdQuaternion
+operator-(const SimdQuaternion& q)
+{
+	return SimdQuaternion(-q.x(), -q.y(), -q.z(), -q[3]);
+}
+
+
+
+
+SIMD_FORCE_INLINE SimdQuaternion
+operator*(const SimdQuaternion& q1, const SimdQuaternion& q2) {
+	return SimdQuaternion(q1[3] * q2.x() + q1.x() * q2[3] + q1.y() * q2.z() - q1.z() * q2.y(),
+		q1[3] * q2.y() + q1.y() * q2[3] + q1.z() * q2.x() - q1.x() * q2.z(),
+		q1[3] * q2.z() + q1.z() * q2[3] + q1.x() * q2.y() - q1.y() * q2.x(),
+		q1[3] * q2[3] - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); 
+}
+
+SIMD_FORCE_INLINE SimdQuaternion
+operator*(const SimdQuaternion& q, const SimdVector3& w)
+{
+	return SimdQuaternion( q[3] * w.x() + q.y() * w.z() - q.z() * w.y(),
+		q[3] * w.y() + q.z() * w.x() - q.x() * w.z(),
+		q[3] * w.z() + q.x() * w.y() - q.y() * w.x(),
+		-q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); 
+}
+
+SIMD_FORCE_INLINE SimdQuaternion
+operator*(const SimdVector3& w, const SimdQuaternion& q)
+{
+	return SimdQuaternion( w.x() * q[3] + w.y() * q.z() - w.z() * q.y(),
+		w.y() * q[3] + w.z() * q.x() - w.x() * q.z(),
+		w.z() * q[3] + w.x() * q.y() - w.y() * q.x(),
+		-w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); 
+}
+
+SIMD_FORCE_INLINE SimdScalar 
+dot(const SimdQuaternion& q1, const SimdQuaternion& q2) 
+{ 
+	return q1.dot(q2); 
+}
+
+
+SIMD_FORCE_INLINE SimdScalar
+length(const SimdQuaternion& q) 
+{ 
+	return q.length(); 
+}
+
+SIMD_FORCE_INLINE SimdScalar
+angle(const SimdQuaternion& q1, const SimdQuaternion& q2) 
+{ 
+	return q1.angle(q2); 
+}
+
+
+SIMD_FORCE_INLINE SimdQuaternion
+inverse(const SimdQuaternion& q) 
+{
+	return q.inverse();
+}
+
+SIMD_FORCE_INLINE SimdQuaternion
+slerp(const SimdQuaternion& q1, const SimdQuaternion& q2, const SimdScalar& t) 
+{
+	return q1.slerp(q2, t);
+}
+
+
+#endif
+
+
+

LinearMath/SimdScalar.h

@@ -0,0 +1,128 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef SIMD___SCALAR_H
+#define SIMD___SCALAR_H
+
+#include <math.h>
+#undef max
+
+
+
+#include <cstdlib>
+#include <cfloat>
+#include <float.h>
+
+#ifdef WIN32
+#pragma warning(disable:4530)
+#pragma warning(disable:4996)
+#ifdef __MINGW32__
+#define SIMD_FORCE_INLINE inline
+#else
+#define SIMD_FORCE_INLINE __forceinline
+#endif //__MINGW32__
+
+//#define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
+#define ATTRIBUTE_ALIGNED16(a) a
+
+#include <assert.h>
+#define ASSERT assert
+#else
+#define SIMD_FORCE_INLINE inline
+#define ATTRIBUTE_ALIGNED16(a) a
+#ifndef assert
+#include <assert.h>
+#endif
+
+
+#define ASSERT assert
+
+#endif
+
+
+
+typedef float    SimdScalar;
+
+#if defined (__sun) || defined (__sun__) || defined (__sparc) || defined (__APPLE__)
+//use double float precision operation on those platforms for Blender
+		
+SIMD_FORCE_INLINE SimdScalar SimdSqrt(SimdScalar x) { return sqrt(x); }
+SIMD_FORCE_INLINE SimdScalar SimdFabs(SimdScalar x) { return fabs(x); }
+SIMD_FORCE_INLINE SimdScalar SimdCos(SimdScalar x) { return cos(x); }
+SIMD_FORCE_INLINE SimdScalar SimdSin(SimdScalar x) { return sin(x); }
+SIMD_FORCE_INLINE SimdScalar SimdTan(SimdScalar x) { return tan(x); }
+SIMD_FORCE_INLINE SimdScalar SimdAcos(SimdScalar x) { return acos(x); }
+SIMD_FORCE_INLINE SimdScalar SimdAsin(SimdScalar x) { return asin(x); }
+SIMD_FORCE_INLINE SimdScalar SimdAtan(SimdScalar x) { return atan(x); }
+SIMD_FORCE_INLINE SimdScalar SimdAtan2(SimdScalar x, SimdScalar y) { return atan2(x, y); }
+SIMD_FORCE_INLINE SimdScalar SimdExp(SimdScalar x) { return exp(x); }
+SIMD_FORCE_INLINE SimdScalar SimdLog(SimdScalar x) { return log(x); }
+SIMD_FORCE_INLINE SimdScalar SimdPow(SimdScalar x,SimdScalar y) { return pow(x,y); }
+
+#else
+		
+SIMD_FORCE_INLINE SimdScalar SimdSqrt(SimdScalar x) { return sqrtf(x); }
+SIMD_FORCE_INLINE SimdScalar SimdFabs(SimdScalar x) { return fabsf(x); }
+SIMD_FORCE_INLINE SimdScalar SimdCos(SimdScalar x) { return cosf(x); }
+SIMD_FORCE_INLINE SimdScalar SimdSin(SimdScalar x) { return sinf(x); }
+SIMD_FORCE_INLINE SimdScalar SimdTan(SimdScalar x) { return tanf(x); }
+SIMD_FORCE_INLINE SimdScalar SimdAcos(SimdScalar x) { return acosf(x); }
+SIMD_FORCE_INLINE SimdScalar SimdAsin(SimdScalar x) { return asinf(x); }
+SIMD_FORCE_INLINE SimdScalar SimdAtan(SimdScalar x) { return atanf(x); }
+SIMD_FORCE_INLINE SimdScalar SimdAtan2(SimdScalar x, SimdScalar y) { return atan2f(x, y); }
+SIMD_FORCE_INLINE SimdScalar SimdExp(SimdScalar x) { return expf(x); }
+SIMD_FORCE_INLINE SimdScalar SimdLog(SimdScalar x) { return logf(x); }
+SIMD_FORCE_INLINE SimdScalar SimdPow(SimdScalar x,SimdScalar y) { return powf(x,y); }
+	
+#endif
+
+
+const SimdScalar  SIMD_2_PI         = 6.283185307179586232f;
+const SimdScalar  SIMD_PI           = SIMD_2_PI * SimdScalar(0.5f);
+const SimdScalar  SIMD_HALF_PI		 = SIMD_2_PI * SimdScalar(0.25f);
+const SimdScalar  SIMD_RADS_PER_DEG = SIMD_2_PI / SimdScalar(360.0f);
+const SimdScalar  SIMD_DEGS_PER_RAD = SimdScalar(360.0f) / SIMD_2_PI;
+const SimdScalar  SIMD_EPSILON      = FLT_EPSILON;
+const SimdScalar  SIMD_INFINITY     = FLT_MAX;
+
+SIMD_FORCE_INLINE bool      SimdFuzzyZero(SimdScalar x) { return SimdFabs(x) < SIMD_EPSILON; }
+
+SIMD_FORCE_INLINE bool	SimdEqual(SimdScalar a, SimdScalar eps) {
+	return (((a) <= eps) && !((a) < -eps));
+}
+SIMD_FORCE_INLINE bool	SimdGreaterEqual (SimdScalar a, SimdScalar eps) {
+	return (!((a) <= eps));
+}
+
+/*SIMD_FORCE_INLINE SimdScalar SimdCos(SimdScalar x) { return cosf(x); }
+SIMD_FORCE_INLINE SimdScalar SimdSin(SimdScalar x) { return sinf(x); }
+SIMD_FORCE_INLINE SimdScalar SimdTan(SimdScalar x) { return tanf(x); }
+SIMD_FORCE_INLINE SimdScalar SimdAcos(SimdScalar x) { return acosf(x); }
+SIMD_FORCE_INLINE SimdScalar SimdAsin(SimdScalar x) { return asinf(x); }
+SIMD_FORCE_INLINE SimdScalar SimdAtan(SimdScalar x) { return atanf(x); }
+SIMD_FORCE_INLINE SimdScalar SimdAtan2(SimdScalar x, SimdScalar y) { return atan2f(x, y); }
+*/
+
+SIMD_FORCE_INLINE int       SimdSign(SimdScalar x) {
+    return x < 0.0f ? -1 : x > 0.0f ? 1 : 0;
+}
+
+SIMD_FORCE_INLINE SimdScalar SimdRadians(SimdScalar x) { return x * SIMD_RADS_PER_DEG; }
+SIMD_FORCE_INLINE SimdScalar SimdDegrees(SimdScalar x) { return x * SIMD_DEGS_PER_RAD; }
+
+
+
+#endif //SIMD___SCALAR_H

LinearMath/SimdTransform.h

@@ -0,0 +1,236 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef SimdTransform_H
+#define SimdTransform_H
+
+#include "SimdVector3.h"
+#include "SimdMatrix3x3.h"
+
+
+
+class SimdTransform {
+	enum { 
+		TRANSLATION = 0x01,
+		ROTATION    = 0x02,
+		RIGID       = TRANSLATION | ROTATION,  
+		SCALING     = 0x04,
+		LINEAR      = ROTATION | SCALING,
+		AFFINE      = TRANSLATION | LINEAR
+	};
+
+public:
+	SimdTransform() {}
+
+	//	template <typename Scalar2>
+	//	explicit Transform(const Scalar2 *m) { setValue(m); }
+
+	explicit SIMD_FORCE_INLINE SimdTransform(const SimdQuaternion& q, 
+		const SimdVector3& c = SimdVector3(SimdScalar(0), SimdScalar(0), SimdScalar(0))) 
+		: m_basis(q),
+		m_origin(c),
+		m_type(RIGID)
+	{}
+
+	explicit SIMD_FORCE_INLINE SimdTransform(const SimdMatrix3x3& b, 
+		const SimdVector3& c = SimdVector3(SimdScalar(0), SimdScalar(0), SimdScalar(0)), 
+		unsigned int type = AFFINE)
+		: m_basis(b),
+		m_origin(c),
+		m_type(type)
+	{}
+
+
+		SIMD_FORCE_INLINE void mult(const SimdTransform& t1, const SimdTransform& t2) {
+			m_basis = t1.m_basis * t2.m_basis;
+			m_origin = t1(t2.m_origin);
+			m_type = t1.m_type | t2.m_type;
+		}
+
+		void multInverseLeft(const SimdTransform& t1, const SimdTransform& t2) {
+			SimdVector3 v = t2.m_origin - t1.m_origin;
+			if (t1.m_type & SCALING) {
+				SimdMatrix3x3 inv = t1.m_basis.inverse();
+				m_basis = inv * t2.m_basis;
+				m_origin = inv * v;
+			}
+			else {
+				m_basis = SimdMultTransposeLeft(t1.m_basis, t2.m_basis);
+				m_origin = v * t1.m_basis;
+			}
+			m_type = t1.m_type | t2.m_type;
+		}
+
+	SIMD_FORCE_INLINE SimdVector3 operator()(const SimdVector3& x) const
+	{
+		return SimdVector3(m_basis[0].dot(x) + m_origin[0], 
+			m_basis[1].dot(x) + m_origin[1], 
+			m_basis[2].dot(x) + m_origin[2]);
+	}
+
+	SIMD_FORCE_INLINE SimdVector3 operator*(const SimdVector3& x) const
+	{
+		return (*this)(x);
+	}
+
+	SIMD_FORCE_INLINE SimdMatrix3x3&       getBasis()          { return m_basis; }
+	SIMD_FORCE_INLINE const SimdMatrix3x3& getBasis()    const { return m_basis; }
+
+	SIMD_FORCE_INLINE SimdVector3&         getOrigin()         { return m_origin; }
+	SIMD_FORCE_INLINE const SimdVector3&   getOrigin()   const { return m_origin; }
+
+	SimdQuaternion getRotation() const { 
+		SimdQuaternion q;
+		m_basis.getRotation(q);
+		return q;
+	}
+	template <typename Scalar2>
+		void setValue(const Scalar2 *m) 
+	{
+		m_basis.setValue(m);
+		m_origin.setValue(&m[12]);
+		m_type = AFFINE;
+	}
+
+	
+	void setFromOpenGLMatrix(const SimdScalar *m)
+	{
+		m_basis.setFromOpenGLSubMatrix(m);
+		m_origin[0] = m[12];
+		m_origin[1] = m[13];
+		m_origin[2] = m[14];
+	}
+
+	void getOpenGLMatrix(SimdScalar *m) const 
+	{
+		m_basis.getOpenGLSubMatrix(m);
+		m[12] = m_origin[0];
+		m[13] = m_origin[1];
+		m[14] = m_origin[2];
+		m[15] = SimdScalar(1.0f);
+	}
+
+	SIMD_FORCE_INLINE void setOrigin(const SimdVector3& origin) 
+	{ 
+		m_origin = origin;
+		m_type |= TRANSLATION;
+	}
+
+	SIMD_FORCE_INLINE SimdVector3 invXform(const SimdVector3& inVec) const;
+
+
+
+	SIMD_FORCE_INLINE void setBasis(const SimdMatrix3x3& basis)
+	{ 
+		m_basis = basis;
+		m_type |= LINEAR;
+	}
+
+	SIMD_FORCE_INLINE void setRotation(const SimdQuaternion& q)
+	{
+		m_basis.setRotation(q);
+		m_type = (m_type & ~LINEAR) | ROTATION;
+	}
+
+	SIMD_FORCE_INLINE void scale(const SimdVector3& scaling)
+	{
+		m_basis = m_basis.scaled(scaling);
+		m_type |= SCALING;
+	}
+
+	void setIdentity()
+	{
+		m_basis.setIdentity();
+		m_origin.setValue(SimdScalar(0.0), SimdScalar(0.0), SimdScalar(0.0));
+		m_type = 0x0;
+	}
+
+	SIMD_FORCE_INLINE bool isIdentity() const { return m_type == 0x0; }
+
+	SimdTransform& operator*=(const SimdTransform& t) 
+	{
+		m_origin += m_basis * t.m_origin;
+		m_basis *= t.m_basis;
+		m_type |= t.m_type; 
+		return *this;
+	}
+
+	SimdTransform inverse() const
+	{ 
+		if (m_type)
+		{
+			SimdMatrix3x3 inv = (m_type & SCALING) ? 
+				m_basis.inverse() : 
+			m_basis.transpose();
+
+			return SimdTransform(inv, inv * -m_origin, m_type);
+		}
+
+		return *this;
+	}
+
+	SimdTransform inverseTimes(const SimdTransform& t) const;  
+
+	SimdTransform operator*(const SimdTransform& t) const;
+
+private:
+
+	SimdMatrix3x3 m_basis;
+	SimdVector3   m_origin;
+	unsigned int      m_type;
+};
+
+
+SIMD_FORCE_INLINE SimdVector3
+SimdTransform::invXform(const SimdVector3& inVec) const
+{
+	SimdVector3 v = inVec - m_origin;
+	return (m_basis.transpose() * v);
+}
+
+SIMD_FORCE_INLINE SimdTransform 
+SimdTransform::inverseTimes(const SimdTransform& t) const  
+{
+	SimdVector3 v = t.getOrigin() - m_origin;
+	if (m_type & SCALING) 
+	{
+		SimdMatrix3x3 inv = m_basis.inverse();
+		return SimdTransform(inv * t.getBasis(), inv * v, 
+			m_type | t.m_type);
+	}
+	else 
+	{
+		return SimdTransform(m_basis.transposeTimes(t.m_basis),
+			v * m_basis, m_type | t.m_type);
+	}
+}
+
+SIMD_FORCE_INLINE SimdTransform 
+SimdTransform::operator*(const SimdTransform& t) const
+{
+	return SimdTransform(m_basis * t.m_basis, 
+		(*this)(t.m_origin), 
+		m_type | t.m_type);
+}	
+
+
+
+#endif
+
+
+
+
+

LinearMath/SimdTransformUtil.h

@@ -0,0 +1,136 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef SIMD_TRANSFORM_UTIL_H
+#define SIMD_TRANSFORM_UTIL_H
+
+#include "SimdTransform.h"
+#define ANGULAR_MOTION_TRESHOLD 0.5f*SIMD_HALF_PI
+
+
+
+#define SIMDSQRT12 SimdScalar(0.7071067811865475244008443621048490)
+
+#define SimdRecipSqrt(x) ((float)(1.0f/SimdSqrt(float(x))))		/* reciprocal square root */
+
+
+inline void SimdPlaneSpace1 (const SimdVector3& n, SimdVector3& p, SimdVector3& q)
+{
+  if (SimdFabs(n[2]) > SIMDSQRT12) {
+    // choose p in y-z plane
+    SimdScalar a = n[1]*n[1] + n[2]*n[2];
+    SimdScalar k = SimdRecipSqrt (a);
+    p[0] = 0;
+    p[1] = -n[2]*k;
+    p[2] = n[1]*k;
+    // set q = n x p
+    q[0] = a*k;
+    q[1] = -n[0]*p[2];
+    q[2] = n[0]*p[1];
+  }
+  else {
+    // choose p in x-y plane
+    SimdScalar a = n[0]*n[0] + n[1]*n[1];
+    SimdScalar k = SimdRecipSqrt (a);
+    p[0] = -n[1]*k;
+    p[1] = n[0]*k;
+    p[2] = 0;
+    // set q = n x p
+    q[0] = -n[2]*p[1];
+    q[1] = n[2]*p[0];
+    q[2] = a*k;
+  }
+}
+
+
+
+/// Utils related to temporal transforms
+class SimdTransformUtil
+{
+
+public:
+
+	static void IntegrateTransform(const SimdTransform& curTrans,const SimdVector3& linvel,const SimdVector3& angvel,SimdScalar timeStep,SimdTransform& predictedTransform)
+	{
+		predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep);
+//	#define QUATERNION_DERIVATIVE
+	#ifdef QUATERNION_DERIVATIVE
+		SimdQuaternion orn = curTrans.getRotation();
+		orn += (angvel * orn) * (timeStep * 0.5f);
+		orn.normalize();
+	#else
+		//exponential map
+		SimdVector3 axis;
+		SimdScalar	fAngle = angvel.length(); 
+		//limit the angular motion
+		if (fAngle*timeStep > ANGULAR_MOTION_TRESHOLD)
+		{
+			fAngle = ANGULAR_MOTION_TRESHOLD / timeStep;
+		}
+
+		if ( fAngle < 0.001f )
+		{
+			// use Taylor's expansions of sync function
+			axis   = angvel*( 0.5f*timeStep-(timeStep*timeStep*timeStep)*(0.020833333333f)*fAngle*fAngle );
+		}
+		else
+		{
+			// sync(fAngle) = sin(c*fAngle)/t
+			axis   = angvel*( SimdSin(0.5f*fAngle*timeStep)/fAngle );
+		}
+		SimdQuaternion dorn (axis.x(),axis.y(),axis.z(),SimdCos( fAngle*timeStep*0.5f ));
+		SimdQuaternion orn0 = curTrans.getRotation();
+
+		SimdQuaternion predictedOrn = dorn * orn0;
+	#endif
+		predictedTransform.setRotation(predictedOrn);
+	}
+
+	static void	CalculateVelocity(const SimdTransform& transform0,const SimdTransform& transform1,SimdScalar timeStep,SimdVector3& linVel,SimdVector3& angVel)
+	{
+		linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
+#ifdef USE_QUATERNION_DIFF
+		SimdQuaternion orn0 = transform0.getRotation();
+		SimdQuaternion orn1a = transform1.getRotation();
+		SimdQuaternion orn1 = orn0.farthest(orn1a);
+		SimdQuaternion dorn = orn1 * orn0.inverse();
+#else
+		SimdMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
+		SimdQuaternion dorn;
+		dmat.getRotation(dorn);
+#endif//USE_QUATERNION_DIFF
+
+		SimdVector3 axis;
+		SimdScalar  angle;
+		angle = dorn.getAngle();
+		axis = SimdVector3(dorn.x(),dorn.y(),dorn.z());
+		axis[3] = 0.f;
+		//check for axis length
+		SimdScalar len = axis.length2();
+		if (len < SIMD_EPSILON*SIMD_EPSILON)
+			axis = SimdVector3(1.f,0.f,0.f);
+		else
+			axis /= SimdSqrt(len);
+
+		
+		angVel = axis * angle / timeStep;
+
+	}
+
+
+};
+
+#endif //SIMD_TRANSFORM_UTIL_H
+

LinearMath/SimdVector3.h

@@ -0,0 +1,403 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef SIMD__VECTOR3_H
+#define SIMD__VECTOR3_H
+
+#include "SimdQuadWord.h"
+
+
+///SimdVector3 is 16byte aligned, and has an extra unused component m_w
+///this extra component can be used by derived classes (Quaternion?) or by user
+class SimdVector3 : public SimdQuadWord {
+
+
+public:
+	SIMD_FORCE_INLINE SimdVector3() {}
+
+	
+
+	SIMD_FORCE_INLINE SimdVector3(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z) 
+		:SimdQuadWord(x,y,z,0.f)
+	{
+	}
+
+//	SIMD_FORCE_INLINE SimdVector3(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z,const SimdScalar& w) 
+//		: SimdQuadWord(x,y,z,w)
+//	{
+//	}
+
+	
+
+	SIMD_FORCE_INLINE SimdVector3& operator+=(const SimdVector3& v)
+	{
+		m_x += v.x(); m_y += v.y(); m_z += v.z();
+		return *this;
+	}
+
+
+
+	SIMD_FORCE_INLINE SimdVector3& operator-=(const SimdVector3& v) 
+	{
+		m_x -= v.x(); m_y -= v.y(); m_z -= v.z();
+		return *this;
+	}
+
+	SIMD_FORCE_INLINE SimdVector3& operator*=(const SimdScalar& s)
+	{
+		m_x *= s; m_y *= s; m_z *= s;
+		return *this;
+	}
+
+	SIMD_FORCE_INLINE SimdVector3& operator/=(const SimdScalar& s) 
+	{
+		assert(s != SimdScalar(0.0));
+		return *this *= SimdScalar(1.0) / s;
+	}
+
+	SIMD_FORCE_INLINE SimdScalar dot(const SimdVector3& v) const
+	{
+		return m_x * v.x() + m_y * v.y() + m_z * v.z();
+	}
+
+	SIMD_FORCE_INLINE SimdScalar length2() const
+	{
+		return dot(*this);
+	}
+
+	SIMD_FORCE_INLINE SimdScalar length() const
+	{
+		return SimdSqrt(length2());
+	}
+
+	SIMD_FORCE_INLINE SimdScalar distance2(const SimdVector3& v) const;
+
+	SIMD_FORCE_INLINE SimdScalar distance(const SimdVector3& v) const;
+
+	SIMD_FORCE_INLINE SimdVector3& normalize() 
+	{
+		return *this /= length();
+	}
+
+	SIMD_FORCE_INLINE SimdVector3 normalized() const;
+
+	SIMD_FORCE_INLINE SimdVector3 rotate( const SimdVector3& wAxis, const SimdScalar angle );
+
+	SIMD_FORCE_INLINE SimdScalar angle(const SimdVector3& v) const 
+	{
+		SimdScalar s = SimdSqrt(length2() * v.length2());
+		assert(s != SimdScalar(0.0));
+		return SimdAcos(dot(v) / s);
+	}
+
+	SIMD_FORCE_INLINE SimdVector3 absolute() const 
+	{
+		return SimdVector3(
+			SimdFabs(m_x), 
+			SimdFabs(m_y), 
+			SimdFabs(m_z));
+	}
+
+	SIMD_FORCE_INLINE SimdVector3 cross(const SimdVector3& v) const
+	{
+		return SimdVector3(
+			m_y * v.z() - m_z * v.y(),
+			m_z * v.x() - m_x * v.z(),
+			m_x * v.y() - m_y * v.x());
+	}
+
+	SIMD_FORCE_INLINE SimdScalar triple(const SimdVector3& v1, const SimdVector3& v2) const
+	{
+		return m_x * (v1.y() * v2.z() - v1.z() * v2.y()) + 
+			m_y * (v1.z() * v2.x() - v1.x() * v2.z()) + 
+			m_z * (v1.x() * v2.y() - v1.y() * v2.x());
+	}
+
+	SIMD_FORCE_INLINE int minAxis() const
+	{
+		return m_x < m_y ? (m_x < m_z ? 0 : 2) : (m_y < m_z ? 1 : 2);
+	}
+
+	SIMD_FORCE_INLINE int maxAxis() const 
+	{
+		return m_x < m_y ? (m_y < m_z ? 2 : 1) : (m_x < m_z ? 2 : 0);
+	}
+
+	SIMD_FORCE_INLINE int furthestAxis() const
+	{
+		return absolute().minAxis();
+	}
+
+	SIMD_FORCE_INLINE int closestAxis() const 
+	{
+		return absolute().maxAxis();
+	}
+
+	SIMD_FORCE_INLINE void setInterpolate3(const SimdVector3& v0, const SimdVector3& v1, SimdScalar rt)
+	{
+		SimdScalar s = 1.0f - rt;
+		m_x = s * v0[0] + rt * v1.x();
+		m_y = s * v0[1] + rt * v1.y();
+		m_z = s * v0[2] + rt * v1.z();
+		//don't do the unused w component
+		//		m_co[3] = s * v0[3] + rt * v1[3];
+	}
+
+	SIMD_FORCE_INLINE SimdVector3 lerp(const SimdVector3& v, const SimdScalar& t) const 
+	{
+		return SimdVector3(m_x + (v.x() - m_x) * t,
+			m_y + (v.y() - m_y) * t,
+			m_z + (v.z() - m_z) * t);
+	}
+
+
+	SIMD_FORCE_INLINE SimdVector3& operator*=(const SimdVector3& v)
+	{
+		m_x *= v.x(); m_y *= v.y(); m_z *= v.z();
+		return *this;
+	}
+
+	
+
+};
+
+SIMD_FORCE_INLINE SimdVector3 
+operator+(const SimdVector3& v1, const SimdVector3& v2) 
+{
+	return SimdVector3(v1.x() + v2.x(), v1.y() + v2.y(), v1.z() + v2.z());
+}
+
+SIMD_FORCE_INLINE SimdVector3 
+operator*(const SimdVector3& v1, const SimdVector3& v2) 
+{
+	return SimdVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() *+ v2.z());
+}
+
+SIMD_FORCE_INLINE SimdVector3 
+operator-(const SimdVector3& v1, const SimdVector3& v2)
+{
+	return SimdVector3(v1.x() - v2.x(), v1.y() - v2.y(), v1.z() - v2.z());
+}
+
+SIMD_FORCE_INLINE SimdVector3 
+operator-(const SimdVector3& v)
+{
+	return SimdVector3(-v.x(), -v.y(), -v.z());
+}
+
+SIMD_FORCE_INLINE SimdVector3 
+operator*(const SimdVector3& v, const SimdScalar& s)
+{
+	return SimdVector3(v.x() * s, v.y() * s, v.z() * s);
+}
+
+SIMD_FORCE_INLINE SimdVector3 
+operator*(const SimdScalar& s, const SimdVector3& v)
+{ 
+	return v * s; 
+}
+
+SIMD_FORCE_INLINE SimdVector3
+operator/(const SimdVector3& v, const SimdScalar& s)
+{
+	assert(s != SimdScalar(0.0));
+	return v * (SimdScalar(1.0) / s);
+}
+
+SIMD_FORCE_INLINE SimdVector3
+operator/(const SimdVector3& v1, const SimdVector3& v2)
+{
+	return SimdVector3(v1.x() / v2.x(),v1.y() / v2.y(),v1.z() / v2.z());
+}
+
+SIMD_FORCE_INLINE SimdScalar 
+dot(const SimdVector3& v1, const SimdVector3& v2) 
+{ 
+	return v1.dot(v2); 
+}
+
+
+
+SIMD_FORCE_INLINE SimdScalar
+distance2(const SimdVector3& v1, const SimdVector3& v2) 
+{ 
+	return v1.distance2(v2); 
+}
+
+
+SIMD_FORCE_INLINE SimdScalar
+distance(const SimdVector3& v1, const SimdVector3& v2) 
+{ 
+	return v1.distance(v2); 
+}
+
+SIMD_FORCE_INLINE SimdScalar
+angle(const SimdVector3& v1, const SimdVector3& v2) 
+{ 
+	return v1.angle(v2); 
+}
+
+SIMD_FORCE_INLINE SimdVector3 
+cross(const SimdVector3& v1, const SimdVector3& v2) 
+{ 
+	return v1.cross(v2); 
+}
+
+SIMD_FORCE_INLINE SimdScalar
+triple(const SimdVector3& v1, const SimdVector3& v2, const SimdVector3& v3)
+{
+	return v1.triple(v2, v3);
+}
+
+SIMD_FORCE_INLINE SimdVector3 
+lerp(const SimdVector3& v1, const SimdVector3& v2, const SimdScalar& t)
+{
+	return v1.lerp(v2, t);
+}
+
+
+SIMD_FORCE_INLINE bool operator==(const SimdVector3& p1, const SimdVector3& p2) 
+{
+	return p1[0] == p2[0] && p1[1] == p2[1] && p1[2] == p2[2];
+}
+
+SIMD_FORCE_INLINE SimdScalar SimdVector3::distance2(const SimdVector3& v) const
+{
+	return (v - *this).length2();
+}
+
+SIMD_FORCE_INLINE SimdScalar SimdVector3::distance(const SimdVector3& v) const
+{
+	return (v - *this).length();
+}
+
+SIMD_FORCE_INLINE SimdVector3 SimdVector3::normalized() const
+{
+	return *this / length();
+} 
+
+SIMD_FORCE_INLINE SimdVector3 SimdVector3::rotate( const SimdVector3& wAxis, const SimdScalar angle )
+{
+	// wAxis must be a unit lenght vector
+
+	SimdVector3 o = wAxis * wAxis.dot( *this );
+	SimdVector3 x = *this - o;
+	SimdVector3 y;
+
+	y = wAxis.cross( *this );
+
+	return ( o + x * SimdCos( angle ) + y * SimdSin( angle ) );
+}
+
+class SimdVector4 : public SimdVector3
+{
+public:
+
+	SIMD_FORCE_INLINE SimdVector4() {}
+
+
+	SIMD_FORCE_INLINE SimdVector4(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z,const SimdScalar& w) 
+		: SimdVector3(x,y,z)
+	{
+		m_unusedW = w;
+	}
+
+
+	SIMD_FORCE_INLINE SimdVector4 absolute4() const 
+	{
+		return SimdVector4(
+			SimdFabs(m_x), 
+			SimdFabs(m_y), 
+			SimdFabs(m_z),
+			SimdFabs(m_unusedW));
+	}
+
+
+
+	float	getW() const { return m_unusedW;}
+
+
+		SIMD_FORCE_INLINE int maxAxis4() const
+	{
+		int maxIndex = -1;
+		float maxVal = -1e30f;
+		if (m_x > maxVal)
+		{
+			maxIndex = 0;
+			maxVal = m_x;
+		}
+		if (m_y > maxVal)
+		{
+			maxIndex = 1;
+			maxVal = m_y;
+		}
+		if (m_z > maxVal)
+		{
+			maxIndex = 2;
+			maxVal = m_z;
+		}
+		if (m_unusedW > maxVal)
+		{
+			maxIndex = 3;
+			maxVal = m_unusedW;
+		}
+		
+		
+		
+
+		return maxIndex;
+
+	}
+
+
+	SIMD_FORCE_INLINE int minAxis4() const
+	{
+		int minIndex = -1;
+		float minVal = 1e30f;
+		if (m_x < minVal)
+		{
+			minIndex = 0;
+			minVal = m_x;
+		}
+		if (m_y < minVal)
+		{
+			minIndex = 1;
+			minVal = m_y;
+		}
+		if (m_z < minVal)
+		{
+			minIndex = 2;
+			minVal = m_z;
+		}
+		if (m_unusedW < minVal)
+		{
+			minIndex = 3;
+			minVal = m_unusedW;
+		}
+		
+		return minIndex;
+
+	}
+
+
+	SIMD_FORCE_INLINE int closestAxis4() const 
+	{
+		return absolute4().maxAxis4();
+	}
+
+};
+
+#endif //SIMD__VECTOR3_H

LinearMath/Vector.cpp

@@ -0,0 +1,55 @@
+// Bullet Continuous Collision Detection and Physics Library
+// Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+//
+//
+// Vector.cpp
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+#ifdef WIN32
+#if _MSC_VER >= 1310
+#include "Vector.h"
+#include <float.h>
+
+////////////////////////////////////////////////////////////////////////////////
+// Vector3
+
+bool Vector3::IsFinite() const
+{
+	if (_finite(GetX()) && _finite(GetY()) && _finite(GetZ()))
+		return true;
+	return false;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Point3
+
+bool Point3::IsFinite() const
+{
+	if (_finite(GetX()) && _finite(GetY()) && _finite(GetZ()))
+		return true;
+	return false;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Vector4
+bool Vector4::IsFinite() const
+{
+	if (_finite(GetX()) && _finite(GetY()) && _finite(GetZ()) && _finite(GetW()))
+		return true;
+	return false;
+}
+
+#endif
+#endif //WIN32

LinearMath/Vector.h

@@ -0,0 +1,349 @@
+// Bullet Continuous Collision Detection and Physics Library
+// Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+//
+// Vector.h
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+#ifndef BULLET_VECTOR_H
+#define BULLET_VECTOR_H
+
+#include "VectorBase.h"
+
+class Point3;
+class Vector4;
+
+////////////////////////////////////////////////////////////////////////////////
+// Vector3
+__declspec(align(16)) class Vector3 : public Vector4Base
+{
+public:
+	BULLET_ALIGNED_NEW_AND_DELETE
+
+	// constructors
+	Vector3();
+	Vector3(const Vector3& v);
+	Vector3(float x, float y, float z);
+	Vector3(const Scalar& x, const Scalar& y, const Scalar& z);
+
+	// construction to constant
+	Vector3(const Maths::ZeroTag&);
+	Vector3(const Maths::UnitXTag&);
+	Vector3(const Maths::UnitYTag&);
+	Vector3(const Maths::UnitZTag&);
+	Vector3(const Maths::UnitNegXTag&);
+	Vector3(const Maths::UnitNegYTag&);
+	Vector3(const Maths::UnitNegZTag&);
+
+	// explicit constructors
+	explicit Vector3(const __m128 b);
+	explicit Vector3(const Vector4Base& v);
+	explicit Vector3(const Scalar& v);
+	explicit Vector3(const Point3& v);
+	explicit Vector3(const Vector4& v);
+	explicit Vector3(const float* p);
+
+	// assignment
+	const Vector3& operator=(const Vector3& v);
+
+	// assignment to constant
+	const Vector3& operator=(const Maths::ZeroTag&);
+	const Vector3& operator=(const Maths::UnitXTag&);
+	const Vector3& operator=(const Maths::UnitYTag&);
+	const Vector3& operator=(const Maths::UnitZTag&);
+	const Vector3& operator=(const Maths::UnitNegXTag&);
+	const Vector3& operator=(const Maths::UnitNegYTag&);
+	const Vector3& operator=(const Maths::UnitNegZTag&);
+
+	// in place operations
+	void operator+=(const Vector3& b);
+	void operator-=(const Vector3& b);
+	void operator*=(const Vector3& b);
+	void operator/=(const Vector3& b);
+	void operator*=(const Scalar& s);
+	void operator/=(const Scalar& s);
+	
+	// operations
+	friend const Vector3 operator-(const Vector3& a);
+
+	friend const Vector3 operator+(const Vector3& a, const Vector3& b);
+	friend const Vector3 operator-(const Vector3& a, const Vector3& b);
+	friend const Vector3 operator*(const Vector3& a, const Vector3& b);
+	friend const Vector3 operator/(const Vector3& a, const Vector3& b);
+	friend const Vector3 operator*(const Vector3& a, const Scalar& s);
+	friend const Vector3 operator*(const Scalar& s,  const Vector3& a);
+	friend const Vector3 operator/(const Vector3& a, const Scalar& s);
+
+	friend const Vector3 Abs(const Vector3& a);
+	friend const Vector3 Rcp(const Vector3& a);
+	friend const Vector3 Rsqrt(const Vector3& a);
+	friend const Vector3 Sqrt(const Vector3& a);
+	friend const Vector3 RcpNr(const Vector3& a);
+	friend const Vector3 RsqrtNr(const Vector3& a);
+
+	friend const Scalar Sum(const Vector3& a);
+	friend const Scalar Dot(const Vector3& a, const Vector3& b);
+
+	friend const Vector3 Cross(const Vector3& a, const Vector3& b);
+
+	friend const Vector3 Min(const Vector3& a, const Vector3& b);
+	friend const Vector3 Max(const Vector3& a, const Vector3& b);
+	friend const Vector3 Clamp(const Vector3& v, const Vector3& min, const Vector3& max);
+
+	friend const Scalar MinComp(const Vector3& a);
+	friend const Scalar MaxComp(const Vector3& a);
+	friend const int MinCompIndex(const Vector3& a);
+	friend const int MaxCompIndex(const Vector3& a);
+
+	friend const Scalar Length(const Vector3& a);
+	friend const Scalar LengthSqr(const Vector3& a);
+	friend const Scalar LengthRcp(const Vector3& a);
+	friend const Vector3 Normalize(const Vector3& a);
+
+	friend const Scalar LengthFast(const Vector3& a);
+	friend const Scalar LengthRcpFast(const Vector3& a);
+	friend const Vector3 NormalizeFast(const Vector3& a);
+
+	friend const Vector3 Lerp(const Vector3& a, const Vector3& b, const Scalar& t);
+
+	// returns an arbitrary perpendicular vector (not normalized)
+	friend const Vector3 Perp(const Vector3& v);
+
+	// comparisons (return 1 bit per component)
+	friend int operator==(const Vector3& a, const Vector3& b);
+	friend int operator!=(const Vector3& a, const Vector3& b);
+	friend int operator<(const Vector3& a, const Vector3& b);
+	friend int operator<=(const Vector3& a, const Vector3& b);
+	friend int operator>(const Vector3& a, const Vector3& b);
+	friend int operator>=(const Vector3& a, const Vector3& b);
+
+	// validation
+	bool IsFinite() const;
+};
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Point3
+__declspec(align(16)) class Point3 : public Vector4Base
+{
+public:
+	// constructors
+	Point3();
+	Point3(const Point3& v);
+	Point3(float x, float y, float z);
+	Point3(const Scalar& x, const Scalar& y, const Scalar& z);
+
+	// construction to constant
+	Point3(const Maths::ZeroTag&);
+	Point3(const Maths::UnitXTag&);
+	Point3(const Maths::UnitYTag&);
+	Point3(const Maths::UnitZTag&);
+	Point3(const Maths::UnitNegXTag&);
+	Point3(const Maths::UnitNegYTag&);
+	Point3(const Maths::UnitNegZTag&);
+
+	// explicit constructors
+	explicit Point3(const __m128 b);
+	explicit Point3(const Vector4Base& v);
+	explicit Point3(const Scalar& v);
+	explicit Point3(const Vector3& v);
+	explicit Point3(const Vector4& v);
+	explicit Point3(const float* p);
+
+	// assignment
+	const Point3& operator=(const Point3& v);
+
+	// assignment to constant
+	const Point3& operator=(const Maths::ZeroTag&);
+	const Point3& operator=(const Maths::UnitXTag&);
+	const Point3& operator=(const Maths::UnitYTag&);
+	const Point3& operator=(const Maths::UnitZTag&);
+	const Point3& operator=(const Maths::UnitNegXTag&);
+	const Point3& operator=(const Maths::UnitNegYTag&);
+	const Point3& operator=(const Maths::UnitNegZTag&);
+
+	// in place operations
+	void operator+=(const Vector3& b);
+	void operator-=(const Vector3& b);
+	
+	// operations
+	friend const Point3 operator-(const Point3& a);
+
+	friend const Point3 operator+(const Point3& a, const Vector3& b);
+	friend const Point3 operator+(const Vector3& a, const Point3& b);
+
+	friend const Point3 operator-(const Point3& a, const Vector3& b);
+	friend const Vector3 operator-(const Point3& a, const Point3& b);
+
+	friend const Vector3 operator*(const Point3& a, const Vector3& b);
+	friend const Vector3 operator*(const Vector3& a, const Point3& b);
+
+	friend const Point3 Abs(const Point3& a);
+
+	friend const Scalar Sum(const Point3& a);
+	friend const Scalar Dot(const Point3& a, const Point3& b);
+	friend const Scalar Dot(const Vector3& a, const Point3& b);
+	friend const Scalar Dot(const Point3& a, const Vector3& b);
+
+	friend const Point3 Min(const Point3& a, const Point3& b);
+	friend const Point3 Max(const Point3& a, const Point3& b);
+	friend const Point3 Clamp(const Point3& v, const Point3& min, const Point3& max);
+
+	friend const Scalar Dist(const Point3& a, const Point3& b);
+	friend const Scalar DistSqr(const Point3& a, const Point3& b);
+	friend const Scalar DistRcp(const Point3& a, const Point3& b);
+	
+	friend const Scalar DistFast(const Point3& a, const Point3& b);
+	friend const Scalar DistRcpFast(const Point3& a, const Point3& b);
+
+	friend const Point3 Lerp(const Point3& a, const Point3& b, const Scalar& t);
+
+	friend const Point3 Homogenize(const Vector4& v);
+	friend const Point3 HomogenizeFast(const Vector4& v);
+
+	// comparisons (return 1 bit per component)
+	friend int operator==(const Point3& a, const Point3& b);
+	friend int operator!=(const Point3& a, const Point3& b);
+	friend int operator<(const Point3& a, const Point3& b);
+	friend int operator<=(const Point3& a, const Point3& b);
+	friend int operator>(const Point3& a, const Point3& b);
+	friend int operator>=(const Point3& a, const Point3& b);
+
+	// validation
+	bool IsFinite() const;
+};
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Vector4
+__declspec(align(16)) class Vector4 : public Vector4Base
+{
+public:
+	// constructors
+	Vector4();
+	Vector4(const Vector4& v);
+	Vector4(float x, float y, float z, float w);
+	Vector4(const Scalar& x, const Scalar& y, const Scalar& z, const Scalar& w);
+	Vector4(const Vector3& xyz, const Scalar& w);
+
+	// construction to constant
+	Vector4(const Maths::ZeroTag&);
+	Vector4(const Maths::UnitXTag&);
+	Vector4(const Maths::UnitYTag&);
+	Vector4(const Maths::UnitZTag&);
+	Vector4(const Maths::UnitWTag&);
+	Vector4(const Maths::UnitNegXTag&);
+	Vector4(const Maths::UnitNegYTag&);
+	Vector4(const Maths::UnitNegZTag&);
+	Vector4(const Maths::UnitNegWTag&);
+
+	// explicit constructors
+	explicit Vector4(const __m128 b);
+	explicit Vector4(const Vector4Base& v);
+	explicit Vector4(const Scalar& v);
+	explicit Vector4(const Vector3& v);
+	explicit Vector4(const Point3& v);
+	explicit Vector4(const float* p);
+
+	// assignment
+	const Vector4& operator=(const Vector4& v);
+
+	// assignment to constant
+	const Vector4& operator=(const Maths::ZeroTag&);
+	const Vector4& operator=(const Maths::UnitXTag&);
+	const Vector4& operator=(const Maths::UnitYTag&);
+	const Vector4& operator=(const Maths::UnitZTag&);
+	const Vector4& operator=(const Maths::UnitWTag&);
+	const Vector4& operator=(const Maths::UnitNegXTag&);
+	const Vector4& operator=(const Maths::UnitNegYTag&);
+	const Vector4& operator=(const Maths::UnitNegZTag&);
+	const Vector4& operator=(const Maths::UnitNegWTag&);
+
+	// in place operations
+	void operator+=(const Vector4& b);
+	void operator-=(const Vector4& b);
+	void operator*=(const Vector4& b);
+	void operator/=(const Vector4& b);
+	void operator*=(const Scalar& s);
+	void operator/=(const Scalar& s);
+	
+	// operations
+	friend const Vector4 operator-(const Vector4& a);
+
+	friend const Vector4 operator+(const Vector4& a, const Vector4& b);
+	friend const Vector4 operator-(const Vector4& a, const Vector4& b);
+	friend const Vector4 operator*(const Vector4& a, const Vector4& b);
+	friend const Vector4 operator/(const Vector4& a, const Vector4& b);
+	friend const Vector4 operator*(const Vector4& a, const Scalar& s);
+	friend const Vector4 operator*(const Scalar& s,  const Vector4& a);
+	friend const Vector4 operator/(const Vector4& a, const Scalar& s);
+
+	friend const Vector4 Abs(const Vector4& a);
+	friend const Vector4 Rcp(const Vector4& a);
+	friend const Vector4 Rsqrt(const Vector4& a);
+	friend const Vector4 Sqrt(const Vector4& a);
+	friend const Vector4 RcpNr(const Vector4& a);
+	friend const Vector4 RsqrtNr(const Vector4& a);
+
+	friend const Scalar Sum(const Vector4& a);
+	friend const Scalar Dot(const Vector4& a, const Vector4& b);
+
+	friend const Vector4 Min(const Vector4& a, const Vector4& b);
+	friend const Vector4 Max(const Vector4& a, const Vector4& b);
+	friend const Vector4 Clamp(const Vector4& v, const Vector4& min, const Vector4& max);
+
+	friend const Scalar MinComp(const Vector4& a);
+	friend const Scalar MaxComp(const Vector4& a);
+
+	friend const Scalar Length(const Vector4& a);
+	friend const Scalar LengthSqr(const Vector4& a);
+	friend const Scalar LengthRcp(const Vector4& a);
+	friend const Vector4 Normalize(const Vector4& a);
+
+	friend const Scalar LengthFast(const Vector4& a);
+	friend const Scalar LengthRcpFast(const Vector4& a);
+	friend const Vector4 NormalizeFast(const Vector4& a);
+
+	friend const Vector4 Lerp(const Vector4& a, const Vector4& b, const Scalar& t);
+
+	// comparisons (return 1 bit per component)
+	friend int operator==(const Vector4& a, const Vector4& b);
+	friend int operator!=(const Vector4& a, const Vector4& b);
+	friend int operator<(const Vector4& a, const Vector4& b);
+	friend int operator<=(const Vector4& a, const Vector4& b);
+	friend int operator>(const Vector4& a, const Vector4& b);
+	friend int operator>=(const Vector4& a, const Vector4& b);
+
+	// validation
+	bool IsFinite() const;
+};
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Vector2
+class Vector2 : public Vector2Base
+{
+public:
+};
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Point2
+class Point2 : public Vector2Base
+{
+public:
+
+};
+
+
+#include "Vector.inl"
+#endif //BULLET_VECTOR_H

LinearMath/VectorBase.cpp

@@ -0,0 +1,49 @@
+// Bullet Continuous Collision Detection and Physics Library
+// Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+//
+// VectorBase.cpp
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+#ifdef WIN32
+#if _MSC_VER >= 1310
+
+#include "VectorBase.h"
+
+const __m128 Vector4Base::Consts::kZero		= _mm_set1_ps(0.0f);
+const __m128 Vector4Base::Consts::kHalf		= _mm_set1_ps(0.5f);
+const __m128 Vector4Base::Consts::kThree	= _mm_set1_ps(3.0f);
+
+const __m128 Vector4Base::Consts::k1000		= _mm_setr_ps(1.0f, 0.0f, 0.0f, 0.0f);
+const __m128 Vector4Base::Consts::k0100		= _mm_setr_ps(0.0f, 1.0f, 0.0f, 0.0f);
+const __m128 Vector4Base::Consts::k0010		= _mm_setr_ps(0.0f, 0.0f, 1.0f, 0.0f);
+const __m128 Vector4Base::Consts::k0001		= _mm_setr_ps(0.0f, 0.0f, 0.0f, 1.0f);
+
+const __m128 Vector4Base::Consts::kNeg1000	= _mm_setr_ps(-1.0f, 0.0f, 0.0f, 0.0f);
+const __m128 Vector4Base::Consts::kNeg0100	= _mm_setr_ps(0.0f, -1.0f, 0.0f, 0.0f);
+const __m128 Vector4Base::Consts::kNeg0010	= _mm_setr_ps(0.0f, 0.0f, -1.0f, 0.0f);
+const __m128 Vector4Base::Consts::kNeg0001	= _mm_setr_ps(0.0f, 0.0f, 0.0f, -1.0f);
+
+const __m128 Vector4Base::Consts::kNeg111_1	= _mm_setr_ps(-1.0f, -1.0f, -1.0f, 1.0f);
+const __m128 Vector4Base::Consts::k1110		= _mm_setr_ps(1.0f, 1.0f, 1.0f, 0.0f);
+
+const unsigned int Vector4Base::Consts::maskAbs = 0x7fffffff;
+const __m128 Vector4Base::Consts::kMaskAbs = _mm_load1_ps((float*)&Vector4Base::Consts::maskAbs);
+
+const unsigned int Vector4Base::Consts::mask1110[4] = {0xffffffff, 0xffffffff, 0xffffffff, 0x00000000};
+const unsigned int Vector4Base::Consts::mask0001[4] = {0x00000000, 0x00000000, 0x00000000, 0xffffffff};
+
+const __m128 Vector4Base::Consts::kMask1110 = _mm_loadu_ps((float*)Vector4Base::Consts::mask1110);
+const __m128 Vector4Base::Consts::kMask0001 = _mm_loadu_ps((float*)Vector4Base::Consts::mask0001);
+
+#endif 
+#endif //WIN32

LinearMath/VectorBase.h

@@ -0,0 +1,142 @@
+// Bullet Continuous Collision Detection and Physics Library
+// Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+//
+//
+// VectorBase.h
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+#ifndef BULLET_VECTOR_BASE_H
+#define BULLET_VECTOR_BASE_H
+
+#include "Scalar.h"
+#include "Memory2.h"
+
+// vector constants
+namespace Maths
+{
+	static const enum ZeroTag { } Zero;
+	static const enum UnitXTag { } UnitX;
+	static const enum UnitYTag { } UnitY;
+	static const enum UnitZTag { } UnitZ;
+	static const enum UnitWTag { } UnitW;
+	static const enum UnitNegXTag { } UnitNegX;
+	static const enum UnitNegYTag { } UnitNegY;
+	static const enum UnitNegZTag { } UnitNegZ;
+	static const enum UnitNegWTag { } UnitNegW;
+
+	static const enum IdentityTag { } Identity;
+	static const enum RotateXTag { } RotateX;
+	static const enum RotateYTag { } RotateY;
+	static const enum RotateZTag { } RotateZ;
+	static const enum ScaleTag { } Scale;
+	static const enum SkewTag { } Skew;
+};
+
+////////////////////////////////////////////////////////////////////////////////
+// Vector4Base
+__declspec(align(16)) class Vector4Base
+{
+public:
+	BULLET_ALIGNED_NEW_AND_DELETE
+
+	__m128 base;
+
+//protected:
+	// useful constants for internal use
+	struct Consts
+	{
+		static const unsigned int maskAbs;
+		static const unsigned int mask1110[4];
+		static const unsigned int mask0001[4];
+
+		static const __m128
+			kZero,
+			kHalf,
+			kThree,
+
+			k1000,
+			k0100,
+			k0010,
+			k0001,
+
+			kNeg1000,
+			kNeg0100,
+			kNeg0010,
+			kNeg0001,
+
+			kNeg111_1,
+
+			k1110,
+			kMaskAbs,
+			kMask1110,
+			kMask0001;
+	};
+
+	// can't construct a Vector4Base
+	Vector4Base();
+
+	// compound operations helpers for use by derived classes
+	void Set(const __m128& x, const __m128& y, const __m128& z, const __m128& w);
+	void Set(const __m128& xyz, const __m128& w);
+
+	static __m128 Dot3(const __m128& v0, const __m128& v1);
+	static __m128 Dot4(const __m128& v0, const __m128& v1);
+
+	static __m128 Sum3(const __m128& a);
+	static __m128 Sum4(const __m128& a);
+
+	static __m128 MinComp3(const __m128& a);
+	static __m128 MinComp4(const __m128& a);
+
+	static __m128 MaxComp3(const __m128& a);
+	static __m128 MaxComp4(const __m128& a);
+
+public:
+	// element access
+	const float& operator[](int i) const;
+	float& operator[](int i);
+
+	// get/set elements
+	const Scalar GetX() const;
+	const Scalar GetY() const;
+	const Scalar GetZ() const;
+	const Scalar GetW() const;
+	const Scalar Get(int i) const;
+
+	void SetX(const Scalar& s);
+	void SetY(const Scalar& s);
+	void SetZ(const Scalar& s);
+	void SetW(const Scalar& s);
+	void Set(int i, const Scalar& s);
+
+	// unaligned load/store
+	void LoadUnaligned3(const float* p);
+	void LoadUnaligned4(const float* p);
+
+	void StoreUnaligned3(float* p) const;
+	void StoreUnaligned4(float* p) const;
+};
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Vector2Base
+class __declspec(align(8)) Vector2Base
+{
+public:
+	float x, y;
+};
+
+
+#include "VectorBase.inl"
+
+#endif //BULLET_VECTOR_BASE_H

LinearMath/VectorBase.inl

@@ -0,0 +1,213 @@
+// Bullet Continuous Collision Detection and Physics Library
+// Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+//
+//
+// VectorBase.inl
+//
+// Copyright (c) 2006 Simon Hobbs
+//
+// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+//
+// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+//
+// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+//
+// 3. This notice may not be removed or altered from any source distribution.
+#pragma once
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Vector4Base
+
+inline Vector4Base::Vector4Base()
+{
+}
+
+inline const float& Vector4Base::operator[](int i) const
+{
+	return *(((float*)&base) + i);
+}
+
+inline float& Vector4Base::operator[](int i)
+{
+	return *(((float*)&base) + i);
+}
+
+inline const Scalar Vector4Base::GetX() const
+{
+	return Scalar(_mm_shuffle_ps(base, base, _MM_SHUFFLE(0, 0, 0, 0)));
+}
+
+inline const Scalar Vector4Base::GetY() const
+{
+	return Scalar(_mm_shuffle_ps(base, base, _MM_SHUFFLE(1, 1, 1, 1)));
+}
+
+inline const Scalar Vector4Base::GetZ() const
+{
+	return Scalar(_mm_shuffle_ps(base, base, _MM_SHUFFLE(2, 2, 2, 2)));
+}
+
+inline const Scalar Vector4Base::GetW() const
+{
+	return Scalar(_mm_shuffle_ps(base, base, _MM_SHUFFLE(3, 3, 3, 3)));
+}
+
+inline const Scalar Vector4Base::Get(int i) const
+{
+	__m128 res;
+
+	switch (i)
+	{
+	case 0: res = _mm_shuffle_ps(base, base, _MM_SHUFFLE(0, 0, 0, 0)); break;
+	case 1: res = _mm_shuffle_ps(base, base, _MM_SHUFFLE(1, 1, 1, 1)); break;
+	case 2: res = _mm_shuffle_ps(base, base, _MM_SHUFFLE(2, 2, 2, 2)); break;
+	case 3: res = _mm_shuffle_ps(base, base, _MM_SHUFFLE(3, 3, 3, 3)); break;
+	}
+
+	return Scalar(res);
+}
+
+inline void Vector4Base::SetX(const Scalar& s)
+{
+	__m128 xxyy = _mm_shuffle_ps(s.base, base, _MM_SHUFFLE(1, 1, 0, 0));
+	base = _mm_shuffle_ps(xxyy, base, _MM_SHUFFLE(3, 2, 2, 0));
+}
+
+inline void Vector4Base::SetY(const Scalar& s)
+{
+	__m128 xxyy = _mm_shuffle_ps(base, s.base, _MM_SHUFFLE(0, 0, 0, 0));
+	base = _mm_shuffle_ps(xxyy, base, _MM_SHUFFLE(3, 2, 2, 0));
+}
+
+inline void Vector4Base::SetZ(const Scalar& s)
+{
+	__m128 zzww = _mm_shuffle_ps(s.base, base, _MM_SHUFFLE(3, 3, 0, 0));
+	base = _mm_shuffle_ps(base, zzww, _MM_SHUFFLE(2, 0, 1, 0));
+}
+
+inline void Vector4Base::SetW(const Scalar& s)
+{
+	__m128 zzww = _mm_shuffle_ps(base, s.base, _MM_SHUFFLE(0, 0, 2, 2));
+	base = _mm_shuffle_ps(base, zzww, _MM_SHUFFLE(2, 0, 1, 0));
+}
+
+inline void Vector4Base::Set(int i, const Scalar& s)
+{
+	switch (i)
+	{
+	case 0: SetX(s); break;
+	case 1: SetY(s); break;
+	case 2: SetZ(s); break;
+	case 3: SetW(s); break;
+	}
+}
+
+inline void Vector4Base::LoadUnaligned3(const float* p)
+{
+	int* dst = (int*)this;
+	dst[0] = ((const int*)p)[0];
+	dst[1] = ((const int*)p)[1];
+	dst[2] = ((const int*)p)[2];
+}
+
+inline void Vector4Base::LoadUnaligned4(const float* p)
+{
+	base = _mm_loadu_ps(p);
+}
+
+inline void Vector4Base::StoreUnaligned3(float* p) const
+{
+	const int* src = (const int*)this;
+	((int*)p)[0] = src[0];
+	((int*)p)[1] = src[1];
+	((int*)p)[2] = src[2];
+}
+
+inline  void Vector4Base::StoreUnaligned4(float* p) const
+{
+	_mm_storeu_ps(p, base);
+}
+
+__forceinline void Vector4Base::Set(const __m128& x, const __m128& y, const __m128& z, const __m128& w)
+{
+	__m128 xy = _mm_unpacklo_ps(x, y);
+	__m128 zw = _mm_unpacklo_ps(z, w);
+	base = _mm_shuffle_ps(xy, zw, _MM_SHUFFLE(1, 0, 1, 0));
+}
+
+__forceinline void Vector4Base::Set(const __m128& xyz, const __m128& w)
+{
+	base = _mm_shuffle_ps(xyz, xyz, _MM_SHUFFLE(0, 1, 2, 3));
+	base = _mm_move_ss(base, w);
+	base = _mm_shuffle_ps(base, base, _MM_SHUFFLE(0, 1, 2, 3));
+}
+
+__forceinline __m128 Vector4Base::Dot3(const __m128& v0, const __m128& v1)
+{
+	__m128 a = _mm_mul_ps(v0, v1);
+	__m128 b = _mm_shuffle_ps(a, a, _MM_SHUFFLE(0, 0, 0, 0));
+	__m128 c = _mm_shuffle_ps(a, a, _MM_SHUFFLE(1, 1, 1, 1));
+	__m128 d = _mm_shuffle_ps(a, a, _MM_SHUFFLE(2, 2, 2, 2));
+	return _mm_add_ps(b, _mm_add_ps(c, d));
+}
+
+__forceinline __m128 Vector4Base::Dot4(const __m128& v0, const __m128& v1)
+{
+	__m128 a = _mm_mul_ps(v0, v1);
+	__m128 b = _mm_shuffle_ps(a, a, _MM_SHUFFLE(0, 3, 2, 1));
+	__m128 c = _mm_shuffle_ps(a, a, _MM_SHUFFLE(1, 0, 3, 2));
+	__m128 d = _mm_shuffle_ps(a, a, _MM_SHUFFLE(2, 1, 0, 3));
+	return _mm_add_ps(a, _mm_add_ps(b, _mm_add_ps(c, d)));
+}
+
+__forceinline __m128 Vector4Base::Sum3(const __m128& a)
+{
+	__m128 b = _mm_shuffle_ps(a, a, _MM_SHUFFLE(0, 0, 0, 0));
+	__m128 c = _mm_shuffle_ps(a, a, _MM_SHUFFLE(1, 1, 1, 1));
+	__m128 d = _mm_shuffle_ps(a, a, _MM_SHUFFLE(2, 2, 2, 2));
+	return _mm_add_ps(b, _mm_add_ps(c, d));
+}
+
+__forceinline __m128 Vector4Base::Sum4(const __m128& a)
+{
+	__m128 b = _mm_shuffle_ps(a, a, _MM_SHUFFLE(0, 3, 2, 1));
+	__m128 c = _mm_shuffle_ps(a, a, _MM_SHUFFLE(1, 0, 3, 2));
+	__m128 d = _mm_shuffle_ps(a, a, _MM_SHUFFLE(2, 1, 0, 3));
+	return _mm_add_ps(a, _mm_add_ps(b, _mm_add_ps(c, d)));
+}
+
+__forceinline __m128 Vector4Base::MinComp3(const __m128& a)
+{
+	__m128 b = _mm_shuffle_ps(a, a, _MM_SHUFFLE(0, 0, 0, 0));
+	__m128 c = _mm_shuffle_ps(a, a, _MM_SHUFFLE(1, 1, 1, 1));
+	__m128 d = _mm_shuffle_ps(a, a, _MM_SHUFFLE(2, 2, 2, 2));
+	return _mm_min_ps(b, _mm_min_ps(c, d));
+}
+
+__forceinline __m128 Vector4Base::MinComp4(const __m128& a)
+{
+	__m128 b = _mm_shuffle_ps(a, a, _MM_SHUFFLE(0, 3, 2, 1));
+	__m128 c = _mm_shuffle_ps(a, a, _MM_SHUFFLE(1, 0, 3, 2));
+	__m128 d = _mm_shuffle_ps(a, a, _MM_SHUFFLE(2, 1, 0, 3));
+	return _mm_min_ps(a, _mm_min_ps(b, _mm_min_ps(c, d)));
+}
+
+__forceinline __m128 Vector4Base::MaxComp3(const __m128& a)
+{
+	__m128 b = _mm_shuffle_ps(a, a, _MM_SHUFFLE(0, 0, 0, 0));
+	__m128 c = _mm_shuffle_ps(a, a, _MM_SHUFFLE(1, 1, 1, 1));
+	__m128 d = _mm_shuffle_ps(a, a, _MM_SHUFFLE(2, 2, 2, 2));
+	return _mm_max_ps(b, _mm_max_ps(c, d));
+}
+
+__forceinline __m128 Vector4Base::MaxComp4(const __m128& a)
+{
+	__m128 b = _mm_shuffle_ps(a, a, _MM_SHUFFLE(0, 3, 2, 1));
+	__m128 c = _mm_shuffle_ps(a, a, _MM_SHUFFLE(1, 0, 3, 2));
+	__m128 d = _mm_shuffle_ps(a, a, _MM_SHUFFLE(2, 1, 0, 3));
+	return _mm_max_ps(a, _mm_max_ps(b, _mm_max_ps(c, d)));
+}
+

LinearMath/quickprof.cpp

@@ -0,0 +1,45 @@
+/************************************************************************
+* QuickProf                                                             *
+* Copyright (C) 2006                                                    *
+* Tyler Streeter  tylerstreeter@gmail.com                               *
+* All rights reserved.                                                  *
+* Web: http://quickprof.sourceforge.net                                 *
+*                                                                       *
+* This library is free software; you can redistribute it and/or         *
+* modify it under the terms of EITHER:                                  *
+*   (1) The GNU Lesser General Public License as published by the Free  *
+*       Software Foundation; either version 2.1 of the License, or (at  *
+*       your option) any later version. The text of the GNU Lesser      *
+*       General Public License is included with this library in the     *
+*       file license-LGPL.txt.                                          *
+*   (2) The BSD-style license that is included with this library in     *
+*       the file license-BSD.txt.                                       *
+*                                                                       *
+* This library is distributed in the hope that it will be useful,       *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
+* license-LGPL.txt and license-BSD.txt for more details.                *
+************************************************************************/
+
+// Please visit the project website (http://quickprof.sourceforge.net) 
+// for usage instructions.
+
+// Credits: The Clock class was inspired by the Timer classes in 
+// Ogre (www.ogre3d.org).
+
+#include "quickprof.h"
+
+#ifdef USE_QUICKPROF
+
+// Note: We must declare these private static variables again here to 
+// avoid link errors.
+bool Profiler::mEnabled = false;
+hidden::Clock Profiler::mClock;
+unsigned long int Profiler::mCurrentCycleStartMicroseconds = 0;
+unsigned long int Profiler::mLastCycleDurationMicroseconds = 0;
+std::map<std::string, hidden::ProfileBlock*> Profiler::mProfileBlocks;
+std::ofstream Profiler::mOutputFile;
+bool Profiler::mFirstFileOutput = true;
+Profiler::BlockTimingMethod Profiler::mFileOutputMethod;
+unsigned long int Profiler::mCycleNumber = 0;
+#endif //USE_QUICKPROF

LinearMath/quickprof.h

@@ -0,0 +1,678 @@
+/************************************************************************
+* QuickProf                                                             *
+* Copyright (C) 2006                                                    *
+* Tyler Streeter  tylerstreeter@gmail.com                               *
+* All rights reserved.                                                  *
+* Web: http://quickprof.sourceforge.net                                 *
+*                                                                       *
+* This library is free software; you can redistribute it and/or         *
+* modify it under the terms of EITHER:                                  *
+*   (1) The GNU Lesser General Public License as published by the Free  *
+*       Software Foundation; either version 2.1 of the License, or (at  *
+*       your option) any later version. The text of the GNU Lesser      *
+*       General Public License is included with this library in the     *
+*       file license-LGPL.txt.                                          *
+*   (2) The BSD-style license that is included with this library in     *
+*       the file license-BSD.txt.                                       *
+*                                                                       *
+* This library is distributed in the hope that it will be useful,       *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
+* license-LGPL.txt and license-BSD.txt for more details.                *
+************************************************************************/
+
+// Please visit the project website (http://quickprof.sourceforge.net) 
+// for usage instructions.
+
+// Credits: The Clock class was inspired by the Timer classes in 
+// Ogre (www.ogre3d.org).
+
+
+//#define USE_QUICKPROF 1
+
+#ifdef USE_QUICKPROF
+
+#ifndef QUICK_PROF_H
+#define QUICK_PROF_H
+
+#include <iostream>
+#include <fstream>
+#include <string>
+#include <map>
+
+#ifdef __PPU__
+#include <sys/sys_time.h>
+#include <stdio.h>
+typedef uint64_t __int64;
+#endif
+
+#if defined(WIN32) || defined(_WIN32)
+	#define USE_WINDOWS_TIMERS
+	#include <windows.h>
+	#include <time.h>
+#else
+	#include <sys/time.h>
+#endif
+
+#define mymin(a,b) (a > b ? a : b)
+namespace hidden
+{
+	/// A simple data structure representing a single timed block 
+	/// of code.
+	struct ProfileBlock
+	{
+		ProfileBlock()
+		{
+			currentBlockStartMicroseconds = 0;
+			currentCycleTotalMicroseconds = 0;
+			lastCycleTotalMicroseconds = 0;
+			totalMicroseconds = 0;
+		}
+
+		/// The starting time (in us) of the current block update.
+		unsigned long int currentBlockStartMicroseconds;
+
+		/// The accumulated time (in us) spent in this block during the 
+		/// current profiling cycle.
+		unsigned long int currentCycleTotalMicroseconds;
+
+		/// The accumulated time (in us) spent in this block during the 
+		/// past profiling cycle.
+		unsigned long int lastCycleTotalMicroseconds;
+
+		/// The total accumulated time (in us) spent in this block.
+		unsigned long int totalMicroseconds;
+	};
+
+	class Clock
+	{
+	public:
+		Clock()
+		{
+#ifdef USE_WINDOWS_TIMERS
+			QueryPerformanceFrequency(&mClockFrequency);
+#endif
+			reset();
+		}
+
+		~Clock()
+		{
+		}
+
+		/// Resets the initial reference time.
+		void reset()
+		{
+#ifdef USE_WINDOWS_TIMERS
+			QueryPerformanceCounter(&mStartTime);
+			mStartTick = GetTickCount();
+			mPrevElapsedTime = 0;
+#else
+#ifdef __PPU__
+
+	typedef uint64_t __int64;
+	typedef __int64  ClockSize;
+	ClockSize newTime;
+	__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
+	mStartTime = newTime;
+#else
+			gettimeofday(&mStartTime, NULL);
+#endif
+
+#endif
+		}
+
+		/// Returns the time in ms since the last call to reset or since 
+		/// the Clock was created.
+		unsigned long int getTimeMilliseconds()
+		{
+#ifdef USE_WINDOWS_TIMERS
+			LARGE_INTEGER currentTime;
+			QueryPerformanceCounter(&currentTime);
+			LONGLONG elapsedTime = currentTime.QuadPart - 
+				mStartTime.QuadPart;
+
+			// Compute the number of millisecond ticks elapsed.
+			unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / 
+				mClockFrequency.QuadPart);
+
+			// Check for unexpected leaps in the Win32 performance counter.  
+			// (This is caused by unexpected data across the PCI to ISA 
+			// bridge, aka south bridge.  See Microsoft KB274323.)
+			unsigned long elapsedTicks = GetTickCount() - mStartTick;
+			signed long msecOff = (signed long)(msecTicks - elapsedTicks);
+			if (msecOff < -100 || msecOff > 100)
+			{
+				// Adjust the starting time forwards.
+				LONGLONG msecAdjustment = mymin(msecOff * 
+					mClockFrequency.QuadPart / 1000, elapsedTime - 
+					mPrevElapsedTime);
+				mStartTime.QuadPart += msecAdjustment;
+				elapsedTime -= msecAdjustment;
+
+				// Recompute the number of millisecond ticks elapsed.
+				msecTicks = (unsigned long)(1000 * elapsedTime / 
+					mClockFrequency.QuadPart);
+			}
+
+			// Store the current elapsed time for adjustments next time.
+			mPrevElapsedTime = elapsedTime;
+
+			return msecTicks;
+#else
+			
+#ifdef __PPU__
+	__int64 freq=sys_time_get_timebase_frequency();
+	 double dFreq=((double) freq) / 1000.0;
+	typedef uint64_t __int64;
+	typedef __int64  ClockSize;
+	ClockSize newTime;
+	__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
+	
+	return (newTime-mStartTime) / dFreq;
+#else
+
+			struct timeval currentTime;
+			gettimeofday(&currentTime, NULL);
+			return (currentTime.tv_sec - mStartTime.tv_sec) * 1000 + 
+				(currentTime.tv_usec - mStartTime.tv_usec) / 1000;
+#endif //__PPU__
+#endif
+		}
+
+		/// Returns the time in us since the last call to reset or since 
+		/// the Clock was created.
+		unsigned long int getTimeMicroseconds()
+		{
+#ifdef USE_WINDOWS_TIMERS
+			LARGE_INTEGER currentTime;
+			QueryPerformanceCounter(&currentTime);
+			LONGLONG elapsedTime = currentTime.QuadPart - 
+				mStartTime.QuadPart;
+
+			// Compute the number of millisecond ticks elapsed.
+			unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / 
+				mClockFrequency.QuadPart);
+
+			// Check for unexpected leaps in the Win32 performance counter.  
+			// (This is caused by unexpected data across the PCI to ISA 
+			// bridge, aka south bridge.  See Microsoft KB274323.)
+			unsigned long elapsedTicks = GetTickCount() - mStartTick;
+			signed long msecOff = (signed long)(msecTicks - elapsedTicks);
+			if (msecOff < -100 || msecOff > 100)
+			{
+				// Adjust the starting time forwards.
+				LONGLONG msecAdjustment = mymin(msecOff * 
+					mClockFrequency.QuadPart / 1000, elapsedTime - 
+					mPrevElapsedTime);
+				mStartTime.QuadPart += msecAdjustment;
+				elapsedTime -= msecAdjustment;
+			}
+
+			// Store the current elapsed time for adjustments next time.
+			mPrevElapsedTime = elapsedTime;
+
+			// Convert to microseconds.
+			unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime / 
+				mClockFrequency.QuadPart);
+
+			return usecTicks;
+#else
+
+#ifdef __PPU__
+	__int64 freq=sys_time_get_timebase_frequency();
+	 double dFreq=((double) freq)/ 1000000.0;
+	typedef uint64_t __int64;
+	typedef __int64  ClockSize;
+	ClockSize newTime;
+	__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
+	
+	return (newTime-mStartTime) / dFreq;
+#else
+
+			struct timeval currentTime;
+			gettimeofday(&currentTime, NULL);
+			return (currentTime.tv_sec - mStartTime.tv_sec) * 1000000 + 
+				(currentTime.tv_usec - mStartTime.tv_usec);
+#endif//__PPU__
+#endif 
+		}
+
+	private:
+#ifdef USE_WINDOWS_TIMERS
+		LARGE_INTEGER mClockFrequency;
+		DWORD mStartTick;
+		LONGLONG mPrevElapsedTime;
+		LARGE_INTEGER mStartTime;
+#else
+#ifdef __PPU__
+		uint64_t	mStartTime;
+#else
+		struct timeval mStartTime;
+#endif
+#endif //__PPU__
+
+	};
+};
+
+/// A static class that manages timing for a set of profiling blocks.
+class Profiler
+{
+public:
+	/// A set of ways to retrieve block timing data.
+	enum BlockTimingMethod
+	{
+		/// The total time spent in the block (in seconds) since the 
+		/// profiler was initialized.
+		BLOCK_TOTAL_SECONDS,
+
+		/// The total time spent in the block (in ms) since the 
+		/// profiler was initialized.
+		BLOCK_TOTAL_MILLISECONDS,
+
+		/// The total time spent in the block (in us) since the 
+		/// profiler was initialized.
+		BLOCK_TOTAL_MICROSECONDS,
+
+		/// The total time spent in the block, as a % of the total 
+		/// elapsed time since the profiler was initialized.
+		BLOCK_TOTAL_PERCENT,
+
+		/// The time spent in the block (in seconds) in the most recent 
+		/// profiling cycle.
+		BLOCK_CYCLE_SECONDS,
+
+		/// The time spent in the block (in ms) in the most recent 
+		/// profiling cycle.
+		BLOCK_CYCLE_MILLISECONDS,
+
+		/// The time spent in the block (in us) in the most recent 
+		/// profiling cycle.
+		BLOCK_CYCLE_MICROSECONDS,
+
+		/// The time spent in the block (in seconds) in the most recent 
+		/// profiling cycle, as a % of the total cycle time.
+		BLOCK_CYCLE_PERCENT
+	};
+
+	/// Initializes the profiler.  This must be called first.  If this is 
+	/// never called, the profiler is effectively disabled; all other 
+	/// functions will return immediately.  The first parameter 
+	/// is the name of an output data file; if this string is not empty, 
+	/// data will be saved on every profiling cycle; if this string is 
+	/// empty, no data will be saved to a file.  The second parameter 
+	/// determines which timing method is used when printing data to the 
+	/// output file.
+	inline static void init(const std::string outputFilename="", 
+		BlockTimingMethod outputMethod=BLOCK_CYCLE_MILLISECONDS);
+
+	/// Cleans up allocated memory.
+	inline static void destroy();
+
+	/// Begins timing the named block of code.
+	inline static void beginBlock(const std::string& name);
+
+	/// Updates the accumulated time spent in the named block by adding 
+	/// the elapsed time since the last call to startBlock for this block 
+	/// name.
+	inline static void endBlock(const std::string& name);
+
+	/// Returns the time spent in the named block according to the 
+	/// given timing method.  See comments on BlockTimingMethod for details.
+	inline static double getBlockTime(const std::string& name, 
+		BlockTimingMethod method=BLOCK_CYCLE_MILLISECONDS);
+
+	/// Defines the end of a profiling cycle.  Use this regularly if you 
+	/// want to generate detailed timing information.  This must not be 
+	/// called within a timing block.
+	inline static void endProfilingCycle();
+
+	/// A helper function that creates a string of statistics for 
+	/// each timing block.  This is mainly for printing an overall 
+	/// summary to the command line.
+	inline static std::string createStatsString(
+		BlockTimingMethod method=BLOCK_TOTAL_PERCENT);
+
+//private:
+	inline Profiler();
+
+	inline ~Profiler();
+
+	/// Prints an error message to standard output.
+	inline static void printError(const std::string& msg)
+	{
+		std::cout << "[QuickProf error] " << msg << std::endl;
+	}
+
+	/// Determines whether the profiler is enabled.
+	static bool mEnabled;
+
+	/// The clock used to time profile blocks.
+	static hidden::Clock mClock;
+
+	/// The starting time (in us) of the current profiling cycle.
+	static unsigned long int mCurrentCycleStartMicroseconds;
+
+	/// The duration (in us) of the most recent profiling cycle.
+	static unsigned long int mLastCycleDurationMicroseconds;
+
+	/// Internal map of named profile blocks.
+	static std::map<std::string, hidden::ProfileBlock*> mProfileBlocks;
+
+	/// The data file used if this feature is enabled in 'init.'
+	static std::ofstream mOutputFile;
+
+	/// Tracks whether we have begun print data to the output file.
+	static bool mFirstFileOutput;
+
+	/// The method used when printing timing data to an output file.
+	static BlockTimingMethod mFileOutputMethod;
+
+	/// The number of the current profiling cycle.
+	static unsigned long int mCycleNumber;
+};
+
+
+Profiler::Profiler()
+{
+	// This never gets called because a Profiler instance is never 
+	// created.
+}
+
+Profiler::~Profiler()
+{
+	// This never gets called because a Profiler instance is never 
+	// created.
+}
+
+void Profiler::init(const std::string outputFilename, 
+	BlockTimingMethod outputMethod)
+{
+	mEnabled = true;
+	
+	if (!outputFilename.empty())
+	{
+		mOutputFile.open(outputFilename.c_str());
+	}
+
+	mFileOutputMethod = outputMethod;
+
+	mClock.reset();
+
+	// Set the start time for the first cycle.
+	mCurrentCycleStartMicroseconds = mClock.getTimeMicroseconds();
+}
+
+void Profiler::destroy()
+{
+	if (!mEnabled)
+	{
+		return;
+	}
+
+	if (mOutputFile.is_open())
+	{
+		mOutputFile.close();
+	}
+
+	// Destroy all ProfileBlocks.
+	while (!mProfileBlocks.empty())
+	{
+		delete (*mProfileBlocks.begin()).second;
+		mProfileBlocks.erase(mProfileBlocks.begin());
+	}
+}
+
+void Profiler::beginBlock(const std::string& name)
+{
+	if (!mEnabled)
+	{
+		return;
+	}
+
+	if (name.empty())
+	{
+		printError("Cannot allow unnamed profile blocks.");
+		return;
+	}
+
+	hidden::ProfileBlock* block = mProfileBlocks[name];
+
+	if (!block)
+	{
+		// Create a new ProfileBlock.
+		mProfileBlocks[name] = new hidden::ProfileBlock();
+		block = mProfileBlocks[name];
+	}
+
+	// We do this at the end to get more accurate results.
+	block->currentBlockStartMicroseconds = mClock.getTimeMicroseconds();
+}
+
+void Profiler::endBlock(const std::string& name)
+{
+	if (!mEnabled)
+	{
+		return;
+	}
+
+	// We do this at the beginning to get more accurate results.
+	unsigned long int endTick = mClock.getTimeMicroseconds();
+
+	hidden::ProfileBlock* block = mProfileBlocks[name];
+
+	if (!block)
+	{
+		// The named block does not exist.  Print an error.
+		printError("The profile block named '" + name + 
+			"' does not exist.");
+		return;
+	}
+
+	unsigned long int blockDuration = endTick - 
+		block->currentBlockStartMicroseconds;
+	block->currentCycleTotalMicroseconds += blockDuration;
+	block->totalMicroseconds += blockDuration;
+}
+
+double Profiler::getBlockTime(const std::string& name, 
+	BlockTimingMethod method)
+{
+	if (!mEnabled)
+	{
+		return 0;
+	}
+
+	hidden::ProfileBlock* block = mProfileBlocks[name];
+
+	if (!block)
+	{
+		// The named block does not exist.  Print an error.
+		printError("The profile block named '" + name + 
+			"' does not exist.");
+		return 0;
+	}
+
+	double result = 0;
+
+	switch(method)
+	{
+		case BLOCK_TOTAL_SECONDS:
+			result = (double)block->totalMicroseconds * (double)0.000001;
+			break;
+		case BLOCK_TOTAL_MILLISECONDS:
+			result = (double)block->totalMicroseconds * (double)0.001;
+			break;
+		case BLOCK_TOTAL_MICROSECONDS:
+			result = (double)block->totalMicroseconds;
+			break;
+		case BLOCK_TOTAL_PERCENT:
+		{
+			double timeSinceInit = (double)mClock.getTimeMicroseconds();
+			if (timeSinceInit <= 0)
+			{
+				result = 0;
+			}
+			else
+			{
+				result = 100.0 * (double)block->totalMicroseconds / 
+					timeSinceInit;
+			}
+			break;
+		}
+		case BLOCK_CYCLE_SECONDS:
+			result = (double)block->lastCycleTotalMicroseconds * 
+				(double)0.000001;
+			break;
+		case BLOCK_CYCLE_MILLISECONDS:
+			result = (double)block->lastCycleTotalMicroseconds * 
+				(double)0.001;
+			break;
+		case BLOCK_CYCLE_MICROSECONDS:
+			result = (double)block->lastCycleTotalMicroseconds;
+			break;
+		case BLOCK_CYCLE_PERCENT:
+		{
+			if (0 == mLastCycleDurationMicroseconds)
+			{
+				// We have not yet finished a cycle, so just return zero 
+				// percent to avoid a divide by zero error.
+				result = 0;
+			}
+			else
+			{
+				result = 100.0 * (double)block->lastCycleTotalMicroseconds / 
+					mLastCycleDurationMicroseconds;
+			}
+			break;
+		}
+		default:
+			break;
+	}
+
+	return result;
+}
+
+void Profiler::endProfilingCycle()
+{
+	if (!mEnabled)
+	{
+		return;
+	}
+
+	// Store the duration of the cycle that just finished.
+	mLastCycleDurationMicroseconds = mClock.getTimeMicroseconds() - 
+		mCurrentCycleStartMicroseconds;
+
+	// For each block, update data for the cycle that just finished.
+	std::map<std::string, hidden::ProfileBlock*>::iterator iter;
+	for (iter = mProfileBlocks.begin(); iter != mProfileBlocks.end(); ++iter)
+	{
+		hidden::ProfileBlock* block = (*iter).second;
+		block->lastCycleTotalMicroseconds = 
+			block->currentCycleTotalMicroseconds;
+		block->currentCycleTotalMicroseconds = 0;
+	}
+
+	if (mOutputFile.is_open())
+	{
+		// Print data to the output file.
+		if (mFirstFileOutput)
+		{
+			// On the first iteration, print a header line that shows the 
+			// names of each profiling block.
+			mOutputFile << "#cycle; ";
+
+			for (iter = mProfileBlocks.begin(); iter != mProfileBlocks.end(); 
+				++iter)
+			{
+				mOutputFile << (*iter).first << "; ";
+			}
+
+			mOutputFile << std::endl;
+			mFirstFileOutput = false;
+		}
+
+		mOutputFile << mCycleNumber << "; ";
+
+		for (iter = mProfileBlocks.begin(); iter != mProfileBlocks.end(); 
+			++iter)
+		{
+			mOutputFile << getBlockTime((*iter).first, mFileOutputMethod) 
+				<< "; ";
+		}
+
+		mOutputFile << std::endl;
+	}
+
+	++mCycleNumber;
+	mCurrentCycleStartMicroseconds = mClock.getTimeMicroseconds();
+}
+
+std::string Profiler::createStatsString(BlockTimingMethod method)
+{
+	if (!mEnabled)
+	{
+		return "";
+	}
+
+	std::string s;
+	std::string suffix;
+
+	switch(method)
+	{
+		case BLOCK_TOTAL_SECONDS:
+			suffix = "s";
+			break;
+		case BLOCK_TOTAL_MILLISECONDS:
+			suffix = "ms";
+			break;
+		case BLOCK_TOTAL_MICROSECONDS:
+			suffix = "us";
+			break;
+		case BLOCK_TOTAL_PERCENT:
+		{
+			suffix = "%";
+			break;
+		}
+		case BLOCK_CYCLE_SECONDS:
+			suffix = "s";
+			break;
+		case BLOCK_CYCLE_MILLISECONDS:
+			suffix = "ms";
+			break;
+		case BLOCK_CYCLE_MICROSECONDS:
+			suffix = "us";
+			break;
+		case BLOCK_CYCLE_PERCENT:
+		{
+			suffix = "%";
+			break;
+		}
+		default:
+			break;
+	}
+
+	std::map<std::string, hidden::ProfileBlock*>::iterator iter;
+	for (iter = mProfileBlocks.begin(); iter != mProfileBlocks.end(); ++iter)
+	{
+		if (iter != mProfileBlocks.begin())
+		{
+			s += "\n";
+		}
+
+		char blockTime[64];
+		sprintf(blockTime, "%lf", getBlockTime((*iter).first, method));
+
+		s += (*iter).first;
+		s += ": ";
+		s += blockTime;
+		s += " ";
+		s += suffix;
+	}
+
+	return s;
+}
+
+#endif
+
+#endif //USE_QUICKPROF