update Bullet physics sdk to latest trunk/version 2.78

add PhysicsConstraints.exportBulletFile(char* fileName) python command
I'll be checking the bf-committers mailing list, in case this commit broke stuff
scons needs to be updated, I'll do that in a second.

1 files changed, 641 insertions, 488 deletions

diff --git a/extern/bullet2/src/LinearMath/btMatrix3x3.h b/extern/bullet2/src/LinearMath/btMatrix3x3.h
index e45afc3c055..d0234a04369 100644
--- a/extern/bullet2/src/LinearMath/btMatrix3x3.h
+++ b/extern/bullet2/src/LinearMath/btMatrix3x3.h
@@ -13,162 +13,179 @@ subject to the following restrictions:
 */
 
 
-#ifndef btMatrix3x3_H
-#define btMatrix3x3_H
-
-#include "btScalar.h"
+#ifndef	BT_MATRIX3x3_H
+#define BT_MATRIX3x3_H
 
 #include "btVector3.h"
 #include "btQuaternion.h"
 
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btMatrix3x3Data	btMatrix3x3DoubleData 
+#else
+#define btMatrix3x3Data	btMatrix3x3FloatData
+#endif //BT_USE_DOUBLE_PRECISION
 
 
 /**@brief The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3.
- * Make sure to only include a pure orthogonal matrix without scaling. */
+* Make sure to only include a pure orthogonal matrix without scaling. */
 class btMatrix3x3 {
-	public:
-  /** @brief No initializaion constructor */
-		btMatrix3x3 () {}
-		
-//		explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); }
-		
-  /**@brief Constructor from Quaternion */
-		explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); }
-		/*
-		template <typename btScalar>
-		Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
-		{ 
-			setEulerYPR(yaw, pitch, roll);
-		}
-		*/
-  /** @brief Constructor with row major formatting */
-		btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz,
-				  const btScalar& yx, const btScalar& yy, const btScalar& yz,
-				  const btScalar& zx, const btScalar& zy, const btScalar& zz)
-		{ 
-			setValue(xx, xy, xz, 
-					 yx, yy, yz, 
-					 zx, zy, zz);
-		}
-  /** @brief Copy constructor */
-		SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other)
-		{
-			m_el[0] = other.m_el[0];
-			m_el[1] = other.m_el[1];
-			m_el[2] = other.m_el[2];
-		}
-  /** @brief Assignment Operator */
-		SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other)
-		{
-			m_el[0] = other.m_el[0];
-			m_el[1] = other.m_el[1];
-			m_el[2] = other.m_el[2];
-			return *this;
-		}
 
-  /** @brief Get a column of the matrix as a vector 
-   *  @param i Column number 0 indexed */
-		SIMD_FORCE_INLINE btVector3 getColumn(int i) const
-		{
-			return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
-		}
-		
+	///Data storage for the matrix, each vector is a row of the matrix
+	btVector3 m_el[3];
 
-  /** @brief Get a row of the matrix as a vector 
-   *  @param i Row number 0 indexed */
-		SIMD_FORCE_INLINE const btVector3& getRow(int i) const
-		{
-			btFullAssert(0 <= i && i < 3);
-			return m_el[i];
-		}
+public:
+	/** @brief No initializaion constructor */
+	btMatrix3x3 () {}
 
-  /** @brief Get a mutable reference to a row of the matrix as a vector 
-   *  @param i Row number 0 indexed */
-		SIMD_FORCE_INLINE btVector3&  operator[](int i)
-		{ 
-			btFullAssert(0 <= i && i < 3);
-			return m_el[i]; 
-		}
-		
-  /** @brief Get a const reference to a row of the matrix as a vector 
-   *  @param i Row number 0 indexed */
-		SIMD_FORCE_INLINE const btVector3& operator[](int i) const
-		{
-			btFullAssert(0 <= i && i < 3);
-			return m_el[i]; 
-		}
-		
-  /** @brief Multiply by the target matrix on the right
-   *  @param m Rotation matrix to be applied 
-   * Equivilant to this = this * m */
-		btMatrix3x3& operator*=(const btMatrix3x3& m); 
-		
-  /** @brief Set from a carray of btScalars 
-   *  @param m A pointer to the beginning of an array of 9 btScalars */
+	//		explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); }
+
+	/**@brief Constructor from Quaternion */
+	explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); }
+	/*
+	template <typename btScalar>
+	Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
+	{ 
+	setEulerYPR(yaw, pitch, roll);
+	}
+	*/
+	/** @brief Constructor with row major formatting */
+	btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz,
+		const btScalar& yx, const btScalar& yy, const btScalar& yz,
+		const btScalar& zx, const btScalar& zy, const btScalar& zz)
+	{ 
+		setValue(xx, xy, xz, 
+			yx, yy, yz, 
+			zx, zy, zz);
+	}
+	/** @brief Copy constructor */
+	SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other)
+	{
+		m_el[0] = other.m_el[0];
+		m_el[1] = other.m_el[1];
+		m_el[2] = other.m_el[2];
+	}
+	/** @brief Assignment Operator */
+	SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other)
+	{
+		m_el[0] = other.m_el[0];
+		m_el[1] = other.m_el[1];
+		m_el[2] = other.m_el[2];
+		return *this;
+	}
+
+	/** @brief Get a column of the matrix as a vector 
+	*  @param i Column number 0 indexed */
+	SIMD_FORCE_INLINE btVector3 getColumn(int i) const
+	{
+		return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
+	}
+
+
+	/** @brief Get a row of the matrix as a vector 
+	*  @param i Row number 0 indexed */
+	SIMD_FORCE_INLINE const btVector3& getRow(int i) const
+	{
+		btFullAssert(0 <= i && i < 3);
+		return m_el[i];
+	}
+
+	/** @brief Get a mutable reference to a row of the matrix as a vector 
+	*  @param i Row number 0 indexed */
+	SIMD_FORCE_INLINE btVector3&  operator[](int i)
+	{ 
+		btFullAssert(0 <= i && i < 3);
+		return m_el[i]; 
+	}
+
+	/** @brief Get a const reference to a row of the matrix as a vector 
+	*  @param i Row number 0 indexed */
+	SIMD_FORCE_INLINE const btVector3& operator[](int i) const
+	{
+		btFullAssert(0 <= i && i < 3);
+		return m_el[i]; 
+	}
+
+	/** @brief Multiply by the target matrix on the right
+	*  @param m Rotation matrix to be applied 
+	* Equivilant to this = this * m */
+	btMatrix3x3& operator*=(const btMatrix3x3& m); 
+
+	/** @brief Adds by the target matrix on the right
+	*  @param m matrix to be applied 
+	* Equivilant to this = this + m */
+	btMatrix3x3& operator+=(const btMatrix3x3& m); 
+
+	/** @brief Substractss by the target matrix on the right
+	*  @param m matrix to be applied 
+	* Equivilant to this = this - m */
+	btMatrix3x3& operator-=(const btMatrix3x3& m); 
+
+	/** @brief Set from the rotational part of a 4x4 OpenGL matrix
+	*  @param m A pointer to the beginning of the array of scalars*/
 	void setFromOpenGLSubMatrix(const btScalar *m)
-		{
-			m_el[0].setValue(m[0],m[4],m[8]);
-			m_el[1].setValue(m[1],m[5],m[9]);
-			m_el[2].setValue(m[2],m[6],m[10]);
+	{
+		m_el[0].setValue(m[0],m[4],m[8]);
+		m_el[1].setValue(m[1],m[5],m[9]);
+		m_el[2].setValue(m[2],m[6],m[10]);
 
-		}
-  /** @brief Set the values of the matrix explicitly (row major)
-   *  @param xx Top left
-   *  @param xy Top Middle
-   *  @param xz Top Right
-   *  @param yx Middle Left
-   *  @param yy Middle Middle
-   *  @param yz Middle Right
-   *  @param zx Bottom Left
-   *  @param zy Bottom Middle
-   *  @param zz Bottom Right*/
-		void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, 
-					  const btScalar& yx, const btScalar& yy, const btScalar& yz, 
-					  const btScalar& zx, const btScalar& zy, const btScalar& zz)
-		{
-			m_el[0].setValue(xx,xy,xz);
-			m_el[1].setValue(yx,yy,yz);
-			m_el[2].setValue(zx,zy,zz);
-		}
+	}
+	/** @brief Set the values of the matrix explicitly (row major)
+	*  @param xx Top left
+	*  @param xy Top Middle
+	*  @param xz Top Right
+	*  @param yx Middle Left
+	*  @param yy Middle Middle
+	*  @param yz Middle Right
+	*  @param zx Bottom Left
+	*  @param zy Bottom Middle
+	*  @param zz Bottom Right*/
+	void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, 
+		const btScalar& yx, const btScalar& yy, const btScalar& yz, 
+		const btScalar& zx, const btScalar& zy, const btScalar& zz)
+	{
+		m_el[0].setValue(xx,xy,xz);
+		m_el[1].setValue(yx,yy,yz);
+		m_el[2].setValue(zx,zy,zz);
+	}
+
+	/** @brief Set the matrix from a quaternion
+	*  @param q The Quaternion to match */  
+	void setRotation(const btQuaternion& q) 
+	{
+		btScalar d = q.length2();
+		btFullAssert(d != btScalar(0.0));
+		btScalar s = btScalar(2.0) / d;
+		btScalar xs = q.x() * s,   ys = q.y() * s,   zs = q.z() * s;
+		btScalar wx = q.w() * xs,  wy = q.w() * ys,  wz = q.w() * zs;
+		btScalar xx = q.x() * xs,  xy = q.x() * ys,  xz = q.x() * zs;
+		btScalar yy = q.y() * ys,  yz = q.y() * zs,  zz = q.z() * zs;
+		setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
+			xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
+			xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
+	}
 
-  /** @brief Set the matrix from a quaternion
-   *  @param q The Quaternion to match */  
-		void setRotation(const btQuaternion& q) 
-		{
-			btScalar d = q.length2();
-			btFullAssert(d != btScalar(0.0));
-			btScalar s = btScalar(2.0) / d;
-			btScalar xs = q.x() * s,   ys = q.y() * s,   zs = q.z() * s;
-			btScalar wx = q.w() * xs,  wy = q.w() * ys,  wz = q.w() * zs;
-			btScalar xx = q.x() * xs,  xy = q.x() * ys,  xz = q.x() * zs;
-			btScalar yy = q.y() * ys,  yz = q.y() * zs,  zz = q.z() * zs;
-			setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
-					 xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
-					 xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
-		}
-		
-
-  /** @brief Set the matrix from euler angles using YPR around YXZ respectively
-   *  @param yaw Yaw about Y axis
-   *  @param pitch Pitch about X axis
-   *  @param roll Roll about Z axis 
-   */
-		void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) 
-		{
-			setEulerZYX(roll, pitch, yaw);
-		}
+
+	/** @brief Set the matrix from euler angles using YPR around YXZ respectively
+	*  @param yaw Yaw about Y axis
+	*  @param pitch Pitch about X axis
+	*  @param roll Roll about Z axis 
+	*/
+	void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) 
+	{
+		setEulerZYX(roll, pitch, yaw);
+	}
 
 	/** @brief Set the matrix from euler angles YPR around ZYX axes
-	 * @param eulerX Roll about X axis
-         * @param eulerY Pitch around Y axis
-         * @param eulerZ Yaw aboud Z axis
-         * 
-	 * 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
-	 **/
+	* @param eulerX Roll about X axis
+	* @param eulerY Pitch around Y axis
+	* @param eulerZ Yaw aboud Z axis
+	* 
+	* 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(btScalar eulerX,btScalar eulerY,btScalar eulerZ) { 
-  ///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code
+		///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code
 		btScalar ci ( btCos(eulerX)); 
 		btScalar cj ( btCos(eulerY)); 
 		btScalar ch ( btCos(eulerZ)); 
@@ -179,227 +196,233 @@ class btMatrix3x3 {
 		btScalar cs = ci * sh; 
 		btScalar sc = si * ch; 
 		btScalar 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);
+			cj * sh, sj * ss + cc, sj * cs - sc, 
+			-sj,      cj * si,      cj * ci);
 	}
 
-  /**@brief Set the matrix to the identity */
-		void setIdentity()
-		{ 
-			setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0), 
-					 btScalar(0.0), btScalar(1.0), btScalar(0.0), 
-					 btScalar(0.0), btScalar(0.0), btScalar(1.0)); 
-		}
+	/**@brief Set the matrix to the identity */
+	void setIdentity()
+	{ 
+		setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0), 
+			btScalar(0.0), btScalar(1.0), btScalar(0.0), 
+			btScalar(0.0), btScalar(0.0), btScalar(1.0)); 
+	}
+
+	static const btMatrix3x3&	getIdentity()
+	{
+		static const btMatrix3x3 identityMatrix(btScalar(1.0), btScalar(0.0), btScalar(0.0), 
+			btScalar(0.0), btScalar(1.0), btScalar(0.0), 
+			btScalar(0.0), btScalar(0.0), btScalar(1.0));
+		return identityMatrix;
+	}
+
+	/**@brief Fill the rotational part of an OpenGL matrix and clear the shear/perspective
+	* @param m The array to be filled */
+	void getOpenGLSubMatrix(btScalar *m) const 
+	{
+		m[0]  = btScalar(m_el[0].x()); 
+		m[1]  = btScalar(m_el[1].x());
+		m[2]  = btScalar(m_el[2].x());
+		m[3]  = btScalar(0.0); 
+		m[4]  = btScalar(m_el[0].y());
+		m[5]  = btScalar(m_el[1].y());
+		m[6]  = btScalar(m_el[2].y());
+		m[7]  = btScalar(0.0); 
+		m[8]  = btScalar(m_el[0].z()); 
+		m[9]  = btScalar(m_el[1].z());
+		m[10] = btScalar(m_el[2].z());
+		m[11] = btScalar(0.0); 
+	}
+
+	/**@brief Get the matrix represented as a quaternion 
+	* @param q The quaternion which will be set */
+	void getRotation(btQuaternion& q) const
+	{
+		btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
+		btScalar temp[4];
 
-		static const btMatrix3x3&	getIdentity()
+		if (trace > btScalar(0.0)) 
+		{
+			btScalar s = btSqrt(trace + btScalar(1.0));
+			temp[3]=(s * btScalar(0.5));
+			s = btScalar(0.5) / s;
+
+			temp[0]=((m_el[2].y() - m_el[1].z()) * s);
+			temp[1]=((m_el[0].z() - m_el[2].x()) * s);
+			temp[2]=((m_el[1].x() - m_el[0].y()) * s);
+		} 
+		else 
 		{
-			static const btMatrix3x3 identityMatrix(btScalar(1.0), btScalar(0.0), btScalar(0.0), 
-					 btScalar(0.0), btScalar(1.0), btScalar(0.0), 
-					 btScalar(0.0), btScalar(0.0), btScalar(1.0));
-			return identityMatrix;
+			int i = m_el[0].x() < m_el[1].y() ? 
+				(m_el[1].y() < m_el[2].z() ? 2 : 1) :
+				(m_el[0].x() < m_el[2].z() ? 2 : 0); 
+			int j = (i + 1) % 3;  
+			int k = (i + 2) % 3;
+
+			btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
+			temp[i] = s * btScalar(0.5);
+			s = btScalar(0.5) / s;
+
+			temp[3] = (m_el[k][j] - m_el[j][k]) * s;
+			temp[j] = (m_el[j][i] + m_el[i][j]) * s;
+			temp[k] = (m_el[k][i] + m_el[i][k]) * s;
 		}
+		q.setValue(temp[0],temp[1],temp[2],temp[3]);
+	}
+
+	/**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR
+	* @param yaw Yaw around Y axis
+	* @param pitch Pitch around X axis
+	* @param roll around Z axis */	
+	void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const
+	{
+
+		// first use the normal calculus
+		yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
+		pitch = btScalar(btAsin(-m_el[2].x()));
+		roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
 
-  /**@brief Fill the values of the matrix into a 9 element array 
-   * @param m The array to be filled */
-		void getOpenGLSubMatrix(btScalar *m) const 
+		// on pitch = +/-HalfPI
+		if (btFabs(pitch)==SIMD_HALF_PI)
 		{
-			m[0]  = btScalar(m_el[0].x()); 
-			m[1]  = btScalar(m_el[1].x());
-			m[2]  = btScalar(m_el[2].x());
-			m[3]  = btScalar(0.0); 
-			m[4]  = btScalar(m_el[0].y());
-			m[5]  = btScalar(m_el[1].y());
-			m[6]  = btScalar(m_el[2].y());
-			m[7]  = btScalar(0.0); 
-			m[8]  = btScalar(m_el[0].z()); 
-			m[9]  = btScalar(m_el[1].z());
-			m[10] = btScalar(m_el[2].z());
-			m[11] = btScalar(0.0); 
+			if (yaw>0)
+				yaw-=SIMD_PI;
+			else
+				yaw+=SIMD_PI;
+
+			if (roll>0)
+				roll-=SIMD_PI;
+			else
+				roll+=SIMD_PI;
 		}
+	};
 
-  /**@brief Get the matrix represented as a quaternion 
-   * @param q The quaternion which will be set */
-		void getRotation(btQuaternion& q) const
+
+	/**@brief Get the matrix represented as euler angles around ZYX
+	* @param yaw Yaw around X axis
+	* @param pitch Pitch around Y axis
+	* @param roll around X axis 
+	* @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/	
+	void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const
+	{
+		struct Euler
 		{
-			btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
-			btScalar temp[4];
-			
-			if (trace > btScalar(0.0)) 
+			btScalar yaw;
+			btScalar pitch;
+			btScalar roll;
+		};
+
+		Euler euler_out;
+		Euler euler_out2; //second solution
+		//get the pointer to the raw data
+
+		// Check that pitch is not at a singularity
+		if (btFabs(m_el[2].x()) >= 1)
+		{
+			euler_out.yaw = 0;
+			euler_out2.yaw = 0;
+
+			// From difference of angles formula
+			btScalar delta = btAtan2(m_el[0].x(),m_el[0].z());
+			if (m_el[2].x() > 0)  //gimbal locked up
 			{
-				btScalar s = btSqrt(trace + btScalar(1.0));
-				temp[3]=(s * btScalar(0.5));
-				s = btScalar(0.5) / s;
-				
-				temp[0]=((m_el[2].y() - m_el[1].z()) * s);
-				temp[1]=((m_el[0].z() - m_el[2].x()) * s);
-				temp[2]=((m_el[1].x() - m_el[0].y()) * s);
-			} 
-			else 
+				euler_out.pitch = SIMD_PI / btScalar(2.0);
+				euler_out2.pitch = SIMD_PI / btScalar(2.0);
+				euler_out.roll = euler_out.pitch + delta;
+				euler_out2.roll = euler_out.pitch + delta;
+			}
+			else // gimbal locked down
 			{
-				int i = m_el[0].x() < m_el[1].y() ? 
-					(m_el[1].y() < m_el[2].z() ? 2 : 1) :
-					(m_el[0].x() < m_el[2].z() ? 2 : 0); 
-				int j = (i + 1) % 3;  
-				int k = (i + 2) % 3;
-				
-				btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
-				temp[i] = s * btScalar(0.5);
-				s = btScalar(0.5) / s;
-				
-				temp[3] = (m_el[k][j] - m_el[j][k]) * s;
-				temp[j] = (m_el[j][i] + m_el[i][j]) * s;
-				temp[k] = (m_el[k][i] + m_el[i][k]) * s;
+				euler_out.pitch = -SIMD_PI / btScalar(2.0);
+				euler_out2.pitch = -SIMD_PI / btScalar(2.0);
+				euler_out.roll = -euler_out.pitch + delta;
+				euler_out2.roll = -euler_out.pitch + delta;
 			}
-			q.setValue(temp[0],temp[1],temp[2],temp[3]);
 		}
-
-  /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR
-   * @param yaw Yaw around Y axis
-   * @param pitch Pitch around X axis
-   * @param roll around Z axis */	
-		void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const
+		else
 		{
-			
-			// first use the normal calculus
-			yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
-			pitch = btScalar(btAsin(-m_el[2].x()));
-			roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
-
-			// on pitch = +/-HalfPI
-			if (btFabs(pitch)==SIMD_HALF_PI)
-			{
-				if (yaw>0)
-					yaw-=SIMD_PI;
-				else
-					yaw+=SIMD_PI;
-
-				if (roll>0)
-					roll-=SIMD_PI;
-				else
-					roll+=SIMD_PI;
-			}
-		};
-
+			euler_out.pitch = - btAsin(m_el[2].x());
+			euler_out2.pitch = SIMD_PI - euler_out.pitch;
 
-  /**@brief Get the matrix represented as euler angles around ZYX
-   * @param yaw Yaw around X axis
-   * @param pitch Pitch around Y axis
-   * @param roll around X axis 
-   * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/	
-  void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const
-  {
-    struct Euler{btScalar yaw, pitch, roll;};
-    Euler euler_out;
-    Euler euler_out2; //second solution
-    //get the pointer to the raw data
-    
-    // Check that pitch is not at a singularity
-    if (btFabs(m_el[2].x()) >= 1)
-    {
-      euler_out.yaw = 0;
-      euler_out2.yaw = 0;
-	
-      // From difference of angles formula
-      btScalar delta = btAtan2(m_el[0].x(),m_el[0].z());
-      if (m_el[2].x() > 0)  //gimbal locked up
-      {
-        euler_out.pitch = SIMD_PI / btScalar(2.0);
-        euler_out2.pitch = SIMD_PI / btScalar(2.0);
-        euler_out.roll = euler_out.pitch + delta;
-        euler_out2.roll = euler_out.pitch + delta;
-      }
-      else // gimbal locked down
-      {
-        euler_out.pitch = -SIMD_PI / btScalar(2.0);
-        euler_out2.pitch = -SIMD_PI / btScalar(2.0);
-        euler_out.roll = -euler_out.pitch + delta;
-        euler_out2.roll = -euler_out.pitch + delta;
-      }
-    }
-    else
-    {
-      euler_out.pitch = - btAsin(m_el[2].x());
-      euler_out2.pitch = SIMD_PI - euler_out.pitch;
-	
-      euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch), 
-			       m_el[2].z()/btCos(euler_out.pitch));
-      euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch), 
+			euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch), 
+				m_el[2].z()/btCos(euler_out.pitch));
+			euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch), 
 				m_el[2].z()/btCos(euler_out2.pitch));
-	
-      euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch), 
-			      m_el[0].x()/btCos(euler_out.pitch));
-      euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch), 
-                               m_el[0].x()/btCos(euler_out2.pitch));
-    }
-    
-    if (solution_number == 1)
-    { 
-		yaw = euler_out.yaw; 
-		pitch = euler_out.pitch;
-		roll = euler_out.roll;
-    }
-    else
-    { 
-		yaw = euler_out2.yaw; 
-		pitch = euler_out2.pitch;
-		roll = euler_out2.roll;
-    }
-  }
-
-  /**@brief Create a scaled copy of the matrix 
-   * @param s Scaling vector The elements of the vector will scale each column */
-		
-		btMatrix3x3 scaled(const btVector3& s) const
-		{
-			return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
-									 m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
-									 m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
-		}
 
-  /**@brief Return the determinant of the matrix */
-		btScalar            determinant() const;
-  /**@brief Return the adjoint of the matrix */
-		btMatrix3x3 adjoint() const;
-  /**@brief Return the matrix with all values non negative */
-		btMatrix3x3 absolute() const;
-  /**@brief Return the transpose of the matrix */
-		btMatrix3x3 transpose() const;
-  /**@brief Return the inverse of the matrix */
-		btMatrix3x3 inverse() const; 
-		
-		btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
-		btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;
-		
-		SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const 
-		{
-			return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
+			euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch), 
+				m_el[0].x()/btCos(euler_out.pitch));
+			euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch), 
+				m_el[0].x()/btCos(euler_out2.pitch));
 		}
-		SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const 
-		{
-			return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
+
+		if (solution_number == 1)
+		{ 
+			yaw = euler_out.yaw; 
+			pitch = euler_out.pitch;
+			roll = euler_out.roll;
 		}
-		SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const 
-		{
-			return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
+		else
+		{ 
+			yaw = euler_out2.yaw; 
+			pitch = euler_out2.pitch;
+			roll = euler_out2.roll;
 		}
-		
-
-  /**@brief diagonalizes this matrix by the Jacobi method.
-   * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
-   * coordinate system, i.e., old_this = rot * new_this * rot^T. 
-   * @param threshold See iteration
-   * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied 
-   * by the sum of the absolute values of the diagonal, or when maxSteps have been executed. 
-   * 
-   * Note that this matrix is assumed to be symmetric. 
-   */
-		void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)
+	}
+
+	/**@brief Create a scaled copy of the matrix 
+	* @param s Scaling vector The elements of the vector will scale each column */
+
+	btMatrix3x3 scaled(const btVector3& s) const
+	{
+		return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
+			m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
+			m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
+	}
+
+	/**@brief Return the determinant of the matrix */
+	btScalar            determinant() const;
+	/**@brief Return the adjoint of the matrix */
+	btMatrix3x3 adjoint() const;
+	/**@brief Return the matrix with all values non negative */
+	btMatrix3x3 absolute() const;
+	/**@brief Return the transpose of the matrix */
+	btMatrix3x3 transpose() const;
+	/**@brief Return the inverse of the matrix */
+	btMatrix3x3 inverse() const; 
+
+	btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
+	btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;
+
+	SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const 
+	{
+		return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
+	}
+	SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const 
+	{
+		return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
+	}
+	SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const 
+	{
+		return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
+	}
+
+
+	/**@brief diagonalizes this matrix by the Jacobi method.
+	* @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
+	* coordinate system, i.e., old_this = rot * new_this * rot^T. 
+	* @param threshold See iteration
+	* @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied 
+	* by the sum of the absolute values of the diagonal, or when maxSteps have been executed. 
+	* 
+	* Note that this matrix is assumed to be symmetric. 
+	*/
+	void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)
+	{
+		rot.setIdentity();
+		for (int step = maxSteps; step > 0; step--)
 		{
-		 rot.setIdentity();
-		 for (int step = maxSteps; step > 0; step--)
-		 {
 			// find off-diagonal element [p][q] with largest magnitude
 			int p = 0;
 			int q = 1;
@@ -408,27 +431,27 @@ class btMatrix3x3 {
 			btScalar v = btFabs(m_el[0][2]);
 			if (v > max)
 			{
-			   q = 2;
-			   r = 1;
-			   max = v;
+				q = 2;
+				r = 1;
+				max = v;
 			}
 			v = btFabs(m_el[1][2]);
 			if (v > max)
 			{
-			   p = 1;
-			   q = 2;
-			   r = 0;
-			   max = v;
+				p = 1;
+				q = 2;
+				r = 0;
+				max = v;
 			}
 
 			btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2]));
 			if (max <= t)
 			{
-			   if (max <= SIMD_EPSILON * t)
-			   {
-				  return;
-			   }
-			   step = 1;
+				if (max <= SIMD_EPSILON * t)
+				{
+					return;
+				}
+				step = 1;
 			}
 
 			// compute Jacobi rotation J which leads to a zero for element [p][q] 
@@ -439,17 +462,17 @@ class btMatrix3x3 {
 			btScalar sin;
 			if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON))
 			{
-			   t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2))
-										: 1 / (theta - btSqrt(1 + theta2));
-			   cos = 1 / btSqrt(1 + t * t);
-			   sin = cos * t;
+				t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2))
+					: 1 / (theta - btSqrt(1 + theta2));
+				cos = 1 / btSqrt(1 + t * t);
+				sin = cos * t;
 			}
 			else
 			{
-			   // approximation for large theta-value, i.e., a nearly diagonal matrix
-			   t = 1 / (theta * (2 + btScalar(0.5) / theta2));
-			   cos = 1 - btScalar(0.5) * t * t;
-			   sin = cos * t;
+				// approximation for large theta-value, i.e., a nearly diagonal matrix
+				t = 1 / (theta * (2 + btScalar(0.5) / theta2));
+				cos = 1 - btScalar(0.5) * t * t;
+				sin = cos * t;
 			}
 
 			// apply rotation to matrix (this = J^T * this * J)
@@ -464,155 +487,285 @@ class btMatrix3x3 {
 			// apply rotation to rot (rot = rot * J)
 			for (int i = 0; i < 3; i++)
 			{
-			   btVector3& row = rot[i];
-			   mrp = row[p];
-			   mrq = row[q];
-			   row[p] = cos * mrp - sin * mrq;
-			   row[q] = cos * mrq + sin * mrp;
+				btVector3& row = rot[i];
+				mrp = row[p];
+				mrq = row[q];
+				row[p] = cos * mrp - sin * mrq;
+				row[q] = cos * mrq + sin * mrp;
 			}
-		 }
 		}
+	}
 
 
-		
-	protected:
-  /**@brief Calculate the matrix cofactor 
-   * @param r1 The first row to use for calculating the cofactor
-   * @param c1 The first column to use for calculating the cofactor
-   * @param r1 The second row to use for calculating the cofactor
-   * @param c1 The second column to use for calculating the cofactor
-   * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details
-   */
-		btScalar 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];
-		}
-  ///Data storage for the matrix, each vector is a row of the matrix
-		btVector3 m_el[3];
-	};
-	
-	SIMD_FORCE_INLINE btMatrix3x3& 
-	btMatrix3x3::operator*=(const btMatrix3x3& m)
-	{
-		setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
-				 m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
-				 m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
-		return *this;
-	}
-	
-	SIMD_FORCE_INLINE btScalar 
-	btMatrix3x3::determinant() const
-	{ 
-		return triple((*this)[0], (*this)[1], (*this)[2]);
-	}
-	
 
-	SIMD_FORCE_INLINE btMatrix3x3 
-	btMatrix3x3::absolute() const
-	{
-		return btMatrix3x3(
-			btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
-			btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
-			btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
-	}
 
-	SIMD_FORCE_INLINE btMatrix3x3 
-	btMatrix3x3::transpose() const 
+	/**@brief Calculate the matrix cofactor 
+	* @param r1 The first row to use for calculating the cofactor
+	* @param c1 The first column to use for calculating the cofactor
+	* @param r1 The second row to use for calculating the cofactor
+	* @param c1 The second column to use for calculating the cofactor
+	* See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details
+	*/
+	btScalar cofac(int r1, int c1, int r2, int c2) const 
 	{
-		return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
-								 m_el[0].y(), m_el[1].y(), m_el[2].y(),
-								 m_el[0].z(), m_el[1].z(), m_el[2].z());
-	}
-	
-	SIMD_FORCE_INLINE btMatrix3x3 
-	btMatrix3x3::adjoint() const 
-	{
-		return btMatrix3x3(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 btMatrix3x3 
-	btMatrix3x3::inverse() const
-	{
-		btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
-		btScalar det = (*this)[0].dot(co);
-		btFullAssert(det != btScalar(0.0));
-		btScalar s = btScalar(1.0) / det;
-		return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
-								 co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
-								 co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
-	}
-	
-	SIMD_FORCE_INLINE btMatrix3x3 
-	btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
-	{
-		return btMatrix3x3(
-			m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
-			m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
-			m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
-			m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
-			m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
-			m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
-			m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
-			m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
-			m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
-	}
-	
-	SIMD_FORCE_INLINE btMatrix3x3 
-	btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
-	{
-		return btMatrix3x3(
-			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]));
-		
+		return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
 	}
 
-	SIMD_FORCE_INLINE btVector3 
-	operator*(const btMatrix3x3& m, const btVector3& v) 
-	{
-		return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
-	}
-	
+	void	serialize(struct	btMatrix3x3Data& dataOut) const;
 
-	SIMD_FORCE_INLINE btVector3
-	operator*(const btVector3& v, const btMatrix3x3& m)
-	{
-		return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
-	}
+	void	serializeFloat(struct	btMatrix3x3FloatData& dataOut) const;
 
-	SIMD_FORCE_INLINE btMatrix3x3 
-	operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
-	{
-		return btMatrix3x3(
-			m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
-			m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
-			m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
-	}
+	void	deSerialize(const struct	btMatrix3x3Data& dataIn);
+
+	void	deSerializeFloat(const struct	btMatrix3x3FloatData& dataIn);
+
+	void	deSerializeDouble(const struct	btMatrix3x3DoubleData& dataIn);
+
+};
+
+
+SIMD_FORCE_INLINE btMatrix3x3& 
+btMatrix3x3::operator*=(const btMatrix3x3& m)
+{
+	setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
+		m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
+		m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
+	return *this;
+}
+
+SIMD_FORCE_INLINE btMatrix3x3& 
+btMatrix3x3::operator+=(const btMatrix3x3& m)
+{
+	setValue(
+		m_el[0][0]+m.m_el[0][0], 
+		m_el[0][1]+m.m_el[0][1],
+		m_el[0][2]+m.m_el[0][2],
+		m_el[1][0]+m.m_el[1][0], 
+		m_el[1][1]+m.m_el[1][1],
+		m_el[1][2]+m.m_el[1][2],
+		m_el[2][0]+m.m_el[2][0], 
+		m_el[2][1]+m.m_el[2][1],
+		m_el[2][2]+m.m_el[2][2]);
+	return *this;
+}
+
+SIMD_FORCE_INLINE btMatrix3x3
+operator*(const btMatrix3x3& m, const btScalar & k)
+{
+	return btMatrix3x3(
+		m[0].x()*k,m[0].y()*k,m[0].z()*k,
+		m[1].x()*k,m[1].y()*k,m[1].z()*k,
+		m[2].x()*k,m[2].y()*k,m[2].z()*k);
+}
+
+ SIMD_FORCE_INLINE btMatrix3x3 
+operator+(const btMatrix3x3& m1, const btMatrix3x3& m2)
+{
+	return btMatrix3x3(
+	m1[0][0]+m2[0][0], 
+	m1[0][1]+m2[0][1],
+	m1[0][2]+m2[0][2],
+	m1[1][0]+m2[1][0], 
+	m1[1][1]+m2[1][1],
+	m1[1][2]+m2[1][2],
+	m1[2][0]+m2[2][0], 
+	m1[2][1]+m2[2][1],
+	m1[2][2]+m2[2][2]);
+}
+
+SIMD_FORCE_INLINE btMatrix3x3 
+operator-(const btMatrix3x3& m1, const btMatrix3x3& m2)
+{
+	return btMatrix3x3(
+	m1[0][0]-m2[0][0], 
+	m1[0][1]-m2[0][1],
+	m1[0][2]-m2[0][2],
+	m1[1][0]-m2[1][0], 
+	m1[1][1]-m2[1][1],
+	m1[1][2]-m2[1][2],
+	m1[2][0]-m2[2][0], 
+	m1[2][1]-m2[2][1],
+	m1[2][2]-m2[2][2]);
+}
+
+
+SIMD_FORCE_INLINE btMatrix3x3& 
+btMatrix3x3::operator-=(const btMatrix3x3& m)
+{
+	setValue(
+	m_el[0][0]-m.m_el[0][0], 
+	m_el[0][1]-m.m_el[0][1],
+	m_el[0][2]-m.m_el[0][2],
+	m_el[1][0]-m.m_el[1][0], 
+	m_el[1][1]-m.m_el[1][1],
+	m_el[1][2]-m.m_el[1][2],
+	m_el[2][0]-m.m_el[2][0], 
+	m_el[2][1]-m.m_el[2][1],
+	m_el[2][2]-m.m_el[2][2]);
+	return *this;
+}
+
+
+SIMD_FORCE_INLINE btScalar 
+btMatrix3x3::determinant() const
+{ 
+	return btTriple((*this)[0], (*this)[1], (*this)[2]);
+}
+
+
+SIMD_FORCE_INLINE btMatrix3x3 
+btMatrix3x3::absolute() const
+{
+	return btMatrix3x3(
+		btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
+		btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
+		btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
+}
+
+SIMD_FORCE_INLINE btMatrix3x3 
+btMatrix3x3::transpose() const 
+{
+	return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
+		m_el[0].y(), m_el[1].y(), m_el[2].y(),
+		m_el[0].z(), m_el[1].z(), m_el[2].z());
+}
+
+SIMD_FORCE_INLINE btMatrix3x3 
+btMatrix3x3::adjoint() const 
+{
+	return btMatrix3x3(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 btMatrix3x3 
+btMatrix3x3::inverse() const
+{
+	btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
+	btScalar det = (*this)[0].dot(co);
+	btFullAssert(det != btScalar(0.0));
+	btScalar s = btScalar(1.0) / det;
+	return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
+		co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
+		co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
+}
+
+SIMD_FORCE_INLINE btMatrix3x3 
+btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
+{
+	return btMatrix3x3(
+		m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
+		m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
+		m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
+		m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
+		m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
+		m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
+		m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
+		m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
+		m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
+}
+
+SIMD_FORCE_INLINE btMatrix3x3 
+btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
+{
+	return btMatrix3x3(
+		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 btVector3 
+operator*(const btMatrix3x3& m, const btVector3& v) 
+{
+	return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
+}
+
+
+SIMD_FORCE_INLINE btVector3
+operator*(const btVector3& v, const btMatrix3x3& m)
+{
+	return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
+}
+
+SIMD_FORCE_INLINE btMatrix3x3 
+operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
+{
+	return btMatrix3x3(
+		m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
+		m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
+		m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
+}
 
 /*
-	SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
-    return btMatrix3x3(
-        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]);
+SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
+return btMatrix3x3(
+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]);
 }
 */
 
 /**@brief Equality operator between two matrices
- * It will test all elements are equal.  */
+* It will test all elements are equal.  */
 SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2)
 {
-   return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
-            m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
-            m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
+	return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
+		m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
+		m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
+}
+
+///for serialization
+struct	btMatrix3x3FloatData
+{
+	btVector3FloatData m_el[3];
+};
+
+///for serialization
+struct	btMatrix3x3DoubleData
+{
+	btVector3DoubleData m_el[3];
+};
+
+
+	
+
+SIMD_FORCE_INLINE	void	btMatrix3x3::serialize(struct	btMatrix3x3Data& dataOut) const
+{
+	for (int i=0;i<3;i++)
+		m_el[i].serialize(dataOut.m_el[i]);
 }
 
-#endif
+SIMD_FORCE_INLINE	void	btMatrix3x3::serializeFloat(struct	btMatrix3x3FloatData& dataOut) const
+{
+	for (int i=0;i<3;i++)
+		m_el[i].serializeFloat(dataOut.m_el[i]);
+}
+
+
+SIMD_FORCE_INLINE	void	btMatrix3x3::deSerialize(const struct	btMatrix3x3Data& dataIn)
+{
+	for (int i=0;i<3;i++)
+		m_el[i].deSerialize(dataIn.m_el[i]);
+}
+
+SIMD_FORCE_INLINE	void	btMatrix3x3::deSerializeFloat(const struct	btMatrix3x3FloatData& dataIn)
+{
+	for (int i=0;i<3;i++)
+		m_el[i].deSerializeFloat(dataIn.m_el[i]);
+}
+
+SIMD_FORCE_INLINE	void	btMatrix3x3::deSerializeDouble(const struct	btMatrix3x3DoubleData& dataIn)
+{
+	for (int i=0;i<3;i++)
+		m_el[i].deSerializeDouble(dataIn.m_el[i]);
+}
+
+#endif //BT_MATRIX3x3_H
+