matrixlib.c

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