SIMD math

benches/build/gen.cfg.py

@@ -9,6 +9,6 @@ executableName = "anki-benches"
 
 compiler = "g++"
 
-compilerFlags = "-DDEBUG_ENABLED=0 -DPLATFORM_LINUX -DMATH_INTEL_SIMD -DREVISION=\\\"`svnversion -c ../..`\\\" -c -msse4 -pedantic-errors -pedantic -ansi -Wall -Wextra -W -Wno-long-long -pipe -s -O3 -mtune=core2 -ffast-math -fsingle-precision-constant"
+compilerFlags = "-DDEBUG_ENABLED=0 -DBOOST_DISABLE_ASSERTS -DNDEBUG -DPLATFORM_LINUX -DMATH_INTEL_SIMD -DREVISION=\\\"`svnversion -c ../..`\\\" -c -msse4 -pedantic-errors -pedantic -ansi -Wall -Wextra -W -Wno-long-long -pipe -s -O3 -mtune=core2 -ffast-math -fsingle-precision-constant"
 
-linkerFlags = "-rdynamic -pg -L../../extern/lib-x86-64-linux -Wl,-Bstatic -lBulletSoftBody -lBulletDynamics -lBulletCollision -lLinearMath -lGLEW -lGLU -Wl,-Bdynamic -lGL -ljpeg -lSDL -lpng -lpython2.6 -lboost_system -lboost_python -lboost_filesystem -lboost_thread -lgtest"
+linkerFlags = "-rdynamic -pg -L../../extern/lib-x86-64-linux -Wl,-Bstatic -lBulletSoftBody -lBulletDynamics -lBulletCollision -lLinearMath -lGLEW -lGLU -Wl,-Bdynamic -lGL -ljpeg -lSDL -lpng -lpython2.6 -lboost_system -lboost_python -lboost_filesystem -lboost_thread"

src/Math/Mat3.h

@@ -11,55 +11,55 @@ namespace M {
 class Mat3
 {
 	public:
-		/// @name Accessors
-		/// @{
-		float& operator()(const uint i, const uint j);
-		const float& operator()(const uint i, const uint j) const;
-		float& operator [](const uint i);
-		const float& operator [](const uint i) const;
-		/// @}
-
 		/// @name Constructors & distructors
 		/// @{
 		explicit Mat3() {};
 		explicit Mat3(float f);
 		explicit Mat3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22);
-		explicit Mat3(float arr []);
+		explicit Mat3(float arr[]);
 		         Mat3(const Mat3& b);
 		explicit Mat3(const Quat& q); ///< Quat to Mat3. 12 muls, 12 adds
 		explicit Mat3(const Euler& eu);
 		explicit Mat3(const Axisang& axisang);
 		/// @}
 
+		/// @name Accessors
+		/// @{
+		float& operator()(const uint i, const uint j);
+		const float& operator()(const uint i, const uint j) const;
+		float& operator[](const uint i);
+		const float& operator[](const uint i) const;
+		/// @}
+
 		/// @name Operators with Mat3
 		/// @{
-		Mat3 operator +(const Mat3& b) const;
-		Mat3& operator +=(const Mat3& b);
-		Mat3 operator -(const Mat3& b) const;
-		Mat3& operator -=(const Mat3& b);
-		Mat3 operator *(const Mat3& b) const; ///< 27 muls, 18 adds
-		Mat3& operator *=(const Mat3& b);
-		Mat3 operator /(const Mat3& b) const;
-		Mat3& operator /=(const Mat3& b);
-		bool operator ==(const Mat3& b) const;
-		bool operator !=(const Mat3& b) const;
+		Mat3 operator+(const Mat3& b) const;
+		Mat3& operator+=(const Mat3& b);
+		Mat3 operator-(const Mat3& b) const;
+		Mat3& operator-=(const Mat3& b);
+		Mat3 operator*(const Mat3& b) const; ///< 27 muls, 18 adds
+		Mat3& operator*=(const Mat3& b);
+		Mat3 operator/(const Mat3& b) const;
+		Mat3& operator/=(const Mat3& b);
+		bool operator==(const Mat3& b) const;
+		bool operator!=(const Mat3& b) const;
 		/// @}
 
 		/// @name Operators with float
 		/// @{
-		Mat3 operator +(float f) const;
-		Mat3& operator +=(float f);
-		Mat3 operator -(float f) const;
-		Mat3& operator -=(float f);
-		Mat3 operator *(float f) const;
-		Mat3& operator *=(float f);
-		Mat3 operator /(float f) const;
-		Mat3& operator /=(float f);
+		Mat3 operator+(float f) const;
+		Mat3& operator+=(float f);
+		Mat3 operator-(float f) const;
+		Mat3& operator-=(float f);
+		Mat3 operator*(float f) const;
+		Mat3& operator*=(float f);
+		Mat3 operator/(float f) const;
+		Mat3& operator/=(float f);
 		/// @}
 
 		/// @name Operators with others
 		/// @{
-		Vec3 operator *(const Vec3& b) const;  ///< 9 muls, 6 adds
+		Vec3 operator*(const Vec3& b) const;  ///< 9 muls, 6 adds
 		/// @}
 
 		/// @name Other
@@ -101,8 +101,8 @@ class Mat3
 		/// @{
 		union
 		{
-			float arr1[9];
-			float arr2[3][3];
+			boost::array<float, 9> arr1;
+			boost::array<boost::array<float, 3>, 3> arr2;
 		};
 		/// @}
 };
@@ -110,10 +110,10 @@ class Mat3
 
 /// @name Other Mat3 operators
 /// @{
-extern Mat3 operator +(float f, const Mat3& m3);
-extern Mat3 operator -(float f, const Mat3& m3);
-extern Mat3 operator *(float f, const Mat3& m3);
-extern Mat3 operator /(float f, const Mat3& m3);
+extern Mat3 operator+(float f, const Mat3& m3);
+extern Mat3 operator-(float f, const Mat3& m3);
+extern Mat3 operator*(float f, const Mat3& m3);
+extern Mat3 operator/(float f, const Mat3& m3);
 extern std::ostream& operator<<(std::ostream& s, const Mat3& m);
 /// @}
 

src/Math/Mat3.inl.h

@@ -1,94 +1,113 @@
 #include "MathCommon.inl.h"
 
 
-#define ME (*this)
+#define SELF (*this)
 
 
 namespace M {
 
-// accessors
-inline float& Mat3::operator ()(const uint i, const uint j)
+//======================================================================================================================
+// Accessor                                                                                                            =
+//======================================================================================================================
+
+inline float& Mat3::operator()(const uint i, const uint j)
 {
 	return arr2[i][j];
 }
 
-inline const float& Mat3::operator ()(const uint i, const uint j) const
+inline const float& Mat3::operator()(const uint i, const uint j) const
 {
 	return arr2[i][j]; 
 }
 
-inline float& Mat3::operator [](const uint i)
+inline float& Mat3::operator[](const uint i)
 {
 	return arr1[i];
 }
 
-inline const float& Mat3::operator [](const uint i) const
+inline const float& Mat3::operator[](const uint i) const
 {
 	return arr1[i];
 }
 
-// constructor [float[]]
+
+//======================================================================================================================
+// Constructors                                                                                                        =
+//======================================================================================================================
+
+// constructor [float]
+inline Mat3::Mat3(float f)
+{
+	for(int i = 0; i < 9; i++)
+	{
+		SELF[i] = f;
+	}
+}
+
+// float[]
 inline Mat3::Mat3(float arr [])
 {
-	for(int i=0; i<9; i++)
-		ME[i] = arr[i];
+	for(int i = 0; i < 9; i++)
+	{
+		SELF[i] = arr[i];
+	}
 }
 
-// constructor [float...........float]
+// many floats
 inline Mat3::Mat3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22)
 {
-	ME(0, 0) = m00;
-	ME(0, 1) = m01;
-	ME(0, 2) = m02;
-	ME(1, 0) = m10;
-	ME(1, 1) = m11;
-	ME(1, 2) = m12;
-	ME(2, 0) = m20;
-	ME(2, 1) = m21;
-	ME(2, 2) = m22;
+	SELF(0, 0) = m00;
+	SELF(0, 1) = m01;
+	SELF(0, 2) = m02;
+	SELF(1, 0) = m10;
+	SELF(1, 1) = m11;
+	SELF(1, 2) = m12;
+	SELF(2, 0) = m20;
+	SELF(2, 1) = m21;
+	SELF(2, 2) = m22;
 }
 
-// constructor [mat3]
+// Copy
 inline Mat3::Mat3(const Mat3& b)
 {
-	for(int i=0; i<9; i++)
-		ME[i] = b[i];
+	for(int i = 0; i < 9; i++)
+		SELF[i] = b[i];
 }
 
-// constructor [quat]
+// Quat
 inline Mat3::Mat3(const Quat& q)
 {
 	RASSERT_THROW_EXCEPTION(fabs(1.0 - q.getLength()) > 0.002); // If length is > 1 + 0.002 or < 1 - 0.002 then not normalized quat
 
 	float xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
 
-	xs = q.x+q.x;
-	ys = q.y+q.y;
-	zs = q.z+q.z;
-	wx = q.w*xs;
-	wy = q.w*ys;
-	wz = q.w*zs;
-	xx = q.x*xs;
-	xy = q.x*ys;
-	xz = q.x*zs;
-	yy = q.y*ys;
-	yz = q.y*zs;
-	zz = q.z*zs;
-
-	ME(0, 0) = 1.0 - (yy + zz);
-	ME(0, 1) = xy - wz;
-	ME(0, 2) = xz + wy;
-
-	ME(1, 0) = xy + wz;
-	ME(1, 1) = 1.0 - (xx + zz);
-	ME(1, 2) = yz - wx;
-
-	ME(2, 0) = xz - wy;
-	ME(2, 1) = yz + wx;
-	ME(2, 2) = 1.0 - (xx + yy);
-}
-
-// constructor [euler]
+	xs = q.x + q.x;
+	ys = q.y + q.y;
+	zs = q.z + q.z;
+	wx = q.w * xs;
+	wy = q.w * ys;
+	wz = q.w * zs;
+	xx = q.x * xs;
+	xy = q.x * ys;
+	xz = q.x * zs;
+	yy = q.y * ys;
+	yz = q.y * zs;
+	zz = q.z * zs;
+
+	SELF(0, 0) = 1.0 - (yy + zz);
+	SELF(0, 1) = xy - wz;
+	SELF(0, 2) = xz + wy;
+
+	SELF(1, 0) = xy + wz;
+	SELF(1, 1) = 1.0 - (xx + zz);
+	SELF(1, 2) = yz - wx;
+
+	SELF(2, 0) = xz - wy;
+	SELF(2, 1) = yz + wx;
+	SELF(2, 2) = 1.0 - (xx + yy);
+}
+
+// Euler
 inline Mat3::Mat3(const Euler& e)
 {
 	float ch, sh, ca, sa, cb, sb;
@@ -96,18 +115,18 @@ inline Mat3::Mat3(const Euler& e)
   sinCos(e.getAttitude(), sa, ca);
   sinCos(e.getBank(), sb, cb);
 
-  ME(0, 0) = ch * ca;
-  ME(0, 1) = sh*sb - ch*sa*cb;
-  ME(0, 2) = ch*sa*sb + sh*cb;
-  ME(1, 0) = sa;
-  ME(1, 1) = ca*cb;
-  ME(1, 2) = -ca*sb;
-  ME(2, 0) = -sh*ca;
-  ME(2, 1) = sh*sa*cb + ch*sb;
-  ME(2, 2) = -sh*sa*sb + ch*cb;
+  SELF(0, 0) = ch * ca;
+  SELF(0, 1) = sh * sb - ch * sa * cb;
+  SELF(0, 2) = ch * sa * sb + sh * cb;
+  SELF(1, 0) = sa;
+  SELF(1, 1) = ca * cb;
+  SELF(1, 2) = -ca * sb;
+  SELF(2, 0) = -sh * ca;
+  SELF(2, 1) = sh * sa * cb + ch * sb;
+  SELF(2, 2) = -sh * sa * sb + ch * cb;
 }
 
-// constructor [axisang]
+// Axisang
 inline Mat3::Mat3(const Axisang& axisang)
 {
 	RASSERT_THROW_EXCEPTION(!isZero(1.0 - axisang.axis.getLength())); // Not normalized axis
@@ -117,296 +136,349 @@ inline Mat3::Mat3(const Axisang& axisang)
 	float t = 1.0 - c;
 
 	const Vec3& axis = axisang.axis;
-	ME(0, 0) = c + axis.x() * axis.x() * t;
-	ME(1, 1) = c + axis.y() * axis.y() * t;
-	ME(2, 2) = c + axis.z() * axis.z() * t;
+	SELF(0, 0) = c + axis.x() * axis.x() * t;
+	SELF(1, 1) = c + axis.y() * axis.y() * t;
+	SELF(2, 2) = c + axis.z() * axis.z() * t;
 
 	float tmp1 = axis.x() * axis.y() * t;
 	float tmp2 = axis.z() * s;
-	ME(1, 0) = tmp1 + tmp2;
-	ME(0, 1) = tmp1 - tmp2;
+	SELF(1, 0) = tmp1 + tmp2;
+	SELF(0, 1) = tmp1 - tmp2;
 	tmp1 = axis.x() * axis.z() * t;
 	tmp2 = axis.y() * s;
-	ME(2, 0) = tmp1 - tmp2;
-	ME(0, 2) = tmp1 + tmp2;    tmp1 = axis.y() * axis.z() * t;
+	SELF(2, 0) = tmp1 - tmp2;
+	SELF(0, 2) = tmp1 + tmp2;    tmp1 = axis.y() * axis.z() * t;
 	tmp2 = axis.x() * s;
-	ME(2, 1) = tmp1 + tmp2;
-	ME(1, 2) = tmp1 - tmp2;
+	SELF(2, 1) = tmp1 + tmp2;
+	SELF(1, 2) = tmp1 - tmp2;
 }
 
-// constructor [float]
-inline Mat3::Mat3(float f)
-{
-	for(int i=0; i<9; i++)
-			ME[i] = f;
-}
 
-// 3x3 + 3x3
-inline Mat3 Mat3::operator +(const Mat3& b) const
+//======================================================================================================================
+// Operators with same                                                                                                 =
+//======================================================================================================================
+
+// +
+inline Mat3 Mat3::operator+(const Mat3& b) const
 {
 	Mat3 c;
-	for(int i=0; i<9; i++)
-		c[i] = ME[i] + b[i];
+	for(int i = 0; i < 9; i++)
+	{
+		c[i] = SELF[i] + b[i];
+	}
 	return c;
 }
 
-// 3x3 += 3x3
-inline Mat3& Mat3::operator +=(const Mat3& b)
+// +=
+inline Mat3& Mat3::operator+=(const Mat3& b)
 {
-	for(int i=0; i<9; i++)
-		ME[i] += b[i];
-	return ME;
+	for(int i = 0; i < 9; i++)
+	{
+		SELF[i] += b[i];
+	}
+	return SELF;
 }
 
-// 3x3 - 3x3
-inline Mat3 Mat3::operator -(const Mat3& b) const
+// -
+inline Mat3 Mat3::operator-(const Mat3& b) const
 {
 	Mat3 c;
-	for(int i=0; i<9; i++)
-		c[i] = ME[i] - b[i];
+	for(int i = 0; i < 9; i++)
+	{
+		c[i] = SELF[i] - b[i];
+	}
 	return c;
 }
 
-// 3x3 -= 3x3
-inline Mat3& Mat3::operator -=(const Mat3& b)
+// -=
+inline Mat3& Mat3::operator-=(const Mat3& b)
 {
-	for(int i=0; i<9; i++)
-		ME[i] -= b[i];
-	return ME;
+	for(int i = 0; i < 9; i++)
+	{
+		SELF[i] -= b[i];
+	}
+	return SELF;
 }
 
-// 3x3 * 3x3
-inline Mat3 Mat3::operator *(const Mat3& b) const
+// *
+inline Mat3 Mat3::operator*(const Mat3& b) const
 {
 	Mat3 c;
-	c(0, 0) = ME(0, 0)*b(0, 0) + ME(0, 1)*b(1, 0) + ME(0, 2)*b(2, 0);
-	c(0, 1) = ME(0, 0)*b(0, 1) + ME(0, 1)*b(1, 1) + ME(0, 2)*b(2, 1);
-	c(0, 2) = ME(0, 0)*b(0, 2) + ME(0, 1)*b(1, 2) + ME(0, 2)*b(2, 2);
-	c(1, 0) = ME(1, 0)*b(0, 0) + ME(1, 1)*b(1, 0) + ME(1, 2)*b(2, 0);
-	c(1, 1) = ME(1, 0)*b(0, 1) + ME(1, 1)*b(1, 1) + ME(1, 2)*b(2, 1);
-	c(1, 2) = ME(1, 0)*b(0, 2) + ME(1, 1)*b(1, 2) + ME(1, 2)*b(2, 2);
-	c(2, 0) = ME(2, 0)*b(0, 0) + ME(2, 1)*b(1, 0) + ME(2, 2)*b(2, 0);
-	c(2, 1) = ME(2, 0)*b(0, 1) + ME(2, 1)*b(1, 1) + ME(2, 2)*b(2, 1);
-	c(2, 2) = ME(2, 0)*b(0, 2) + ME(2, 1)*b(1, 2) + ME(2, 2)*b(2, 2);
+	c(0, 0) = SELF(0, 0) * b(0, 0) + SELF(0, 1) * b(1, 0) + SELF(0, 2) * b(2, 0);
+	c(0, 1) = SELF(0, 0) * b(0, 1) + SELF(0, 1) * b(1, 1) + SELF(0, 2) * b(2, 1);
+	c(0, 2) = SELF(0, 0) * b(0, 2) + SELF(0, 1) * b(1, 2) + SELF(0, 2) * b(2, 2);
+	c(1, 0) = SELF(1, 0) * b(0, 0) + SELF(1, 1) * b(1, 0) + SELF(1, 2) * b(2, 0);
+	c(1, 1) = SELF(1, 0) * b(0, 1) + SELF(1, 1) * b(1, 1) + SELF(1, 2) * b(2, 1);
+	c(1, 2) = SELF(1, 0) * b(0, 2) + SELF(1, 1) * b(1, 2) + SELF(1, 2) * b(2, 2);
+	c(2, 0) = SELF(2, 0) * b(0, 0) + SELF(2, 1) * b(1, 0) + SELF(2, 2) * b(2, 0);
+	c(2, 1) = SELF(2, 0) * b(0, 1) + SELF(2, 1) * b(1, 1) + SELF(2, 2) * b(2, 1);
+	c(2, 2) = SELF(2, 0) * b(0, 2) + SELF(2, 1) * b(1, 2) + SELF(2, 2) * b(2, 2);
 	return c;
 }
 
-// 3x3 *= 3x3
-inline Mat3& Mat3::operator *=(const Mat3& b)
+// *=
+inline Mat3& Mat3::operator*=(const Mat3& b)
+{
+	SELF = SELF * b;
+	return SELF;
+}
+
+// ==
+inline bool Mat3::operator==(const Mat3& b) const
 {
-	ME = ME * b;
-	return ME;
+	for(int i = 0; i < 9; i++)
+	{
+		if(!isZero(SELF[i]-b[i]))
+		{
+			return false;
+		}
+	}
+	return true;
 }
 
+// !=
+inline bool Mat3::operator!=(const Mat3& b) const
+{
+	for(int i = 0; i < 9; i++)
+	{
+		if(!isZero(SELF[i]-b[i]))
+		{
+			return true;
+		}
+	}
+	return false;
+}
+
+
+//======================================================================================================================
+// Operators with float                                                                                                =
+//======================================================================================================================
+
 // 3x3 + float
-inline Mat3 Mat3::operator +(float f) const
+inline Mat3 Mat3::operator+(float f) const
 {
 	Mat3 c;
-	for(uint i=0; i<9; i++)
-		c[i] = ME[i] + f;
+	for(uint i = 0; i < 9; i++)
+	{
+		c[i] = SELF[i] + f;
+	}
 	return c;
 }
 
 // float + 3x3
-inline Mat3 operator +(float f, const Mat3& m3)
+inline Mat3 operator+(float f, const Mat3& m3)
 {
-	return m3+f;
+	return m3 + f;
 }
 
 // 3x3 += float
-inline Mat3& Mat3::operator +=(float f)
+inline Mat3& Mat3::operator+=(float f)
 {
-	for(uint i=0; i<9; i++)
-		ME[i] += f;
-	return ME;
+	for(uint i = 0; i < 9; i++)
+	{
+		SELF[i] += f;
+	}
+	return SELF;
 }
 
 // 3x3 - float
-inline Mat3 Mat3::operator -(float f) const
+inline Mat3 Mat3::operator-(float f) const
 {
 	Mat3 c;
-	for(uint i=0; i<9; i++)
-		c[i] = ME[i] - f;
+	for(uint i = 0; i < 9; i++)
+	{
+		c[i] = SELF[i] - f;
+	}
 	return c;
 }
 
 // float - 3x3
-inline Mat3 operator -(float f, const Mat3& m3)
+inline Mat3 operator-(float f, const Mat3& m3)
 {
 	Mat3 out;
-	for(uint i=0; i<9; i++)
+	for(uint i = 0; i < 9; i++)
+	{
 		out[i] = f - m3[i];
+	}
 	return out;
 }
 
 // 3x3 -= float
-inline Mat3& Mat3::operator -=(float f)
+inline Mat3& Mat3::operator-=(float f)
 {
-	for(uint i=0; i<9; i++)
-		ME[i] -= f;
-	return ME;
+	for(uint i = 0; i < 9; i++)
+	{
+		SELF[i] -= f;
+	}
+	return SELF;
 }
 
 // 3x3 * float
-inline Mat3 Mat3::operator *(float f) const
+inline Mat3 Mat3::operator*(float f) const
 {
 	Mat3 c;
-	for(uint i=0; i<9; i++)
-		c[i] = ME[i] * f;
+	for(uint i = 0; i < 9; i++)
+	{
+		c[i] = SELF[i] * f;
+	}
 	return c;
 }
 
 // float * 3x3
-inline Mat3 operator *(float f, const Mat3& m3)
+inline Mat3 operator*(float f, const Mat3& m3)
 {
 	Mat3 out;
-	for(uint i=0; i<9; i++)
+	for(uint i = 0; i < 9; i++)
+	{
 		out[i] = f * m3[i];
+	}
 	return out;
 }
 
 // 3x3 *= float
-inline Mat3& Mat3::operator *=(float f)
+inline Mat3& Mat3::operator*=(float f)
 {
-	for(uint i=0; i<9; i++)
-		ME[i] *= f;
-	return ME;
+	for(uint i = 0; i < 9; i++)
+	{
+		SELF[i] *= f;
+	}
+	return SELF;
 }
 
 // 3x3 / float
-inline Mat3 Mat3::operator /(float f) const
+inline Mat3 Mat3::operator/(float f) const
 {
 	Mat3 c;
-	for(uint i=0; i<9; i++)
-		c[i] = ME[i] / f;
+	for(uint i = 0; i < 9; i++)
+	{
+		c[i] = SELF[i] / f;
+	}
 	return c;
 }
 
 // float / 3x3
-inline Mat3 operator /(float f, const Mat3& m3)
+inline Mat3 operator/(float f, const Mat3& m3)
 {
 	Mat3 out;
-	for(uint i=0; i<9; i++)
+	for(uint i = 0; i < 9; i++)
+	{
 		out[i] = f / m3[i];
+	}
 	return out;
 }
 
 // 3x3 / float (self)
-inline Mat3& Mat3::operator /=(float f)
+inline Mat3& Mat3::operator/=(float f)
 {
-	for(uint i=0; i<9; i++)
-		ME[i] /= f;
-	return ME;
+	for(uint i = 0; i < 9; i++)
+	{
+		SELF[i] /= f;
+	}
+	return SELF;
 }
 
+
+//======================================================================================================================
+// Operators with other                                                                                                =
+//======================================================================================================================
+
 // 3x3 * vec3 (cross products with rows)
-inline Vec3 Mat3::operator *(const Vec3& b) const
+inline Vec3 Mat3::operator*(const Vec3& b) const
 {
 	return Vec3(
-		ME(0, 0) * b.x() + ME(0, 1) * b.y() + ME(0, 2) * b.z(),
-		ME(1, 0) * b.x() + ME(1, 1) * b.y() + ME(1, 2) * b.z(),
-		ME(2, 0) * b.x() + ME(2, 1) * b.y() + ME(2, 2) * b.z()
+		SELF(0, 0) * b.x() + SELF(0, 1) * b.y() + SELF(0, 2) * b.z(),
+		SELF(1, 0) * b.x() + SELF(1, 1) * b.y() + SELF(1, 2) * b.z(),
+		SELF(2, 0) * b.x() + SELF(2, 1) * b.y() + SELF(2, 2) * b.z()
 	);
 }
 
-// ==
-inline bool Mat3::operator ==(const Mat3& b) const
-{
-	for(int i=0; i<9; i++)
-		if(!isZero(ME[i]-b[i])) return false;
-	return true;
-}
 
-// !=
-inline bool Mat3::operator !=(const Mat3& b) const
-{
-	for(int i=0; i<9; i++)
-		if(!isZero(ME[i]-b[i])) return true;
-	return false;
-}
+//======================================================================================================================
+// Other                                                                                                               =
+//======================================================================================================================
 
 // setRows
 inline void Mat3::setRows(const Vec3& a, const Vec3& b, const Vec3& c)
 {
-	ME(0, 0) = a.x();
-	ME(0, 1) = a.y();
-	ME(0, 2) = a.z();
-	ME(1, 0) = b.x();
-	ME(1, 1) = b.y();
-	ME(1, 2) = b.z();
-	ME(2, 0) = c.x();
-	ME(2, 1) = c.y();
-	ME(2, 2) = c.z();
+	SELF(0, 0) = a.x();
+	SELF(0, 1) = a.y();
+	SELF(0, 2) = a.z();
+	SELF(1, 0) = b.x();
+	SELF(1, 1) = b.y();
+	SELF(1, 2) = b.z();
+	SELF(2, 0) = c.x();
+	SELF(2, 1) = c.y();
+	SELF(2, 2) = c.z();
 }
 
 // setColumns
 inline void Mat3::setColumns(const Vec3& a, const Vec3& b, const Vec3& c)
 {
-	ME(0, 0) = a.x();
-	ME(1, 0) = a.y();
-	ME(2, 0) = a.z();
-	ME(0, 1) = b.x();
-	ME(1, 1) = b.y();
-	ME(2, 1) = b.z();
-	ME(0, 2) = c.x();
-	ME(1, 2) = c.y();
-	ME(2, 2) = c.z();
+	SELF(0, 0) = a.x();
+	SELF(1, 0) = a.y();
+	SELF(2, 0) = a.z();
+	SELF(0, 1) = b.x();
+	SELF(1, 1) = b.y();
+	SELF(2, 1) = b.z();
+	SELF(0, 2) = c.x();
+	SELF(1, 2) = c.y();
+	SELF(2, 2) = c.z();
 }
 
 // getRows
 inline void Mat3::getRows(Vec3& a, Vec3& b, Vec3& c) const
 {
-	a.x() = ME(0, 0);
-	a.y() = ME(0, 1);
-	a.z() = ME(0, 2);
-	b.x() = ME(1, 0);
-	b.y() = ME(1, 1);
-	b.z() = ME(1, 2);
-	c.x() = ME(2, 0);
-	c.y() = ME(2, 1);
-	c.z() = ME(2, 2);
+	a.x() = SELF(0, 0);
+	a.y() = SELF(0, 1);
+	a.z() = SELF(0, 2);
+	b.x() = SELF(1, 0);
+	b.y() = SELF(1, 1);
+	b.z() = SELF(1, 2);
+	c.x() = SELF(2, 0);
+	c.y() = SELF(2, 1);
+	c.z() = SELF(2, 2);
 }
 
 // getColumns
 inline void Mat3::getColumns(Vec3& a, Vec3& b, Vec3& c) const
 {
-	a.x() = ME(0, 0);
-	a.y() = ME(1, 0);
-	a.z() = ME(2, 0);
-	b.x() = ME(0, 1);
-	b.y() = ME(1, 1);
-	b.z() = ME(2, 1);
-	c.x() = ME(0, 2);
-	c.y() = ME(1, 2);
-	c.z() = ME(2, 2);
+	a.x() = SELF(0, 0);
+	a.y() = SELF(1, 0);
+	a.z() = SELF(2, 0);
+	b.x() = SELF(0, 1);
+	b.y() = SELF(1, 1);
+	b.z() = SELF(2, 1);
+	c.x() = SELF(0, 2);
+	c.y() = SELF(1, 2);
+	c.z() = SELF(2, 2);
 }
 
 // setRow
 inline void Mat3::setRow(const uint i, const Vec3& v)
 {
-	ME(i, 0) = v.x();
-	ME(i, 1) = v.y();
-	ME(i, 2) = v.z();
+	SELF(i, 0) = v.x();
+	SELF(i, 1) = v.y();
+	SELF(i, 2) = v.z();
 }
 
 // getRow
 inline Vec3 Mat3::getRow(const uint i) const
 {
-	return Vec3(ME(i, 0), ME(i, 1), ME(i, 2));
+	return Vec3(SELF(i, 0), SELF(i, 1), SELF(i, 2));
 }
 
 // setColumn
 inline void Mat3::setColumn(const uint i, const Vec3& v)
 {
-	ME(0, i) = v.x();
-	ME(1, i) = v.y();
-	ME(2, i) = v.z();
+	SELF(0, i) = v.x();
+	SELF(1, i) = v.y();
+	SELF(2, i) = v.z();
 }
 
 // getColumn
 inline Vec3 Mat3::getColumn(const uint i) const
 {
-	return Vec3(ME(0,i), ME(1,i), ME(2,i));
+	return Vec3(SELF(0,i), SELF(1,i), SELF(2,i));
 }
 
 // getXAxis
@@ -451,15 +523,15 @@ inline void Mat3::setRotationX(float rad)
 	float sintheta, costheta;
 	sinCos(rad, sintheta, costheta);
 
-	ME(0, 0) = 1.0;
-	ME(0, 1) = 0.0;
-	ME(0, 2) = 0.0;
-	ME(1, 0) = 0.0;
-	ME(1, 1) = costheta;
-	ME(1, 2) = -sintheta;
-	ME(2, 0) = 0.0;
-	ME(2, 1) = sintheta;
-	ME(2, 2) = costheta;
+	SELF(0, 0) = 1.0;
+	SELF(0, 1) = 0.0;
+	SELF(0, 2) = 0.0;
+	SELF(1, 0) = 0.0;
+	SELF(1, 1) = costheta;
+	SELF(1, 2) = -sintheta;
+	SELF(2, 0) = 0.0;
+	SELF(2, 1) = sintheta;
+	SELF(2, 2) = costheta;
 }
 
 // setRotationY
@@ -468,15 +540,15 @@ inline void Mat3::setRotationY(float rad)
 	float sintheta, costheta;
 	sinCos(rad, sintheta, costheta);
 
-	ME(0, 0) = costheta;
-	ME(0, 1) = 0.0;
-	ME(0, 2) = sintheta;
-	ME(1, 0) = 0.0;
-	ME(1, 1) = 1.0;
-	ME(1, 2) = 0.0;
-	ME(2, 0) = -sintheta;
-	ME(2, 1) = 0.0;
-	ME(2, 2) = costheta;
+	SELF(0, 0) = costheta;
+	SELF(0, 1) = 0.0;
+	SELF(0, 2) = sintheta;
+	SELF(1, 0) = 0.0;
+	SELF(1, 1) = 1.0;
+	SELF(1, 2) = 0.0;
+	SELF(2, 0) = -sintheta;
+	SELF(2, 1) = 0.0;
+	SELF(2, 2) = costheta;
 }
 
 // loadRotationZ
@@ -485,15 +557,15 @@ inline void Mat3::setRotationZ(float rad)
 	float sintheta, costheta;
 	sinCos(rad, sintheta, costheta);
 
-	ME(0, 0) = costheta;
-	ME(0, 1) = -sintheta;
-	ME(0, 2) = 0.0;
-	ME(1, 0) = sintheta;
-	ME(1, 1) = costheta;
-	ME(1, 2) = 0.0;
-	ME(2, 0) = 0.0;
-	ME(2, 1) = 0.0;
-	ME(2, 2) = 1.0;
+	SELF(0, 0) = costheta;
+	SELF(0, 1) = -sintheta;
+	SELF(0, 2) = 0.0;
+	SELF(1, 0) = sintheta;
+	SELF(1, 1) = costheta;
+	SELF(1, 2) = 0.0;
+	SELF(2, 0) = 0.0;
+	SELF(2, 1) = 0.0;
+	SELF(2, 2) = 1.0;
 }
 
 // rotateXAxis
@@ -512,26 +584,26 @@ inline void Mat3::rotateXAxis(float rad)
 	getColumns(xAxis, yAxis, zAxis);*/
 
 	// zAxis = zAxis*cosa - yAxis*sina;
-	ME(0, 2) = ME(0, 2)*cosa - ME(0, 1)*sina;
-	ME(1, 2) = ME(1, 2)*cosa - ME(1, 1)*sina;
-	ME(2, 2) = ME(2, 2)*cosa - ME(2, 1)*sina;
+	SELF(0, 2) = SELF(0, 2) * cosa - SELF(0, 1) * sina;
+	SELF(1, 2) = SELF(1, 2) * cosa - SELF(1, 1) * sina;
+	SELF(2, 2) = SELF(2, 2) * cosa - SELF(2, 1) * sina;
 
 	// zAxis.normalize();
-	float len = sqrt(ME(0, 2)*ME(0, 2) + ME(1, 2)*ME(1, 2) + ME(2, 2)*ME(2, 2));
-	ME(0, 2) /= len;
-	ME(1, 2) /= len;
-	ME(2, 2) /= len;
+	float len = sqrt(SELF(0, 2) * SELF(0, 2) + SELF(1, 2) * SELF(1, 2) + SELF(2, 2) * SELF(2, 2));
+	SELF(0, 2) /= len;
+	SELF(1, 2) /= len;
+	SELF(2, 2) /= len;
 
 	// yAxis = zAxis * xAxis;
-	ME(0, 1) = ME(1, 2)*ME(2, 0) - ME(2, 2)*ME(1, 0);
-	ME(1, 1) = ME(2, 2)*ME(0, 0) - ME(0, 2)*ME(2, 0);
-	ME(2, 1) = ME(0, 2)*ME(1, 0) - ME(1, 2)*ME(0, 0);
+	SELF(0, 1) = SELF(1, 2) * SELF(2, 0) - SELF(2, 2) * SELF(1, 0);
+	SELF(1, 1) = SELF(2, 2) * SELF(0, 0) - SELF(0, 2) * SELF(2, 0);
+	SELF(2, 1) = SELF(0, 2) * SELF(1, 0) - SELF(1, 2) * SELF(0, 0);
 
 	// yAxis.normalize();
-	/*len = invSqrt(ME(0, 1)*ME(0, 1) + ME(1, 1)*ME(1, 1) + ME(2, 1)*ME(2, 1));
-	ME(0, 1) *= len;
-	ME(1, 1) *= len;
-	ME(2, 1) *= len;*/
+	/*len = invSqrt(SELF(0, 1) * SELF(0, 1) + SELF(1, 1) * SELF(1, 1) + SELF(2, 1) * SELF(2, 1));
+	SELF(0, 1) *= len;
+	SELF(1, 1) *= len;
+	SELF(2, 1) *= len;*/
 
 	// setColumns(xAxis, yAxis, zAxis);
 
@@ -547,26 +619,26 @@ inline void Mat3::rotateYAxis(float rad)
 	getColumns(xAxis, yAxis, zAxis);*/
 
 	// zAxis = zAxis*cosa + xAxis*sina;
-	ME(0, 2) = ME(0, 2)*cosa + ME(0, 0)*sina;
-	ME(1, 2) = ME(1, 2)*cosa + ME(1, 0)*sina;
-	ME(2, 2) = ME(2, 2)*cosa + ME(2, 0)*sina;
+	SELF(0, 2) = SELF(0, 2)*cosa + SELF(0, 0)*sina;
+	SELF(1, 2) = SELF(1, 2)*cosa + SELF(1, 0)*sina;
+	SELF(2, 2) = SELF(2, 2)*cosa + SELF(2, 0)*sina;
 
 	// zAxis.normalize();
-	float len = sqrt(ME(0, 2)*ME(0, 2) + ME(1, 2)*ME(1, 2) + ME(2, 2)*ME(2, 2));
-	ME(0, 2) /= len;
-	ME(1, 2) /= len;
-	ME(2, 2) /= len;
+	float len = sqrt(SELF(0, 2) * SELF(0, 2) + SELF(1, 2) * SELF(1, 2) + SELF(2, 2) * SELF(2, 2));
+	SELF(0, 2) /= len;
+	SELF(1, 2) /= len;
+	SELF(2, 2) /= len;
 
 	// xAxis = (zAxis*yAxis) * -1.0f;
-	ME(0, 0) = ME(2, 2)*ME(1, 1) - ME(1, 2)*ME(2, 1);
-	ME(1, 0) = ME(0, 2)*ME(2, 1) - ME(2, 2)*ME(0, 1);
-	ME(2, 0) = ME(1, 2)*ME(0, 1) - ME(0, 2)*ME(1, 1);
+	SELF(0, 0) = SELF(2, 2) * SELF(1, 1) - SELF(1, 2) * SELF(2, 1);
+	SELF(1, 0) = SELF(0, 2) * SELF(2, 1) - SELF(2, 2) * SELF(0, 1);
+	SELF(2, 0) = SELF(1, 2) * SELF(0, 1) - SELF(0, 2) * SELF(1, 1);
 
 	// xAxis.normalize();
-	/*len = invSqrt(ME(0, 0)*ME(0, 0) + ME(1, 0)*ME(1, 0) + ME(2, 0)*ME(2, 0));
-	ME(0, 0) *= len;
-	ME(1, 0) *= len;
-	ME(2, 0) *= len;*/
+	/*len = invSqrt(SELF(0, 0) * SELF(0, 0) + SELF(1, 0) * SELF(1, 0) + SELF(2, 0) * SELF(2, 0));
+	SELF(0, 0) *= len;
+	SELF(1, 0) *= len;
+	SELF(2, 0) *= len;*/
 
 	// setColumns(xAxis, yAxis, zAxis);
 }
@@ -582,26 +654,26 @@ inline void Mat3::rotateZAxis(float rad)
 	getColumns(xAxis, yAxis, zAxis);*/
 
 	// xAxis = xAxis*cosa + yAxis*sina;
-	ME(0, 0) = ME(0, 0)*cosa + ME(0, 1)*sina;
-	ME(1, 0) = ME(1, 0)*cosa + ME(1, 1)*sina;
-	ME(2, 0) = ME(2, 0)*cosa + ME(2, 1)*sina;
+	SELF(0, 0) = SELF(0, 0)*cosa + SELF(0, 1)*sina;
+	SELF(1, 0) = SELF(1, 0)*cosa + SELF(1, 1)*sina;
+	SELF(2, 0) = SELF(2, 0)*cosa + SELF(2, 1)*sina;
 
 	// xAxis.normalize();
-	float len = sqrt(ME(0, 0)*ME(0, 0) + ME(1, 0)*ME(1, 0) + ME(2, 0)*ME(2, 0));
-	ME(0, 0) /= len;
-	ME(1, 0) /= len;
-	ME(2, 0) /= len;
+	float len = sqrt(SELF(0, 0) * SELF(0, 0) + SELF(1, 0) * SELF(1, 0) + SELF(2, 0) * SELF(2, 0));
+	SELF(0, 0) /= len;
+	SELF(1, 0) /= len;
+	SELF(2, 0) /= len;
 
 	// yAxis = zAxis*xAxis;
-	ME(0, 1) = ME(1, 2)*ME(2, 0) - ME(2, 2)*ME(1, 0);
-	ME(1, 1) = ME(2, 2)*ME(0, 0) - ME(0, 2)*ME(2, 0);
-	ME(2, 1) = ME(0, 2)*ME(1, 0) - ME(1, 2)*ME(0, 0);
+	SELF(0, 1) = SELF(1, 2) * SELF(2, 0) - SELF(2, 2) * SELF(1, 0);
+	SELF(1, 1) = SELF(2, 2) * SELF(0, 0) - SELF(0, 2) * SELF(2, 0);
+	SELF(2, 1) = SELF(0, 2) * SELF(1, 0) - SELF(1, 2) * SELF(0, 0);
 
 	// yAxis.normalize();
-	/*len = invSqrt(ME(0, 1)*ME(0, 1) + ME(1, 1)*ME(1, 1) + ME(2, 1)*ME(2, 1));
-	ME(0, 1) *= len;
-	ME(1, 1) *= len;
-	ME(2, 1) *= len;*/
+	/*len = invSqrt(SELF(0, 1) * SELF(0, 1) + SELF(1, 1) * SELF(1, 1) + SELF(2, 1) * SELF(2, 1));
+	SELF(0, 1) *= len;
+	SELF(1, 1) *= len;
+	SELF(2, 1) *= len;*/
 
 	//setColumns(xAxis, yAxis, zAxis);
 }
@@ -609,30 +681,30 @@ inline void Mat3::rotateZAxis(float rad)
 // transpose
 inline void Mat3::transpose()
 {
-	float temp = ME(0, 1);
-	ME(0, 1) = ME(1, 0);
-	ME(1, 0) = temp;
-	temp = ME(0, 2);
-	ME(0, 2) = ME(2, 0);
-	ME(2, 0) = temp;
-	temp = ME(1, 2);
-	ME(1, 2) = ME(2, 1);
-	ME(2, 1) = temp;
+	float temp = SELF(0, 1);
+	SELF(0, 1) = SELF(1, 0);
+	SELF(1, 0) = temp;
+	temp = SELF(0, 2);
+	SELF(0, 2) = SELF(2, 0);
+	SELF(2, 0) = temp;
+	temp = SELF(1, 2);
+	SELF(1, 2) = SELF(2, 1);
+	SELF(2, 1) = temp;
 }
 
 // transposed
 inline Mat3 Mat3::getTransposed() const
 {
 	Mat3 m3;
-	m3[0] = ME[0];
-	m3[1] = ME[3];
-	m3[2] = ME[6];
-	m3[3] = ME[1];
-	m3[4] = ME[4];
-	m3[5] = ME[7];
-	m3[6] = ME[2];
-	m3[7] = ME[5];
-	m3[8] = ME[8];
+	m3[0] = SELF[0];
+	m3[1] = SELF[3];
+	m3[2] = SELF[6];
+	m3[3] = SELF[1];
+	m3[4] = SELF[4];
+	m3[5] = SELF[7];
+	m3[6] = SELF[2];
+	m3[7] = SELF[5];
+	m3[8] = SELF[8];
 	return m3;
 }
 
@@ -643,10 +715,10 @@ inline void Mat3::reorthogonalize()
 	/*Mat3 correction_m3 =
 	(
 		(Mat3::ident * 3.0f) -
-		(ME * ME.transposed())
+		(SELF * SELF.transposed())
 	) * 0.5f;
 
-	ME = correction_m3 * ME;*/
+	SELF = correction_m3 * SELF;*/
 
 	// method 2: Gram-Schmidt method with a twist for zAxis
 	Vec3 xAxis, yAxis, zAxis;
@@ -666,55 +738,55 @@ inline void Mat3::reorthogonalize()
 inline float Mat3::getDet() const
 {
 	/* accurate method:
-	return ME(0, 0)*ME(1, 1)*ME(2, 2) + ME(0, 1)*ME(1, 2)*ME(2, 0) + ME(0, 2)*ME(1, 0)*ME(2, 1)
-	- ME(0, 0)*ME(1, 2)*ME(2, 1) - ME(0, 1)*ME(1, 0)*ME(2, 2) - ME(0, 2)*ME(1, 1)*ME(2, 0);*/
-	return ME(0, 0)*(ME(1, 1)*ME(2, 2) - ME(1, 2)*ME(2, 1)) -
-	ME(0, 1)*(ME(1, 0)*ME(2, 2) - ME(1, 2)*ME(2, 0)) +
-	ME(0, 2)*(ME(0, 1)*ME(2, 1) - ME(1, 1)*ME(2, 0));
+	return SELF(0, 0) * SELF(1, 1) * SELF(2, 2) + SELF(0, 1) * SELF(1, 2) * SELF(2, 0) + SELF(0, 2) * SELF(1, 0) * SELF(2, 1)
+	- SELF(0, 0) * SELF(1, 2) * SELF(2, 1) - SELF(0, 1) * SELF(1, 0) * SELF(2, 2) - SELF(0, 2) * SELF(1, 1) * SELF(2, 0);*/
+	return SELF(0, 0)*(SELF(1, 1) * SELF(2, 2) - SELF(1, 2) * SELF(2, 1)) -
+	SELF(0, 1)*(SELF(1, 0) * SELF(2, 2) - SELF(1, 2) * SELF(2, 0)) +
+	SELF(0, 2)*(SELF(0, 1) * SELF(2, 1) - SELF(1, 1) * SELF(2, 0));
 }
 
 // getInverse
 // using Gramer's method (Inv(A) = (1/getDet(A)) * Adj(A))
 inline Mat3 Mat3::getInverse() const
 {
-	Mat3 result;
+	Mat3 r;
 
 	// compute determinant
-	float cofactor0 = ME(1, 1)*ME(2, 2) - ME(1, 2)*ME(2, 1);
-	float cofactor3 = ME(0, 2)*ME(2, 1) - ME(0, 1)*ME(2, 2);
-	float cofactor6 = ME(0, 1)*ME(1, 2) - ME(0, 2)*ME(1, 1);
-	float det = ME(0, 0)*cofactor0 + ME(1, 0)*cofactor3 + ME(2, 0)*cofactor6;
+	float cofactor0 = SELF(1, 1) * SELF(2, 2) - SELF(1, 2) * SELF(2, 1);
+	float cofactor3 = SELF(0, 2) * SELF(2, 1) - SELF(0, 1) * SELF(2, 2);
+	float cofactor6 = SELF(0, 1) * SELF(1, 2) - SELF(0, 2) * SELF(1, 1);
+	float det = SELF(0, 0) * cofactor0 + SELF(1, 0) * cofactor3 + SELF(2, 0) * cofactor6;
 
 	RASSERT_THROW_EXCEPTION(isZero(det)); // Cannot invert det == 0
 
 	// create adjoint matrix and multiply by 1/det to get inverse
-	float invDet = 1.0f/det;
-	result(0, 0) = invDet*cofactor0;
-	result(0, 1) = invDet*cofactor3;
-	result(0, 2) = invDet*cofactor6;
+	float invDet = 1.0 / det;
+	r(0, 0) = invDet * cofactor0;
+	r(0, 1) = invDet * cofactor3;
+	r(0, 2) = invDet * cofactor6;
 
-	result(1, 0) = invDet*(ME(1, 2)*ME(2, 0) - ME(1, 0)*ME(2, 2));
-	result(1, 1) = invDet*(ME(0, 0)*ME(2, 2) - ME(0, 2)*ME(2, 0));
-	result(1, 2) = invDet*(ME(0, 2)*ME(1, 0) - ME(0, 0)*ME(1, 2));
+	r(1, 0) = invDet * (SELF(1, 2) * SELF(2, 0) - SELF(1, 0) * SELF(2, 2));
+	r(1, 1) = invDet * (SELF(0, 0) * SELF(2, 2) - SELF(0, 2) * SELF(2, 0));
+	r(1, 2) = invDet * (SELF(0, 2) * SELF(1, 0) - SELF(0, 0) * SELF(1, 2));
 
-	result(2, 0) = invDet*(ME(1, 0)*ME(2, 1) - ME(1, 1)*ME(2, 0));
-	result(2, 1) = invDet*(ME(0, 1)*ME(2, 0) - ME(0, 0)*ME(2, 1));
-	result(2, 2) = invDet*(ME(0, 0)*ME(1, 1) - ME(0, 1)*ME(1, 0));
+	r(2, 0) = invDet * (SELF(1, 0) * SELF(2, 1) - SELF(1, 1) * SELF(2, 0));
+	r(2, 1) = invDet * (SELF(0, 1) * SELF(2, 0) - SELF(0, 0) * SELF(2, 1));
+	r(2, 2) = invDet * (SELF(0, 0) * SELF(1, 1) - SELF(0, 1) * SELF(1, 0));
 
-	return result;
+	return r;
 }
 
 // setIdentity
 inline void Mat3::setIdentity()
 {
-	ME = getIdentity();
+	SELF = getIdentity();
 }
 
 // invert
 // see above
 inline void Mat3::invert()
 {
-	ME = getInverse();
+	SELF = getInverse();
 }
 
 // getZero

src/Math/Mat4.h

@@ -10,6 +10,11 @@ namespace M {
 /// Used mainly for transformations but not necessarily. Its row major
 class Mat4
 {
+	friend Mat4 operator+(float f, const Mat4& m4);
+	friend Mat4 operator-(float f, const Mat4& m4);
+	friend Mat4 operator*(float f, const Mat4& m4);
+	friend Mat4 operator/(float f, const Mat4& m4);
+
 	public:
 		/// @name Constructors & distructors
 		/// @{
@@ -80,8 +85,8 @@ class Mat4
 		void  transpose();
 		Mat4  getTransposed() const;
 		float getDet() const;
-		void  invert();
-		Mat4  getInverse() const;
+		Mat4  getInverse() const; ///< Invert using Cramer's rule
+		void  invert(); ///< See getInverse
 		Mat4  getInverseTransformation() const;
 		Mat4  lerp(const Mat4& b, float t) const;
 		void  setIdentity();

src/Math/Mat4.inl.h

@@ -357,54 +357,99 @@ inline Mat4 operator+(float f, const Mat4& m4)
 // 4x4 += float
 inline Mat4& Mat4::operator+=(float f)
 {
-	for(int i = 0; i < 16; i++)
-	{
-		SELF[i] += f;
-	}
+	#if defined(MATH_INTEL_SIMD)
+		__m128 mm;
+		mm = _mm_set1_ps(f);
+		for(int i = 0; i < 4; i++)
+		{
+			arrMm[i] = _mm_add_ps(arrMm[i], mm);
+		}
+	#else
+		for(int i = 0; i < 16; i++)
+		{
+			SELF[i] += f;
+		}
+	#endif
 	return SELF;
 }
 
 // 4x4 - float
 inline Mat4 Mat4::operator-(float f) const
 {
-	Mat4 c;
-	for(int i = 0; i < 16; i++)
-	{
-		c[i] = SELF[i] - f;
-	}
-	return c;
+	Mat4 r;
+	#if defined(MATH_INTEL_SIMD)
+		__m128 mm;
+		mm = _mm_set1_ps(f);
+		for(int i = 0; i < 4; i++)
+		{
+			r.arrMm[i] = _mm_sub_ps(arrMm[i], mm);
+		}
+	#else
+		for(int i = 0; i < 16; i++)
+		{
+			r[i] = SELF[i] - f;
+		}
+	#endif
+	return r;
 }
 
 // float - 4x4
 inline Mat4 operator-(float f, const Mat4& m4)
 {
-	Mat4 out;
-	for(int i = 0; i < 16; i++)
-	{
-		out[i] = f- m4[i];
-	}
-	return out;
+	Mat4 r;
+	#if defined(MATH_INTEL_SIMD)
+		__m128 mm;
+		mm = _mm_set1_ps(f);
+		for(int i = 0; i < 4; i++)
+		{
+			r.arrMm[i] = _mm_sub_ps(mm, m4.arrMm[i]);
+		}
+	#else
+		for(int i = 0; i < 16; i++)
+		{
+			r[i] = f - m4[i];
+		}
+	#endif
+	return r;
 }
 
-// 4x4 - float (self)
+// 4x4 -= float
 inline Mat4& Mat4::operator-=(float f)
 {
-	for(int i = 0; i < 16; i++)
-	{
-		SELF[i] -= f;
-	}
+	#if defined(MATH_INTEL_SIMD)
+		__m128 mm;
+		mm = _mm_set1_ps(f);
+		for(int i = 0; i < 4; i++)
+		{
+			arrMm[i] = _mm_sub_ps(arrMm[i], mm);
+		}
+	#else
+		for(int i = 0; i < 16; i++)
+		{
+			SELF[i] -= f;
+		}
+	#endif
 	return SELF;
 }
 
 // 4x4 * float
 inline Mat4 Mat4::operator*(float f) const
 {
-	Mat4 c;
-	for(int i = 0; i < 16; i++)
-	{
-		c[i] = SELF[i] * f;
-	}
-	return c;
+	Mat4 r;
+	#if defined(MATH_INTEL_SIMD)
+		__m128 mm;
+		mm = _mm_set1_ps(f);
+		for(int i = 0; i < 4; i++)
+		{
+			r.arrMm[i] = _mm_mul_ps(arrMm[i], mm);
+		}
+	#else
+		for(int i = 0; i < 16; i++)
+		{
+			r[i] = SELF[i] * f;
+		}
+	#endif
+	return r;
 }
 
 // float * 4x4
@@ -416,42 +461,78 @@ inline Mat4 operator*(float f, const Mat4& m4)
 // 4x4 *= float
 inline Mat4& Mat4::operator*=(float f)
 {
-	for(int i = 0; i < 16; i++)
-	{
-		SELF[i] *= f;
-	}
+	#if defined(MATH_INTEL_SIMD)
+		__m128 mm;
+		mm = _mm_set1_ps(f);
+		for(int i = 0; i < 4; i++)
+		{
+			arrMm[i] = _mm_mul_ps(arrMm[i], mm);
+		}
+	#else
+		for(int i = 0; i < 16; i++)
+		{
+			SELF[i] *= f;
+		}
+	#endif
 	return SELF;
 }
 
 // 4x4 / float
 inline Mat4 Mat4::operator/(float f) const
 {
-	Mat4 c;
-	for(int i = 0; i < 16; i++)
-	{
-		c[i] = SELF[i] / f;
-	}
-	return c;
+	Mat4 r;
+	#if defined(MATH_INTEL_SIMD)
+		__m128 mm;
+		mm = _mm_set1_ps(f);
+		for(int i = 0; i < 4; i++)
+		{
+			r.arrMm[i] = _mm_mul_ps(arrMm[i], mm);
+		}
+	#else
+		for(int i = 0; i < 16; i++)
+		{
+			r[i] = SELF[i] / f;
+		}
+	#endif
+	return r;
 }
 
 // float / 4x4
 inline Mat4 operator/(float f, const Mat4& m4)
 {
-	Mat4 out;
-	for(uint i = 0; i < 16; i++)
-	{
-		out[i] = f / m4[i];
-	}
-	return out;
+	Mat4 r;
+	#if defined(MATH_INTEL_SIMD)
+		__m128 mm;
+		mm = _mm_set1_ps(f);
+		for(int i = 0; i < 4; i++)
+		{
+			r.arrMm[i] = _mm_div_ps(mm, m4.arrMm[i]);
+		}
+	#else
+		for(int i = 0; i < 16; i++)
+		{
+			r[i] = f / m4[i];
+		}
+	#endif
+	return r;
 }
 
 // 4x4 /= float
 inline Mat4& Mat4::operator/=(float f)
 {
-	for(int i = 0; i < 16; i++)
-	{
-		SELF[i] /= f;
-	}
+	#if defined(MATH_INTEL_SIMD)
+		__m128 mm;
+		mm = _mm_set1_ps(f);
+		for(int i = 0; i < 4; i++)
+		{
+			arrMm[i] = _mm_div_ps(arrMm[i], mm);
+		}
+	#else
+		for(int i = 0; i < 16; i++)
+		{
+			SELF[i] /= f;
+		}
+	#endif
 	return SELF;
 }
 
@@ -582,7 +663,6 @@ inline void Mat4::transpose()
 }
 
 // getTransposed
-// return the transposed
 inline Mat4 Mat4::getTransposed() const
 {
 	Mat4 m4;
@@ -690,96 +770,175 @@ inline float Mat4::getDet() const
 	SELF(0, 1) * SELF(1, 0) * SELF(2, 2) * SELF(3, 3) + SELF(0, 0) * SELF(1, 1) * SELF(2, 2) * SELF(3, 3);
 }
 
-// invert
-inline void Mat4::invert()
+// getInverse
+inline Mat4 Mat4::getInverse() const
 {
-	SELF = getInverse();
+	#if defined(MATH_INTEL_SIMD)
+		Mat4 r(SELF);
+		__m128 minor0, minor1, minor2, minor3;
+		__m128 det, tmp1;
+
+		// Transpose
+		r.transpose();
+
+		// Calc coeffs
+		tmp1 = _mm_mul_ps(r.arrMm[2], r.arrMm[3]);
+		tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
+		minor0 = _mm_mul_ps(r.arrMm[1], tmp1);
+		minor1 = _mm_mul_ps(r.arrMm[0], tmp1);
+		tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
+		minor0 = _mm_sub_ps(_mm_mul_ps(r.arrMm[1], tmp1), minor0);
+		minor1 = _mm_sub_ps(_mm_mul_ps(r.arrMm[0], tmp1), minor1);
+		minor1 = _mm_shuffle_ps(minor1, minor1, 0x4E);
+
+		tmp1 = _mm_mul_ps(r.arrMm[1], r.arrMm[2]);
+		tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
+		minor0 = _mm_add_ps(_mm_mul_ps(r.arrMm[3], tmp1), minor0);
+		minor3 = _mm_mul_ps(r.arrMm[0], tmp1);
+		tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
+		minor0 = _mm_sub_ps(minor0, _mm_mul_ps(r.arrMm[3], tmp1));
+		minor3 = _mm_sub_ps(_mm_mul_ps(r.arrMm[0], tmp1), minor3);
+		minor3 = _mm_shuffle_ps(minor3, minor3, 0x4E);
+
+		tmp1 = _mm_mul_ps(_mm_shuffle_ps(r.arrMm[1], r.arrMm[1], 0x4E), r.arrMm[3]);
+		tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
+		r.arrMm[2] = _mm_shuffle_ps(r.arrMm[2], r.arrMm[2], 0x4E);
+		minor0 = _mm_add_ps(_mm_mul_ps(r.arrMm[2], tmp1), minor0);
+		minor2 = _mm_mul_ps(r.arrMm[0], tmp1);
+		tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
+		minor0 = _mm_sub_ps(minor0, _mm_mul_ps(r.arrMm[2], tmp1));
+		minor2 = _mm_sub_ps(_mm_mul_ps(r.arrMm[0], tmp1), minor2);
+		minor2 = _mm_shuffle_ps(minor2, minor2, 0x4E);
+
+		tmp1 = _mm_mul_ps(r.arrMm[0], r.arrMm[1]);
+		tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
+		minor2 = _mm_add_ps(_mm_mul_ps(r.arrMm[3], tmp1), minor2);
+		minor3 = _mm_sub_ps(_mm_mul_ps(r.arrMm[2], tmp1), minor3);
+		tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
+		minor2 = _mm_sub_ps(_mm_mul_ps(r.arrMm[3], tmp1), minor2);
+		minor3 = _mm_sub_ps(minor3, _mm_mul_ps(r.arrMm[2], tmp1));
+
+		tmp1 = _mm_mul_ps(r.arrMm[0], r.arrMm[3]);
+		tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
+		minor1 = _mm_sub_ps(minor1, _mm_mul_ps(r.arrMm[2], tmp1));
+		minor2 = _mm_add_ps(_mm_mul_ps(r.arrMm[1], tmp1), minor2);
+		tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
+		minor1 = _mm_add_ps(_mm_mul_ps(r.arrMm[2], tmp1), minor1);
+		minor2 = _mm_sub_ps(minor2, _mm_mul_ps(r.arrMm[1], tmp1));
+
+		tmp1 = _mm_mul_ps(r.arrMm[0], r.arrMm[2]);
+		tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
+		minor1 = _mm_add_ps(_mm_mul_ps(r.arrMm[3], tmp1), minor1);
+		minor3 = _mm_sub_ps(minor3, _mm_mul_ps(r.arrMm[1], tmp1));
+		tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
+		minor1 = _mm_sub_ps(minor1, _mm_mul_ps(r.arrMm[3], tmp1));
+		minor3 = _mm_add_ps(_mm_mul_ps(r.arrMm[1], tmp1), minor3);
+
+		// 1 / det
+		det = _mm_mul_ps(r.arrMm[0], minor0);
+		det = _mm_add_ps(_mm_shuffle_ps(det, det, 0x4E), det);
+		det = _mm_add_ss(_mm_shuffle_ps(det, det, 0xB1), det);
+		tmp1 = _mm_rcp_ss(det);
+		det = _mm_sub_ss(_mm_add_ss(tmp1, tmp1), _mm_mul_ss(det, _mm_mul_ss(tmp1, tmp1)));
+		det = _mm_shuffle_ps(det, det, 0x00);
+
+		// Mul and store
+		minor0 = _mm_mul_ps(det, minor0);
+		r.arrMm[0] = minor0;
+		minor1 = _mm_mul_ps(det, minor1);
+		r.arrMm[1] = minor1;
+		minor2 = _mm_mul_ps(det, minor2);
+		r.arrMm[2] = minor2;
+		minor3 = _mm_mul_ps(det, minor3);
+		r.arrMm[3] = minor3;
+
+		return r;
+	#else
+		float tmp[12];
+		float det;
+		const Mat4& in = SELF;
+		Mat4 m4;
+
+		tmp[0] = in(2, 2) * in(3, 3);
+		tmp[1] = in(3, 2) * in(2, 3);
+		tmp[2] = in(1, 2) * in(3, 3);
+		tmp[3] = in(3, 2) * in(1, 3);
+		tmp[4] = in(1, 2) * in(2, 3);
+		tmp[5] = in(2, 2) * in(1, 3);
+		tmp[6] = in(0, 2) * in(3, 3);
+		tmp[7] = in(3, 2) * in(0, 3);
+		tmp[8] = in(0, 2) * in(2, 3);
+		tmp[9] = in(2, 2) * in(0, 3);
+		tmp[10] = in(0, 2) * in(1, 3);
+		tmp[11] = in(1, 2) * in(0, 3);
+
+		m4(0, 0) =  tmp[0] * in(1, 1) + tmp[3] * in(2, 1) + tmp[4] * in(3, 1);
+		m4(0, 0) -= tmp[1] * in(1, 1) + tmp[2] * in(2, 1) + tmp[5] * in(3, 1);
+		m4(0, 1) =  tmp[1] * in(0, 1) + tmp[6] * in(2, 1) + tmp[9] * in(3, 1);
+		m4(0, 1) -= tmp[0] * in(0, 1) + tmp[7] * in(2, 1) + tmp[8] * in(3, 1);
+		m4(0, 2) =  tmp[2] * in(0, 1) + tmp[7] * in(1, 1) + tmp[10] * in(3, 1);
+		m4(0, 2) -= tmp[3] * in(0, 1) + tmp[6] * in(1, 1) + tmp[11] * in(3, 1);
+		m4(0, 3) =  tmp[5] * in(0, 1) + tmp[8] * in(1, 1) + tmp[11] * in(2, 1);
+		m4(0, 3) -= tmp[4] * in(0, 1) + tmp[9] * in(1, 1) + tmp[10] * in(2, 1);
+		m4(1, 0) =  tmp[1] * in(1, 0) + tmp[2] * in(2, 0) + tmp[5] * in(3, 0);
+		m4(1, 0) -= tmp[0] * in(1, 0) + tmp[3] * in(2, 0) + tmp[4] * in(3, 0);
+		m4(1, 1) =  tmp[0] * in(0, 0) + tmp[7] * in(2, 0) + tmp[8] * in(3, 0);
+		m4(1, 1) -= tmp[1] * in(0, 0) + tmp[6] * in(2, 0) + tmp[9] * in(3, 0);
+		m4(1, 2) =  tmp[3] * in(0, 0) + tmp[6] * in(1, 0) + tmp[11] * in(3, 0);
+		m4(1, 2) -= tmp[2] * in(0, 0) + tmp[7] * in(1, 0) + tmp[10] * in(3, 0);
+		m4(1, 3) =  tmp[4] * in(0, 0) + tmp[9] * in(1, 0) + tmp[10] * in(2, 0);
+		m4(1, 3) -= tmp[5] * in(0, 0) + tmp[8] * in(1, 0) + tmp[11] * in(2, 0);
+
+		tmp[0] = in(2, 0) * in(3, 1);
+		tmp[1] = in(3, 0) * in(2, 1);
+		tmp[2] = in(1, 0) * in(3, 1);
+		tmp[3] = in(3, 0) * in(1, 1);
+		tmp[4] = in(1, 0) * in(2, 1);
+		tmp[5] = in(2, 0) * in(1, 1);
+		tmp[6] = in(0, 0) * in(3, 1);
+		tmp[7] = in(3, 0) * in(0, 1);
+		tmp[8] = in(0, 0) * in(2, 1);
+		tmp[9] = in(2, 0) * in(0, 1);
+		tmp[10] = in(0, 0) * in(1, 1);
+		tmp[11] = in(1, 0) * in(0, 1);
+
+		m4(2, 0) = tmp[0] * in(1, 3) + tmp[3] * in(2, 3) + tmp[4] * in(3, 3);
+		m4(2, 0)-= tmp[1] * in(1, 3) + tmp[2] * in(2, 3) + tmp[5] * in(3, 3);
+		m4(2, 1) = tmp[1] * in(0, 3) + tmp[6] * in(2, 3) + tmp[9] * in(3, 3);
+		m4(2, 1)-= tmp[0] * in(0, 3) + tmp[7] * in(2, 3) + tmp[8] * in(3, 3);
+		m4(2, 2) = tmp[2] * in(0, 3) + tmp[7] * in(1, 3) + tmp[10] * in(3, 3);
+		m4(2, 2)-= tmp[3] * in(0, 3) + tmp[6] * in(1, 3) + tmp[11] * in(3, 3);
+		m4(2, 3) = tmp[5] * in(0, 3) + tmp[8] * in(1, 3) + tmp[11] * in(2, 3);
+		m4(2, 3)-= tmp[4] * in(0, 3) + tmp[9] * in(1, 3) + tmp[10] * in(2, 3);
+		m4(3, 0) = tmp[2] * in(2, 2) + tmp[5] * in(3, 2) + tmp[1] * in(1, 2);
+		m4(3, 0)-= tmp[4] * in(3, 2) + tmp[0] * in(1, 2) + tmp[3] * in(2, 2);
+		m4(3, 1) = tmp[8] * in(3, 2) + tmp[0] * in(0, 2) + tmp[7] * in(2, 2);
+		m4(3, 1)-= tmp[6] * in(2, 2) + tmp[9] * in(3, 2) + tmp[1] * in(0, 2);
+		m4(3, 2) = tmp[6] * in(1, 2) + tmp[11] * in(3, 2) + tmp[3] * in(0, 2);
+		m4(3, 2)-= tmp[10] * in(3, 2) + tmp[2] * in(0, 2) + tmp[7] * in(1, 2);
+		m4(3, 3) = tmp[10] * in(2, 2) + tmp[4] * in(0, 2) + tmp[9] * in(1, 2);
+		m4(3, 3)-= tmp[8] * in(1, 2) + tmp[11] * in(2, 2) + tmp[5] * in(0, 2);
+
+		det = SELF(0, 0) * m4(0, 0) + SELF(1, 0) * m4(0, 1) + SELF(2, 0) * m4(0, 2) + SELF(3, 0) * m4(0, 3);
+
+		RASSERT_THROW_EXCEPTION(isZero(det)); // Cannot invert, det == 0
+		det = 1.0 / det;
+		m4 *= det;
+		return m4;
+	#endif
 }
 
-// Inverted
-inline Mat4 Mat4::getInverse() const
+// invert
+inline void Mat4::invert()
 {
-	float tmp[12];
-	float det;
-	const Mat4& in = SELF;
-
-	Mat4 m4;
-
-
-	tmp[0] = in(2, 2) * in(3, 3);
-	tmp[1] = in(3, 2) * in(2, 3);
-	tmp[2] = in(1, 2) * in(3, 3);
-	tmp[3] = in(3, 2) * in(1, 3);
-	tmp[4] = in(1, 2) * in(2, 3);
-	tmp[5] = in(2, 2) * in(1, 3);
-	tmp[6] = in(0, 2) * in(3, 3);
-	tmp[7] = in(3, 2) * in(0, 3);
-	tmp[8] = in(0, 2) * in(2, 3);
-	tmp[9] = in(2, 2) * in(0, 3);
-	tmp[10] = in(0, 2) * in(1, 3);
-	tmp[11] = in(1, 2) * in(0, 3);
-
-	m4(0, 0) =  tmp[0]*in(1, 1) + tmp[3]*in(2, 1) + tmp[4]*in(3, 1);
-	m4(0, 0) -= tmp[1]*in(1, 1) + tmp[2]*in(2, 1) + tmp[5]*in(3, 1);
-	m4(0, 1) =  tmp[1]*in(0, 1) + tmp[6]*in(2, 1) + tmp[9]*in(3, 1);
-	m4(0, 1) -= tmp[0]*in(0, 1) + tmp[7]*in(2, 1) + tmp[8]*in(3, 1);
-	m4(0, 2) =  tmp[2]*in(0, 1) + tmp[7]*in(1, 1) + tmp[10]*in(3, 1);
-	m4(0, 2) -= tmp[3]*in(0, 1) + tmp[6]*in(1, 1) + tmp[11]*in(3, 1);
-	m4(0, 3) =  tmp[5]*in(0, 1) + tmp[8]*in(1, 1) + tmp[11]*in(2, 1);
-	m4(0, 3) -= tmp[4]*in(0, 1) + tmp[9]*in(1, 1) + tmp[10]*in(2, 1);
-	m4(1, 0) =  tmp[1]*in(1, 0) + tmp[2]*in(2, 0) + tmp[5]*in(3, 0);
-	m4(1, 0) -= tmp[0]*in(1, 0) + tmp[3]*in(2, 0) + tmp[4]*in(3, 0);
-	m4(1, 1) =  tmp[0]*in(0, 0) + tmp[7]*in(2, 0) + tmp[8]*in(3, 0);
-	m4(1, 1) -= tmp[1]*in(0, 0) + tmp[6]*in(2, 0) + tmp[9]*in(3, 0);
-	m4(1, 2) =  tmp[3]*in(0, 0) + tmp[6]*in(1, 0) + tmp[11]*in(3, 0);
-	m4(1, 2) -= tmp[2]*in(0, 0) + tmp[7]*in(1, 0) + tmp[10]*in(3, 0);
-	m4(1, 3) =  tmp[4]*in(0, 0) + tmp[9]*in(1, 0) + tmp[10]*in(2, 0);
-	m4(1, 3) -= tmp[5]*in(0, 0) + tmp[8]*in(1, 0) + tmp[11]*in(2, 0);
-
-
-	tmp[0] = in(2, 0)*in(3, 1);
-	tmp[1] = in(3, 0)*in(2, 1);
-	tmp[2] = in(1, 0)*in(3, 1);
-	tmp[3] = in(3, 0)*in(1, 1);
-	tmp[4] = in(1, 0)*in(2, 1);
-	tmp[5] = in(2, 0)*in(1, 1);
-	tmp[6] = in(0, 0)*in(3, 1);
-	tmp[7] = in(3, 0)*in(0, 1);
-	tmp[8] = in(0, 0)*in(2, 1);
-	tmp[9] = in(2, 0)*in(0, 1);
-	tmp[10] = in(0, 0)*in(1, 1);
-	tmp[11] = in(1, 0)*in(0, 1);
-
-	m4(2, 0) = tmp[0]*in(1, 3) + tmp[3]*in(2, 3) + tmp[4]*in(3, 3);
-	m4(2, 0)-= tmp[1]*in(1, 3) + tmp[2]*in(2, 3) + tmp[5]*in(3, 3);
-	m4(2, 1) = tmp[1]*in(0, 3) + tmp[6]*in(2, 3) + tmp[9]*in(3, 3);
-	m4(2, 1)-= tmp[0]*in(0, 3) + tmp[7]*in(2, 3) + tmp[8]*in(3, 3);
-	m4(2, 2) = tmp[2]*in(0, 3) + tmp[7]*in(1, 3) + tmp[10]*in(3, 3);
-	m4(2, 2)-= tmp[3]*in(0, 3) + tmp[6]*in(1, 3) + tmp[11]*in(3, 3);
-	m4(2, 3) = tmp[5]*in(0, 3) + tmp[8]*in(1, 3) + tmp[11]*in(2, 3);
-	m4(2, 3)-= tmp[4]*in(0, 3) + tmp[9]*in(1, 3) + tmp[10]*in(2, 3);
-	m4(3, 0) = tmp[2]*in(2, 2) + tmp[5]*in(3, 2) + tmp[1]*in(1, 2);
-	m4(3, 0)-= tmp[4]*in(3, 2) + tmp[0]*in(1, 2) + tmp[3]*in(2, 2);
-	m4(3, 1) = tmp[8]*in(3, 2) + tmp[0]*in(0, 2) + tmp[7]*in(2, 2);
-	m4(3, 1)-= tmp[6]*in(2, 2) + tmp[9]*in(3, 2) + tmp[1]*in(0, 2);
-	m4(3, 2) = tmp[6]*in(1, 2) + tmp[11]*in(3, 2) + tmp[3]*in(0, 2);
-	m4(3, 2)-= tmp[10]*in(3, 2) + tmp[2]*in(0, 2) + tmp[7]*in(1, 2);
-	m4(3, 3) = tmp[10]*in(2, 2) + tmp[4]*in(0, 2) + tmp[9]*in(1, 2);
-	m4(3, 3)-= tmp[8]*in(1, 2) + tmp[11]*in(2, 2) + tmp[5]*in(0, 2);
-
-	det = SELF(0, 0)*m4(0, 0)+SELF(1, 0)*m4(0, 1)+SELF(2, 0)*m4(0, 2)+SELF(3, 0)*m4(0, 3);
-
-	RASSERT_THROW_EXCEPTION(isZero(det)); // Cannot invert, det == 0
-	det = 1.0/det;
-	m4 *= det;
-	return m4;
+	SELF = getInverse();
 }
 
 
 // getInverseTransformation
 inline Mat4 Mat4::getInverseTransformation() const
 {
-	Mat3 invertedRot = (getRotationPart()).getTransposed();
+	Mat3 invertedRot = getRotationPart().getTransposed();
 	Vec3 invertedTsl = getTranslationPart();
 	invertedTsl = -(invertedRot * invertedTsl);
 	return Mat4(invertedTsl, invertedRot);
@@ -788,7 +947,7 @@ inline Mat4 Mat4::getInverseTransformation() const
 // lerp
 inline Mat4 Mat4::lerp(const Mat4& b, float t) const
 {
-	return (SELF*(1.0-t))+(b*t);
+	return (SELF * (1.0 - t)) + (b * t);
 }
 
 // setIdentity
@@ -801,12 +960,12 @@ inline void Mat4::setIdentity()
 inline Mat4 Mat4::combineTransformations(const Mat4& m0, const Mat4& m1)
 {
 	/*
-	 * The clean code is:
-	 * Mat3 rot = m0.getRotationPart() * m1.getRotationPart();  // combine the rotations
-	 * Vec3 tra = (m1.getTranslationPart()).Transformed(m0.getTranslationPart(), m0.getRotationPart(), 1.0);
-	 * return Mat4(tra, rot);
-	 * and the optimized:
-	 */
+	The clean code is:
+	Mat3 rot = m0.getRotationPart() * m1.getRotationPart();  // combine the rotations
+	Vec3 tra = (m1.getTranslationPart()).Transformed(m0.getTranslationPart(), m0.getRotationPart(), 1.0);
+	return Mat4(tra, rot);
+	and the optimized:
+	*/
 	RASSERT_THROW_EXCEPTION(!isZero(m0(3, 0) + m0(3, 1) + m0(3, 2) + m0(3, 3)-1.0) ||
 	          !isZero(m1(3, 0) + m1(3, 1) + m1(3, 2) + m1(3, 3)-1.0)); // one of the 2 mat4 doesnt represent transformation
 
@@ -832,14 +991,15 @@ inline Mat4 Mat4::combineTransformations(const Mat4& m0, const Mat4& m1)
 	return m4;
 }
 
+
 //======================================================================================================================
 // Print                                                                                                               =
 //======================================================================================================================
 inline std::ostream& operator<<(std::ostream& s, const Mat4& m)
 {
-	for(int i=0; i<4; i++)
+	for(int i = 0; i < 4; i++)
 	{
-		for(int j=0; j<4; j++)
+		for(int j = 0; j < 4; j++)
 		{
 			s << m(i, j) << ' ';
 		}
@@ -852,4 +1012,5 @@ inline std::ostream& operator<<(std::ostream& s, const Mat4& m)
 	return s;
 }
 
+
 } // end namespace