matrixlib.c

   1 #include "quakedef.h"
   2
   3 #include <math.h>
   4 #include "matrixlib.h"
   5
   6 #ifdef _MSC_VER
   7 #pragma warning(disable : 4244)     // LordHavoc: MSVC++ 4 x86, double/float
   8 #pragma warning(disable : 4305)         // LordHavoc: MSVC++ 6 x86, double/float
   9 #endif
  10
  11 const matrix4x4_t identitymatrix =
  12 {
  13         {
  14                 {1, 0, 0, 0},
  15                 {0, 1, 0, 0},
  16                 {0, 0, 1, 0},
  17                 {0, 0, 0, 1}
  18         }
  19 };
  20
  21 void Matrix4x4_Copy (matrix4x4_t *out, const matrix4x4_t *in)
  22 {
  23         *out = *in;
  24 }
  25
  26 void Matrix4x4_CopyRotateOnly (matrix4x4_t *out, const matrix4x4_t *in)
  27 {
  28         out->m[0][0] = in->m[0][0];
  29         out->m[0][1] = in->m[0][1];
  30         out->m[0][2] = in->m[0][2];
  31         out->m[0][3] = 0.0f;
  32         out->m[1][0] = in->m[1][0];
  33         out->m[1][1] = in->m[1][1];
  34         out->m[1][2] = in->m[1][2];
  35         out->m[1][3] = 0.0f;
  36         out->m[2][0] = in->m[2][0];
  37         out->m[2][1] = in->m[2][1];
  38         out->m[2][2] = in->m[2][2];
  39         out->m[2][3] = 0.0f;
  40         out->m[3][0] = 0.0f;
  41         out->m[3][1] = 0.0f;
  42         out->m[3][2] = 0.0f;
  43         out->m[3][3] = 1.0f;
  44 }
  45
  46 void Matrix4x4_CopyTranslateOnly (matrix4x4_t *out, const matrix4x4_t *in)
  47 {
  48 #ifdef MATRIX4x4_OPENGLORIENTATION
  49         out->m[0][0] = 1.0f;
  50         out->m[1][0] = 0.0f;
  51         out->m[2][0] = 0.0f;
  52         out->m[3][0] = in->m[0][3];
  53         out->m[0][1] = 0.0f;
  54         out->m[1][1] = 1.0f;
  55         out->m[2][1] = 0.0f;
  56         out->m[3][1] = in->m[1][3];
  57         out->m[0][2] = 0.0f;
  58         out->m[1][2] = 0.0f;
  59         out->m[2][2] = 1.0f;
  60         out->m[3][2] = in->m[2][3];
  61         out->m[0][3] = 0.0f;
  62         out->m[1][3] = 0.0f;
  63         out->m[2][3] = 0.0f;
  64         out->m[3][3] = 1.0f;
  65 #else
  66         out->m[0][0] = 1.0f;
  67         out->m[0][1] = 0.0f;
  68         out->m[0][2] = 0.0f;
  69         out->m[0][3] = in->m[0][3];
  70         out->m[1][0] = 0.0f;
  71         out->m[1][1] = 1.0f;
  72         out->m[1][2] = 0.0f;
  73         out->m[1][3] = in->m[1][3];
  74         out->m[2][0] = 0.0f;
  75         out->m[2][1] = 0.0f;
  76         out->m[2][2] = 1.0f;
  77         out->m[2][3] = in->m[2][3];
  78         out->m[3][0] = 0.0f;
  79         out->m[3][1] = 0.0f;
  80         out->m[3][2] = 0.0f;
  81         out->m[3][3] = 1.0f;
  82 #endif
  83 }
  84
  85 void Matrix4x4_Concat (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2)
  86 {
  87 #ifdef MATRIX4x4_OPENGLORIENTATION
  88         out->m[0][0] = in1->m[0][0] * in2->m[0][0] + in1->m[1][0] * in2->m[0][1] + in1->m[2][0] * in2->m[0][2] + in1->m[3][0] * in2->m[0][3];
  89         out->m[1][0] = in1->m[0][0] * in2->m[1][0] + in1->m[1][0] * in2->m[1][1] + in1->m[2][0] * in2->m[1][2] + in1->m[3][0] * in2->m[1][3];
  90         out->m[2][0] = in1->m[0][0] * in2->m[2][0] + in1->m[1][0] * in2->m[2][1] + in1->m[2][0] * in2->m[2][2] + in1->m[3][0] * in2->m[2][3];
  91         out->m[3][0] = in1->m[0][0] * in2->m[3][0] + in1->m[1][0] * in2->m[3][1] + in1->m[2][0] * in2->m[3][2] + in1->m[3][0] * in2->m[3][3];
  92         out->m[0][1] = in1->m[0][1] * in2->m[0][0] + in1->m[1][1] * in2->m[0][1] + in1->m[2][1] * in2->m[0][2] + in1->m[3][1] * in2->m[0][3];
  93         out->m[1][1] = in1->m[0][1] * in2->m[1][0] + in1->m[1][1] * in2->m[1][1] + in1->m[2][1] * in2->m[1][2] + in1->m[3][1] * in2->m[1][3];
  94         out->m[2][1] = in1->m[0][1] * in2->m[2][0] + in1->m[1][1] * in2->m[2][1] + in1->m[2][1] * in2->m[2][2] + in1->m[3][1] * in2->m[2][3];
  95         out->m[3][1] = in1->m[0][1] * in2->m[3][0] + in1->m[1][1] * in2->m[3][1] + in1->m[2][1] * in2->m[3][2] + in1->m[3][1] * in2->m[3][3];
  96         out->m[0][2] = in1->m[0][2] * in2->m[0][0] + in1->m[1][2] * in2->m[0][1] + in1->m[2][2] * in2->m[0][2] + in1->m[3][2] * in2->m[0][3];
  97         out->m[1][2] = in1->m[0][2] * in2->m[1][0] + in1->m[1][2] * in2->m[1][1] + in1->m[2][2] * in2->m[1][2] + in1->m[3][2] * in2->m[1][3];
  98         out->m[2][2] = in1->m[0][2] * in2->m[2][0] + in1->m[1][2] * in2->m[2][1] + in1->m[2][2] * in2->m[2][2] + in1->m[3][2] * in2->m[2][3];
  99         out->m[3][2] = in1->m[0][2] * in2->m[3][0] + in1->m[1][2] * in2->m[3][1] + in1->m[2][2] * in2->m[3][2] + in1->m[3][2] * in2->m[3][3];
 100         out->m[0][3] = in1->m[0][3] * in2->m[0][0] + in1->m[1][3] * in2->m[0][1] + in1->m[2][3] * in2->m[0][2] + in1->m[3][3] * in2->m[0][3];
 101         out->m[1][3] = in1->m[0][3] * in2->m[1][0] + in1->m[1][3] * in2->m[1][1] + in1->m[2][3] * in2->m[1][2] + in1->m[3][3] * in2->m[1][3];
 102         out->m[2][3] = in1->m[0][3] * in2->m[2][0] + in1->m[1][3] * in2->m[2][1] + in1->m[2][3] * in2->m[2][2] + in1->m[3][3] * in2->m[2][3];
 103         out->m[3][3] = in1->m[0][3] * in2->m[3][0] + in1->m[1][3] * in2->m[3][1] + in1->m[2][3] * in2->m[3][2] + in1->m[3][3] * in2->m[3][3];
 104 #else
 105         out->m[0][0] = in1->m[0][0] * in2->m[0][0] + in1->m[0][1] * in2->m[1][0] + in1->m[0][2] * in2->m[2][0] + in1->m[0][3] * in2->m[3][0];
 106         out->m[0][1] = in1->m[0][0] * in2->m[0][1] + in1->m[0][1] * in2->m[1][1] + in1->m[0][2] * in2->m[2][1] + in1->m[0][3] * in2->m[3][1];
 107         out->m[0][2] = in1->m[0][0] * in2->m[0][2] + in1->m[0][1] * in2->m[1][2] + in1->m[0][2] * in2->m[2][2] + in1->m[0][3] * in2->m[3][2];
 108         out->m[0][3] = in1->m[0][0] * in2->m[0][3] + in1->m[0][1] * in2->m[1][3] + in1->m[0][2] * in2->m[2][3] + in1->m[0][3] * in2->m[3][3];
 109         out->m[1][0] = in1->m[1][0] * in2->m[0][0] + in1->m[1][1] * in2->m[1][0] + in1->m[1][2] * in2->m[2][0] + in1->m[1][3] * in2->m[3][0];
 110         out->m[1][1] = in1->m[1][0] * in2->m[0][1] + in1->m[1][1] * in2->m[1][1] + in1->m[1][2] * in2->m[2][1] + in1->m[1][3] * in2->m[3][1];
 111         out->m[1][2] = in1->m[1][0] * in2->m[0][2] + in1->m[1][1] * in2->m[1][2] + in1->m[1][2] * in2->m[2][2] + in1->m[1][3] * in2->m[3][2];
 112         out->m[1][3] = in1->m[1][0] * in2->m[0][3] + in1->m[1][1] * in2->m[1][3] + in1->m[1][2] * in2->m[2][3] + in1->m[1][3] * in2->m[3][3];
 113         out->m[2][0] = in1->m[2][0] * in2->m[0][0] + in1->m[2][1] * in2->m[1][0] + in1->m[2][2] * in2->m[2][0] + in1->m[2][3] * in2->m[3][0];
 114         out->m[2][1] = in1->m[2][0] * in2->m[0][1] + in1->m[2][1] * in2->m[1][1] + in1->m[2][2] * in2->m[2][1] + in1->m[2][3] * in2->m[3][1];
 115         out->m[2][2] = in1->m[2][0] * in2->m[0][2] + in1->m[2][1] * in2->m[1][2] + in1->m[2][2] * in2->m[2][2] + in1->m[2][3] * in2->m[3][2];
 116         out->m[2][3] = in1->m[2][0] * in2->m[0][3] + in1->m[2][1] * in2->m[1][3] + in1->m[2][2] * in2->m[2][3] + in1->m[2][3] * in2->m[3][3];
 117         out->m[3][0] = in1->m[3][0] * in2->m[0][0] + in1->m[3][1] * in2->m[1][0] + in1->m[3][2] * in2->m[2][0] + in1->m[3][3] * in2->m[3][0];
 118         out->m[3][1] = in1->m[3][0] * in2->m[0][1] + in1->m[3][1] * in2->m[1][1] + in1->m[3][2] * in2->m[2][1] + in1->m[3][3] * in2->m[3][1];
 119         out->m[3][2] = in1->m[3][0] * in2->m[0][2] + in1->m[3][1] * in2->m[1][2] + in1->m[3][2] * in2->m[2][2] + in1->m[3][3] * in2->m[3][2];
 120         out->m[3][3] = in1->m[3][0] * in2->m[0][3] + in1->m[3][1] * in2->m[1][3] + in1->m[3][2] * in2->m[2][3] + in1->m[3][3] * in2->m[3][3];
 121 #endif
 122 }
 123
 124 void Matrix4x4_Transpose (matrix4x4_t *out, const matrix4x4_t *in1)
 125 {
 126         out->m[0][0] = in1->m[0][0];
 127         out->m[0][1] = in1->m[1][0];
 128         out->m[0][2] = in1->m[2][0];
 129         out->m[0][3] = in1->m[3][0];
 130         out->m[1][0] = in1->m[0][1];
 131         out->m[1][1] = in1->m[1][1];
 132         out->m[1][2] = in1->m[2][1];
 133         out->m[1][3] = in1->m[3][1];
 134         out->m[2][0] = in1->m[0][2];
 135         out->m[2][1] = in1->m[1][2];
 136         out->m[2][2] = in1->m[2][2];
 137         out->m[2][3] = in1->m[3][2];
 138         out->m[3][0] = in1->m[0][3];
 139         out->m[3][1] = in1->m[1][3];
 140         out->m[3][2] = in1->m[2][3];
 141         out->m[3][3] = in1->m[3][3];
 142 }
 143
 144 #if 1
 145 // Adapted from code contributed to Mesa by David Moore (Mesa 7.6 under SGI Free License B - which is MIT/X11-type)
 146 // added helper for common subexpression elimination by eihrul, and other optimizations by div0
 147 int Matrix4x4_Invert_Full (matrix4x4_t *out, const matrix4x4_t *in1)
 148 {
 149                 float det;
 150
 151                 // note: orientation does not matter, as transpose(invert(transpose(m))) == invert(m), proof:
 152                 //   transpose(invert(transpose(m))) * m
 153                 // = transpose(invert(transpose(m))) * transpose(transpose(m))
 154                 // = transpose(transpose(m) * invert(transpose(m)))
 155                 // = transpose(identity)
 156                 // = identity
 157
 158                 // this seems to help gcc's common subexpression elimination, and also makes the code look nicer
 159                 float   m00 = in1->m[0][0], m01 = in1->m[0][1], m02 = in1->m[0][2], m03 = in1->m[0][3],
 160                                 m10 = in1->m[1][0], m11 = in1->m[1][1], m12 = in1->m[1][2], m13 = in1->m[1][3],
 161                                 m20 = in1->m[2][0], m21 = in1->m[2][1], m22 = in1->m[2][2], m23 = in1->m[2][3],
 162                                 m30 = in1->m[3][0], m31 = in1->m[3][1], m32 = in1->m[3][2], m33 = in1->m[3][3];
 163
 164                 // calculate the adjoint
 165                 out->m[0][0] =  (m11*(m22*m33 - m23*m32) - m21*(m12*m33 - m13*m32) + m31*(m12*m23 - m13*m22));
 166                 out->m[0][1] = -(m01*(m22*m33 - m23*m32) - m21*(m02*m33 - m03*m32) + m31*(m02*m23 - m03*m22));
 167                 out->m[0][2] =  (m01*(m12*m33 - m13*m32) - m11*(m02*m33 - m03*m32) + m31*(m02*m13 - m03*m12));
 168                 out->m[0][3] = -(m01*(m12*m23 - m13*m22) - m11*(m02*m23 - m03*m22) + m21*(m02*m13 - m03*m12));
 169                 out->m[1][0] = -(m10*(m22*m33 - m23*m32) - m20*(m12*m33 - m13*m32) + m30*(m12*m23 - m13*m22));
 170                 out->m[1][1] =  (m00*(m22*m33 - m23*m32) - m20*(m02*m33 - m03*m32) + m30*(m02*m23 - m03*m22));
 171                 out->m[1][2] = -(m00*(m12*m33 - m13*m32) - m10*(m02*m33 - m03*m32) + m30*(m02*m13 - m03*m12));
 172                 out->m[1][3] =  (m00*(m12*m23 - m13*m22) - m10*(m02*m23 - m03*m22) + m20*(m02*m13 - m03*m12));
 173                 out->m[2][0] =  (m10*(m21*m33 - m23*m31) - m20*(m11*m33 - m13*m31) + m30*(m11*m23 - m13*m21));
 174                 out->m[2][1] = -(m00*(m21*m33 - m23*m31) - m20*(m01*m33 - m03*m31) + m30*(m01*m23 - m03*m21));
 175                 out->m[2][2] =  (m00*(m11*m33 - m13*m31) - m10*(m01*m33 - m03*m31) + m30*(m01*m13 - m03*m11));
 176                 out->m[2][3] = -(m00*(m11*m23 - m13*m21) - m10*(m01*m23 - m03*m21) + m20*(m01*m13 - m03*m11));
 177                 out->m[3][0] = -(m10*(m21*m32 - m22*m31) - m20*(m11*m32 - m12*m31) + m30*(m11*m22 - m12*m21));
 178                 out->m[3][1] =  (m00*(m21*m32 - m22*m31) - m20*(m01*m32 - m02*m31) + m30*(m01*m22 - m02*m21));
 179                 out->m[3][2] = -(m00*(m11*m32 - m12*m31) - m10*(m01*m32 - m02*m31) + m30*(m01*m12 - m02*m11));
 180                 out->m[3][3] =  (m00*(m11*m22 - m12*m21) - m10*(m01*m22 - m02*m21) + m20*(m01*m12 - m02*m11));
 181
 182                 // calculate the determinant (as inverse == 1/det * adjoint, adjoint * m == identity * det, so this calculates the det)
 183                 det = m00*out->m[0][0] + m10*out->m[0][1] + m20*out->m[0][2] + m30*out->m[0][3];
 184                 if (det == 0.0f)
 185                                 return 0;
 186
 187                 // multiplications are faster than divisions, usually
 188                 det = 1.0f / det;
 189
 190                 // manually unrolled loop to multiply all matrix elements by 1/det
 191                 out->m[0][0] *= det; out->m[0][1] *= det; out->m[0][2] *= det; out->m[0][3] *= det;
 192                 out->m[1][0] *= det; out->m[1][1] *= det; out->m[1][2] *= det; out->m[1][3] *= det;
 193                 out->m[2][0] *= det; out->m[2][1] *= det; out->m[2][2] *= det; out->m[2][3] *= det;
 194                 out->m[3][0] *= det; out->m[3][1] *= det; out->m[3][2] *= det; out->m[3][3] *= det;
 195
 196                 return 1;
 197 }
 198 #elif 1
 199 // Adapted from code contributed to Mesa by David Moore (Mesa 7.6 under SGI Free License B - which is MIT/X11-type)
 200 int Matrix4x4_Invert_Full (matrix4x4_t *out, const matrix4x4_t *in1)
 201 {
 202         matrix4x4_t temp;
 203         double det;
 204         int i, j;
 205
 206 #ifdef MATRIX4x4_OPENGLORIENTATION
 207         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];
 208         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];
 209         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];
 210         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];
 211         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];
 212         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];
 213         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];
 214         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];
 215         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];
 216         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];
 217         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];
 218         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];
 219         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];
 220         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];
 221         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];
 222         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];
 223 #else
 224         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];
 225         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];
 226         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];
 227         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];
 228         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];
 229         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];
 230         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];
 231         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];
 232         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];
 233         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];
 234         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];
 235         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];
 236         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];
 237         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];
 238         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];
 239         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];
 240 #endif
 241
 242         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];
 243         if (det == 0.0f)
 244                 return 0;
 245
 246         det = 1.0f / det;
 247
 248         for (i = 0;i < 4;i++)
 249                 for (j = 0;j < 4;j++)
 250                         out->m[i][j] = temp.m[i][j] * det;
 251
 252         return 1;
 253 }
 254 #else
 255 int Matrix4x4_Invert_Full (matrix4x4_t *out, const matrix4x4_t *in1)
 256 {
 257         double  *temp;
 258         double  *r[4];
 259         double  rtemp[4][8];
 260         double  m[4];
 261         double  s;
 262
 263         r[0]    = rtemp[0];
 264         r[1]    = rtemp[1];
 265         r[2]    = rtemp[2];
 266         r[3]    = rtemp[3];
 267
 268 #ifdef MATRIX4x4_OPENGLORIENTATION
 269         r[0][0] = in1->m[0][0]; r[0][1] = in1->m[1][0]; r[0][2] = in1->m[2][0]; r[0][3] = in1->m[3][0];
 270         r[0][4] = 1.0;                  r[0][5] =                               r[0][6] =                               r[0][7] = 0.0;
 271
 272         r[1][0] = in1->m[0][1]; r[1][1] = in1->m[1][1]; r[1][2] = in1->m[2][1]; r[1][3] = in1->m[3][1];
 273         r[1][5] = 1.0;                  r[1][4] =                               r[1][6] =                               r[1][7] = 0.0;
 274
 275         r[2][0] = in1->m[0][2]; r[2][1] = in1->m[1][2]; r[2][2] = in1->m[2][2]; r[2][3] = in1->m[3][2];
 276         r[2][6] = 1.0;                  r[2][4] =                               r[2][5] =                               r[2][7] = 0.0;
 277
 278         r[3][0] = in1->m[0][3]; r[3][1] = in1->m[1][3]; r[3][2] = in1->m[2][3]; r[3][3] = in1->m[3][3];
 279         r[3][7] = 1.0;                  r[3][4] =                               r[3][5] =                               r[3][6] = 0.0;
 280 #else
 281         r[0][0] = in1->m[0][0]; r[0][1] = in1->m[0][1]; r[0][2] = in1->m[0][2]; r[0][3] = in1->m[0][3];
 282         r[0][4] = 1.0;                  r[0][5] =                               r[0][6] =                               r[0][7] = 0.0;
 283
 284         r[1][0] = in1->m[1][0]; r[1][1] = in1->m[1][1]; r[1][2] = in1->m[1][2]; r[1][3] = in1->m[1][3];
 285         r[1][5] = 1.0;                  r[1][4] =                               r[1][6] =                               r[1][7] = 0.0;
 286
 287         r[2][0] = in1->m[2][0]; r[2][1] = in1->m[2][1]; r[2][2] = in1->m[2][2]; r[2][3] = in1->m[2][3];
 288         r[2][6] = 1.0;                  r[2][4] =                               r[2][5] =                               r[2][7] = 0.0;
 289
 290         r[3][0] = in1->m[3][0]; r[3][1] = in1->m[3][1]; r[3][2] = in1->m[3][2]; r[3][3] = in1->m[3][3];
 291         r[3][7] = 1.0;                  r[3][4] =                               r[3][5] =                               r[3][6] = 0.0;
 292 #endif
 293
 294         if (fabs (r[3][0]) > fabs (r[2][0])) { temp = r[3]; r[3] = r[2]; r[2] = temp; }
 295         if (fabs (r[2][0]) > fabs (r[1][0])) { temp = r[2]; r[2] = r[1]; r[1] = temp; }
 296         if (fabs (r[1][0]) > fabs (r[0][0])) { temp = r[1]; r[1] = r[0]; r[0] = temp; }
 297
 298         if (r[0][0])
 299         {
 300                 m[1]    = r[1][0] / r[0][0];
 301                 m[2]    = r[2][0] / r[0][0];
 302                 m[3]    = r[3][0] / r[0][0];
 303
 304                 s       = r[0][1]; r[1][1] -= m[1] * s; r[2][1] -= m[2] * s; r[3][1] -= m[3] * s;
 305                 s       = r[0][2]; r[1][2] -= m[1] * s; r[2][2] -= m[2] * s; r[3][2] -= m[3] * s;
 306                 s       = r[0][3]; r[1][3] -= m[1] * s; r[2][3] -= m[2] * s; r[3][3] -= m[3] * s;
 307
 308                 s       = r[0][4]; if (s) { r[1][4] -= m[1] * s; r[2][4] -= m[2] * s; r[3][4] -= m[3] * s; }
 309                 s       = r[0][5]; if (s) { r[1][5] -= m[1] * s; r[2][5] -= m[2] * s; r[3][5] -= m[3] * s; }
 310                 s       = r[0][6]; if (s) { r[1][6] -= m[1] * s; r[2][6] -= m[2] * s; r[3][6] -= m[3] * s; }
 311                 s       = r[0][7]; if (s) { r[1][7] -= m[1] * s; r[2][7] -= m[2] * s; r[3][7] -= m[3] * s; }
 312
 313                 if (fabs (r[3][1]) > fabs (r[2][1])) { temp = r[3]; r[3] = r[2]; r[2] = temp; }
 314                 if (fabs (r[2][1]) > fabs (r[1][1])) { temp = r[2]; r[2] = r[1]; r[1] = temp; }
 315
 316                 if (r[1][1])
 317                 {
 318                         m[2]            = r[2][1] / r[1][1];
 319                         m[3]            = r[3][1] / r[1][1];
 320                         r[2][2] -= m[2] * r[1][2];
 321                         r[3][2] -= m[3] * r[1][2];
 322                         r[2][3] -= m[2] * r[1][3];
 323                         r[3][3] -= m[3] * r[1][3];
 324
 325                         s       = r[1][4]; if (s) { r[2][4] -= m[2] * s; r[3][4] -= m[3] * s; }
 326                         s       = r[1][5]; if (s) { r[2][5] -= m[2] * s; r[3][5] -= m[3] * s; }
 327                         s       = r[1][6]; if (s) { r[2][6] -= m[2] * s; r[3][6] -= m[3] * s; }
 328                         s       = r[1][7]; if (s) { r[2][7] -= m[2] * s; r[3][7] -= m[3] * s; }
 329
 330                         if (fabs (r[3][2]) > fabs (r[2][2])) { temp = r[3]; r[3] = r[2]; r[2] = temp; }
 331
 332                         if (r[2][2])
 333                         {
 334                                 m[3]            = r[3][2] / r[2][2];
 335                                 r[3][3] -= m[3] * r[2][3];
 336                                 r[3][4] -= m[3] * r[2][4];
 337                                 r[3][5] -= m[3] * r[2][5];
 338                                 r[3][6] -= m[3] * r[2][6];
 339                                 r[3][7] -= m[3] * r[2][7];
 340
 341                                 if (r[3][3])
 342                                 {
 343                                         s                       = 1.0 / r[3][3];
 344                                         r[3][4] *= s;
 345                                         r[3][5] *= s;
 346                                         r[3][6] *= s;
 347                                         r[3][7] *= s;
 348
 349                                         m[2]            = r[2][3];
 350                                         s                       = 1.0 / r[2][2];
 351                                         r[2][4] = s * (r[2][4] - r[3][4] * m[2]);
 352                                         r[2][5] = s * (r[2][5] - r[3][5] * m[2]);
 353                                         r[2][6] = s * (r[2][6] - r[3][6] * m[2]);
 354                                         r[2][7] = s * (r[2][7] - r[3][7] * m[2]);
 355
 356                                         m[1]            = r[1][3];
 357                                         r[1][4] -= r[3][4] * m[1], r[1][5] -= r[3][5] * m[1];
 358                                         r[1][6] -= r[3][6] * m[1], r[1][7] -= r[3][7] * m[1];
 359
 360                                         m[0]            = r[0][3];
 361                                         r[0][4] -= r[3][4] * m[0], r[0][5] -= r[3][5] * m[0];
 362                                         r[0][6] -= r[3][6] * m[0], r[0][7] -= r[3][7] * m[0];
 363
 364                                         m[1]            = r[1][2];
 365                                         s                       = 1.0 / r[1][1];
 366                                         r[1][4] = s * (r[1][4] - r[2][4] * m[1]), r[1][5] = s * (r[1][5] - r[2][5] * m[1]);
 367                                         r[1][6] = s * (r[1][6] - r[2][6] * m[1]), r[1][7] = s * (r[1][7] - r[2][7] * m[1]);
 368
 369                                         m[0]            = r[0][2];
 370                                         r[0][4] -= r[2][4] * m[0], r[0][5] -= r[2][5] * m[0];
 371                                         r[0][6] -= r[2][6] * m[0], r[0][7] -= r[2][7] * m[0];
 372
 373                                         m[0]            = r[0][1];
 374                                         s                       = 1.0 / r[0][0];
 375                                         r[0][4] = s * (r[0][4] - r[1][4] * m[0]), r[0][5] = s * (r[0][5] - r[1][5] * m[0]);
 376                                         r[0][6] = s * (r[0][6] - r[1][6] * m[0]), r[0][7] = s * (r[0][7] - r[1][7] * m[0]);
 377
 378 #ifdef MATRIX4x4_OPENGLORIENTATION
 379                                         out->m[0][0]    = r[0][4];
 380                                         out->m[0][1]    = r[1][4];
 381                                         out->m[0][2]    = r[2][4];
 382                                         out->m[0][3]    = r[3][4];
 383                                         out->m[1][0]    = r[0][5];
 384                                         out->m[1][1]    = r[1][5];
 385                                         out->m[1][2]    = r[2][5];
 386                                         out->m[1][3]    = r[3][5];
 387                                         out->m[2][0]    = r[0][6];
 388                                         out->m[2][1]    = r[1][6];
 389                                         out->m[2][2]    = r[2][6];
 390                                         out->m[2][3]    = r[3][6];
 391                                         out->m[3][0]    = r[0][7];
 392                                         out->m[3][1]    = r[1][7];
 393                                         out->m[3][2]    = r[2][7];
 394                                         out->m[3][3]    = r[3][7];
 395 #else
 396                                         out->m[0][0]    = r[0][4];
 397                                         out->m[0][1]    = r[0][5];
 398                                         out->m[0][2]    = r[0][6];
 399                                         out->m[0][3]    = r[0][7];
 400                                         out->m[1][0]    = r[1][4];
 401                                         out->m[1][1]    = r[1][5];
 402                                         out->m[1][2]    = r[1][6];
 403                                         out->m[1][3]    = r[1][7];
 404                                         out->m[2][0]    = r[2][4];
 405                                         out->m[2][1]    = r[2][5];
 406                                         out->m[2][2]    = r[2][6];
 407                                         out->m[2][3]    = r[2][7];
 408                                         out->m[3][0]    = r[3][4];
 409                                         out->m[3][1]    = r[3][5];
 410                                         out->m[3][2]    = r[3][6];
 411                                         out->m[3][3]    = r[3][7];
 412 #endif
 413
 414                                         return 1;
 415                                 }
 416                         }
 417                 }
 418         }
 419
 420         return 0;
 421 }
 422 #endif
 423
 424 void Matrix4x4_Invert_Simple (matrix4x4_t *out, const matrix4x4_t *in1)
 425 {
 426         // we only support uniform scaling, so assume the first row is enough
 427         // (note the lack of sqrt here, because we're trying to undo the scaling,
 428         // this means multiplying by the inverse scale twice - squaring it, which
 429         // makes the sqrt a waste of time)
 430 #if 1
 431         double scale = 1.0 / (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]);
 432 #else
 433         double scale = 3.0 / sqrt
 434                  (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]
 435                 + in1->m[1][0] * in1->m[1][0] + in1->m[1][1] * in1->m[1][1] + in1->m[1][2] * in1->m[1][2]
 436                 + in1->m[2][0] * in1->m[2][0] + in1->m[2][1] * in1->m[2][1] + in1->m[2][2] * in1->m[2][2]);
 437         scale *= scale;
 438 #endif
 439
 440         // invert the rotation by transposing and multiplying by the squared
 441         // recipricol of the input matrix scale as described above
 442         out->m[0][0] = in1->m[0][0] * scale;
 443         out->m[0][1] = in1->m[1][0] * scale;
 444         out->m[0][2] = in1->m[2][0] * scale;
 445         out->m[1][0] = in1->m[0][1] * scale;
 446         out->m[1][1] = in1->m[1][1] * scale;
 447         out->m[1][2] = in1->m[2][1] * scale;
 448         out->m[2][0] = in1->m[0][2] * scale;
 449         out->m[2][1] = in1->m[1][2] * scale;
 450         out->m[2][2] = in1->m[2][2] * scale;
 451
 452 #ifdef MATRIX4x4_OPENGLORIENTATION
 453         // invert the translate
 454         out->m[3][0] = -(in1->m[3][0] * out->m[0][0] + in1->m[3][1] * out->m[1][0] + in1->m[3][2] * out->m[2][0]);
 455         out->m[3][1] = -(in1->m[3][0] * out->m[0][1] + in1->m[3][1] * out->m[1][1] + in1->m[3][2] * out->m[2][1]);
 456         out->m[3][2] = -(in1->m[3][0] * out->m[0][2] + in1->m[3][1] * out->m[1][2] + in1->m[3][2] * out->m[2][2]);
 457
 458         // don't know if there's anything worth doing here
 459         out->m[0][3] = 0;
 460         out->m[1][3] = 0;
 461         out->m[2][3] = 0;
 462         out->m[3][3] = 1;
 463 #else
 464         // invert the translate
 465         out->m[0][3] = -(in1->m[0][3] * out->m[0][0] + in1->m[1][3] * out->m[0][1] + in1->m[2][3] * out->m[0][2]);
 466         out->m[1][3] = -(in1->m[0][3] * out->m[1][0] + in1->m[1][3] * out->m[1][1] + in1->m[2][3] * out->m[1][2]);
 467         out->m[2][3] = -(in1->m[0][3] * out->m[2][0] + in1->m[1][3] * out->m[2][1] + in1->m[2][3] * out->m[2][2]);
 468
 469         // don't know if there's anything worth doing here
 470         out->m[3][0] = 0;
 471         out->m[3][1] = 0;
 472         out->m[3][2] = 0;
 473         out->m[3][3] = 1;
 474 #endif
 475 }
 476
 477 void Matrix4x4_Interpolate (matrix4x4_t *out, matrix4x4_t *in1, matrix4x4_t *in2, double frac)
 478 {
 479         int i, j;
 480         for (i = 0;i < 4;i++)
 481                 for (j = 0;j < 4;j++)
 482                         out->m[i][j] = in1->m[i][j] + frac * (in2->m[i][j] - in1->m[i][j]);
 483 }
 484
 485 void Matrix4x4_Clear (matrix4x4_t *out)
 486 {
 487         int i, j;
 488         for (i = 0;i < 4;i++)
 489                 for (j = 0;j < 4;j++)
 490                         out->m[i][j] = 0;
 491 }
 492
 493 void Matrix4x4_Accumulate (matrix4x4_t *out, matrix4x4_t *in, double weight)
 494 {
 495         int i, j;
 496         for (i = 0;i < 4;i++)
 497                 for (j = 0;j < 4;j++)
 498                         out->m[i][j] += in->m[i][j] * weight;
 499 }
 500
 501 void Matrix4x4_Normalize (matrix4x4_t *out, matrix4x4_t *in1)
 502 {
 503         // scale rotation matrix vectors to a length of 1
 504         // note: this is only designed to undo uniform scaling
 505         double scale = 1.0 / sqrt(in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]);
 506         *out = *in1;
 507         Matrix4x4_Scale(out, scale, 1);
 508 }
 509
 510 void Matrix4x4_Normalize3 (matrix4x4_t *out, matrix4x4_t *in1)
 511 {
 512         int i;
 513         double scale;
 514         // scale each rotation matrix vector to a length of 1
 515         // intended for use after Matrix4x4_Interpolate or Matrix4x4_Accumulate
 516         *out = *in1;
 517         for (i = 0;i < 3;i++)
 518         {
 519 #ifdef MATRIX4x4_OPENGLORIENTATION
 520                 scale = sqrt(in1->m[i][0] * in1->m[i][0] + in1->m[i][1] * in1->m[i][1] + in1->m[i][2] * in1->m[i][2]);
 521                 if (scale)
 522                         scale = 1.0 / scale;
 523                 out->m[i][0] *= scale;
 524                 out->m[i][1] *= scale;
 525                 out->m[i][2] *= scale;
 526 #else
 527                 scale = sqrt(in1->m[0][i] * in1->m[0][i] + in1->m[1][i] * in1->m[1][i] + in1->m[2][i] * in1->m[2][i]);
 528                 if (scale)
 529                         scale = 1.0 / scale;
 530                 out->m[0][i] *= scale;
 531                 out->m[1][i] *= scale;
 532                 out->m[2][i] *= scale;
 533 #endif
 534         }
 535 }
 536
 537 void Matrix4x4_Reflect (matrix4x4_t *out, double normalx, double normaly, double normalz, double dist, double axisscale)
 538 {
 539         int i;
 540         double d;
 541         double p[4], p2[4];
 542         p[0] = normalx;
 543         p[1] = normaly;
 544         p[2] = normalz;
 545         p[3] = -dist;
 546         p2[0] = p[0] * axisscale;
 547         p2[1] = p[1] * axisscale;
 548         p2[2] = p[2] * axisscale;
 549         p2[3] = 0;
 550         for (i = 0;i < 4;i++)
 551         {
 552 #ifdef MATRIX4x4_OPENGLORIENTATION
 553                 d = out->m[i][0] * p[0] + out->m[i][1] * p[1] + out->m[i][2] * p[2] + out->m[i][3] * p[3];
 554                 out->m[i][0] += p2[0] * d;
 555                 out->m[i][1] += p2[1] * d;
 556                 out->m[i][2] += p2[2] * d;
 557 #else
 558                 d = out->m[0][i] * p[0] + out->m[1][i] * p[1] + out->m[2][i] * p[2] + out->m[3][i] * p[3];
 559                 out->m[0][i] += p2[0] * d;
 560                 out->m[1][i] += p2[1] * d;
 561                 out->m[2][i] += p2[2] * d;
 562 #endif
 563         }
 564 }
 565
 566 void Matrix4x4_CreateIdentity (matrix4x4_t *out)
 567 {
 568         out->m[0][0]=1.0f;
 569         out->m[0][1]=0.0f;
 570         out->m[0][2]=0.0f;
 571         out->m[0][3]=0.0f;
 572         out->m[1][0]=0.0f;
 573         out->m[1][1]=1.0f;
 574         out->m[1][2]=0.0f;
 575         out->m[1][3]=0.0f;
 576         out->m[2][0]=0.0f;
 577         out->m[2][1]=0.0f;
 578         out->m[2][2]=1.0f;
 579         out->m[2][3]=0.0f;
 580         out->m[3][0]=0.0f;
 581         out->m[3][1]=0.0f;
 582         out->m[3][2]=0.0f;
 583         out->m[3][3]=1.0f;
 584 }
 585
 586 void Matrix4x4_CreateTranslate (matrix4x4_t *out, double x, double y, double z)
 587 {
 588 #ifdef MATRIX4x4_OPENGLORIENTATION
 589         out->m[0][0]=1.0f;
 590         out->m[1][0]=0.0f;
 591         out->m[2][0]=0.0f;
 592         out->m[3][0]=x;
 593         out->m[0][1]=0.0f;
 594         out->m[1][1]=1.0f;
 595         out->m[2][1]=0.0f;
 596         out->m[3][1]=y;
 597         out->m[0][2]=0.0f;
 598         out->m[1][2]=0.0f;
 599         out->m[2][2]=1.0f;
 600         out->m[3][2]=z;
 601         out->m[0][3]=0.0f;
 602         out->m[1][3]=0.0f;
 603         out->m[2][3]=0.0f;
 604         out->m[3][3]=1.0f;
 605 #else
 606         out->m[0][0]=1.0f;
 607         out->m[0][1]=0.0f;
 608         out->m[0][2]=0.0f;
 609         out->m[0][3]=x;
 610         out->m[1][0]=0.0f;
 611         out->m[1][1]=1.0f;
 612         out->m[1][2]=0.0f;
 613         out->m[1][3]=y;
 614         out->m[2][0]=0.0f;
 615         out->m[2][1]=0.0f;
 616         out->m[2][2]=1.0f;
 617         out->m[2][3]=z;
 618         out->m[3][0]=0.0f;
 619         out->m[3][1]=0.0f;
 620         out->m[3][2]=0.0f;
 621         out->m[3][3]=1.0f;
 622 #endif
 623 }
 624
 625 void Matrix4x4_CreateRotate (matrix4x4_t *out, double angle, double x, double y, double z)
 626 {
 627         double len, c, s;
 628
 629         len = x*x+y*y+z*z;
 630         if (len != 0.0f)
 631                 len = 1.0f / sqrt(len);
 632         x *= len;
 633         y *= len;
 634         z *= len;
 635
 636         angle *= (-M_PI / 180.0);
 637         c = cos(angle);
 638         s = sin(angle);
 639
 640 #ifdef MATRIX4x4_OPENGLORIENTATION
 641         out->m[0][0]=x * x + c * (1 - x * x);
 642         out->m[1][0]=x * y * (1 - c) + z * s;
 643         out->m[2][0]=z * x * (1 - c) - y * s;
 644         out->m[3][0]=0.0f;
 645         out->m[0][1]=x * y * (1 - c) - z * s;
 646         out->m[1][1]=y * y + c * (1 - y * y);
 647         out->m[2][1]=y * z * (1 - c) + x * s;
 648         out->m[3][1]=0.0f;
 649         out->m[0][2]=z * x * (1 - c) + y * s;
 650         out->m[1][2]=y * z * (1 - c) - x * s;
 651         out->m[2][2]=z * z + c * (1 - z * z);
 652         out->m[3][2]=0.0f;
 653         out->m[0][3]=0.0f;
 654         out->m[1][3]=0.0f;
 655         out->m[2][3]=0.0f;
 656         out->m[3][3]=1.0f;
 657 #else
 658         out->m[0][0]=x * x + c * (1 - x * x);
 659         out->m[0][1]=x * y * (1 - c) + z * s;
 660         out->m[0][2]=z * x * (1 - c) - y * s;
 661         out->m[0][3]=0.0f;
 662         out->m[1][0]=x * y * (1 - c) - z * s;
 663         out->m[1][1]=y * y + c * (1 - y * y);
 664         out->m[1][2]=y * z * (1 - c) + x * s;
 665         out->m[1][3]=0.0f;
 666         out->m[2][0]=z * x * (1 - c) + y * s;
 667         out->m[2][1]=y * z * (1 - c) - x * s;
 668         out->m[2][2]=z * z + c * (1 - z * z);
 669         out->m[2][3]=0.0f;
 670         out->m[3][0]=0.0f;
 671         out->m[3][1]=0.0f;
 672         out->m[3][2]=0.0f;
 673         out->m[3][3]=1.0f;
 674 #endif
 675 }
 676
 677 void Matrix4x4_CreateScale (matrix4x4_t *out, double x)
 678 {
 679         out->m[0][0]=x;
 680         out->m[0][1]=0.0f;
 681         out->m[0][2]=0.0f;
 682         out->m[0][3]=0.0f;
 683         out->m[1][0]=0.0f;
 684         out->m[1][1]=x;
 685         out->m[1][2]=0.0f;
 686         out->m[1][3]=0.0f;
 687         out->m[2][0]=0.0f;
 688         out->m[2][1]=0.0f;
 689         out->m[2][2]=x;
 690         out->m[2][3]=0.0f;
 691         out->m[3][0]=0.0f;
 692         out->m[3][1]=0.0f;
 693         out->m[3][2]=0.0f;
 694         out->m[3][3]=1.0f;
 695 }
 696
 697 void Matrix4x4_CreateScale3 (matrix4x4_t *out, double x, double y, double z)
 698 {
 699         out->m[0][0]=x;
 700         out->m[0][1]=0.0f;
 701         out->m[0][2]=0.0f;
 702         out->m[0][3]=0.0f;
 703         out->m[1][0]=0.0f;
 704         out->m[1][1]=y;
 705         out->m[1][2]=0.0f;
 706         out->m[1][3]=0.0f;
 707         out->m[2][0]=0.0f;
 708         out->m[2][1]=0.0f;
 709         out->m[2][2]=z;
 710         out->m[2][3]=0.0f;
 711         out->m[3][0]=0.0f;
 712         out->m[3][1]=0.0f;
 713         out->m[3][2]=0.0f;
 714         out->m[3][3]=1.0f;
 715 }
 716
 717 void Matrix4x4_CreateFromQuakeEntity(matrix4x4_t *out, double x, double y, double z, double pitch, double yaw, double roll, double scale)
 718 {
 719         double angle, sr, sp, sy, cr, cp, cy;
 720
 721         if (roll)
 722         {
 723                 angle = yaw * (M_PI*2 / 360);
 724                 sy = sin(angle);
 725                 cy = cos(angle);
 726                 angle = pitch * (M_PI*2 / 360);
 727                 sp = sin(angle);
 728                 cp = cos(angle);
 729                 angle = roll * (M_PI*2 / 360);
 730                 sr = sin(angle);
 731                 cr = cos(angle);
 732 #ifdef MATRIX4x4_OPENGLORIENTATION
 733                 out->m[0][0] = (cp*cy) * scale;
 734                 out->m[1][0] = (sr*sp*cy+cr*-sy) * scale;
 735                 out->m[2][0] = (cr*sp*cy+-sr*-sy) * scale;
 736                 out->m[3][0] = x;
 737                 out->m[0][1] = (cp*sy) * scale;
 738                 out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
 739                 out->m[2][1] = (cr*sp*sy+-sr*cy) * scale;
 740                 out->m[3][1] = y;
 741                 out->m[0][2] = (-sp) * scale;
 742                 out->m[1][2] = (sr*cp) * scale;
 743                 out->m[2][2] = (cr*cp) * scale;
 744                 out->m[3][2] = z;
 745                 out->m[0][3] = 0;
 746                 out->m[1][3] = 0;
 747                 out->m[2][3] = 0;
 748                 out->m[3][3] = 1;
 749 #else
 750                 out->m[0][0] = (cp*cy) * scale;
 751                 out->m[0][1] = (sr*sp*cy+cr*-sy) * scale;
 752                 out->m[0][2] = (cr*sp*cy+-sr*-sy) * scale;
 753                 out->m[0][3] = x;
 754                 out->m[1][0] = (cp*sy) * scale;
 755                 out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
 756                 out->m[1][2] = (cr*sp*sy+-sr*cy) * scale;
 757                 out->m[1][3] = y;
 758                 out->m[2][0] = (-sp) * scale;
 759                 out->m[2][1] = (sr*cp) * scale;
 760                 out->m[2][2] = (cr*cp) * scale;
 761                 out->m[2][3] = z;
 762                 out->m[3][0] = 0;
 763                 out->m[3][1] = 0;
 764                 out->m[3][2] = 0;
 765                 out->m[3][3] = 1;
 766 #endif
 767         }
 768         else if (pitch)
 769         {
 770                 angle = yaw * (M_PI*2 / 360);
 771                 sy = sin(angle);
 772                 cy = cos(angle);
 773                 angle = pitch * (M_PI*2 / 360);
 774                 sp = sin(angle);
 775                 cp = cos(angle);
 776 #ifdef MATRIX4x4_OPENGLORIENTATION
 777                 out->m[0][0] = (cp*cy) * scale;
 778                 out->m[1][0] = (-sy) * scale;
 779                 out->m[2][0] = (sp*cy) * scale;
 780                 out->m[3][0] = x;
 781                 out->m[0][1] = (cp*sy) * scale;
 782                 out->m[1][1] = (cy) * scale;
 783                 out->m[2][1] = (sp*sy) * scale;
 784                 out->m[3][1] = y;
 785                 out->m[0][2] = (-sp) * scale;
 786                 out->m[1][2] = 0;
 787                 out->m[2][2] = (cp) * scale;
 788                 out->m[3][2] = z;
 789                 out->m[0][3] = 0;
 790                 out->m[1][3] = 0;
 791                 out->m[2][3] = 0;
 792                 out->m[3][3] = 1;
 793 #else
 794                 out->m[0][0] = (cp*cy) * scale;
 795                 out->m[0][1] = (-sy) * scale;
 796                 out->m[0][2] = (sp*cy) * scale;
 797                 out->m[0][3] = x;
 798                 out->m[1][0] = (cp*sy) * scale;
 799                 out->m[1][1] = (cy) * scale;
 800                 out->m[1][2] = (sp*sy) * scale;
 801                 out->m[1][3] = y;
 802                 out->m[2][0] = (-sp) * scale;
 803                 out->m[2][1] = 0;
 804                 out->m[2][2] = (cp) * scale;
 805                 out->m[2][3] = z;
 806                 out->m[3][0] = 0;
 807                 out->m[3][1] = 0;
 808                 out->m[3][2] = 0;
 809                 out->m[3][3] = 1;
 810 #endif
 811         }
 812         else if (yaw)
 813         {
 814                 angle = yaw * (M_PI*2 / 360);
 815                 sy = sin(angle);
 816                 cy = cos(angle);
 817 #ifdef MATRIX4x4_OPENGLORIENTATION
 818                 out->m[0][0] = (cy) * scale;
 819                 out->m[1][0] = (-sy) * scale;
 820                 out->m[2][0] = 0;
 821                 out->m[3][0] = x;
 822                 out->m[0][1] = (sy) * scale;
 823                 out->m[1][1] = (cy) * scale;
 824                 out->m[2][1] = 0;
 825                 out->m[3][1] = y;
 826                 out->m[0][2] = 0;
 827                 out->m[1][2] = 0;
 828                 out->m[2][2] = scale;
 829                 out->m[3][2] = z;
 830                 out->m[0][3] = 0;
 831                 out->m[1][3] = 0;
 832                 out->m[2][3] = 0;
 833                 out->m[3][3] = 1;
 834 #else
 835                 out->m[0][0] = (cy) * scale;
 836                 out->m[0][1] = (-sy) * scale;
 837                 out->m[0][2] = 0;
 838                 out->m[0][3] = x;
 839                 out->m[1][0] = (sy) * scale;
 840                 out->m[1][1] = (cy) * scale;
 841                 out->m[1][2] = 0;
 842                 out->m[1][3] = y;
 843                 out->m[2][0] = 0;
 844                 out->m[2][1] = 0;
 845                 out->m[2][2] = scale;
 846                 out->m[2][3] = z;
 847                 out->m[3][0] = 0;
 848                 out->m[3][1] = 0;
 849                 out->m[3][2] = 0;
 850                 out->m[3][3] = 1;
 851 #endif
 852         }
 853         else
 854         {
 855 #ifdef MATRIX4x4_OPENGLORIENTATION
 856                 out->m[0][0] = scale;
 857                 out->m[1][0] = 0;
 858                 out->m[2][0] = 0;
 859                 out->m[3][0] = x;
 860                 out->m[0][1] = 0;
 861                 out->m[1][1] = scale;
 862                 out->m[2][1] = 0;
 863                 out->m[3][1] = y;
 864                 out->m[0][2] = 0;
 865                 out->m[1][2] = 0;
 866                 out->m[2][2] = scale;
 867                 out->m[3][2] = z;
 868                 out->m[0][3] = 0;
 869                 out->m[1][3] = 0;
 870                 out->m[2][3] = 0;
 871                 out->m[3][3] = 1;
 872 #else
 873                 out->m[0][0] = scale;
 874                 out->m[0][1] = 0;
 875                 out->m[0][2] = 0;
 876                 out->m[0][3] = x;
 877                 out->m[1][0] = 0;
 878                 out->m[1][1] = scale;
 879                 out->m[1][2] = 0;
 880                 out->m[1][3] = y;
 881                 out->m[2][0] = 0;
 882                 out->m[2][1] = 0;
 883                 out->m[2][2] = scale;
 884                 out->m[2][3] = z;
 885                 out->m[3][0] = 0;
 886                 out->m[3][1] = 0;
 887                 out->m[3][2] = 0;
 888                 out->m[3][3] = 1;
 889 #endif
 890         }
 891 }
 892
 893 void Matrix4x4_QuakeToDuke3D(const matrix4x4_t *in, matrix4x4_t *out, double maxShearAngle)
 894 {
 895         // Sorry - this isn't direct at all. We can't just use an alternative to
 896         // Matrix4x4_CreateFromQuakeEntity as in some cases the input for
 897         // generating the view matrix is generated externally.
 898         vec3_t forward, left, up, angles;
 899         double scaleforward, scaleleft, scaleup;
 900 #ifdef MATRIX4x4_OPENGLORIENTATION
 901         VectorSet(forward, in->m[0][0], in->m[0][1], in->m[0][2]);
 902         VectorSet(left, in->m[1][0], in->m[1][1], in->m[1][2]);
 903         VectorSet(up, in->m[2][0], in->m[2][1], in->m[2][2]);
 904 #else
 905         VectorSet(forward, in->m[0][0], in->m[1][0], in->m[2][0]);
 906         VectorSet(left, in->m[0][1], in->m[1][1], in->m[2][1]);
 907         VectorSet(up, in->m[0][2], in->m[1][2], in->m[2][2]);
 908 #endif
 909         scaleforward = VectorNormalizeLength(forward);
 910         scaleleft = VectorNormalizeLength(left);
 911         scaleup = VectorNormalizeLength(up);
 912         AnglesFromVectors(angles, forward, up, false);
 913         AngleVectorsDuke3DFLU(angles, forward, left, up, maxShearAngle);
 914         VectorScale(forward, scaleforward, forward);
 915         VectorScale(left, scaleleft, left);
 916         VectorScale(up, scaleup, up);
 917         *out = *in;
 918 #ifdef MATRIX4x4_OPENGLORIENTATION
 919         out->m[0][0] = forward[0];
 920         out->m[1][0] = left[0];
 921         out->m[2][0] = up[0];
 922         out->m[0][1] = forward[1];
 923         out->m[1][1] = left[1];
 924         out->m[2][1] = up[1];
 925         out->m[0][2] = forward[2];
 926         out->m[1][2] = left[2];
 927         out->m[2][2] = up[2];
 928 #else
 929         out->m[0][0] = forward[0];
 930         out->m[0][1] = left[0];
 931         out->m[0][2] = up[0];
 932         out->m[1][0] = forward[1];
 933         out->m[1][1] = left[1];
 934         out->m[1][2] = up[1];
 935         out->m[2][0] = forward[2];
 936         out->m[2][1] = left[2];
 937         out->m[2][2] = up[2];
 938 #endif
 939 }
 940
 941 void Matrix4x4_ToVectors(const matrix4x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
 942 {
 943 #ifdef MATRIX4x4_OPENGLORIENTATION
 944         vx[0] = in->m[0][0];
 945         vx[1] = in->m[0][1];
 946         vx[2] = in->m[0][2];
 947         vy[0] = in->m[1][0];
 948         vy[1] = in->m[1][1];
 949         vy[2] = in->m[1][2];
 950         vz[0] = in->m[2][0];
 951         vz[1] = in->m[2][1];
 952         vz[2] = in->m[2][2];
 953         t [0] = in->m[3][0];
 954         t [1] = in->m[3][1];
 955         t [2] = in->m[3][2];
 956 #else
 957         vx[0] = in->m[0][0];
 958         vx[1] = in->m[1][0];
 959         vx[2] = in->m[2][0];
 960         vy[0] = in->m[0][1];
 961         vy[1] = in->m[1][1];
 962         vy[2] = in->m[2][1];
 963         vz[0] = in->m[0][2];
 964         vz[1] = in->m[1][2];
 965         vz[2] = in->m[2][2];
 966         t [0] = in->m[0][3];
 967         t [1] = in->m[1][3];
 968         t [2] = in->m[2][3];
 969 #endif
 970 }
 971
 972 void Matrix4x4_FromVectors(matrix4x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
 973 {
 974 #ifdef MATRIX4x4_OPENGLORIENTATION
 975         out->m[0][0] = vx[0];
 976         out->m[1][0] = vy[0];
 977         out->m[2][0] = vz[0];
 978         out->m[3][0] = t[0];
 979         out->m[0][1] = vx[1];
 980         out->m[1][1] = vy[1];
 981         out->m[2][1] = vz[1];
 982         out->m[3][1] = t[1];
 983         out->m[0][2] = vx[2];
 984         out->m[1][2] = vy[2];
 985         out->m[2][2] = vz[2];
 986         out->m[3][2] = t[2];
 987         out->m[0][3] = 0.0f;
 988         out->m[1][3] = 0.0f;
 989         out->m[2][3] = 0.0f;
 990         out->m[3][3] = 1.0f;
 991 #else
 992         out->m[0][0] = vx[0];
 993         out->m[0][1] = vy[0];
 994         out->m[0][2] = vz[0];
 995         out->m[0][3] = t[0];
 996         out->m[1][0] = vx[1];
 997         out->m[1][1] = vy[1];
 998         out->m[1][2] = vz[1];
 999         out->m[1][3] = t[1];
1000         out->m[2][0] = vx[2];
1001         out->m[2][1] = vy[2];
1002         out->m[2][2] = vz[2];
1003         out->m[2][3] = t[2];
1004         out->m[3][0] = 0.0f;
1005         out->m[3][1] = 0.0f;
1006         out->m[3][2] = 0.0f;
1007         out->m[3][3] = 1.0f;
1008 #endif
1009 }
1010
1011 void Matrix4x4_ToArrayDoubleGL(const matrix4x4_t *in, double out[16])
1012 {
1013 #ifdef MATRIX4x4_OPENGLORIENTATION
1014         out[ 0] = in->m[0][0];
1015         out[ 1] = in->m[0][1];
1016         out[ 2] = in->m[0][2];
1017         out[ 3] = in->m[0][3];
1018         out[ 4] = in->m[1][0];
1019         out[ 5] = in->m[1][1];
1020         out[ 6] = in->m[1][2];
1021         out[ 7] = in->m[1][3];
1022         out[ 8] = in->m[2][0];
1023         out[ 9] = in->m[2][1];
1024         out[10] = in->m[2][2];
1025         out[11] = in->m[2][3];
1026         out[12] = in->m[3][0];
1027         out[13] = in->m[3][1];
1028         out[14] = in->m[3][2];
1029         out[15] = in->m[3][3];
1030 #else
1031         out[ 0] = in->m[0][0];
1032         out[ 1] = in->m[1][0];
1033         out[ 2] = in->m[2][0];
1034         out[ 3] = in->m[3][0];
1035         out[ 4] = in->m[0][1];
1036         out[ 5] = in->m[1][1];
1037         out[ 6] = in->m[2][1];
1038         out[ 7] = in->m[3][1];
1039         out[ 8] = in->m[0][2];
1040         out[ 9] = in->m[1][2];
1041         out[10] = in->m[2][2];
1042         out[11] = in->m[3][2];
1043         out[12] = in->m[0][3];
1044         out[13] = in->m[1][3];
1045         out[14] = in->m[2][3];
1046         out[15] = in->m[3][3];
1047 #endif
1048 }
1049
1050 void Matrix4x4_FromArrayDoubleGL (matrix4x4_t *out, const double in[16])
1051 {
1052 #ifdef MATRIX4x4_OPENGLORIENTATION
1053         out->m[0][0] = in[0];
1054         out->m[0][1] = in[1];
1055         out->m[0][2] = in[2];
1056         out->m[0][3] = in[3];
1057         out->m[1][0] = in[4];
1058         out->m[1][1] = in[5];
1059         out->m[1][2] = in[6];
1060         out->m[1][3] = in[7];
1061         out->m[2][0] = in[8];
1062         out->m[2][1] = in[9];
1063         out->m[2][2] = in[10];
1064         out->m[2][3] = in[11];
1065         out->m[3][0] = in[12];
1066         out->m[3][1] = in[13];
1067         out->m[3][2] = in[14];
1068         out->m[3][3] = in[15];
1069 #else
1070         out->m[0][0] = in[0];
1071         out->m[1][0] = in[1];
1072         out->m[2][0] = in[2];
1073         out->m[3][0] = in[3];
1074         out->m[0][1] = in[4];
1075         out->m[1][1] = in[5];
1076         out->m[2][1] = in[6];
1077         out->m[3][1] = in[7];
1078         out->m[0][2] = in[8];
1079         out->m[1][2] = in[9];
1080         out->m[2][2] = in[10];
1081         out->m[3][2] = in[11];
1082         out->m[0][3] = in[12];
1083         out->m[1][3] = in[13];
1084         out->m[2][3] = in[14];
1085         out->m[3][3] = in[15];
1086 #endif
1087 }
1088
1089 void Matrix4x4_ToArrayDoubleD3D(const matrix4x4_t *in, double out[16])
1090 {
1091 #ifdef MATRIX4x4_OPENGLORIENTATION
1092         out[ 0] = in->m[0][0];
1093         out[ 1] = in->m[1][0];
1094         out[ 2] = in->m[2][0];
1095         out[ 3] = in->m[3][0];
1096         out[ 4] = in->m[0][1];
1097         out[ 5] = in->m[1][1];
1098         out[ 6] = in->m[2][1];
1099         out[ 7] = in->m[3][1];
1100         out[ 8] = in->m[0][2];
1101         out[ 9] = in->m[1][2];
1102         out[10] = in->m[2][2];
1103         out[11] = in->m[3][2];
1104         out[12] = in->m[0][3];
1105         out[13] = in->m[1][3];
1106         out[14] = in->m[2][3];
1107         out[15] = in->m[3][3];
1108 #else
1109         out[ 0] = in->m[0][0];
1110         out[ 1] = in->m[0][1];
1111         out[ 2] = in->m[0][2];
1112         out[ 3] = in->m[0][3];
1113         out[ 4] = in->m[1][0];
1114         out[ 5] = in->m[1][1];
1115         out[ 6] = in->m[1][2];
1116         out[ 7] = in->m[1][3];
1117         out[ 8] = in->m[2][0];
1118         out[ 9] = in->m[2][1];
1119         out[10] = in->m[2][2];
1120         out[11] = in->m[2][3];
1121         out[12] = in->m[3][0];
1122         out[13] = in->m[3][1];
1123         out[14] = in->m[3][2];
1124         out[15] = in->m[3][3];
1125 #endif
1126 }
1127
1128 void Matrix4x4_FromArrayDoubleD3D (matrix4x4_t *out, const double in[16])
1129 {
1130 #ifdef MATRIX4x4_OPENGLORIENTATION
1131         out->m[0][0] = in[0];
1132         out->m[1][0] = in[1];
1133         out->m[2][0] = in[2];
1134         out->m[3][0] = in[3];
1135         out->m[0][1] = in[4];
1136         out->m[1][1] = in[5];
1137         out->m[2][1] = in[6];
1138         out->m[3][1] = in[7];
1139         out->m[0][2] = in[8];
1140         out->m[1][2] = in[9];
1141         out->m[2][2] = in[10];
1142         out->m[3][2] = in[11];
1143         out->m[0][3] = in[12];
1144         out->m[1][3] = in[13];
1145         out->m[2][3] = in[14];
1146         out->m[3][3] = in[15];
1147 #else
1148         out->m[0][0] = in[0];
1149         out->m[0][1] = in[1];
1150         out->m[0][2] = in[2];
1151         out->m[0][3] = in[3];
1152         out->m[1][0] = in[4];
1153         out->m[1][1] = in[5];
1154         out->m[1][2] = in[6];
1155         out->m[1][3] = in[7];
1156         out->m[2][0] = in[8];
1157         out->m[2][1] = in[9];
1158         out->m[2][2] = in[10];
1159         out->m[2][3] = in[11];
1160         out->m[3][0] = in[12];
1161         out->m[3][1] = in[13];
1162         out->m[3][2] = in[14];
1163         out->m[3][3] = in[15];
1164 #endif
1165 }
1166
1167 void Matrix4x4_ToArrayFloatGL(const matrix4x4_t *in, float out[16])
1168 {
1169 #ifdef MATRIX4x4_OPENGLORIENTATION
1170         out[ 0] = in->m[0][0];
1171         out[ 1] = in->m[0][1];
1172         out[ 2] = in->m[0][2];
1173         out[ 3] = in->m[0][3];
1174         out[ 4] = in->m[1][0];
1175         out[ 5] = in->m[1][1];
1176         out[ 6] = in->m[1][2];
1177         out[ 7] = in->m[1][3];
1178         out[ 8] = in->m[2][0];
1179         out[ 9] = in->m[2][1];
1180         out[10] = in->m[2][2];
1181         out[11] = in->m[2][3];
1182         out[12] = in->m[3][0];
1183         out[13] = in->m[3][1];
1184         out[14] = in->m[3][2];
1185         out[15] = in->m[3][3];
1186 #else
1187         out[ 0] = in->m[0][0];
1188         out[ 1] = in->m[1][0];
1189         out[ 2] = in->m[2][0];
1190         out[ 3] = in->m[3][0];
1191         out[ 4] = in->m[0][1];
1192         out[ 5] = in->m[1][1];
1193         out[ 6] = in->m[2][1];
1194         out[ 7] = in->m[3][1];
1195         out[ 8] = in->m[0][2];
1196         out[ 9] = in->m[1][2];
1197         out[10] = in->m[2][2];
1198         out[11] = in->m[3][2];
1199         out[12] = in->m[0][3];
1200         out[13] = in->m[1][3];
1201         out[14] = in->m[2][3];
1202         out[15] = in->m[3][3];
1203 #endif
1204 }
1205
1206 void Matrix4x4_FromArrayFloatGL (matrix4x4_t *out, const float in[16])
1207 {
1208 #ifdef MATRIX4x4_OPENGLORIENTATION
1209         out->m[0][0] = in[0];
1210         out->m[0][1] = in[1];
1211         out->m[0][2] = in[2];
1212         out->m[0][3] = in[3];
1213         out->m[1][0] = in[4];
1214         out->m[1][1] = in[5];
1215         out->m[1][2] = in[6];
1216         out->m[1][3] = in[7];
1217         out->m[2][0] = in[8];
1218         out->m[2][1] = in[9];
1219         out->m[2][2] = in[10];
1220         out->m[2][3] = in[11];
1221         out->m[3][0] = in[12];
1222         out->m[3][1] = in[13];
1223         out->m[3][2] = in[14];
1224         out->m[3][3] = in[15];
1225 #else
1226         out->m[0][0] = in[0];
1227         out->m[1][0] = in[1];
1228         out->m[2][0] = in[2];
1229         out->m[3][0] = in[3];
1230         out->m[0][1] = in[4];
1231         out->m[1][1] = in[5];
1232         out->m[2][1] = in[6];
1233         out->m[3][1] = in[7];
1234         out->m[0][2] = in[8];
1235         out->m[1][2] = in[9];
1236         out->m[2][2] = in[10];
1237         out->m[3][2] = in[11];
1238         out->m[0][3] = in[12];
1239         out->m[1][3] = in[13];
1240         out->m[2][3] = in[14];
1241         out->m[3][3] = in[15];
1242 #endif
1243 }
1244
1245 void Matrix4x4_ToArrayFloatD3D(const matrix4x4_t *in, float out[16])
1246 {
1247 #ifdef MATRIX4x4_OPENGLORIENTATION
1248         out[ 0] = in->m[0][0];
1249         out[ 1] = in->m[1][0];
1250         out[ 2] = in->m[2][0];
1251         out[ 3] = in->m[3][0];
1252         out[ 4] = in->m[0][1];
1253         out[ 5] = in->m[1][1];
1254         out[ 6] = in->m[2][1];
1255         out[ 7] = in->m[3][1];
1256         out[ 8] = in->m[0][2];
1257         out[ 9] = in->m[1][2];
1258         out[10] = in->m[2][2];
1259         out[11] = in->m[3][2];
1260         out[12] = in->m[0][3];
1261         out[13] = in->m[1][3];
1262         out[14] = in->m[2][3];
1263         out[15] = in->m[3][3];
1264 #else
1265         out[ 0] = in->m[0][0];
1266         out[ 1] = in->m[0][1];
1267         out[ 2] = in->m[0][2];
1268         out[ 3] = in->m[0][3];
1269         out[ 4] = in->m[1][0];
1270         out[ 5] = in->m[1][1];
1271         out[ 6] = in->m[1][2];
1272         out[ 7] = in->m[1][3];
1273         out[ 8] = in->m[2][0];
1274         out[ 9] = in->m[2][1];
1275         out[10] = in->m[2][2];
1276         out[11] = in->m[2][3];
1277         out[12] = in->m[3][0];
1278         out[13] = in->m[3][1];
1279         out[14] = in->m[3][2];
1280         out[15] = in->m[3][3];
1281 #endif
1282 }
1283
1284 void Matrix4x4_FromArrayFloatD3D (matrix4x4_t *out, const float in[16])
1285 {
1286 #ifdef MATRIX4x4_OPENGLORIENTATION
1287         out->m[0][0] = in[0];
1288         out->m[1][0] = in[1];
1289         out->m[2][0] = in[2];
1290         out->m[3][0] = in[3];
1291         out->m[0][1] = in[4];
1292         out->m[1][1] = in[5];
1293         out->m[2][1] = in[6];
1294         out->m[3][1] = in[7];
1295         out->m[0][2] = in[8];
1296         out->m[1][2] = in[9];
1297         out->m[2][2] = in[10];
1298         out->m[3][2] = in[11];
1299         out->m[0][3] = in[12];
1300         out->m[1][3] = in[13];
1301         out->m[2][3] = in[14];
1302         out->m[3][3] = in[15];
1303 #else
1304         out->m[0][0] = in[0];
1305         out->m[0][1] = in[1];
1306         out->m[0][2] = in[2];
1307         out->m[0][3] = in[3];
1308         out->m[1][0] = in[4];
1309         out->m[1][1] = in[5];
1310         out->m[1][2] = in[6];
1311         out->m[1][3] = in[7];
1312         out->m[2][0] = in[8];
1313         out->m[2][1] = in[9];
1314         out->m[2][2] = in[10];
1315         out->m[2][3] = in[11];
1316         out->m[3][0] = in[12];
1317         out->m[3][1] = in[13];
1318         out->m[3][2] = in[14];
1319         out->m[3][3] = in[15];
1320 #endif
1321 }
1322
1323 void Matrix4x4_ToArray12FloatGL(const matrix4x4_t *in, float out[12])
1324 {
1325 #ifdef MATRIX4x4_OPENGLORIENTATION
1326         out[ 0] = in->m[0][0];
1327         out[ 1] = in->m[0][1];
1328         out[ 2] = in->m[0][2];
1329         out[ 3] = in->m[1][0];
1330         out[ 4] = in->m[1][1];
1331         out[ 5] = in->m[1][2];
1332         out[ 6] = in->m[2][0];
1333         out[ 7] = in->m[2][1];
1334         out[ 8] = in->m[2][2];
1335         out[ 9] = in->m[3][0];
1336         out[10] = in->m[3][1];
1337         out[11] = in->m[3][2];
1338 #else
1339         out[ 0] = in->m[0][0];
1340         out[ 1] = in->m[1][0];
1341         out[ 2] = in->m[2][0];
1342         out[ 3] = in->m[0][1];
1343         out[ 4] = in->m[1][1];
1344         out[ 5] = in->m[2][1];
1345         out[ 6] = in->m[0][2];
1346         out[ 7] = in->m[1][2];
1347         out[ 8] = in->m[2][2];
1348         out[ 9] = in->m[0][3];
1349         out[10] = in->m[1][3];
1350         out[11] = in->m[2][3];
1351 #endif
1352 }
1353
1354 void Matrix4x4_FromArray12FloatGL(matrix4x4_t *out, const float in[12])
1355 {
1356 #ifdef MATRIX4x4_OPENGLORIENTATION
1357         out->m[0][0] = in[0];
1358         out->m[0][1] = in[1];
1359         out->m[0][2] = in[2];
1360         out->m[0][3] = 0;
1361         out->m[1][0] = in[3];
1362         out->m[1][1] = in[4];
1363         out->m[1][2] = in[5];
1364         out->m[1][3] = 0;
1365         out->m[2][0] = in[6];
1366         out->m[2][1] = in[7];
1367         out->m[2][2] = in[8];
1368         out->m[2][3] = 0;
1369         out->m[3][0] = in[9];
1370         out->m[3][1] = in[10];
1371         out->m[3][2] = in[11];
1372         out->m[3][3] = 1;
1373 #else
1374         out->m[0][0] = in[0];
1375         out->m[1][0] = in[1];
1376         out->m[2][0] = in[2];
1377         out->m[3][0] = 0;
1378         out->m[0][1] = in[3];
1379         out->m[1][1] = in[4];
1380         out->m[2][1] = in[5];
1381         out->m[3][1] = 0;
1382         out->m[0][2] = in[6];
1383         out->m[1][2] = in[7];
1384         out->m[2][2] = in[8];
1385         out->m[3][2] = 0;
1386         out->m[0][3] = in[9];
1387         out->m[1][3] = in[10];
1388         out->m[2][3] = in[11];
1389         out->m[3][3] = 1;
1390 #endif
1391 }
1392
1393 void Matrix4x4_ToArray12FloatD3D(const matrix4x4_t *in, float out[12])
1394 {
1395 #ifdef MATRIX4x4_OPENGLORIENTATION
1396         out[ 0] = in->m[0][0];
1397         out[ 1] = in->m[1][0];
1398         out[ 2] = in->m[2][0];
1399         out[ 3] = in->m[3][0];
1400         out[ 4] = in->m[0][1];
1401         out[ 5] = in->m[1][1];
1402         out[ 6] = in->m[2][1];
1403         out[ 7] = in->m[3][1];
1404         out[ 8] = in->m[0][2];
1405         out[ 9] = in->m[1][2];
1406         out[10] = in->m[2][2];
1407         out[11] = in->m[3][2];
1408 #else
1409         out[ 0] = in->m[0][0];
1410         out[ 1] = in->m[0][1];
1411         out[ 2] = in->m[0][2];
1412         out[ 3] = in->m[0][3];
1413         out[ 4] = in->m[1][0];
1414         out[ 5] = in->m[1][1];
1415         out[ 6] = in->m[1][2];
1416         out[ 7] = in->m[1][3];
1417         out[ 8] = in->m[2][0];
1418         out[ 9] = in->m[2][1];
1419         out[10] = in->m[2][2];
1420         out[11] = in->m[2][3];
1421 #endif
1422 }
1423
1424 void Matrix4x4_FromArray12FloatD3D(matrix4x4_t *out, const float in[12])
1425 {
1426 #ifdef MATRIX4x4_OPENGLORIENTATION
1427         out->m[0][0] = in[0];
1428         out->m[1][0] = in[1];
1429         out->m[2][0] = in[2];
1430         out->m[3][0] = in[3];
1431         out->m[0][1] = in[4];
1432         out->m[1][1] = in[5];
1433         out->m[2][1] = in[6];
1434         out->m[3][1] = in[7];
1435         out->m[0][2] = in[8];
1436         out->m[1][2] = in[9];
1437         out->m[2][2] = in[10];
1438         out->m[3][2] = in[11];
1439         out->m[0][3] = 0;
1440         out->m[1][3] = 0;
1441         out->m[2][3] = 0;
1442         out->m[3][3] = 1;
1443 #else
1444         out->m[0][0] = in[0];
1445         out->m[0][1] = in[1];
1446         out->m[0][2] = in[2];
1447         out->m[0][3] = in[3];
1448         out->m[1][0] = in[4];
1449         out->m[1][1] = in[5];
1450         out->m[1][2] = in[6];
1451         out->m[1][3] = in[7];
1452         out->m[2][0] = in[8];
1453         out->m[2][1] = in[9];
1454         out->m[2][2] = in[10];
1455         out->m[2][3] = in[11];
1456         out->m[3][0] = 0;
1457         out->m[3][1] = 0;
1458         out->m[3][2] = 0;
1459         out->m[3][3] = 1;
1460 #endif
1461 }
1462
1463 void Matrix4x4_FromOriginQuat(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z, double w)
1464 {
1465 #ifdef MATRIX4x4_OPENGLORIENTATION
1466         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;
1467         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;
1468         m->m[0][2]=  2*(x*z-y*w);m->m[1][2]=  2*(y*z+x*w);m->m[2][2]=1-2*(x*x+y*y);m->m[3][2]=oz;
1469         m->m[0][3]=  0          ;m->m[1][3]=  0          ;m->m[2][3]=  0          ;m->m[3][3]=1;
1470 #else
1471         m->m[0][0]=1-2*(y*y+z*z);m->m[0][1]=  2*(x*y-z*w);m->m[0][2]=  2*(x*z+y*w);m->m[0][3]=ox;
1472         m->m[1][0]=  2*(x*y+z*w);m->m[1][1]=1-2*(x*x+z*z);m->m[1][2]=  2*(y*z-x*w);m->m[1][3]=oy;
1473         m->m[2][0]=  2*(x*z-y*w);m->m[2][1]=  2*(y*z+x*w);m->m[2][2]=1-2*(x*x+y*y);m->m[2][3]=oz;
1474         m->m[3][0]=  0          ;m->m[3][1]=  0          ;m->m[3][2]=  0          ;m->m[3][3]=1;
1475 #endif
1476 }
1477
1478 // see http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
1479 void Matrix4x4_ToOrigin3Quat4Float(const matrix4x4_t *m, float *origin, float *quat)
1480 {
1481 #if 0
1482         float s;
1483         quat[3] = sqrt(1.0f + m->m[0][0] + m->m[1][1] + m->m[2][2]) * 0.5f;
1484         s = 0.25f / quat[3];
1485 #ifdef MATRIX4x4_OPENGLORIENTATION
1486         origin[0] = m->m[3][0];
1487         origin[1] = m->m[3][1];
1488         origin[2] = m->m[3][2];
1489         quat[0] = (m->m[1][2] - m->m[2][1]) * s;
1490         quat[1] = (m->m[2][0] - m->m[0][2]) * s;
1491         quat[2] = (m->m[0][1] - m->m[1][0]) * s;
1492 #else
1493         origin[0] = m->m[0][3];
1494         origin[1] = m->m[1][3];
1495         origin[2] = m->m[2][3];
1496         quat[0] = (m->m[2][1] - m->m[1][2]) * s;
1497         quat[1] = (m->m[0][2] - m->m[2][0]) * s;
1498         quat[2] = (m->m[1][0] - m->m[0][1]) * s;
1499 #endif
1500
1501 #else
1502
1503 #ifdef MATRIX4x4_OPENGLORIENTATION
1504         float trace = m->m[0][0] + m->m[1][1] + m->m[2][2];
1505         origin[0] = m->m[3][0];
1506         origin[1] = m->m[3][1];
1507         origin[2] = m->m[3][2];
1508         if(trace > 0)
1509         {
1510                 float r = sqrt(1.0f + trace), inv = 0.5f / r;
1511                 quat[0] = (m->m[1][2] - m->m[2][1]) * inv;
1512                 quat[1] = (m->m[2][0] - m->m[0][2]) * inv;
1513                 quat[2] = (m->m[0][1] - m->m[1][0]) * inv;
1514                 quat[3] = 0.5f * r;
1515         }
1516         else if(m->m[0][0] > m->m[1][1] && m->m[0][0] > m->m[2][2])
1517         {
1518                 float r = sqrt(1.0f + m->m[0][0] - m->m[1][1] - m->m[2][2]), inv = 0.5f / r;
1519                 quat[0] = 0.5f * r;
1520                 quat[1] = (m->m[0][1] + m->m[1][0]) * inv;
1521                 quat[2] = (m->m[2][0] + m->m[0][2]) * inv;
1522                 quat[3] = (m->m[1][2] - m->m[2][1]) * inv;
1523         }
1524         else if(m->m[1][1] > m->m[2][2])
1525         {
1526                 float r = sqrt(1.0f + m->m[1][1] - m->m[0][0] - m->m[2][2]), inv = 0.5f / r;
1527                 quat[0] = (m->m[0][1] + m->m[1][0]) * inv;
1528                 quat[1] = 0.5f * r;
1529                 quat[2] = (m->m[1][2] + m->m[2][1]) * inv;
1530                 quat[3] = (m->m[2][0] - m->m[0][2]) * inv;
1531         }
1532         else
1533         {
1534                 float r = sqrt(1.0f + m->m[2][2] - m->m[0][0] - m->m[1][1]), inv = 0.5f / r;
1535                 quat[0] = (m->m[2][0] + m->m[0][2]) * inv;
1536                 quat[1] = (m->m[1][2] + m->m[2][1]) * inv;
1537                 quat[2] = 0.5f * r;
1538                 quat[3] = (m->m[0][1] - m->m[1][0]) * inv;
1539         }
1540 #else
1541         float trace = m->m[0][0] + m->m[1][1] + m->m[2][2];
1542         origin[0] = m->m[0][3];
1543         origin[1] = m->m[1][3];
1544         origin[2] = m->m[2][3];
1545         if(trace > 0)
1546         {
1547                 float r = sqrt(1.0f + trace), inv = 0.5f / r;
1548                 quat[0] = (m->m[2][1] - m->m[1][2]) * inv;
1549                 quat[1] = (m->m[0][2] - m->m[2][0]) * inv;
1550                 quat[2] = (m->m[1][0] - m->m[0][1]) * inv;
1551                 quat[3] = 0.5f * r;
1552         }
1553         else if(m->m[0][0] > m->m[1][1] && m->m[0][0] > m->m[2][2])
1554         {
1555                 float r = sqrt(1.0f + m->m[0][0] - m->m[1][1] - m->m[2][2]), inv = 0.5f / r;
1556                 quat[0] = 0.5f * r;
1557                 quat[1] = (m->m[1][0] + m->m[0][1]) * inv;
1558                 quat[2] = (m->m[0][2] + m->m[2][0]) * inv;
1559                 quat[3] = (m->m[2][1] - m->m[1][2]) * inv;
1560         }
1561         else if(m->m[1][1] > m->m[2][2])
1562         {
1563                 float r = sqrt(1.0f + m->m[1][1] - m->m[0][0] - m->m[2][2]), inv = 0.5f / r;
1564                 quat[0] = (m->m[1][0] + m->m[0][1]) * inv;
1565                 quat[1] = 0.5f * r;
1566                 quat[2] = (m->m[2][1] + m->m[1][2]) * inv;
1567                 quat[3] = (m->m[0][2] - m->m[2][0]) * inv;
1568         }
1569         else
1570         {
1571                 float r = sqrt(1.0f + m->m[2][2] - m->m[0][0] - m->m[1][1]), inv = 0.5f / r;
1572                 quat[0] = (m->m[0][2] + m->m[2][0]) * inv;
1573                 quat[1] = (m->m[2][1] + m->m[1][2]) * inv;
1574                 quat[2] = 0.5f * r;
1575                 quat[3] = (m->m[1][0] - m->m[0][1]) * inv;
1576         }
1577 #endif
1578
1579 #endif
1580 }
1581
1582 // LordHavoc: I got this code from:
1583 //http://www.doom3world.org/phpbb2/viewtopic.php?t=2884
1584 void Matrix4x4_FromDoom3Joint(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z)
1585 {
1586         double w = 1.0f - (x*x+y*y+z*z);
1587         w = w > 0.0f ? -sqrt(w) : 0.0f;
1588 #ifdef MATRIX4x4_OPENGLORIENTATION
1589         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;
1590         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;
1591         m->m[0][2]=  2*(x*z-y*w);m->m[1][2]=  2*(y*z+x*w);m->m[2][2]=1-2*(x*x+y*y);m->m[3][2]=oz;
1592         m->m[0][3]=  0          ;m->m[1][3]=  0          ;m->m[2][3]=  0          ;m->m[3][3]=1;
1593 #else
1594         m->m[0][0]=1-2*(y*y+z*z);m->m[0][1]=  2*(x*y-z*w);m->m[0][2]=  2*(x*z+y*w);m->m[0][3]=ox;
1595         m->m[1][0]=  2*(x*y+z*w);m->m[1][1]=1-2*(x*x+z*z);m->m[1][2]=  2*(y*z-x*w);m->m[1][3]=oy;
1596         m->m[2][0]=  2*(x*z-y*w);m->m[2][1]=  2*(y*z+x*w);m->m[2][2]=1-2*(x*x+y*y);m->m[2][3]=oz;
1597         m->m[3][0]=  0          ;m->m[3][1]=  0          ;m->m[3][2]=  0          ;m->m[3][3]=1;
1598 #endif
1599 }
1600
1601 void Matrix4x4_FromBonePose7s(matrix4x4_t *m, float originscale, const short *pose7s)
1602 {
1603         float origin[3];
1604         float quat[4];
1605         float quatscale = pose7s[6] > 0 ? -1.0f / 32767.0f : 1.0f / 32767.0f;
1606         origin[0] = pose7s[0] * originscale;
1607         origin[1] = pose7s[1] * originscale;
1608         origin[2] = pose7s[2] * originscale;
1609         quat[0] = pose7s[3] * quatscale;
1610         quat[1] = pose7s[4] * quatscale;
1611         quat[2] = pose7s[5] * quatscale;
1612         quat[3] = pose7s[6] * quatscale;
1613         Matrix4x4_FromOriginQuat(m, origin[0], origin[1], origin[2], quat[0], quat[1], quat[2], quat[3]);
1614 }
1615
1616 void Matrix4x4_ToBonePose7s(const matrix4x4_t *m, float origininvscale, short *pose7s)
1617 {
1618         float origin[3];
1619         float quat[4];
1620         float quatscale;
1621         Matrix4x4_ToOrigin3Quat4Float(m, origin, quat);
1622         // normalize quaternion so that it is unit length
1623         quatscale = quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3];
1624         if (quatscale)
1625                 quatscale = (quat[3] >= 0 ? -32767.0f : 32767.0f) / sqrt(quatscale);
1626         // use a negative scale on the quat because the above function produces a
1627         // positive quat[3] and canonical quaternions have negative quat[3]
1628         pose7s[0] = origin[0] * origininvscale;
1629         pose7s[1] = origin[1] * origininvscale;
1630         pose7s[2] = origin[2] * origininvscale;
1631         pose7s[3] = quat[0] * quatscale;
1632         pose7s[4] = quat[1] * quatscale;
1633         pose7s[5] = quat[2] * quatscale;
1634         pose7s[6] = quat[3] * quatscale;
1635 }
1636
1637 void Matrix4x4_Blend (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2, double blend)
1638 {
1639         double iblend = 1 - blend;
1640         out->m[0][0] = in1->m[0][0] * iblend + in2->m[0][0] * blend;
1641         out->m[0][1] = in1->m[0][1] * iblend + in2->m[0][1] * blend;
1642         out->m[0][2] = in1->m[0][2] * iblend + in2->m[0][2] * blend;
1643         out->m[0][3] = in1->m[0][3] * iblend + in2->m[0][3] * blend;
1644         out->m[1][0] = in1->m[1][0] * iblend + in2->m[1][0] * blend;
1645         out->m[1][1] = in1->m[1][1] * iblend + in2->m[1][1] * blend;
1646         out->m[1][2] = in1->m[1][2] * iblend + in2->m[1][2] * blend;
1647         out->m[1][3] = in1->m[1][3] * iblend + in2->m[1][3] * blend;
1648         out->m[2][0] = in1->m[2][0] * iblend + in2->m[2][0] * blend;
1649         out->m[2][1] = in1->m[2][1] * iblend + in2->m[2][1] * blend;
1650         out->m[2][2] = in1->m[2][2] * iblend + in2->m[2][2] * blend;
1651         out->m[2][3] = in1->m[2][3] * iblend + in2->m[2][3] * blend;
1652         out->m[3][0] = in1->m[3][0] * iblend + in2->m[3][0] * blend;
1653         out->m[3][1] = in1->m[3][1] * iblend + in2->m[3][1] * blend;
1654         out->m[3][2] = in1->m[3][2] * iblend + in2->m[3][2] * blend;
1655         out->m[3][3] = in1->m[3][3] * iblend + in2->m[3][3] * blend;
1656 }
1657
1658
1659 void Matrix4x4_Transform (const matrix4x4_t *in, const float v[3], float out[3])
1660 {
1661 #ifdef MATRIX4x4_OPENGLORIENTATION
1662         out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0] + in->m[3][0];
1663         out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1] + in->m[3][1];
1664         out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2] + in->m[3][2];
1665 #else
1666         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
1667         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
1668         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
1669 #endif
1670 }
1671
1672 void Matrix4x4_Transform4 (const matrix4x4_t *in, const float v[4], float out[4])
1673 {
1674 #ifdef MATRIX4x4_OPENGLORIENTATION
1675         out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0] + v[3] * in->m[3][0];
1676         out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1] + v[3] * in->m[3][1];
1677         out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2] + v[3] * in->m[3][2];
1678         out[3] = v[0] * in->m[0][3] + v[1] * in->m[1][3] + v[2] * in->m[2][3] + v[3] * in->m[3][3];
1679 #else
1680         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + v[3] * in->m[0][3];
1681         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + v[3] * in->m[1][3];
1682         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + v[3] * in->m[2][3];
1683         out[3] = v[0] * in->m[3][0] + v[1] * in->m[3][1] + v[2] * in->m[3][2] + v[3] * in->m[3][3];
1684 #endif
1685 }
1686
1687 void Matrix4x4_Transform3x3 (const matrix4x4_t *in, const float v[3], float out[3])
1688 {
1689 #ifdef MATRIX4x4_OPENGLORIENTATION
1690         out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0];
1691         out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1];
1692         out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2];
1693 #else
1694         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2];
1695         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2];
1696         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2];
1697 #endif
1698 }
1699
1700 // transforms a positive distance plane (A*x+B*y+C*z-D=0) through a rotation or translation matrix
1701 void Matrix4x4_TransformPositivePlane(const matrix4x4_t *in, float x, float y, float z, float d, float *o)
1702 {
1703         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]);
1704         float iscale = 1.0f / scale;
1705 #ifdef MATRIX4x4_OPENGLORIENTATION
1706         o[0] = (x * in->m[0][0] + y * in->m[1][0] + z * in->m[2][0]) * iscale;
1707         o[1] = (x * in->m[0][1] + y * in->m[1][1] + z * in->m[2][1]) * iscale;
1708         o[2] = (x * in->m[0][2] + y * in->m[1][2] + z * in->m[2][2]) * iscale;
1709         o[3] = d * scale + (o[0] * in->m[3][0] + o[1] * in->m[3][1] + o[2] * in->m[3][2]);
1710 #else
1711         o[0] = (x * in->m[0][0] + y * in->m[0][1] + z * in->m[0][2]) * iscale;
1712         o[1] = (x * in->m[1][0] + y * in->m[1][1] + z * in->m[1][2]) * iscale;
1713         o[2] = (x * in->m[2][0] + y * in->m[2][1] + z * in->m[2][2]) * iscale;
1714         o[3] = d * scale + (o[0] * in->m[0][3] + o[1] * in->m[1][3] + o[2] * in->m[2][3]);
1715 #endif
1716 }
1717
1718 // transforms a standard plane (A*x+B*y+C*z+D=0) through a rotation or translation matrix
1719 void Matrix4x4_TransformStandardPlane(const matrix4x4_t *in, float x, float y, float z, float d, float *o)
1720 {
1721         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]);
1722         float iscale = 1.0f / scale;
1723 #ifdef MATRIX4x4_OPENGLORIENTATION
1724         o[0] = (x * in->m[0][0] + y * in->m[1][0] + z * in->m[2][0]) * iscale;
1725         o[1] = (x * in->m[0][1] + y * in->m[1][1] + z * in->m[2][1]) * iscale;
1726         o[2] = (x * in->m[0][2] + y * in->m[1][2] + z * in->m[2][2]) * iscale;
1727         o[3] = d * scale - (o[0] * in->m[3][0] + o[1] * in->m[3][1] + o[2] * in->m[3][2]);
1728 #else
1729         o[0] = (x * in->m[0][0] + y * in->m[0][1] + z * in->m[0][2]) * iscale;
1730         o[1] = (x * in->m[1][0] + y * in->m[1][1] + z * in->m[1][2]) * iscale;
1731         o[2] = (x * in->m[2][0] + y * in->m[2][1] + z * in->m[2][2]) * iscale;
1732         o[3] = d * scale - (o[0] * in->m[0][3] + o[1] * in->m[1][3] + o[2] * in->m[2][3]);
1733 #endif
1734 }
1735
1736 /*
1737 void Matrix4x4_SimpleUntransform (const matrix4x4_t *in, const float v[3], float out[3])
1738 {
1739         double t[3];
1740 #ifdef MATRIX4x4_OPENGLORIENTATION
1741         t[0] = v[0] - in->m[3][0];
1742         t[1] = v[1] - in->m[3][1];
1743         t[2] = v[2] - in->m[3][2];
1744         out[0] = t[0] * in->m[0][0] + t[1] * in->m[0][1] + t[2] * in->m[0][2];
1745         out[1] = t[0] * in->m[1][0] + t[1] * in->m[1][1] + t[2] * in->m[1][2];
1746         out[2] = t[0] * in->m[2][0] + t[1] * in->m[2][1] + t[2] * in->m[2][2];
1747 #else
1748         t[0] = v[0] - in->m[0][3];
1749         t[1] = v[1] - in->m[1][3];
1750         t[2] = v[2] - in->m[2][3];
1751         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
1752         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
1753         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
1754 #endif
1755 }
1756 */
1757
1758 // FIXME: optimize
1759 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, double x, double y, double z)
1760 {
1761         matrix4x4_t base, temp;
1762         base = *out;
1763         Matrix4x4_CreateTranslate(&temp, x, y, z);
1764         Matrix4x4_Concat(out, &base, &temp);
1765 }
1766
1767 // FIXME: optimize
1768 void Matrix4x4_ConcatRotate (matrix4x4_t *out, double angle, double x, double y, double z)
1769 {
1770         matrix4x4_t base, temp;
1771         base = *out;
1772         Matrix4x4_CreateRotate(&temp, angle, x, y, z);
1773         Matrix4x4_Concat(out, &base, &temp);
1774 }
1775
1776 // FIXME: optimize
1777 void Matrix4x4_ConcatScale (matrix4x4_t *out, double x)
1778 {
1779         matrix4x4_t base, temp;
1780         base = *out;
1781         Matrix4x4_CreateScale(&temp, x);
1782         Matrix4x4_Concat(out, &base, &temp);
1783 }
1784
1785 // FIXME: optimize
1786 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, double x, double y, double z)
1787 {
1788         matrix4x4_t base, temp;
1789         base = *out;
1790         Matrix4x4_CreateScale3(&temp, x, y, z);
1791         Matrix4x4_Concat(out, &base, &temp);
1792 }
1793
1794 void Matrix4x4_OriginFromMatrix (const matrix4x4_t *in, float *out)
1795 {
1796 #ifdef MATRIX4x4_OPENGLORIENTATION
1797         out[0] = in->m[3][0];
1798         out[1] = in->m[3][1];
1799         out[2] = in->m[3][2];
1800 #else
1801         out[0] = in->m[0][3];
1802         out[1] = in->m[1][3];
1803         out[2] = in->m[2][3];
1804 #endif
1805 }
1806
1807 double Matrix4x4_ScaleFromMatrix (const matrix4x4_t *in)
1808 {
1809         // we only support uniform scaling, so assume the first row is enough
1810         return 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]);
1811 }
1812
1813 void Matrix4x4_SetOrigin (matrix4x4_t *out, double x, double y, double z)
1814 {
1815 #ifdef MATRIX4x4_OPENGLORIENTATION
1816         out->m[3][0] = x;
1817         out->m[3][1] = y;
1818         out->m[3][2] = z;
1819 #else
1820         out->m[0][3] = x;
1821         out->m[1][3] = y;
1822         out->m[2][3] = z;
1823 #endif
1824 }
1825
1826 void Matrix4x4_AdjustOrigin (matrix4x4_t *out, double x, double y, double z)
1827 {
1828 #ifdef MATRIX4x4_OPENGLORIENTATION
1829         out->m[3][0] += x;
1830         out->m[3][1] += y;
1831         out->m[3][2] += z;
1832 #else
1833         out->m[0][3] += x;
1834         out->m[1][3] += y;
1835         out->m[2][3] += z;
1836 #endif
1837 }
1838
1839 void Matrix4x4_Scale (matrix4x4_t *out, double rotatescale, double originscale)
1840 {
1841         out->m[0][0] *= rotatescale;
1842         out->m[0][1] *= rotatescale;
1843         out->m[0][2] *= rotatescale;
1844         out->m[1][0] *= rotatescale;
1845         out->m[1][1] *= rotatescale;
1846         out->m[1][2] *= rotatescale;
1847         out->m[2][0] *= rotatescale;
1848         out->m[2][1] *= rotatescale;
1849         out->m[2][2] *= rotatescale;
1850 #ifdef MATRIX4x4_OPENGLORIENTATION
1851         out->m[3][0] *= originscale;
1852         out->m[3][1] *= originscale;
1853         out->m[3][2] *= originscale;
1854 #else
1855         out->m[0][3] *= originscale;
1856         out->m[1][3] *= originscale;
1857         out->m[2][3] *= originscale;
1858 #endif
1859 }
1860
1861 void Matrix4x4_Abs (matrix4x4_t *out)
1862 {
1863         out->m[0][0] = fabs(out->m[0][0]);
1864         out->m[0][1] = fabs(out->m[0][1]);
1865         out->m[0][2] = fabs(out->m[0][2]);
1866         out->m[1][0] = fabs(out->m[1][0]);
1867         out->m[1][1] = fabs(out->m[1][1]);
1868         out->m[1][2] = fabs(out->m[1][2]);
1869         out->m[2][0] = fabs(out->m[2][0]);
1870         out->m[2][1] = fabs(out->m[2][1]);
1871         out->m[2][2] = fabs(out->m[2][2]);
1872 }
1873