First stage in refactoring Bullet: moved Bullet Collision and Dynamics and LinearMath into src folder, and all files in Collision Detection and Dynamics have bt prefix.

Made all buildsystems to work again (jam, msvc, cmake)

src/LinearMath/CMakeLists.txt

@@ -0,0 +1,9 @@
+
+INCLUDE_DIRECTORIES(
+${BULLET_PHYSICS_SOURCE_DIR}/src }
+)
+
+ADD_LIBRARY(LibLinearMath
+GenQuickprof.cpp
+)
+

src/LinearMath/GenAabbUtil2.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 "LinearMath/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
+

src/LinearMath/GenIDebugDraw.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 "LinearMath/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
+

src/LinearMath/GenList.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
+
+
+

src/LinearMath/GenMinMax.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

src/LinearMath/GenQuickprof.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 "LinearMath/GenQuickprof.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

src/LinearMath/GenQuickprof.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

src/LinearMath/GenRandom.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
+

src/LinearMath/Jamfile

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

src/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 "LinearMath/SimdScalar.h"
+
+#include "LinearMath/SimdVector3.h"
+#include "LinearMath/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

src/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

src/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 "LinearMath/SimdVector3.h"
+
+typedef SimdVector3 SimdPoint3;
+
+#endif

src/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 "LinearMath/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

src/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 "LinearMath/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
+
+
+

src/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
+
+		#if defined(__MINGW32__) || defined(__CYGWIN__)
+			#define SIMD_FORCE_INLINE inline
+		#else
+			#pragma warning(disable:4530)
+			#pragma warning(disable:4996)
+			#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
+	
+	//non-windows systems
+
+		#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

src/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 "LinearMath/SimdVector3.h"
+#include "LinearMath/SimdMatrix3x3.h"
+
+
+
+class SimdTransform {
+	
+
+public:
+	
+	enum { 
+		TRANSLATION = 0x01,
+		ROTATION    = 0x02,
+		RIGID       = TRANSLATION | ROTATION,  
+		SCALING     = 0x04,
+		LINEAR      = ROTATION | SCALING,
+		AFFINE      = TRANSLATION | LINEAR
+	};
+
+	SimdTransform() {}
+
+	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
+
+
+
+
+

src/LinearMath/SimdTransformUtil.h

@@ -0,0 +1,143 @@
+/*
+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 "LinearMath/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 SimdVector3 SimdAabbSupport(const SimdVector3& halfExtents,const SimdVector3& supportDir)
+{
+	return SimdVector3(supportDir.x() < SimdScalar(0.0f) ? -halfExtents.x() : halfExtents.x(),
+      supportDir.y() < SimdScalar(0.0f) ? -halfExtents.y() : halfExtents.y(),
+      supportDir.z() < SimdScalar(0.0f) ? -halfExtents.z() : halfExtents.z()); 
+}
+
+
+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
+

src/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