X-Git-Url: http://git.xonotic.org/?p=xonotic%2Fdarkplaces.git;a=blobdiff_plain;f=matrixlib.c;h=dd6f3c6a348919f14b7620cf2a0ac1daea2e352e;hp=505f7d6e5ffcb560ffd9e0fc552fe158dded6f4f;hb=7c62e71683de5672379f033664504eb10d431544;hpb=f40047a4c83d9d5cd57c731c101296131e509332

diff --git a/matrixlib.c b/matrixlib.c
index 505f7d6e..dd6f3c6a 100644
--- a/matrixlib.c
+++ b/matrixlib.c
@@ -1,11 +1,9 @@
-#include "quakedef.h"
-
+#include "darkplaces.h"
 #include <math.h>
-#include "matrixlib.h"
 
 #ifdef _MSC_VER
-#pragma warning(disable : 4244)     // LordHavoc: MSVC++ 4 x86, double/float
-#pragma warning(disable : 4305)         // LordHavoc: MSVC++ 6 x86, double/float
+#pragma warning(disable : 4244)     // LadyHavoc: MSVC++ 4 x86, double/float
+#pragma warning(disable : 4305)         // LadyHavoc: MSVC++ 6 x86, double/float
 #endif
 
 const matrix4x4_t identitymatrix =
@@ -141,6 +139,117 @@ void Matrix4x4_Transpose (matrix4x4_t *out, const matrix4x4_t *in1)
 	out->m[3][3] = in1->m[3][3];
 }
 
+#if 1
+// Adapted from code contributed to Mesa by David Moore (Mesa 7.6 under SGI Free License B - which is MIT/X11-type)
+// added helper for common subexpression elimination by eihrul, and other optimizations by div0
+int Matrix4x4_Invert_Full (matrix4x4_t *out, const matrix4x4_t *in1)
+{
+		float det;
+
+		// note: orientation does not matter, as transpose(invert(transpose(m))) == invert(m), proof:
+		//   transpose(invert(transpose(m))) * m
+		// = transpose(invert(transpose(m))) * transpose(transpose(m))
+		// = transpose(transpose(m) * invert(transpose(m)))
+		// = transpose(identity)
+		// = identity
+
+		// this seems to help gcc's common subexpression elimination, and also makes the code look nicer
+		float   m00 = in1->m[0][0], m01 = in1->m[0][1], m02 = in1->m[0][2], m03 = in1->m[0][3],
+				m10 = in1->m[1][0], m11 = in1->m[1][1], m12 = in1->m[1][2], m13 = in1->m[1][3],
+				m20 = in1->m[2][0], m21 = in1->m[2][1], m22 = in1->m[2][2], m23 = in1->m[2][3],
+				m30 = in1->m[3][0], m31 = in1->m[3][1], m32 = in1->m[3][2], m33 = in1->m[3][3];
+
+		// calculate the adjoint
+		out->m[0][0] =  (m11*(m22*m33 - m23*m32) - m21*(m12*m33 - m13*m32) + m31*(m12*m23 - m13*m22));
+		out->m[0][1] = -(m01*(m22*m33 - m23*m32) - m21*(m02*m33 - m03*m32) + m31*(m02*m23 - m03*m22));
+		out->m[0][2] =  (m01*(m12*m33 - m13*m32) - m11*(m02*m33 - m03*m32) + m31*(m02*m13 - m03*m12));
+		out->m[0][3] = -(m01*(m12*m23 - m13*m22) - m11*(m02*m23 - m03*m22) + m21*(m02*m13 - m03*m12));
+		out->m[1][0] = -(m10*(m22*m33 - m23*m32) - m20*(m12*m33 - m13*m32) + m30*(m12*m23 - m13*m22));
+		out->m[1][1] =  (m00*(m22*m33 - m23*m32) - m20*(m02*m33 - m03*m32) + m30*(m02*m23 - m03*m22));
+		out->m[1][2] = -(m00*(m12*m33 - m13*m32) - m10*(m02*m33 - m03*m32) + m30*(m02*m13 - m03*m12));
+		out->m[1][3] =  (m00*(m12*m23 - m13*m22) - m10*(m02*m23 - m03*m22) + m20*(m02*m13 - m03*m12));
+		out->m[2][0] =  (m10*(m21*m33 - m23*m31) - m20*(m11*m33 - m13*m31) + m30*(m11*m23 - m13*m21));
+		out->m[2][1] = -(m00*(m21*m33 - m23*m31) - m20*(m01*m33 - m03*m31) + m30*(m01*m23 - m03*m21));
+		out->m[2][2] =  (m00*(m11*m33 - m13*m31) - m10*(m01*m33 - m03*m31) + m30*(m01*m13 - m03*m11));
+		out->m[2][3] = -(m00*(m11*m23 - m13*m21) - m10*(m01*m23 - m03*m21) + m20*(m01*m13 - m03*m11));
+		out->m[3][0] = -(m10*(m21*m32 - m22*m31) - m20*(m11*m32 - m12*m31) + m30*(m11*m22 - m12*m21));
+		out->m[3][1] =  (m00*(m21*m32 - m22*m31) - m20*(m01*m32 - m02*m31) + m30*(m01*m22 - m02*m21));
+		out->m[3][2] = -(m00*(m11*m32 - m12*m31) - m10*(m01*m32 - m02*m31) + m30*(m01*m12 - m02*m11));
+		out->m[3][3] =  (m00*(m11*m22 - m12*m21) - m10*(m01*m22 - m02*m21) + m20*(m01*m12 - m02*m11));
+
+		// calculate the determinant (as inverse == 1/det * adjoint, adjoint * m == identity * det, so this calculates the det)
+		det = m00*out->m[0][0] + m10*out->m[0][1] + m20*out->m[0][2] + m30*out->m[0][3];
+		if (det == 0.0f)
+				return 0;
+
+		// multiplications are faster than divisions, usually
+		det = 1.0f / det;
+
+		// manually unrolled loop to multiply all matrix elements by 1/det
+		out->m[0][0] *= det; out->m[0][1] *= det; out->m[0][2] *= det; out->m[0][3] *= det;
+		out->m[1][0] *= det; out->m[1][1] *= det; out->m[1][2] *= det; out->m[1][3] *= det;
+		out->m[2][0] *= det; out->m[2][1] *= det; out->m[2][2] *= det; out->m[2][3] *= det;
+		out->m[3][0] *= det; out->m[3][1] *= det; out->m[3][2] *= det; out->m[3][3] *= det;
+
+		return 1;
+}
+#elif 1
+// Adapted from code contributed to Mesa by David Moore (Mesa 7.6 under SGI Free License B - which is MIT/X11-type)
+int Matrix4x4_Invert_Full (matrix4x4_t *out, const matrix4x4_t *in1)
+{
+	matrix4x4_t temp;
+	double det;
+	int i, j;
+
+#ifdef MATRIX4x4_OPENGLORIENTATION
+	temp.m[0][0] =  in1->m[1][1]*in1->m[2][2]*in1->m[3][3] - in1->m[1][1]*in1->m[2][3]*in1->m[3][2] - in1->m[2][1]*in1->m[1][2]*in1->m[3][3] + in1->m[2][1]*in1->m[1][3]*in1->m[3][2] + in1->m[3][1]*in1->m[1][2]*in1->m[2][3] - in1->m[3][1]*in1->m[1][3]*in1->m[2][2];
+	temp.m[1][0] = -in1->m[1][0]*in1->m[2][2]*in1->m[3][3] + in1->m[1][0]*in1->m[2][3]*in1->m[3][2] + in1->m[2][0]*in1->m[1][2]*in1->m[3][3] - in1->m[2][0]*in1->m[1][3]*in1->m[3][2] - in1->m[3][0]*in1->m[1][2]*in1->m[2][3] + in1->m[3][0]*in1->m[1][3]*in1->m[2][2];
+	temp.m[2][0] =  in1->m[1][0]*in1->m[2][1]*in1->m[3][3] - in1->m[1][0]*in1->m[2][3]*in1->m[3][1] - in1->m[2][0]*in1->m[1][1]*in1->m[3][3] + in1->m[2][0]*in1->m[1][3]*in1->m[3][1] + in1->m[3][0]*in1->m[1][1]*in1->m[2][3] - in1->m[3][0]*in1->m[1][3]*in1->m[2][1];
+	temp.m[3][0] = -in1->m[1][0]*in1->m[2][1]*in1->m[3][2] + in1->m[1][0]*in1->m[2][2]*in1->m[3][1] + in1->m[2][0]*in1->m[1][1]*in1->m[3][2] - in1->m[2][0]*in1->m[1][2]*in1->m[3][1] - in1->m[3][0]*in1->m[1][1]*in1->m[2][2] + in1->m[3][0]*in1->m[1][2]*in1->m[2][1];
+	temp.m[0][1] = -in1->m[0][1]*in1->m[2][2]*in1->m[3][3] + in1->m[0][1]*in1->m[2][3]*in1->m[3][2] + in1->m[2][1]*in1->m[0][2]*in1->m[3][3] - in1->m[2][1]*in1->m[0][3]*in1->m[3][2] - in1->m[3][1]*in1->m[0][2]*in1->m[2][3] + in1->m[3][1]*in1->m[0][3]*in1->m[2][2];
+	temp.m[1][1] =  in1->m[0][0]*in1->m[2][2]*in1->m[3][3] - in1->m[0][0]*in1->m[2][3]*in1->m[3][2] - in1->m[2][0]*in1->m[0][2]*in1->m[3][3] + in1->m[2][0]*in1->m[0][3]*in1->m[3][2] + in1->m[3][0]*in1->m[0][2]*in1->m[2][3] - in1->m[3][0]*in1->m[0][3]*in1->m[2][2];
+	temp.m[2][1] = -in1->m[0][0]*in1->m[2][1]*in1->m[3][3] + in1->m[0][0]*in1->m[2][3]*in1->m[3][1] + in1->m[2][0]*in1->m[0][1]*in1->m[3][3] - in1->m[2][0]*in1->m[0][3]*in1->m[3][1] - in1->m[3][0]*in1->m[0][1]*in1->m[2][3] + in1->m[3][0]*in1->m[0][3]*in1->m[2][1];
+	temp.m[3][1] =  in1->m[0][0]*in1->m[2][1]*in1->m[3][2] - in1->m[0][0]*in1->m[2][2]*in1->m[3][1] - in1->m[2][0]*in1->m[0][1]*in1->m[3][2] + in1->m[2][0]*in1->m[0][2]*in1->m[3][1] + in1->m[3][0]*in1->m[0][1]*in1->m[2][2] - in1->m[3][0]*in1->m[0][2]*in1->m[2][1];
+	temp.m[0][2] =  in1->m[0][1]*in1->m[1][2]*in1->m[3][3] - in1->m[0][1]*in1->m[1][3]*in1->m[3][2] - in1->m[1][1]*in1->m[0][2]*in1->m[3][3] + in1->m[1][1]*in1->m[0][3]*in1->m[3][2] + in1->m[3][1]*in1->m[0][2]*in1->m[1][3] - in1->m[3][1]*in1->m[0][3]*in1->m[1][2];
+	temp.m[1][2] = -in1->m[0][0]*in1->m[1][2]*in1->m[3][3] + in1->m[0][0]*in1->m[1][3]*in1->m[3][2] + in1->m[1][0]*in1->m[0][2]*in1->m[3][3] - in1->m[1][0]*in1->m[0][3]*in1->m[3][2] - in1->m[3][0]*in1->m[0][2]*in1->m[1][3] + in1->m[3][0]*in1->m[0][3]*in1->m[1][2];
+	temp.m[2][2] =  in1->m[0][0]*in1->m[1][1]*in1->m[3][3] - in1->m[0][0]*in1->m[1][3]*in1->m[3][1] - in1->m[1][0]*in1->m[0][1]*in1->m[3][3] + in1->m[1][0]*in1->m[0][3]*in1->m[3][1] + in1->m[3][0]*in1->m[0][1]*in1->m[1][3] - in1->m[3][0]*in1->m[0][3]*in1->m[1][1];
+	temp.m[3][2] = -in1->m[0][0]*in1->m[1][1]*in1->m[3][2] + in1->m[0][0]*in1->m[1][2]*in1->m[3][1] + in1->m[1][0]*in1->m[0][1]*in1->m[3][2] - in1->m[1][0]*in1->m[0][2]*in1->m[3][1] - in1->m[3][0]*in1->m[0][1]*in1->m[1][2] + in1->m[3][0]*in1->m[0][2]*in1->m[1][1];
+	temp.m[0][3] = -in1->m[0][1]*in1->m[1][2]*in1->m[2][3] + in1->m[0][1]*in1->m[1][3]*in1->m[2][2] + in1->m[1][1]*in1->m[0][2]*in1->m[2][3] - in1->m[1][1]*in1->m[0][3]*in1->m[2][2] - in1->m[2][1]*in1->m[0][2]*in1->m[1][3] + in1->m[2][1]*in1->m[0][3]*in1->m[1][2];
+	temp.m[1][3] =  in1->m[0][0]*in1->m[1][2]*in1->m[2][3] - in1->m[0][0]*in1->m[1][3]*in1->m[2][2] - in1->m[1][0]*in1->m[0][2]*in1->m[2][3] + in1->m[1][0]*in1->m[0][3]*in1->m[2][2] + in1->m[2][0]*in1->m[0][2]*in1->m[1][3] - in1->m[2][0]*in1->m[0][3]*in1->m[1][2];
+	temp.m[2][3] = -in1->m[0][0]*in1->m[1][1]*in1->m[2][3] + in1->m[0][0]*in1->m[1][3]*in1->m[2][1] + in1->m[1][0]*in1->m[0][1]*in1->m[2][3] - in1->m[1][0]*in1->m[0][3]*in1->m[2][1] - in1->m[2][0]*in1->m[0][1]*in1->m[1][3] + in1->m[2][0]*in1->m[0][3]*in1->m[1][1];
+	temp.m[3][3] =  in1->m[0][0]*in1->m[1][1]*in1->m[2][2] - in1->m[0][0]*in1->m[1][2]*in1->m[2][1] - in1->m[1][0]*in1->m[0][1]*in1->m[2][2] + in1->m[1][0]*in1->m[0][2]*in1->m[2][1] + in1->m[2][0]*in1->m[0][1]*in1->m[1][2] - in1->m[2][0]*in1->m[0][2]*in1->m[1][1];
+#else
+	temp.m[0][0] =  in1->m[1][1]*in1->m[2][2]*in1->m[3][3] - in1->m[1][1]*in1->m[3][2]*in1->m[2][3] - in1->m[1][2]*in1->m[2][1]*in1->m[3][3] + in1->m[1][2]*in1->m[3][1]*in1->m[2][3] + in1->m[1][3]*in1->m[2][1]*in1->m[3][2] - in1->m[1][3]*in1->m[3][1]*in1->m[2][2];
+	temp.m[0][1] = -in1->m[0][1]*in1->m[2][2]*in1->m[3][3] + in1->m[0][1]*in1->m[3][2]*in1->m[2][3] + in1->m[0][2]*in1->m[2][1]*in1->m[3][3] - in1->m[0][2]*in1->m[3][1]*in1->m[2][3] - in1->m[0][3]*in1->m[2][1]*in1->m[3][2] + in1->m[0][3]*in1->m[3][1]*in1->m[2][2];
+	temp.m[0][2] =  in1->m[0][1]*in1->m[1][2]*in1->m[3][3] - in1->m[0][1]*in1->m[3][2]*in1->m[1][3] - in1->m[0][2]*in1->m[1][1]*in1->m[3][3] + in1->m[0][2]*in1->m[3][1]*in1->m[1][3] + in1->m[0][3]*in1->m[1][1]*in1->m[3][2] - in1->m[0][3]*in1->m[3][1]*in1->m[1][2];
+	temp.m[0][3] = -in1->m[0][1]*in1->m[1][2]*in1->m[2][3] + in1->m[0][1]*in1->m[2][2]*in1->m[1][3] + in1->m[0][2]*in1->m[1][1]*in1->m[2][3] - in1->m[0][2]*in1->m[2][1]*in1->m[1][3] - in1->m[0][3]*in1->m[1][1]*in1->m[2][2] + in1->m[0][3]*in1->m[2][1]*in1->m[1][2];
+	temp.m[1][0] = -in1->m[1][0]*in1->m[2][2]*in1->m[3][3] + in1->m[1][0]*in1->m[3][2]*in1->m[2][3] + in1->m[1][2]*in1->m[2][0]*in1->m[3][3] - in1->m[1][2]*in1->m[3][0]*in1->m[2][3] - in1->m[1][3]*in1->m[2][0]*in1->m[3][2] + in1->m[1][3]*in1->m[3][0]*in1->m[2][2];
+	temp.m[1][1] =  in1->m[0][0]*in1->m[2][2]*in1->m[3][3] - in1->m[0][0]*in1->m[3][2]*in1->m[2][3] - in1->m[0][2]*in1->m[2][0]*in1->m[3][3] + in1->m[0][2]*in1->m[3][0]*in1->m[2][3] + in1->m[0][3]*in1->m[2][0]*in1->m[3][2] - in1->m[0][3]*in1->m[3][0]*in1->m[2][2];
+	temp.m[1][2] = -in1->m[0][0]*in1->m[1][2]*in1->m[3][3] + in1->m[0][0]*in1->m[3][2]*in1->m[1][3] + in1->m[0][2]*in1->m[1][0]*in1->m[3][3] - in1->m[0][2]*in1->m[3][0]*in1->m[1][3] - in1->m[0][3]*in1->m[1][0]*in1->m[3][2] + in1->m[0][3]*in1->m[3][0]*in1->m[1][2];
+	temp.m[1][3] =  in1->m[0][0]*in1->m[1][2]*in1->m[2][3] - in1->m[0][0]*in1->m[2][2]*in1->m[1][3] - in1->m[0][2]*in1->m[1][0]*in1->m[2][3] + in1->m[0][2]*in1->m[2][0]*in1->m[1][3] + in1->m[0][3]*in1->m[1][0]*in1->m[2][2] - in1->m[0][3]*in1->m[2][0]*in1->m[1][2];
+	temp.m[2][0] =  in1->m[1][0]*in1->m[2][1]*in1->m[3][3] - in1->m[1][0]*in1->m[3][1]*in1->m[2][3] - in1->m[1][1]*in1->m[2][0]*in1->m[3][3] + in1->m[1][1]*in1->m[3][0]*in1->m[2][3] + in1->m[1][3]*in1->m[2][0]*in1->m[3][1] - in1->m[1][3]*in1->m[3][0]*in1->m[2][1];
+	temp.m[2][1] = -in1->m[0][0]*in1->m[2][1]*in1->m[3][3] + in1->m[0][0]*in1->m[3][1]*in1->m[2][3] + in1->m[0][1]*in1->m[2][0]*in1->m[3][3] - in1->m[0][1]*in1->m[3][0]*in1->m[2][3] - in1->m[0][3]*in1->m[2][0]*in1->m[3][1] + in1->m[0][3]*in1->m[3][0]*in1->m[2][1];
+	temp.m[2][2] =  in1->m[0][0]*in1->m[1][1]*in1->m[3][3] - in1->m[0][0]*in1->m[3][1]*in1->m[1][3] - in1->m[0][1]*in1->m[1][0]*in1->m[3][3] + in1->m[0][1]*in1->m[3][0]*in1->m[1][3] + in1->m[0][3]*in1->m[1][0]*in1->m[3][1] - in1->m[0][3]*in1->m[3][0]*in1->m[1][1];
+	temp.m[2][3] = -in1->m[0][0]*in1->m[1][1]*in1->m[2][3] + in1->m[0][0]*in1->m[2][1]*in1->m[1][3] + in1->m[0][1]*in1->m[1][0]*in1->m[2][3] - in1->m[0][1]*in1->m[2][0]*in1->m[1][3] - in1->m[0][3]*in1->m[1][0]*in1->m[2][1] + in1->m[0][3]*in1->m[2][0]*in1->m[1][1];
+	temp.m[3][0] = -in1->m[1][0]*in1->m[2][1]*in1->m[3][2] + in1->m[1][0]*in1->m[3][1]*in1->m[2][2] + in1->m[1][1]*in1->m[2][0]*in1->m[3][2] - in1->m[1][1]*in1->m[3][0]*in1->m[2][2] - in1->m[1][2]*in1->m[2][0]*in1->m[3][1] + in1->m[1][2]*in1->m[3][0]*in1->m[2][1];
+	temp.m[3][1] =  in1->m[0][0]*in1->m[2][1]*in1->m[3][2] - in1->m[0][0]*in1->m[3][1]*in1->m[2][2] - in1->m[0][1]*in1->m[2][0]*in1->m[3][2] + in1->m[0][1]*in1->m[3][0]*in1->m[2][2] + in1->m[0][2]*in1->m[2][0]*in1->m[3][1] - in1->m[0][2]*in1->m[3][0]*in1->m[2][1];
+	temp.m[3][2] = -in1->m[0][0]*in1->m[1][1]*in1->m[3][2] + in1->m[0][0]*in1->m[3][1]*in1->m[1][2] + in1->m[0][1]*in1->m[1][0]*in1->m[3][2] - in1->m[0][1]*in1->m[3][0]*in1->m[1][2] - in1->m[0][2]*in1->m[1][0]*in1->m[3][1] + in1->m[0][2]*in1->m[3][0]*in1->m[1][1];
+	temp.m[3][3] =  in1->m[0][0]*in1->m[1][1]*in1->m[2][2] - in1->m[0][0]*in1->m[2][1]*in1->m[1][2] - in1->m[0][1]*in1->m[1][0]*in1->m[2][2] + in1->m[0][1]*in1->m[2][0]*in1->m[1][2] + in1->m[0][2]*in1->m[1][0]*in1->m[2][1] - in1->m[0][2]*in1->m[2][0]*in1->m[1][1];
+#endif
+
+	det = in1->m[0][0]*temp.m[0][0] + in1->m[1][0]*temp.m[0][1] + in1->m[2][0]*temp.m[0][2] + in1->m[3][0]*temp.m[0][3];
+	if (det == 0.0f)
+		return 0;
+
+	det = 1.0f / det;
+
+	for (i = 0;i < 4;i++)
+		for (j = 0;j < 4;j++)
+			out->m[i][j] = temp.m[i][j] * det;
+
+	return 1;
+}
+#else
 int Matrix4x4_Invert_Full (matrix4x4_t *out, const matrix4x4_t *in1)
 {
 	double	*temp;
@@ -308,6 +417,7 @@ int Matrix4x4_Invert_Full (matrix4x4_t *out, const matrix4x4_t *in1)
 
 	return 0;
 }
+#endif
 
 void Matrix4x4_Invert_Simple (matrix4x4_t *out, const matrix4x4_t *in1)
 {
@@ -778,6 +888,54 @@ void Matrix4x4_CreateFromQuakeEntity(matrix4x4_t *out, double x, double y, doubl
 	}
 }
 
+void Matrix4x4_QuakeToDuke3D(const matrix4x4_t *in, matrix4x4_t *out, double maxShearAngle)
+{
+	// Sorry - this isn't direct at all. We can't just use an alternative to
+	// Matrix4x4_CreateFromQuakeEntity as in some cases the input for
+	// generating the view matrix is generated externally.
+	vec3_t forward, left, up, angles;
+	double scaleforward, scaleleft, scaleup;
+#ifdef MATRIX4x4_OPENGLORIENTATION
+	VectorSet(forward, in->m[0][0], in->m[0][1], in->m[0][2]);
+	VectorSet(left, in->m[1][0], in->m[1][1], in->m[1][2]);
+	VectorSet(up, in->m[2][0], in->m[2][1], in->m[2][2]);
+#else
+	VectorSet(forward, in->m[0][0], in->m[1][0], in->m[2][0]);
+	VectorSet(left, in->m[0][1], in->m[1][1], in->m[2][1]);
+	VectorSet(up, in->m[0][2], in->m[1][2], in->m[2][2]);
+#endif
+	scaleforward = VectorNormalizeLength(forward);
+	scaleleft = VectorNormalizeLength(left);
+	scaleup = VectorNormalizeLength(up);
+	AnglesFromVectors(angles, forward, up, false);
+	AngleVectorsDuke3DFLU(angles, forward, left, up, maxShearAngle);
+	VectorScale(forward, scaleforward, forward);
+	VectorScale(left, scaleleft, left);
+	VectorScale(up, scaleup, up);
+	*out = *in;
+#ifdef MATRIX4x4_OPENGLORIENTATION
+	out->m[0][0] = forward[0];
+	out->m[1][0] = left[0];
+	out->m[2][0] = up[0];
+	out->m[0][1] = forward[1];
+	out->m[1][1] = left[1];
+	out->m[2][1] = up[1];
+	out->m[0][2] = forward[2];
+	out->m[1][2] = left[2];
+	out->m[2][2] = up[2];
+#else
+	out->m[0][0] = forward[0];
+	out->m[0][1] = left[0];
+	out->m[0][2] = up[0];
+	out->m[1][0] = forward[1];
+	out->m[1][1] = left[1];
+	out->m[1][2] = up[1];
+	out->m[2][0] = forward[2];
+	out->m[2][1] = left[2];
+	out->m[2][2] = up[2];
+#endif
+}
+
 void Matrix4x4_ToVectors(const matrix4x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
 {
 #ifdef MATRIX4x4_OPENGLORIENTATION
@@ -1315,12 +1473,116 @@ void Matrix4x4_FromOriginQuat(matrix4x4_t *m, double ox, double oy, double oz, d
 #endif
 }
 
-// LordHavoc: I got this code from:
+// see http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
+void Matrix4x4_ToOrigin3Quat4Float(const matrix4x4_t *m, float *origin, float *quat)
+{
+#if 0
+	float s;
+	quat[3] = sqrt(1.0f + m->m[0][0] + m->m[1][1] + m->m[2][2]) * 0.5f;
+	s = 0.25f / quat[3];
+#ifdef MATRIX4x4_OPENGLORIENTATION
+	origin[0] = m->m[3][0];
+	origin[1] = m->m[3][1];
+	origin[2] = m->m[3][2];
+	quat[0] = (m->m[1][2] - m->m[2][1]) * s;
+	quat[1] = (m->m[2][0] - m->m[0][2]) * s;
+	quat[2] = (m->m[0][1] - m->m[1][0]) * s;
+#else
+	origin[0] = m->m[0][3];
+	origin[1] = m->m[1][3];
+	origin[2] = m->m[2][3];
+	quat[0] = (m->m[2][1] - m->m[1][2]) * s;
+	quat[1] = (m->m[0][2] - m->m[2][0]) * s;
+	quat[2] = (m->m[1][0] - m->m[0][1]) * s;
+#endif
+
+#else
+
+#ifdef MATRIX4x4_OPENGLORIENTATION
+	float trace = m->m[0][0] + m->m[1][1] + m->m[2][2];
+	origin[0] = m->m[3][0];
+	origin[1] = m->m[3][1];
+	origin[2] = m->m[3][2];
+	if(trace > 0)
+	{
+		float r = sqrt(1.0f + trace), inv = 0.5f / r;
+		quat[0] = (m->m[1][2] - m->m[2][1]) * inv;
+		quat[1] = (m->m[2][0] - m->m[0][2]) * inv;
+		quat[2] = (m->m[0][1] - m->m[1][0]) * inv;
+		quat[3] = 0.5f * r;
+	}
+	else if(m->m[0][0] > m->m[1][1] && m->m[0][0] > m->m[2][2])
+	{
+		float r = sqrt(1.0f + m->m[0][0] - m->m[1][1] - m->m[2][2]), inv = 0.5f / r;
+		quat[0] = 0.5f * r;
+		quat[1] = (m->m[0][1] + m->m[1][0]) * inv;
+		quat[2] = (m->m[2][0] + m->m[0][2]) * inv;
+		quat[3] = (m->m[1][2] - m->m[2][1]) * inv;
+	}
+	else if(m->m[1][1] > m->m[2][2])
+	{
+		float r = sqrt(1.0f + m->m[1][1] - m->m[0][0] - m->m[2][2]), inv = 0.5f / r;
+		quat[0] = (m->m[0][1] + m->m[1][0]) * inv;
+		quat[1] = 0.5f * r;
+		quat[2] = (m->m[1][2] + m->m[2][1]) * inv;
+		quat[3] = (m->m[2][0] - m->m[0][2]) * inv;
+	}
+	else
+	{
+		float r = sqrt(1.0f + m->m[2][2] - m->m[0][0] - m->m[1][1]), inv = 0.5f / r;
+		quat[0] = (m->m[2][0] + m->m[0][2]) * inv;
+		quat[1] = (m->m[1][2] + m->m[2][1]) * inv;
+		quat[2] = 0.5f * r;
+		quat[3] = (m->m[0][1] - m->m[1][0]) * inv;
+	}
+#else
+	float trace = m->m[0][0] + m->m[1][1] + m->m[2][2];
+	origin[0] = m->m[0][3];
+	origin[1] = m->m[1][3];
+	origin[2] = m->m[2][3];
+	if(trace > 0)
+	{
+		float r = sqrt(1.0f + trace), inv = 0.5f / r;
+		quat[0] = (m->m[2][1] - m->m[1][2]) * inv;
+		quat[1] = (m->m[0][2] - m->m[2][0]) * inv;
+		quat[2] = (m->m[1][0] - m->m[0][1]) * inv;
+		quat[3] = 0.5f * r;
+	}
+	else if(m->m[0][0] > m->m[1][1] && m->m[0][0] > m->m[2][2])
+	{
+		float r = sqrt(1.0f + m->m[0][0] - m->m[1][1] - m->m[2][2]), inv = 0.5f / r;
+		quat[0] = 0.5f * r;
+		quat[1] = (m->m[1][0] + m->m[0][1]) * inv;
+		quat[2] = (m->m[0][2] + m->m[2][0]) * inv;
+		quat[3] = (m->m[2][1] - m->m[1][2]) * inv;
+	}
+	else if(m->m[1][1] > m->m[2][2])
+	{
+		float r = sqrt(1.0f + m->m[1][1] - m->m[0][0] - m->m[2][2]), inv = 0.5f / r;
+		quat[0] = (m->m[1][0] + m->m[0][1]) * inv;
+		quat[1] = 0.5f * r;
+		quat[2] = (m->m[2][1] + m->m[1][2]) * inv;
+		quat[3] = (m->m[0][2] - m->m[2][0]) * inv;
+	}
+	else
+	{
+		float r = sqrt(1.0f + m->m[2][2] - m->m[0][0] - m->m[1][1]), inv = 0.5f / r;
+		quat[0] = (m->m[0][2] + m->m[2][0]) * inv;
+		quat[1] = (m->m[2][1] + m->m[1][2]) * inv;
+		quat[2] = 0.5f * r;
+		quat[3] = (m->m[1][0] - m->m[0][1]) * inv;
+	}
+#endif
+
+#endif
+}
+
+// LadyHavoc: I got this code from:
 //http://www.doom3world.org/phpbb2/viewtopic.php?t=2884
 void Matrix4x4_FromDoom3Joint(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z)
 {
-	double w = 1.0 - (x*x+y*y+z*z);
-	w = w > 0.0 ? -sqrt(w) : 0.0;
+	double w = 1.0f - (x*x+y*y+z*z);
+	w = w > 0.0f ? -sqrt(w) : 0.0f;
 #ifdef MATRIX4x4_OPENGLORIENTATION
 	m->m[0][0]=1-2*(y*y+z*z);m->m[1][0]=  2*(x*y-z*w);m->m[2][0]=  2*(x*z+y*w);m->m[3][0]=ox;
 	m->m[0][1]=  2*(x*y+z*w);m->m[1][1]=1-2*(x*x+z*z);m->m[2][1]=  2*(y*z-x*w);m->m[3][1]=oy;
@@ -1334,6 +1596,42 @@ void Matrix4x4_FromDoom3Joint(matrix4x4_t *m, double ox, double oy, double oz, d
 #endif
 }
 
+void Matrix4x4_FromBonePose7s(matrix4x4_t *m, float originscale, const short *pose7s)
+{
+	float origin[3];
+	float quat[4];
+	float quatscale = pose7s[6] > 0 ? -1.0f / 32767.0f : 1.0f / 32767.0f;
+	origin[0] = pose7s[0] * originscale;
+	origin[1] = pose7s[1] * originscale;
+	origin[2] = pose7s[2] * originscale;
+	quat[0] = pose7s[3] * quatscale;
+	quat[1] = pose7s[4] * quatscale;
+	quat[2] = pose7s[5] * quatscale;
+	quat[3] = pose7s[6] * quatscale;
+	Matrix4x4_FromOriginQuat(m, origin[0], origin[1], origin[2], quat[0], quat[1], quat[2], quat[3]);
+}
+
+void Matrix4x4_ToBonePose7s(const matrix4x4_t *m, float origininvscale, short *pose7s)
+{
+	float origin[3];
+	float quat[4];
+	float quatscale;
+	Matrix4x4_ToOrigin3Quat4Float(m, origin, quat);
+	// normalize quaternion so that it is unit length
+	quatscale = quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3];
+	if (quatscale)
+		quatscale = (quat[3] >= 0 ? -32767.0f : 32767.0f) / sqrt(quatscale);
+	// use a negative scale on the quat because the above function produces a
+	// positive quat[3] and canonical quaternions have negative quat[3]
+	pose7s[0] = origin[0] * origininvscale;
+	pose7s[1] = origin[1] * origininvscale;
+	pose7s[2] = origin[2] * origininvscale;
+	pose7s[3] = quat[0] * quatscale;
+	pose7s[4] = quat[1] * quatscale;
+	pose7s[5] = quat[2] * quatscale;
+	pose7s[6] = quat[3] * quatscale;
+}
+
 void Matrix4x4_Blend (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2, double blend)
 {
 	double iblend = 1 - blend;
@@ -1397,33 +1695,39 @@ void Matrix4x4_Transform3x3 (const matrix4x4_t *in, const float v[3], float out[
 #endif
 }
 
+// transforms a positive distance plane (A*x+B*y+C*z-D=0) through a rotation or translation matrix
 void Matrix4x4_TransformPositivePlane(const matrix4x4_t *in, float x, float y, float z, float d, float *o)
 {
+	float scale = sqrt(in->m[0][0] * in->m[0][0] + in->m[0][1] * in->m[0][1] + in->m[0][2] * in->m[0][2]);
+	float iscale = 1.0f / scale;
 #ifdef MATRIX4x4_OPENGLORIENTATION
-	o[0] = x * in->m[0][0] + y * in->m[1][0] + z * in->m[2][0];
-	o[1] = x * in->m[0][1] + y * in->m[1][1] + z * in->m[2][1];
-	o[2] = x * in->m[0][2] + y * in->m[1][2] + z * in->m[2][2];
-	o[3] = d + (x * in->m[3][0] + y * in->m[3][1] + z * in->m[3][2]);
+	o[0] = (x * in->m[0][0] + y * in->m[1][0] + z * in->m[2][0]) * iscale;
+	o[1] = (x * in->m[0][1] + y * in->m[1][1] + z * in->m[2][1]) * iscale;
+	o[2] = (x * in->m[0][2] + y * in->m[1][2] + z * in->m[2][2]) * iscale;
+	o[3] = d * scale + (o[0] * in->m[3][0] + o[1] * in->m[3][1] + o[2] * in->m[3][2]);
 #else
-	o[0] = x * in->m[0][0] + y * in->m[0][1] + z * in->m[0][2];
-	o[1] = x * in->m[1][0] + y * in->m[1][1] + z * in->m[1][2];
-	o[2] = x * in->m[2][0] + y * in->m[2][1] + z * in->m[2][2];
-	o[3] = d + (x * in->m[0][3] + y * in->m[1][3] + z * in->m[2][3]);
+	o[0] = (x * in->m[0][0] + y * in->m[0][1] + z * in->m[0][2]) * iscale;
+	o[1] = (x * in->m[1][0] + y * in->m[1][1] + z * in->m[1][2]) * iscale;
+	o[2] = (x * in->m[2][0] + y * in->m[2][1] + z * in->m[2][2]) * iscale;
+	o[3] = d * scale + (o[0] * in->m[0][3] + o[1] * in->m[1][3] + o[2] * in->m[2][3]);
 #endif
 }
 
+// transforms a standard plane (A*x+B*y+C*z+D=0) through a rotation or translation matrix
 void Matrix4x4_TransformStandardPlane(const matrix4x4_t *in, float x, float y, float z, float d, float *o)
 {
+	float scale = sqrt(in->m[0][0] * in->m[0][0] + in->m[0][1] * in->m[0][1] + in->m[0][2] * in->m[0][2]);
+	float iscale = 1.0f / scale;
 #ifdef MATRIX4x4_OPENGLORIENTATION
-	o[0] = x * in->m[0][0] + y * in->m[1][0] + z * in->m[2][0];
-	o[1] = x * in->m[0][1] + y * in->m[1][1] + z * in->m[2][1];
-	o[2] = x * in->m[0][2] + y * in->m[1][2] + z * in->m[2][2];
-	o[3] = d - (x * in->m[3][0] + y * in->m[3][1] + z * in->m[3][2]);
+	o[0] = (x * in->m[0][0] + y * in->m[1][0] + z * in->m[2][0]) * iscale;
+	o[1] = (x * in->m[0][1] + y * in->m[1][1] + z * in->m[2][1]) * iscale;
+	o[2] = (x * in->m[0][2] + y * in->m[1][2] + z * in->m[2][2]) * iscale;
+	o[3] = d * scale - (o[0] * in->m[3][0] + o[1] * in->m[3][1] + o[2] * in->m[3][2]);
 #else
-	o[0] = x * in->m[0][0] + y * in->m[0][1] + z * in->m[0][2];
-	o[1] = x * in->m[1][0] + y * in->m[1][1] + z * in->m[1][2];
-	o[2] = x * in->m[2][0] + y * in->m[2][1] + z * in->m[2][2];
-	o[3] = d - (x * in->m[0][3] + y * in->m[1][3] + z * in->m[2][3]);
+	o[0] = (x * in->m[0][0] + y * in->m[0][1] + z * in->m[0][2]) * iscale;
+	o[1] = (x * in->m[1][0] + y * in->m[1][1] + z * in->m[1][2]) * iscale;
+	o[2] = (x * in->m[2][0] + y * in->m[2][1] + z * in->m[2][2]) * iscale;
+	o[3] = d * scale - (o[0] * in->m[0][3] + o[1] * in->m[1][3] + o[2] * in->m[2][3]);
 #endif
 }
 
@@ -1551,3 +1855,17 @@ void Matrix4x4_Scale (matrix4x4_t *out, double rotatescale, double originscale)
 	out->m[2][3] *= originscale;
 #endif
 }
+
+void Matrix4x4_Abs (matrix4x4_t *out)
+{
+	out->m[0][0] = fabs(out->m[0][0]);
+	out->m[0][1] = fabs(out->m[0][1]);
+	out->m[0][2] = fabs(out->m[0][2]);
+	out->m[1][0] = fabs(out->m[1][0]);
+	out->m[1][1] = fabs(out->m[1][1]);
+	out->m[1][2] = fabs(out->m[1][2]);
+	out->m[2][0] = fabs(out->m[2][0]);
+	out->m[2][1] = fabs(out->m[2][1]);
+	out->m[2][2] = fabs(out->m[2][2]);
+}
+