7 files changed, 64 insertions, 10 deletions
diff --git a/indra/llmath/llmatrix4a.h b/indra/llmath/llmatrix4a.h
index 7ba347062f..5291a05607 100644
--- a/indra/llmath/llmatrix4a.h
+++ b/indra/llmath/llmatrix4a.h
@@ -36,6 +36,26 @@ class LLMatrix4a
 public:
 	LL_ALIGN_16(LLVector4a mMatrix[4]);
 
+    LLMatrix4a()
+    {
+
+    }
+
+    explicit LLMatrix4a(const LLMatrix4& val)
+    {
+        loadu(val);
+    }
+
+    inline F32* getF32ptr()
+    {
+        return (F32*) &mMatrix;
+    }
+
+    inline const F32* getF32ptr() const
+    {
+        return (F32*)&mMatrix;
+    }
+
 	inline void clear()
 	{
 		mMatrix[0].clear();
@@ -44,6 +64,14 @@ public:
 		mMatrix[3].clear();
 	}
 
+    inline void setIdentity()
+    {
+        mMatrix[0].set(1.f, 0.f, 0.f, 0.f);
+        mMatrix[1].set(0.f, 1.f, 0.f, 0.f);
+        mMatrix[2].set(0.f, 0.f, 1.f, 0.f);
+        mMatrix[3].set(0.f, 0.f, 0.f, 1.f);
+    }
+
 	inline void loadu(const LLMatrix4& src)
 	{
 		mMatrix[0] = _mm_loadu_ps(src.mMatrix[0]);
@@ -105,7 +133,7 @@ public:
 		mMatrix[3].setAdd(a.mMatrix[3],d3);
 	}
 
-	inline void rotate(const LLVector4a& v, LLVector4a& res)
+	inline void rotate(const LLVector4a& v, LLVector4a& res) const
 	{
 		LLVector4a y,z;
 
@@ -151,6 +179,8 @@ public:
     {
         affineTransformSSE(v,res);
     }
+
+    const LLVector4a& getTranslation() const { return mMatrix[3]; }
 };
 
 inline LLVector4a rowMul(const LLVector4a &row, const LLMatrix4a &mat)
@@ -176,6 +206,15 @@ inline void matMul(const LLMatrix4a &a, const LLMatrix4a &b, LLMatrix4a &res)
     res.mMatrix[3] = row3;
 }
 
+//Faster version of matMul wehere res must not be a or b
+inline void matMulUnsafe(const LLMatrix4a &a, const LLMatrix4a &b, LLMatrix4a &res)
+{
+    res.mMatrix[0] = rowMul(a.mMatrix[0], b);
+    res.mMatrix[1] = rowMul(a.mMatrix[1], b);
+    res.mMatrix[2] = rowMul(a.mMatrix[2], b);
+    res.mMatrix[3] = rowMul(a.mMatrix[3], b);
+}
+
 inline std::ostream& operator<<(std::ostream& s, const LLMatrix4a& m)
 {
     s << "[" << m.mMatrix[0] << ", " << m.mMatrix[1] << ", " << m.mMatrix[2] << ", " << m.mMatrix[3] << "]";
diff --git a/indra/llmath/llvector4a.h b/indra/llmath/llvector4a.h
index 27abf39537..5a02928374 100644
--- a/indra/llmath/llvector4a.h
+++ b/indra/llmath/llvector4a.h
@@ -46,10 +46,9 @@ class LLRotation;
 // of this writing, July 08, 2010) about getting it implemented before you resort to
 // LLVector3/LLVector4. 
 /////////////////////////////////
-struct LLVector4a;
 
 LL_ALIGN_PREFIX(16)
-struct LLVector4a
+class LLVector4a
 {
 public:
 
@@ -138,10 +137,10 @@ public:
 	// BASIC GET/SET 
 	////////////////////////////////////
 	
-	// Return a "this" as an F32 pointer. Do not use unless you have a very good reason.  (Not sure? Ask Falcon)
+	// Return a "this" as an F32 pointer.
 	inline F32* getF32ptr();
 	
-	// Return a "this" as a const F32 pointer. Do not use unless you have a very good reason.  (Not sure? Ask Falcon)
+	// Return a "this" as a const F32 pointer.
 	inline const F32* const getF32ptr() const;
 	
 	// Read-only access a single float in this vector. Do not use in proximity to any function call that manipulates
diff --git a/indra/llmath/llvector4a.inl b/indra/llmath/llvector4a.inl
index 69d3d01efe..8be1c1b114 100644
--- a/indra/llmath/llvector4a.inl
+++ b/indra/llmath/llvector4a.inl
@@ -58,13 +58,13 @@ inline void LLVector4a::store4a(F32* dst) const
 // BASIC GET/SET 
 ////////////////////////////////////
 
-// Return a "this" as an F32 pointer. Do not use unless you have a very good reason.  (Not sure? Ask Falcon)
+// Return a "this" as an F32 pointer.
 F32* LLVector4a::getF32ptr()
 {
 	return (F32*) &mQ;
 }
 
-// Return a "this" as a const F32 pointer. Do not use unless you have a very good reason.  (Not sure? Ask Falcon)
+// Return a "this" as a const F32 pointer.
 const F32* const LLVector4a::getF32ptr() const
 {
 	return (const F32* const) &mQ;
diff --git a/indra/llmath/m4math.cpp b/indra/llmath/m4math.cpp
index 3baf1bad18..6e40dae30b 100644
--- a/indra/llmath/m4math.cpp
+++ b/indra/llmath/m4math.cpp
@@ -32,8 +32,7 @@
 #include "m4math.h"
 #include "m3math.h"
 #include "llquaternion.h"
-
-
+#include "llmatrix4a.h"
 
 
 // LLMatrix4
@@ -115,6 +114,12 @@ LLMatrix4::LLMatrix4(const LLQuaternion &q)
 	*this = initRotation(q);
 }
 
+LLMatrix4::LLMatrix4(const LLMatrix4a& mat)
+    : LLMatrix4(mat.getF32ptr())
+{
+    
+}
+
 LLMatrix4::LLMatrix4(const LLQuaternion &q, const LLVector4 &pos)
 {
 	*this = initRotTrans(q, pos);
diff --git a/indra/llmath/m4math.h b/indra/llmath/m4math.h
index bf60adb9b6..b9da970cde 100644
--- a/indra/llmath/m4math.h
+++ b/indra/llmath/m4math.h
@@ -32,6 +32,7 @@
 class LLVector4;
 class LLMatrix3;
 class LLQuaternion;
+class LLMatrix4a;
 
 // NOTA BENE: Currently assuming a right-handed, x-forward, y-left, z-up universe
 
@@ -104,6 +105,7 @@ public:
 	explicit LLMatrix4(const F32 *mat);								// Initializes Matrix to values in mat
 	explicit LLMatrix4(const LLMatrix3 &mat);						// Initializes Matrix to values in mat and sets position to (0,0,0)
 	explicit LLMatrix4(const LLQuaternion &q);						// Initializes Matrix with rotation q and sets position to (0,0,0)
+    explicit LLMatrix4(const LLMatrix4a& mat);
 
 	LLMatrix4(const LLMatrix3 &mat, const LLVector4 &pos);	// Initializes Matrix to values in mat and pos
 
diff --git a/indra/llmath/v3math.cpp b/indra/llmath/v3math.cpp
index b04c67d926..93010d2250 100644
--- a/indra/llmath/v3math.cpp
+++ b/indra/llmath/v3math.cpp
@@ -316,6 +316,12 @@ LLVector3::LLVector3(const LLVector4 &vec)
 	mV[VZ] = (F32)vec.mV[VZ];
 }
 
+LLVector3::LLVector3(const LLVector4a& vec)
+    : LLVector3(vec.getF32ptr())
+{
+
+}
+
 LLVector3::LLVector3(const LLSD& sd)
 {
 	setValue(sd);
diff --git a/indra/llmath/v3math.h b/indra/llmath/v3math.h
index 6f857d7061..068f489020 100644
--- a/indra/llmath/v3math.h
+++ b/indra/llmath/v3math.h
@@ -33,6 +33,7 @@
 #include "llsd.h"
 class LLVector2;
 class LLVector4;
+class LLVector4a;
 class LLMatrix3;
 class LLMatrix4;
 class LLVector3d;
@@ -62,7 +63,9 @@ class LLVector3
 		explicit LLVector3(const LLVector2 &vec);				// Initializes LLVector3 to (vec[0]. vec[1], 0)
 		explicit LLVector3(const LLVector3d &vec);				// Initializes LLVector3 to (vec[0]. vec[1], vec[2])
 		explicit LLVector3(const LLVector4 &vec);				// Initializes LLVector4 to (vec[0]. vec[1], vec[2])
-		explicit LLVector3(const LLSD& sd);
+        explicit LLVector3(const LLVector4a& vec);              // Initializes LLVector4 to (vec[0]. vec[1], vec[2])
+        explicit LLVector3(const LLSD& sd);
+        
 
 		LLSD getValue() const;