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 int Matrix4x4_Invert_Full (matrix4x4_t *out, const matrix4x4_t *in1)
 147 {
 148         matrix4x4_t temp;
 149         double det;
 150         int i, j;
 151
 152 #ifdef MATRIX4x4_OPENGLORIENTATION
 153         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];
 154         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];
 155         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];
 156         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];
 157         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];
 158         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];
 159         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];
 160         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];
 161         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];
 162         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];
 163         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];
 164         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];
 165         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];
 166         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];
 167         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];
 168         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];
 169 #else
 170         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];
 171         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];
 172         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];
 173         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];
 174         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];
 175         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];
 176         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];
 177         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];
 178         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];
 179         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];
 180         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];
 181         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];
 182         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];
 183         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];
 184         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];
 185         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];
 186 #endif
 187
 188         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];
 189         if (det == 0.0f)
 190                 return 0;
 191
 192         det = 1.0f / det;
 193
 194         for (i = 0;i < 4;i++)
 195                 for (j = 0;j < 4;j++)
 196                         out->m[i][j] = temp.m[i][j] * det;
 197
 198         return 1;
 199 }
 200 #elif 1
 201 // Adapted from code contributed to Mesa by David Moore (Mesa 7.6 under SGI Free License B - which is MIT/X11-type)
 202 // number of multiplications reduced by divVerent, yet to be benchmarked
 203 int Matrix4x4_Invert_Full (matrix4x4_t *out, const matrix4x4_t *in1)
 204 {
 205         matrix4x4_t temp;
 206         double det;
 207         int i, j;
 208
 209 #ifdef MATRIX4x4_OPENGLORIENTATION
 210         temp.m[0][0] =  in1->m[1][1]*(in1->m[2][2]*in1->m[3][3] - in1->m[2][3]*in1->m[3][2]) - in1->m[2][1]*(in1->m[1][2]*in1->m[3][3] - in1->m[1][3]*in1->m[3][2]) + in1->m[3][1]*(in1->m[1][2]*in1->m[2][3] - in1->m[1][3]*in1->m[2][2]);
 211         temp.m[1][0] = -in1->m[1][0]*(in1->m[2][2]*in1->m[3][3] - in1->m[2][3]*in1->m[3][2]) + in1->m[2][0]*(in1->m[1][2]*in1->m[3][3] - in1->m[1][3]*in1->m[3][2]) - in1->m[3][0]*(in1->m[1][2]*in1->m[2][3] - in1->m[1][3]*in1->m[2][2]);
 212         temp.m[2][0] =  in1->m[1][0]*(in1->m[2][1]*in1->m[3][3] - in1->m[2][3]*in1->m[3][1]) - in1->m[2][0]*(in1->m[1][1]*in1->m[3][3] - in1->m[1][3]*in1->m[3][1]) + in1->m[3][0]*(in1->m[1][1]*in1->m[2][3] - in1->m[1][3]*in1->m[2][1]);
 213         temp.m[3][0] = -in1->m[1][0]*(in1->m[2][1]*in1->m[3][2] - in1->m[2][2]*in1->m[3][1]) + in1->m[2][0]*(in1->m[1][1]*in1->m[3][2] - in1->m[1][2]*in1->m[3][1]) - in1->m[3][0]*(in1->m[1][1]*in1->m[2][2] - in1->m[1][2]*in1->m[2][1]);
 214         temp.m[0][1] = -in1->m[0][1]*(in1->m[2][2]*in1->m[3][3] - in1->m[2][3]*in1->m[3][2]) + in1->m[2][1]*(in1->m[0][2]*in1->m[3][3] - in1->m[0][3]*in1->m[3][2]) - in1->m[3][1]*(in1->m[0][2]*in1->m[2][3] - in1->m[0][3]*in1->m[2][2]);
 215         temp.m[1][1] =  in1->m[0][0]*(in1->m[2][2]*in1->m[3][3] - in1->m[2][3]*in1->m[3][2]) - in1->m[2][0]*(in1->m[0][2]*in1->m[3][3] - in1->m[0][3]*in1->m[3][2]) + in1->m[3][0]*(in1->m[0][2]*in1->m[2][3] - in1->m[0][3]*in1->m[2][2]);
 216         temp.m[2][1] = -in1->m[0][0]*(in1->m[2][1]*in1->m[3][3] - in1->m[2][3]*in1->m[3][1]) + in1->m[2][0]*(in1->m[0][1]*in1->m[3][3] - in1->m[0][3]*in1->m[3][1]) - in1->m[3][0]*(in1->m[0][1]*in1->m[2][3] - in1->m[0][3]*in1->m[2][1]);
 217         temp.m[3][1] =  in1->m[0][0]*(in1->m[2][1]*in1->m[3][2] - in1->m[2][2]*in1->m[3][1]) - in1->m[2][0]*(in1->m[0][1]*in1->m[3][2] - in1->m[0][2]*in1->m[3][1]) + in1->m[3][0]*(in1->m[0][1]*in1->m[2][2] - in1->m[0][2]*in1->m[2][1]);
 218         temp.m[0][2] =  in1->m[0][1]*(in1->m[1][2]*in1->m[3][3] - in1->m[1][3]*in1->m[3][2]) - in1->m[1][1]*(in1->m[0][2]*in1->m[3][3] - in1->m[0][3]*in1->m[3][2]) + in1->m[3][1]*(in1->m[0][2]*in1->m[1][3] - in1->m[0][3]*in1->m[1][2]);
 219         temp.m[1][2] = -in1->m[0][0]*(in1->m[1][2]*in1->m[3][3] - in1->m[1][3]*in1->m[3][2]) + in1->m[1][0]*(in1->m[0][2]*in1->m[3][3] - in1->m[0][3]*in1->m[3][2]) - in1->m[3][0]*(in1->m[0][2]*in1->m[1][3] - in1->m[0][3]*in1->m[1][2]);
 220         temp.m[2][2] =  in1->m[0][0]*(in1->m[1][1]*in1->m[3][3] - in1->m[1][3]*in1->m[3][1]) - in1->m[1][0]*(in1->m[0][1]*in1->m[3][3] - in1->m[0][3]*in1->m[3][1]) + in1->m[3][0]*(in1->m[0][1]*in1->m[1][3] - in1->m[0][3]*in1->m[1][1]);
 221         temp.m[3][2] = -in1->m[0][0]*(in1->m[1][1]*in1->m[3][2] - in1->m[1][2]*in1->m[3][1]) + in1->m[1][0]*(in1->m[0][1]*in1->m[3][2] - in1->m[0][2]*in1->m[3][1]) - in1->m[3][0]*(in1->m[0][1]*in1->m[1][2] - in1->m[0][2]*in1->m[1][1]);
 222         temp.m[0][3] = -in1->m[0][1]*(in1->m[1][2]*in1->m[2][3] - in1->m[1][3]*in1->m[2][2]) + in1->m[1][1]*(in1->m[0][2]*in1->m[2][3] - in1->m[0][3]*in1->m[2][2]) - in1->m[2][1]*(in1->m[0][2]*in1->m[1][3] - in1->m[0][3]*in1->m[1][2]);
 223         temp.m[1][3] =  in1->m[0][0]*(in1->m[1][2]*in1->m[2][3] - in1->m[1][3]*in1->m[2][2]) - in1->m[1][0]*(in1->m[0][2]*in1->m[2][3] - in1->m[0][3]*in1->m[2][2]) + in1->m[2][0]*(in1->m[0][2]*in1->m[1][3] - in1->m[0][3]*in1->m[1][2]);
 224         temp.m[2][3] = -in1->m[0][0]*(in1->m[1][1]*in1->m[2][3] - in1->m[1][3]*in1->m[2][1]) + in1->m[1][0]*(in1->m[0][1]*in1->m[2][3] - in1->m[0][3]*in1->m[2][1]) - in1->m[2][0]*(in1->m[0][1]*in1->m[1][3] - in1->m[0][3]*in1->m[1][1]);
 225         temp.m[3][3] =  in1->m[0][0]*(in1->m[1][1]*in1->m[2][2] - in1->m[1][2]*in1->m[2][1]) - in1->m[1][0]*(in1->m[0][1]*in1->m[2][2] - in1->m[0][2]*in1->m[2][1]) + in1->m[2][0]*(in1->m[0][1]*in1->m[1][2] - in1->m[0][2]*in1->m[1][1]);
 226 #else
 227         temp.m[0][0] =  in1->m[1][1]*(in1->m[2][2]*in1->m[3][3] - in1->m[3][2]*in1->m[2][3]) - in1->m[1][2]*(in1->m[2][1]*in1->m[3][3] - in1->m[3][1]*in1->m[2][3]) + in1->m[1][3]*(in1->m[2][1]*in1->m[3][2] - in1->m[3][1]*in1->m[2][2]);
 228         temp.m[0][1] = -in1->m[0][1]*(in1->m[2][2]*in1->m[3][3] - in1->m[3][2]*in1->m[2][3]) + in1->m[0][2]*(in1->m[2][1]*in1->m[3][3] - in1->m[3][1]*in1->m[2][3]) - in1->m[0][3]*(in1->m[2][1]*in1->m[3][2] + in1->m[3][1]*in1->m[2][2]);
 229         temp.m[0][2] =  in1->m[0][1]*(in1->m[1][2]*in1->m[3][3] - in1->m[3][2]*in1->m[1][3]) - in1->m[0][2]*(in1->m[1][1]*in1->m[3][3] - in1->m[3][1]*in1->m[1][3]) + in1->m[0][3]*(in1->m[1][1]*in1->m[3][2] - in1->m[3][1]*in1->m[1][2]);
 230         temp.m[0][3] = -in1->m[0][1]*(in1->m[1][2]*in1->m[2][3] - in1->m[2][2]*in1->m[1][3]) + in1->m[0][2]*(in1->m[1][1]*in1->m[2][3] - in1->m[2][1]*in1->m[1][3]) - in1->m[0][3]*(in1->m[1][1]*in1->m[2][2] + in1->m[2][1]*in1->m[1][2]);
 231         temp.m[1][0] = -in1->m[1][0]*(in1->m[2][2]*in1->m[3][3] - in1->m[3][2]*in1->m[2][3]) + in1->m[1][2]*(in1->m[2][0]*in1->m[3][3] - in1->m[3][0]*in1->m[2][3]) - in1->m[1][3]*(in1->m[2][0]*in1->m[3][2] + in1->m[3][0]*in1->m[2][2]);
 232         temp.m[1][1] =  in1->m[0][0]*(in1->m[2][2]*in1->m[3][3] - in1->m[3][2]*in1->m[2][3]) - in1->m[0][2]*(in1->m[2][0]*in1->m[3][3] - in1->m[3][0]*in1->m[2][3]) + in1->m[0][3]*(in1->m[2][0]*in1->m[3][2] - in1->m[3][0]*in1->m[2][2]);
 233         temp.m[1][2] = -in1->m[0][0]*(in1->m[1][2]*in1->m[3][3] - in1->m[3][2]*in1->m[1][3]) + in1->m[0][2]*(in1->m[1][0]*in1->m[3][3] - in1->m[3][0]*in1->m[1][3]) - in1->m[0][3]*(in1->m[1][0]*in1->m[3][2] + in1->m[3][0]*in1->m[1][2]);
 234         temp.m[1][3] =  in1->m[0][0]*(in1->m[1][2]*in1->m[2][3] - in1->m[2][2]*in1->m[1][3]) - in1->m[0][2]*(in1->m[1][0]*in1->m[2][3] - in1->m[2][0]*in1->m[1][3]) + in1->m[0][3]*(in1->m[1][0]*in1->m[2][2] - in1->m[2][0]*in1->m[1][2]);
 235         temp.m[2][0] =  in1->m[1][0]*(in1->m[2][1]*in1->m[3][3] - in1->m[3][1]*in1->m[2][3]) - in1->m[1][1]*(in1->m[2][0]*in1->m[3][3] - in1->m[3][0]*in1->m[2][3]) + in1->m[1][3]*(in1->m[2][0]*in1->m[3][1] - in1->m[3][0]*in1->m[2][1]);
 236         temp.m[2][1] = -in1->m[0][0]*(in1->m[2][1]*in1->m[3][3] - in1->m[3][1]*in1->m[2][3]) + in1->m[0][1]*(in1->m[2][0]*in1->m[3][3] - in1->m[3][0]*in1->m[2][3]) - in1->m[0][3]*(in1->m[2][0]*in1->m[3][1] + in1->m[3][0]*in1->m[2][1]);
 237         temp.m[2][2] =  in1->m[0][0]*(in1->m[1][1]*in1->m[3][3] - in1->m[3][1]*in1->m[1][3]) - in1->m[0][1]*(in1->m[1][0]*in1->m[3][3] - in1->m[3][0]*in1->m[1][3]) + in1->m[0][3]*(in1->m[1][0]*in1->m[3][1] - in1->m[3][0]*in1->m[1][1]);
 238         temp.m[2][3] = -in1->m[0][0]*(in1->m[1][1]*in1->m[2][3] - in1->m[2][1]*in1->m[1][3]) + in1->m[0][1]*(in1->m[1][0]*in1->m[2][3] - in1->m[2][0]*in1->m[1][3]) - in1->m[0][3]*(in1->m[1][0]*in1->m[2][1] + in1->m[2][0]*in1->m[1][1]);
 239         temp.m[3][0] = -in1->m[1][0]*(in1->m[2][1]*in1->m[3][2] - in1->m[3][1]*in1->m[2][2]) + in1->m[1][1]*(in1->m[2][0]*in1->m[3][2] - in1->m[3][0]*in1->m[2][2]) - in1->m[1][2]*(in1->m[2][0]*in1->m[3][1] + in1->m[3][0]*in1->m[2][1]);
 240         temp.m[3][1] =  in1->m[0][0]*(in1->m[2][1]*in1->m[3][2] - in1->m[3][1]*in1->m[2][2]) - in1->m[0][1]*(in1->m[2][0]*in1->m[3][2] - in1->m[3][0]*in1->m[2][2]) + in1->m[0][2]*(in1->m[2][0]*in1->m[3][1] - in1->m[3][0]*in1->m[2][1]);
 241         temp.m[3][2] = -in1->m[0][0]*(in1->m[1][1]*in1->m[3][2] - in1->m[3][1]*in1->m[1][2]) + in1->m[0][1]*(in1->m[1][0]*in1->m[3][2] - in1->m[3][0]*in1->m[1][2]) - in1->m[0][2]*(in1->m[1][0]*in1->m[3][1] + in1->m[3][0]*in1->m[1][1]);
 242         temp.m[3][3] =  in1->m[0][0]*(in1->m[1][1]*in1->m[2][2] - in1->m[2][1]*in1->m[1][2]) - in1->m[0][1]*(in1->m[1][0]*in1->m[2][2] - in1->m[2][0]*in1->m[1][2]) + in1->m[0][2]*(in1->m[1][0]*in1->m[2][1] - in1->m[2][0]*in1->m[1][1]);
 243 #endif
 244
 245         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];
 246         if (det == 0.0f)
 247                 return 0;
 248
 249         det = 1.0f / det;
 250
 251         for (i = 0;i < 4;i++)
 252                 for (j = 0;j < 4;j++)
 253                         out->m[i][j] = temp.m[i][j] * det;
 254
 255         return 1;
 256 }
 257 #else
 258 int Matrix4x4_Invert_Full (matrix4x4_t *out, const matrix4x4_t *in1)
 259 {
 260         double  *temp;
 261         double  *r[4];
 262         double  rtemp[4][8];
 263         double  m[4];
 264         double  s;
 265
 266         r[0]    = rtemp[0];
 267         r[1]    = rtemp[1];
 268         r[2]    = rtemp[2];
 269         r[3]    = rtemp[3];
 270
 271 #ifdef MATRIX4x4_OPENGLORIENTATION
 272         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];
 273         r[0][4] = 1.0;                  r[0][5] =                               r[0][6] =                               r[0][7] = 0.0;
 274
 275         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];
 276         r[1][5] = 1.0;                  r[1][4] =                               r[1][6] =                               r[1][7] = 0.0;
 277
 278         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];
 279         r[2][6] = 1.0;                  r[2][4] =                               r[2][5] =                               r[2][7] = 0.0;
 280
 281         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];
 282         r[3][7] = 1.0;                  r[3][4] =                               r[3][5] =                               r[3][6] = 0.0;
 283 #else
 284         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];
 285         r[0][4] = 1.0;                  r[0][5] =                               r[0][6] =                               r[0][7] = 0.0;
 286
 287         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];
 288         r[1][5] = 1.0;                  r[1][4] =                               r[1][6] =                               r[1][7] = 0.0;
 289
 290         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];
 291         r[2][6] = 1.0;                  r[2][4] =                               r[2][5] =                               r[2][7] = 0.0;
 292
 293         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];
 294         r[3][7] = 1.0;                  r[3][4] =                               r[3][5] =                               r[3][6] = 0.0;
 295 #endif
 296
 297         if (fabs (r[3][0]) > fabs (r[2][0])) { temp = r[3]; r[3] = r[2]; r[2] = temp; }
 298         if (fabs (r[2][0]) > fabs (r[1][0])) { temp = r[2]; r[2] = r[1]; r[1] = temp; }
 299         if (fabs (r[1][0]) > fabs (r[0][0])) { temp = r[1]; r[1] = r[0]; r[0] = temp; }
 300
 301         if (r[0][0])
 302         {
 303                 m[1]    = r[1][0] / r[0][0];
 304                 m[2]    = r[2][0] / r[0][0];
 305                 m[3]    = r[3][0] / r[0][0];
 306
 307                 s       = r[0][1]; r[1][1] -= m[1] * s; r[2][1] -= m[2] * s; r[3][1] -= m[3] * s;
 308                 s       = r[0][2]; r[1][2] -= m[1] * s; r[2][2] -= m[2] * s; r[3][2] -= m[3] * s;
 309                 s       = r[0][3]; r[1][3] -= m[1] * s; r[2][3] -= m[2] * s; r[3][3] -= m[3] * s;
 310
 311                 s       = r[0][4]; if (s) { r[1][4] -= m[1] * s; r[2][4] -= m[2] * s; r[3][4] -= m[3] * s; }
 312                 s       = r[0][5]; if (s) { r[1][5] -= m[1] * s; r[2][5] -= m[2] * s; r[3][5] -= m[3] * s; }
 313                 s       = r[0][6]; if (s) { r[1][6] -= m[1] * s; r[2][6] -= m[2] * s; r[3][6] -= m[3] * s; }
 314                 s       = r[0][7]; if (s) { r[1][7] -= m[1] * s; r[2][7] -= m[2] * s; r[3][7] -= m[3] * s; }
 315
 316                 if (fabs (r[3][1]) > fabs (r[2][1])) { temp = r[3]; r[3] = r[2]; r[2] = temp; }
 317                 if (fabs (r[2][1]) > fabs (r[1][1])) { temp = r[2]; r[2] = r[1]; r[1] = temp; }
 318
 319                 if (r[1][1])
 320                 {
 321                         m[2]            = r[2][1] / r[1][1];
 322                         m[3]            = r[3][1] / r[1][1];
 323                         r[2][2] -= m[2] * r[1][2];
 324                         r[3][2] -= m[3] * r[1][2];
 325                         r[2][3] -= m[2] * r[1][3];
 326                         r[3][3] -= m[3] * r[1][3];
 327
 328                         s       = r[1][4]; if (s) { r[2][4] -= m[2] * s; r[3][4] -= m[3] * s; }
 329                         s       = r[1][5]; if (s) { r[2][5] -= m[2] * s; r[3][5] -= m[3] * s; }
 330                         s       = r[1][6]; if (s) { r[2][6] -= m[2] * s; r[3][6] -= m[3] * s; }
 331                         s       = r[1][7]; if (s) { r[2][7] -= m[2] * s; r[3][7] -= m[3] * s; }
 332
 333                         if (fabs (r[3][2]) > fabs (r[2][2])) { temp = r[3]; r[3] = r[2]; r[2] = temp; }
 334
 335                         if (r[2][2])
 336                         {
 337                                 m[3]            = r[3][2] / r[2][2];
 338                                 r[3][3] -= m[3] * r[2][3];
 339                                 r[3][4] -= m[3] * r[2][4];
 340                                 r[3][5] -= m[3] * r[2][5];
 341                                 r[3][6] -= m[3] * r[2][6];
 342                                 r[3][7] -= m[3] * r[2][7];
 343
 344                                 if (r[3][3])
 345                                 {
 346                                         s                       = 1.0 / r[3][3];
 347                                         r[3][4] *= s;
 348                                         r[3][5] *= s;
 349                                         r[3][6] *= s;
 350                                         r[3][7] *= s;
 351
 352                                         m[2]            = r[2][3];
 353                                         s                       = 1.0 / r[2][2];
 354                                         r[2][4] = s * (r[2][4] - r[3][4] * m[2]);
 355                                         r[2][5] = s * (r[2][5] - r[3][5] * m[2]);
 356                                         r[2][6] = s * (r[2][6] - r[3][6] * m[2]);
 357                                         r[2][7] = s * (r[2][7] - r[3][7] * m[2]);
 358
 359                                         m[1]            = r[1][3];
 360                                         r[1][4] -= r[3][4] * m[1], r[1][5] -= r[3][5] * m[1];
 361                                         r[1][6] -= r[3][6] * m[1], r[1][7] -= r[3][7] * m[1];
 362
 363                                         m[0]            = r[0][3];
 364                                         r[0][4] -= r[3][4] * m[0], r[0][5] -= r[3][5] * m[0];
 365                                         r[0][6] -= r[3][6] * m[0], r[0][7] -= r[3][7] * m[0];
 366
 367                                         m[1]            = r[1][2];
 368                                         s                       = 1.0 / r[1][1];
 369                                         r[1][4] = s * (r[1][4] - r[2][4] * m[1]), r[1][5] = s * (r[1][5] - r[2][5] * m[1]);
 370                                         r[1][6] = s * (r[1][6] - r[2][6] * m[1]), r[1][7] = s * (r[1][7] - r[2][7] * m[1]);
 371
 372                                         m[0]            = r[0][2];
 373                                         r[0][4] -= r[2][4] * m[0], r[0][5] -= r[2][5] * m[0];
 374                                         r[0][6] -= r[2][6] * m[0], r[0][7] -= r[2][7] * m[0];
 375
 376                                         m[0]            = r[0][1];
 377                                         s                       = 1.0 / r[0][0];
 378                                         r[0][4] = s * (r[0][4] - r[1][4] * m[0]), r[0][5] = s * (r[0][5] - r[1][5] * m[0]);
 379                                         r[0][6] = s * (r[0][6] - r[1][6] * m[0]), r[0][7] = s * (r[0][7] - r[1][7] * m[0]);
 380
 381 #ifdef MATRIX4x4_OPENGLORIENTATION
 382                                         out->m[0][0]    = r[0][4];
 383                                         out->m[0][1]    = r[1][4];
 384                                         out->m[0][2]    = r[2][4];
 385                                         out->m[0][3]    = r[3][4];
 386                                         out->m[1][0]    = r[0][5];
 387                                         out->m[1][1]    = r[1][5];
 388                                         out->m[1][2]    = r[2][5];
 389                                         out->m[1][3]    = r[3][5];
 390                                         out->m[2][0]    = r[0][6];
 391                                         out->m[2][1]    = r[1][6];
 392                                         out->m[2][2]    = r[2][6];
 393                                         out->m[2][3]    = r[3][6];
 394                                         out->m[3][0]    = r[0][7];
 395                                         out->m[3][1]    = r[1][7];
 396                                         out->m[3][2]    = r[2][7];
 397                                         out->m[3][3]    = r[3][7];
 398 #else
 399                                         out->m[0][0]    = r[0][4];
 400                                         out->m[0][1]    = r[0][5];
 401                                         out->m[0][2]    = r[0][6];
 402                                         out->m[0][3]    = r[0][7];
 403                                         out->m[1][0]    = r[1][4];
 404                                         out->m[1][1]    = r[1][5];
 405                                         out->m[1][2]    = r[1][6];
 406                                         out->m[1][3]    = r[1][7];
 407                                         out->m[2][0]    = r[2][4];
 408                                         out->m[2][1]    = r[2][5];
 409                                         out->m[2][2]    = r[2][6];
 410                                         out->m[2][3]    = r[2][7];
 411                                         out->m[3][0]    = r[3][4];
 412                                         out->m[3][1]    = r[3][5];
 413                                         out->m[3][2]    = r[3][6];
 414                                         out->m[3][3]    = r[3][7];
 415 #endif
 416
 417                                         return 1;
 418                                 }
 419                         }
 420                 }
 421         }
 422
 423         return 0;
 424 }
 425 #endif
 426
 427 void Matrix4x4_Invert_Simple (matrix4x4_t *out, const matrix4x4_t *in1)
 428 {
 429         // we only support uniform scaling, so assume the first row is enough
 430         // (note the lack of sqrt here, because we're trying to undo the scaling,
 431         // this means multiplying by the inverse scale twice - squaring it, which
 432         // makes the sqrt a waste of time)
 433 #if 1
 434         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]);
 435 #else
 436         double scale = 3.0 / sqrt
 437                  (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]
 438                 + in1->m[1][0] * in1->m[1][0] + in1->m[1][1] * in1->m[1][1] + in1->m[1][2] * in1->m[1][2]
 439                 + in1->m[2][0] * in1->m[2][0] + in1->m[2][1] * in1->m[2][1] + in1->m[2][2] * in1->m[2][2]);
 440         scale *= scale;
 441 #endif
 442
 443         // invert the rotation by transposing and multiplying by the squared
 444         // recipricol of the input matrix scale as described above
 445         out->m[0][0] = in1->m[0][0] * scale;
 446         out->m[0][1] = in1->m[1][0] * scale;
 447         out->m[0][2] = in1->m[2][0] * scale;
 448         out->m[1][0] = in1->m[0][1] * scale;
 449         out->m[1][1] = in1->m[1][1] * scale;
 450         out->m[1][2] = in1->m[2][1] * scale;
 451         out->m[2][0] = in1->m[0][2] * scale;
 452         out->m[2][1] = in1->m[1][2] * scale;
 453         out->m[2][2] = in1->m[2][2] * scale;
 454
 455 #ifdef MATRIX4x4_OPENGLORIENTATION
 456         // invert the translate
 457         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]);
 458         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]);
 459         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]);
 460
 461         // don't know if there's anything worth doing here
 462         out->m[0][3] = 0;
 463         out->m[1][3] = 0;
 464         out->m[2][3] = 0;
 465         out->m[3][3] = 1;
 466 #else
 467         // invert the translate
 468         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]);
 469         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]);
 470         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]);
 471
 472         // don't know if there's anything worth doing here
 473         out->m[3][0] = 0;
 474         out->m[3][1] = 0;
 475         out->m[3][2] = 0;
 476         out->m[3][3] = 1;
 477 #endif
 478 }
 479
 480 void Matrix4x4_Interpolate (matrix4x4_t *out, matrix4x4_t *in1, matrix4x4_t *in2, double frac)
 481 {
 482         int i, j;
 483         for (i = 0;i < 4;i++)
 484                 for (j = 0;j < 4;j++)
 485                         out->m[i][j] = in1->m[i][j] + frac * (in2->m[i][j] - in1->m[i][j]);
 486 }
 487
 488 void Matrix4x4_Clear (matrix4x4_t *out)
 489 {
 490         int i, j;
 491         for (i = 0;i < 4;i++)
 492                 for (j = 0;j < 4;j++)
 493                         out->m[i][j] = 0;
 494 }
 495
 496 void Matrix4x4_Accumulate (matrix4x4_t *out, matrix4x4_t *in, double weight)
 497 {
 498         int i, j;
 499         for (i = 0;i < 4;i++)
 500                 for (j = 0;j < 4;j++)
 501                         out->m[i][j] += in->m[i][j] * weight;
 502 }
 503
 504 void Matrix4x4_Normalize (matrix4x4_t *out, matrix4x4_t *in1)
 505 {
 506         // scale rotation matrix vectors to a length of 1
 507         // note: this is only designed to undo uniform scaling
 508         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]);
 509         *out = *in1;
 510         Matrix4x4_Scale(out, scale, 1);
 511 }
 512
 513 void Matrix4x4_Normalize3 (matrix4x4_t *out, matrix4x4_t *in1)
 514 {
 515         int i;
 516         double scale;
 517         // scale each rotation matrix vector to a length of 1
 518         // intended for use after Matrix4x4_Interpolate or Matrix4x4_Accumulate
 519         *out = *in1;
 520         for (i = 0;i < 3;i++)
 521         {
 522 #ifdef MATRIX4x4_OPENGLORIENTATION
 523                 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]);
 524                 if (scale)
 525                         scale = 1.0 / scale;
 526                 out->m[i][0] *= scale;
 527                 out->m[i][1] *= scale;
 528                 out->m[i][2] *= scale;
 529 #else
 530                 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]);
 531                 if (scale)
 532                         scale = 1.0 / scale;
 533                 out->m[0][i] *= scale;
 534                 out->m[1][i] *= scale;
 535                 out->m[2][i] *= scale;
 536 #endif
 537         }
 538 }
 539
 540 void Matrix4x4_Reflect (matrix4x4_t *out, double normalx, double normaly, double normalz, double dist, double axisscale)
 541 {
 542         int i;
 543         double d;
 544         double p[4], p2[4];
 545         p[0] = normalx;
 546         p[1] = normaly;
 547         p[2] = normalz;
 548         p[3] = -dist;
 549         p2[0] = p[0] * axisscale;
 550         p2[1] = p[1] * axisscale;
 551         p2[2] = p[2] * axisscale;
 552         p2[3] = 0;
 553         for (i = 0;i < 4;i++)
 554         {
 555 #ifdef MATRIX4x4_OPENGLORIENTATION
 556                 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];
 557                 out->m[i][0] += p2[0] * d;
 558                 out->m[i][1] += p2[1] * d;
 559                 out->m[i][2] += p2[2] * d;
 560 #else
 561                 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];
 562                 out->m[0][i] += p2[0] * d;
 563                 out->m[1][i] += p2[1] * d;
 564                 out->m[2][i] += p2[2] * d;
 565 #endif
 566         }
 567 }
 568
 569 void Matrix4x4_CreateIdentity (matrix4x4_t *out)
 570 {
 571         out->m[0][0]=1.0f;
 572         out->m[0][1]=0.0f;
 573         out->m[0][2]=0.0f;
 574         out->m[0][3]=0.0f;
 575         out->m[1][0]=0.0f;
 576         out->m[1][1]=1.0f;
 577         out->m[1][2]=0.0f;
 578         out->m[1][3]=0.0f;
 579         out->m[2][0]=0.0f;
 580         out->m[2][1]=0.0f;
 581         out->m[2][2]=1.0f;
 582         out->m[2][3]=0.0f;
 583         out->m[3][0]=0.0f;
 584         out->m[3][1]=0.0f;
 585         out->m[3][2]=0.0f;
 586         out->m[3][3]=1.0f;
 587 }
 588
 589 void Matrix4x4_CreateTranslate (matrix4x4_t *out, double x, double y, double z)
 590 {
 591 #ifdef MATRIX4x4_OPENGLORIENTATION
 592         out->m[0][0]=1.0f;
 593         out->m[1][0]=0.0f;
 594         out->m[2][0]=0.0f;
 595         out->m[3][0]=x;
 596         out->m[0][1]=0.0f;
 597         out->m[1][1]=1.0f;
 598         out->m[2][1]=0.0f;
 599         out->m[3][1]=y;
 600         out->m[0][2]=0.0f;
 601         out->m[1][2]=0.0f;
 602         out->m[2][2]=1.0f;
 603         out->m[3][2]=z;
 604         out->m[0][3]=0.0f;
 605         out->m[1][3]=0.0f;
 606         out->m[2][3]=0.0f;
 607         out->m[3][3]=1.0f;
 608 #else
 609         out->m[0][0]=1.0f;
 610         out->m[0][1]=0.0f;
 611         out->m[0][2]=0.0f;
 612         out->m[0][3]=x;
 613         out->m[1][0]=0.0f;
 614         out->m[1][1]=1.0f;
 615         out->m[1][2]=0.0f;
 616         out->m[1][3]=y;
 617         out->m[2][0]=0.0f;
 618         out->m[2][1]=0.0f;
 619         out->m[2][2]=1.0f;
 620         out->m[2][3]=z;
 621         out->m[3][0]=0.0f;
 622         out->m[3][1]=0.0f;
 623         out->m[3][2]=0.0f;
 624         out->m[3][3]=1.0f;
 625 #endif
 626 }
 627
 628 void Matrix4x4_CreateRotate (matrix4x4_t *out, double angle, double x, double y, double z)
 629 {
 630         double len, c, s;
 631
 632         len = x*x+y*y+z*z;
 633         if (len != 0.0f)
 634                 len = 1.0f / sqrt(len);
 635         x *= len;
 636         y *= len;
 637         z *= len;
 638
 639         angle *= (-M_PI / 180.0);
 640         c = cos(angle);
 641         s = sin(angle);
 642
 643 #ifdef MATRIX4x4_OPENGLORIENTATION
 644         out->m[0][0]=x * x + c * (1 - x * x);
 645         out->m[1][0]=x * y * (1 - c) + z * s;
 646         out->m[2][0]=z * x * (1 - c) - y * s;
 647         out->m[3][0]=0.0f;
 648         out->m[0][1]=x * y * (1 - c) - z * s;
 649         out->m[1][1]=y * y + c * (1 - y * y);
 650         out->m[2][1]=y * z * (1 - c) + x * s;
 651         out->m[3][1]=0.0f;
 652         out->m[0][2]=z * x * (1 - c) + y * s;
 653         out->m[1][2]=y * z * (1 - c) - x * s;
 654         out->m[2][2]=z * z + c * (1 - z * z);
 655         out->m[3][2]=0.0f;
 656         out->m[0][3]=0.0f;
 657         out->m[1][3]=0.0f;
 658         out->m[2][3]=0.0f;
 659         out->m[3][3]=1.0f;
 660 #else
 661         out->m[0][0]=x * x + c * (1 - x * x);
 662         out->m[0][1]=x * y * (1 - c) + z * s;
 663         out->m[0][2]=z * x * (1 - c) - y * s;
 664         out->m[0][3]=0.0f;
 665         out->m[1][0]=x * y * (1 - c) - z * s;
 666         out->m[1][1]=y * y + c * (1 - y * y);
 667         out->m[1][2]=y * z * (1 - c) + x * s;
 668         out->m[1][3]=0.0f;
 669         out->m[2][0]=z * x * (1 - c) + y * s;
 670         out->m[2][1]=y * z * (1 - c) - x * s;
 671         out->m[2][2]=z * z + c * (1 - z * z);
 672         out->m[2][3]=0.0f;
 673         out->m[3][0]=0.0f;
 674         out->m[3][1]=0.0f;
 675         out->m[3][2]=0.0f;
 676         out->m[3][3]=1.0f;
 677 #endif
 678 }
 679
 680 void Matrix4x4_CreateScale (matrix4x4_t *out, double x)
 681 {
 682         out->m[0][0]=x;
 683         out->m[0][1]=0.0f;
 684         out->m[0][2]=0.0f;
 685         out->m[0][3]=0.0f;
 686         out->m[1][0]=0.0f;
 687         out->m[1][1]=x;
 688         out->m[1][2]=0.0f;
 689         out->m[1][3]=0.0f;
 690         out->m[2][0]=0.0f;
 691         out->m[2][1]=0.0f;
 692         out->m[2][2]=x;
 693         out->m[2][3]=0.0f;
 694         out->m[3][0]=0.0f;
 695         out->m[3][1]=0.0f;
 696         out->m[3][2]=0.0f;
 697         out->m[3][3]=1.0f;
 698 }
 699
 700 void Matrix4x4_CreateScale3 (matrix4x4_t *out, double x, double y, double z)
 701 {
 702         out->m[0][0]=x;
 703         out->m[0][1]=0.0f;
 704         out->m[0][2]=0.0f;
 705         out->m[0][3]=0.0f;
 706         out->m[1][0]=0.0f;
 707         out->m[1][1]=y;
 708         out->m[1][2]=0.0f;
 709         out->m[1][3]=0.0f;
 710         out->m[2][0]=0.0f;
 711         out->m[2][1]=0.0f;
 712         out->m[2][2]=z;
 713         out->m[2][3]=0.0f;
 714         out->m[3][0]=0.0f;
 715         out->m[3][1]=0.0f;
 716         out->m[3][2]=0.0f;
 717         out->m[3][3]=1.0f;
 718 }
 719
 720 void Matrix4x4_CreateFromQuakeEntity(matrix4x4_t *out, double x, double y, double z, double pitch, double yaw, double roll, double scale)
 721 {
 722         double angle, sr, sp, sy, cr, cp, cy;
 723
 724         if (roll)
 725         {
 726                 angle = yaw * (M_PI*2 / 360);
 727                 sy = sin(angle);
 728                 cy = cos(angle);
 729                 angle = pitch * (M_PI*2 / 360);
 730                 sp = sin(angle);
 731                 cp = cos(angle);
 732                 angle = roll * (M_PI*2 / 360);
 733                 sr = sin(angle);
 734                 cr = cos(angle);
 735 #ifdef MATRIX4x4_OPENGLORIENTATION
 736                 out->m[0][0] = (cp*cy) * scale;
 737                 out->m[1][0] = (sr*sp*cy+cr*-sy) * scale;
 738                 out->m[2][0] = (cr*sp*cy+-sr*-sy) * scale;
 739                 out->m[3][0] = x;
 740                 out->m[0][1] = (cp*sy) * scale;
 741                 out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
 742                 out->m[2][1] = (cr*sp*sy+-sr*cy) * scale;
 743                 out->m[3][1] = y;
 744                 out->m[0][2] = (-sp) * scale;
 745                 out->m[1][2] = (sr*cp) * scale;
 746                 out->m[2][2] = (cr*cp) * scale;
 747                 out->m[3][2] = z;
 748                 out->m[0][3] = 0;
 749                 out->m[1][3] = 0;
 750                 out->m[2][3] = 0;
 751                 out->m[3][3] = 1;
 752 #else
 753                 out->m[0][0] = (cp*cy) * scale;
 754                 out->m[0][1] = (sr*sp*cy+cr*-sy) * scale;
 755                 out->m[0][2] = (cr*sp*cy+-sr*-sy) * scale;
 756                 out->m[0][3] = x;
 757                 out->m[1][0] = (cp*sy) * scale;
 758                 out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
 759                 out->m[1][2] = (cr*sp*sy+-sr*cy) * scale;
 760                 out->m[1][3] = y;
 761                 out->m[2][0] = (-sp) * scale;
 762                 out->m[2][1] = (sr*cp) * scale;
 763                 out->m[2][2] = (cr*cp) * scale;
 764                 out->m[2][3] = z;
 765                 out->m[3][0] = 0;
 766                 out->m[3][1] = 0;
 767                 out->m[3][2] = 0;
 768                 out->m[3][3] = 1;
 769 #endif
 770         }
 771         else if (pitch)
 772         {
 773                 angle = yaw * (M_PI*2 / 360);
 774                 sy = sin(angle);
 775                 cy = cos(angle);
 776                 angle = pitch * (M_PI*2 / 360);
 777                 sp = sin(angle);
 778                 cp = cos(angle);
 779 #ifdef MATRIX4x4_OPENGLORIENTATION
 780                 out->m[0][0] = (cp*cy) * scale;
 781                 out->m[1][0] = (-sy) * scale;
 782                 out->m[2][0] = (sp*cy) * scale;
 783                 out->m[3][0] = x;
 784                 out->m[0][1] = (cp*sy) * scale;
 785                 out->m[1][1] = (cy) * scale;
 786                 out->m[2][1] = (sp*sy) * scale;
 787                 out->m[3][1] = y;
 788                 out->m[0][2] = (-sp) * scale;
 789                 out->m[1][2] = 0;
 790                 out->m[2][2] = (cp) * scale;
 791                 out->m[3][2] = z;
 792                 out->m[0][3] = 0;
 793                 out->m[1][3] = 0;
 794                 out->m[2][3] = 0;
 795                 out->m[3][3] = 1;
 796 #else
 797                 out->m[0][0] = (cp*cy) * scale;
 798                 out->m[0][1] = (-sy) * scale;
 799                 out->m[0][2] = (sp*cy) * scale;
 800                 out->m[0][3] = x;
 801                 out->m[1][0] = (cp*sy) * scale;
 802                 out->m[1][1] = (cy) * scale;
 803                 out->m[1][2] = (sp*sy) * scale;
 804                 out->m[1][3] = y;
 805                 out->m[2][0] = (-sp) * scale;
 806                 out->m[2][1] = 0;
 807                 out->m[2][2] = (cp) * scale;
 808                 out->m[2][3] = z;
 809                 out->m[3][0] = 0;
 810                 out->m[3][1] = 0;
 811                 out->m[3][2] = 0;
 812                 out->m[3][3] = 1;
 813 #endif
 814         }
 815         else if (yaw)
 816         {
 817                 angle = yaw * (M_PI*2 / 360);
 818                 sy = sin(angle);
 819                 cy = cos(angle);
 820 #ifdef MATRIX4x4_OPENGLORIENTATION
 821                 out->m[0][0] = (cy) * scale;
 822                 out->m[1][0] = (-sy) * scale;
 823                 out->m[2][0] = 0;
 824                 out->m[3][0] = x;
 825                 out->m[0][1] = (sy) * scale;
 826                 out->m[1][1] = (cy) * scale;
 827                 out->m[2][1] = 0;
 828                 out->m[3][1] = y;
 829                 out->m[0][2] = 0;
 830                 out->m[1][2] = 0;
 831                 out->m[2][2] = scale;
 832                 out->m[3][2] = z;
 833                 out->m[0][3] = 0;
 834                 out->m[1][3] = 0;
 835                 out->m[2][3] = 0;
 836                 out->m[3][3] = 1;
 837 #else
 838                 out->m[0][0] = (cy) * scale;
 839                 out->m[0][1] = (-sy) * scale;
 840                 out->m[0][2] = 0;
 841                 out->m[0][3] = x;
 842                 out->m[1][0] = (sy) * scale;
 843                 out->m[1][1] = (cy) * scale;
 844                 out->m[1][2] = 0;
 845                 out->m[1][3] = y;
 846                 out->m[2][0] = 0;
 847                 out->m[2][1] = 0;
 848                 out->m[2][2] = scale;
 849                 out->m[2][3] = z;
 850                 out->m[3][0] = 0;
 851                 out->m[3][1] = 0;
 852                 out->m[3][2] = 0;
 853                 out->m[3][3] = 1;
 854 #endif
 855         }
 856         else
 857         {
 858 #ifdef MATRIX4x4_OPENGLORIENTATION
 859                 out->m[0][0] = scale;
 860                 out->m[1][0] = 0;
 861                 out->m[2][0] = 0;
 862                 out->m[3][0] = x;
 863                 out->m[0][1] = 0;
 864                 out->m[1][1] = scale;
 865                 out->m[2][1] = 0;
 866                 out->m[3][1] = y;
 867                 out->m[0][2] = 0;
 868                 out->m[1][2] = 0;
 869                 out->m[2][2] = scale;
 870                 out->m[3][2] = z;
 871                 out->m[0][3] = 0;
 872                 out->m[1][3] = 0;
 873                 out->m[2][3] = 0;
 874                 out->m[3][3] = 1;
 875 #else
 876                 out->m[0][0] = scale;
 877                 out->m[0][1] = 0;
 878                 out->m[0][2] = 0;
 879                 out->m[0][3] = x;
 880                 out->m[1][0] = 0;
 881                 out->m[1][1] = scale;
 882                 out->m[1][2] = 0;
 883                 out->m[1][3] = y;
 884                 out->m[2][0] = 0;
 885                 out->m[2][1] = 0;
 886                 out->m[2][2] = scale;
 887                 out->m[2][3] = z;
 888                 out->m[3][0] = 0;
 889                 out->m[3][1] = 0;
 890                 out->m[3][2] = 0;
 891                 out->m[3][3] = 1;
 892 #endif
 893         }
 894 }
 895
 896 void Matrix4x4_ToVectors(const matrix4x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
 897 {
 898 #ifdef MATRIX4x4_OPENGLORIENTATION
 899         vx[0] = in->m[0][0];
 900         vx[1] = in->m[0][1];
 901         vx[2] = in->m[0][2];
 902         vy[0] = in->m[1][0];
 903         vy[1] = in->m[1][1];
 904         vy[2] = in->m[1][2];
 905         vz[0] = in->m[2][0];
 906         vz[1] = in->m[2][1];
 907         vz[2] = in->m[2][2];
 908         t [0] = in->m[3][0];
 909         t [1] = in->m[3][1];
 910         t [2] = in->m[3][2];
 911 #else
 912         vx[0] = in->m[0][0];
 913         vx[1] = in->m[1][0];
 914         vx[2] = in->m[2][0];
 915         vy[0] = in->m[0][1];
 916         vy[1] = in->m[1][1];
 917         vy[2] = in->m[2][1];
 918         vz[0] = in->m[0][2];
 919         vz[1] = in->m[1][2];
 920         vz[2] = in->m[2][2];
 921         t [0] = in->m[0][3];
 922         t [1] = in->m[1][3];
 923         t [2] = in->m[2][3];
 924 #endif
 925 }
 926
 927 void Matrix4x4_FromVectors(matrix4x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
 928 {
 929 #ifdef MATRIX4x4_OPENGLORIENTATION
 930         out->m[0][0] = vx[0];
 931         out->m[1][0] = vy[0];
 932         out->m[2][0] = vz[0];
 933         out->m[3][0] = t[0];
 934         out->m[0][1] = vx[1];
 935         out->m[1][1] = vy[1];
 936         out->m[2][1] = vz[1];
 937         out->m[3][1] = t[1];
 938         out->m[0][2] = vx[2];
 939         out->m[1][2] = vy[2];
 940         out->m[2][2] = vz[2];
 941         out->m[3][2] = t[2];
 942         out->m[0][3] = 0.0f;
 943         out->m[1][3] = 0.0f;
 944         out->m[2][3] = 0.0f;
 945         out->m[3][3] = 1.0f;
 946 #else
 947         out->m[0][0] = vx[0];
 948         out->m[0][1] = vy[0];
 949         out->m[0][2] = vz[0];
 950         out->m[0][3] = t[0];
 951         out->m[1][0] = vx[1];
 952         out->m[1][1] = vy[1];
 953         out->m[1][2] = vz[1];
 954         out->m[1][3] = t[1];
 955         out->m[2][0] = vx[2];
 956         out->m[2][1] = vy[2];
 957         out->m[2][2] = vz[2];
 958         out->m[2][3] = t[2];
 959         out->m[3][0] = 0.0f;
 960         out->m[3][1] = 0.0f;
 961         out->m[3][2] = 0.0f;
 962         out->m[3][3] = 1.0f;
 963 #endif
 964 }
 965
 966 void Matrix4x4_ToArrayDoubleGL(const matrix4x4_t *in, double out[16])
 967 {
 968 #ifdef MATRIX4x4_OPENGLORIENTATION
 969         out[ 0] = in->m[0][0];
 970         out[ 1] = in->m[0][1];
 971         out[ 2] = in->m[0][2];
 972         out[ 3] = in->m[0][3];
 973         out[ 4] = in->m[1][0];
 974         out[ 5] = in->m[1][1];
 975         out[ 6] = in->m[1][2];
 976         out[ 7] = in->m[1][3];
 977         out[ 8] = in->m[2][0];
 978         out[ 9] = in->m[2][1];
 979         out[10] = in->m[2][2];
 980         out[11] = in->m[2][3];
 981         out[12] = in->m[3][0];
 982         out[13] = in->m[3][1];
 983         out[14] = in->m[3][2];
 984         out[15] = in->m[3][3];
 985 #else
 986         out[ 0] = in->m[0][0];
 987         out[ 1] = in->m[1][0];
 988         out[ 2] = in->m[2][0];
 989         out[ 3] = in->m[3][0];
 990         out[ 4] = in->m[0][1];
 991         out[ 5] = in->m[1][1];
 992         out[ 6] = in->m[2][1];
 993         out[ 7] = in->m[3][1];
 994         out[ 8] = in->m[0][2];
 995         out[ 9] = in->m[1][2];
 996         out[10] = in->m[2][2];
 997         out[11] = in->m[3][2];
 998         out[12] = in->m[0][3];
 999         out[13] = in->m[1][3];
1000         out[14] = in->m[2][3];
1001         out[15] = in->m[3][3];
1002 #endif
1003 }
1004
1005 void Matrix4x4_FromArrayDoubleGL (matrix4x4_t *out, const double in[16])
1006 {
1007 #ifdef MATRIX4x4_OPENGLORIENTATION
1008         out->m[0][0] = in[0];
1009         out->m[0][1] = in[1];
1010         out->m[0][2] = in[2];
1011         out->m[0][3] = in[3];
1012         out->m[1][0] = in[4];
1013         out->m[1][1] = in[5];
1014         out->m[1][2] = in[6];
1015         out->m[1][3] = in[7];
1016         out->m[2][0] = in[8];
1017         out->m[2][1] = in[9];
1018         out->m[2][2] = in[10];
1019         out->m[2][3] = in[11];
1020         out->m[3][0] = in[12];
1021         out->m[3][1] = in[13];
1022         out->m[3][2] = in[14];
1023         out->m[3][3] = in[15];
1024 #else
1025         out->m[0][0] = in[0];
1026         out->m[1][0] = in[1];
1027         out->m[2][0] = in[2];
1028         out->m[3][0] = in[3];
1029         out->m[0][1] = in[4];
1030         out->m[1][1] = in[5];
1031         out->m[2][1] = in[6];
1032         out->m[3][1] = in[7];
1033         out->m[0][2] = in[8];
1034         out->m[1][2] = in[9];
1035         out->m[2][2] = in[10];
1036         out->m[3][2] = in[11];
1037         out->m[0][3] = in[12];
1038         out->m[1][3] = in[13];
1039         out->m[2][3] = in[14];
1040         out->m[3][3] = in[15];
1041 #endif
1042 }
1043
1044 void Matrix4x4_ToArrayDoubleD3D(const matrix4x4_t *in, double out[16])
1045 {
1046 #ifdef MATRIX4x4_OPENGLORIENTATION
1047         out[ 0] = in->m[0][0];
1048         out[ 1] = in->m[1][0];
1049         out[ 2] = in->m[2][0];
1050         out[ 3] = in->m[3][0];
1051         out[ 4] = in->m[0][1];
1052         out[ 5] = in->m[1][1];
1053         out[ 6] = in->m[2][1];
1054         out[ 7] = in->m[3][1];
1055         out[ 8] = in->m[0][2];
1056         out[ 9] = in->m[1][2];
1057         out[10] = in->m[2][2];
1058         out[11] = in->m[3][2];
1059         out[12] = in->m[0][3];
1060         out[13] = in->m[1][3];
1061         out[14] = in->m[2][3];
1062         out[15] = in->m[3][3];
1063 #else
1064         out[ 0] = in->m[0][0];
1065         out[ 1] = in->m[0][1];
1066         out[ 2] = in->m[0][2];
1067         out[ 3] = in->m[0][3];
1068         out[ 4] = in->m[1][0];
1069         out[ 5] = in->m[1][1];
1070         out[ 6] = in->m[1][2];
1071         out[ 7] = in->m[1][3];
1072         out[ 8] = in->m[2][0];
1073         out[ 9] = in->m[2][1];
1074         out[10] = in->m[2][2];
1075         out[11] = in->m[2][3];
1076         out[12] = in->m[3][0];
1077         out[13] = in->m[3][1];
1078         out[14] = in->m[3][2];
1079         out[15] = in->m[3][3];
1080 #endif
1081 }
1082
1083 void Matrix4x4_FromArrayDoubleD3D (matrix4x4_t *out, const double in[16])
1084 {
1085 #ifdef MATRIX4x4_OPENGLORIENTATION
1086         out->m[0][0] = in[0];
1087         out->m[1][0] = in[1];
1088         out->m[2][0] = in[2];
1089         out->m[3][0] = in[3];
1090         out->m[0][1] = in[4];
1091         out->m[1][1] = in[5];
1092         out->m[2][1] = in[6];
1093         out->m[3][1] = in[7];
1094         out->m[0][2] = in[8];
1095         out->m[1][2] = in[9];
1096         out->m[2][2] = in[10];
1097         out->m[3][2] = in[11];
1098         out->m[0][3] = in[12];
1099         out->m[1][3] = in[13];
1100         out->m[2][3] = in[14];
1101         out->m[3][3] = in[15];
1102 #else
1103         out->m[0][0] = in[0];
1104         out->m[0][1] = in[1];
1105         out->m[0][2] = in[2];
1106         out->m[0][3] = in[3];
1107         out->m[1][0] = in[4];
1108         out->m[1][1] = in[5];
1109         out->m[1][2] = in[6];
1110         out->m[1][3] = in[7];
1111         out->m[2][0] = in[8];
1112         out->m[2][1] = in[9];
1113         out->m[2][2] = in[10];
1114         out->m[2][3] = in[11];
1115         out->m[3][0] = in[12];
1116         out->m[3][1] = in[13];
1117         out->m[3][2] = in[14];
1118         out->m[3][3] = in[15];
1119 #endif
1120 }
1121
1122 void Matrix4x4_ToArrayFloatGL(const matrix4x4_t *in, float out[16])
1123 {
1124 #ifdef MATRIX4x4_OPENGLORIENTATION
1125         out[ 0] = in->m[0][0];
1126         out[ 1] = in->m[0][1];
1127         out[ 2] = in->m[0][2];
1128         out[ 3] = in->m[0][3];
1129         out[ 4] = in->m[1][0];
1130         out[ 5] = in->m[1][1];
1131         out[ 6] = in->m[1][2];
1132         out[ 7] = in->m[1][3];
1133         out[ 8] = in->m[2][0];
1134         out[ 9] = in->m[2][1];
1135         out[10] = in->m[2][2];
1136         out[11] = in->m[2][3];
1137         out[12] = in->m[3][0];
1138         out[13] = in->m[3][1];
1139         out[14] = in->m[3][2];
1140         out[15] = in->m[3][3];
1141 #else
1142         out[ 0] = in->m[0][0];
1143         out[ 1] = in->m[1][0];
1144         out[ 2] = in->m[2][0];
1145         out[ 3] = in->m[3][0];
1146         out[ 4] = in->m[0][1];
1147         out[ 5] = in->m[1][1];
1148         out[ 6] = in->m[2][1];
1149         out[ 7] = in->m[3][1];
1150         out[ 8] = in->m[0][2];
1151         out[ 9] = in->m[1][2];
1152         out[10] = in->m[2][2];
1153         out[11] = in->m[3][2];
1154         out[12] = in->m[0][3];
1155         out[13] = in->m[1][3];
1156         out[14] = in->m[2][3];
1157         out[15] = in->m[3][3];
1158 #endif
1159 }
1160
1161 void Matrix4x4_FromArrayFloatGL (matrix4x4_t *out, const float in[16])
1162 {
1163 #ifdef MATRIX4x4_OPENGLORIENTATION
1164         out->m[0][0] = in[0];
1165         out->m[0][1] = in[1];
1166         out->m[0][2] = in[2];
1167         out->m[0][3] = in[3];
1168         out->m[1][0] = in[4];
1169         out->m[1][1] = in[5];
1170         out->m[1][2] = in[6];
1171         out->m[1][3] = in[7];
1172         out->m[2][0] = in[8];
1173         out->m[2][1] = in[9];
1174         out->m[2][2] = in[10];
1175         out->m[2][3] = in[11];
1176         out->m[3][0] = in[12];
1177         out->m[3][1] = in[13];
1178         out->m[3][2] = in[14];
1179         out->m[3][3] = in[15];
1180 #else
1181         out->m[0][0] = in[0];
1182         out->m[1][0] = in[1];
1183         out->m[2][0] = in[2];
1184         out->m[3][0] = in[3];
1185         out->m[0][1] = in[4];
1186         out->m[1][1] = in[5];
1187         out->m[2][1] = in[6];
1188         out->m[3][1] = in[7];
1189         out->m[0][2] = in[8];
1190         out->m[1][2] = in[9];
1191         out->m[2][2] = in[10];
1192         out->m[3][2] = in[11];
1193         out->m[0][3] = in[12];
1194         out->m[1][3] = in[13];
1195         out->m[2][3] = in[14];
1196         out->m[3][3] = in[15];
1197 #endif
1198 }
1199
1200 void Matrix4x4_ToArrayFloatD3D(const matrix4x4_t *in, float out[16])
1201 {
1202 #ifdef MATRIX4x4_OPENGLORIENTATION
1203         out[ 0] = in->m[0][0];
1204         out[ 1] = in->m[1][0];
1205         out[ 2] = in->m[2][0];
1206         out[ 3] = in->m[3][0];
1207         out[ 4] = in->m[0][1];
1208         out[ 5] = in->m[1][1];
1209         out[ 6] = in->m[2][1];
1210         out[ 7] = in->m[3][1];
1211         out[ 8] = in->m[0][2];
1212         out[ 9] = in->m[1][2];
1213         out[10] = in->m[2][2];
1214         out[11] = in->m[3][2];
1215         out[12] = in->m[0][3];
1216         out[13] = in->m[1][3];
1217         out[14] = in->m[2][3];
1218         out[15] = in->m[3][3];
1219 #else
1220         out[ 0] = in->m[0][0];
1221         out[ 1] = in->m[0][1];
1222         out[ 2] = in->m[0][2];
1223         out[ 3] = in->m[0][3];
1224         out[ 4] = in->m[1][0];
1225         out[ 5] = in->m[1][1];
1226         out[ 6] = in->m[1][2];
1227         out[ 7] = in->m[1][3];
1228         out[ 8] = in->m[2][0];
1229         out[ 9] = in->m[2][1];
1230         out[10] = in->m[2][2];
1231         out[11] = in->m[2][3];
1232         out[12] = in->m[3][0];
1233         out[13] = in->m[3][1];
1234         out[14] = in->m[3][2];
1235         out[15] = in->m[3][3];
1236 #endif
1237 }
1238
1239 void Matrix4x4_FromArrayFloatD3D (matrix4x4_t *out, const float in[16])
1240 {
1241 #ifdef MATRIX4x4_OPENGLORIENTATION
1242         out->m[0][0] = in[0];
1243         out->m[1][0] = in[1];
1244         out->m[2][0] = in[2];
1245         out->m[3][0] = in[3];
1246         out->m[0][1] = in[4];
1247         out->m[1][1] = in[5];
1248         out->m[2][1] = in[6];
1249         out->m[3][1] = in[7];
1250         out->m[0][2] = in[8];
1251         out->m[1][2] = in[9];
1252         out->m[2][2] = in[10];
1253         out->m[3][2] = in[11];
1254         out->m[0][3] = in[12];
1255         out->m[1][3] = in[13];
1256         out->m[2][3] = in[14];
1257         out->m[3][3] = in[15];
1258 #else
1259         out->m[0][0] = in[0];
1260         out->m[0][1] = in[1];
1261         out->m[0][2] = in[2];
1262         out->m[0][3] = in[3];
1263         out->m[1][0] = in[4];
1264         out->m[1][1] = in[5];
1265         out->m[1][2] = in[6];
1266         out->m[1][3] = in[7];
1267         out->m[2][0] = in[8];
1268         out->m[2][1] = in[9];
1269         out->m[2][2] = in[10];
1270         out->m[2][3] = in[11];
1271         out->m[3][0] = in[12];
1272         out->m[3][1] = in[13];
1273         out->m[3][2] = in[14];
1274         out->m[3][3] = in[15];
1275 #endif
1276 }
1277
1278 void Matrix4x4_ToArray12FloatGL(const matrix4x4_t *in, float out[12])
1279 {
1280 #ifdef MATRIX4x4_OPENGLORIENTATION
1281         out[ 0] = in->m[0][0];
1282         out[ 1] = in->m[0][1];
1283         out[ 2] = in->m[0][2];
1284         out[ 3] = in->m[1][0];
1285         out[ 4] = in->m[1][1];
1286         out[ 5] = in->m[1][2];
1287         out[ 6] = in->m[2][0];
1288         out[ 7] = in->m[2][1];
1289         out[ 8] = in->m[2][2];
1290         out[ 9] = in->m[3][0];
1291         out[10] = in->m[3][1];
1292         out[11] = in->m[3][2];
1293 #else
1294         out[ 0] = in->m[0][0];
1295         out[ 1] = in->m[1][0];
1296         out[ 2] = in->m[2][0];
1297         out[ 3] = in->m[0][1];
1298         out[ 4] = in->m[1][1];
1299         out[ 5] = in->m[2][1];
1300         out[ 6] = in->m[0][2];
1301         out[ 7] = in->m[1][2];
1302         out[ 8] = in->m[2][2];
1303         out[ 9] = in->m[0][3];
1304         out[10] = in->m[1][3];
1305         out[11] = in->m[2][3];
1306 #endif
1307 }
1308
1309 void Matrix4x4_FromArray12FloatGL(matrix4x4_t *out, const float in[12])
1310 {
1311 #ifdef MATRIX4x4_OPENGLORIENTATION
1312         out->m[0][0] = in[0];
1313         out->m[0][1] = in[1];
1314         out->m[0][2] = in[2];
1315         out->m[0][3] = 0;
1316         out->m[1][0] = in[3];
1317         out->m[1][1] = in[4];
1318         out->m[1][2] = in[5];
1319         out->m[1][3] = 0;
1320         out->m[2][0] = in[6];
1321         out->m[2][1] = in[7];
1322         out->m[2][2] = in[8];
1323         out->m[2][3] = 0;
1324         out->m[3][0] = in[9];
1325         out->m[3][1] = in[10];
1326         out->m[3][2] = in[11];
1327         out->m[3][3] = 1;
1328 #else
1329         out->m[0][0] = in[0];
1330         out->m[1][0] = in[1];
1331         out->m[2][0] = in[2];
1332         out->m[3][0] = 0;
1333         out->m[0][1] = in[3];
1334         out->m[1][1] = in[4];
1335         out->m[2][1] = in[5];
1336         out->m[3][1] = 0;
1337         out->m[0][2] = in[6];
1338         out->m[1][2] = in[7];
1339         out->m[2][2] = in[8];
1340         out->m[3][2] = 0;
1341         out->m[0][3] = in[9];
1342         out->m[1][3] = in[10];
1343         out->m[2][3] = in[11];
1344         out->m[3][3] = 1;
1345 #endif
1346 }
1347
1348 void Matrix4x4_ToArray12FloatD3D(const matrix4x4_t *in, float out[12])
1349 {
1350 #ifdef MATRIX4x4_OPENGLORIENTATION
1351         out[ 0] = in->m[0][0];
1352         out[ 1] = in->m[1][0];
1353         out[ 2] = in->m[2][0];
1354         out[ 3] = in->m[3][0];
1355         out[ 4] = in->m[0][1];
1356         out[ 5] = in->m[1][1];
1357         out[ 6] = in->m[2][1];
1358         out[ 7] = in->m[3][1];
1359         out[ 8] = in->m[0][2];
1360         out[ 9] = in->m[1][2];
1361         out[10] = in->m[2][2];
1362         out[11] = in->m[3][2];
1363 #else
1364         out[ 0] = in->m[0][0];
1365         out[ 1] = in->m[0][1];
1366         out[ 2] = in->m[0][2];
1367         out[ 3] = in->m[0][3];
1368         out[ 4] = in->m[1][0];
1369         out[ 5] = in->m[1][1];
1370         out[ 6] = in->m[1][2];
1371         out[ 7] = in->m[1][3];
1372         out[ 8] = in->m[2][0];
1373         out[ 9] = in->m[2][1];
1374         out[10] = in->m[2][2];
1375         out[11] = in->m[2][3];
1376 #endif
1377 }
1378
1379 void Matrix4x4_FromArray12FloatD3D(matrix4x4_t *out, const float in[12])
1380 {
1381 #ifdef MATRIX4x4_OPENGLORIENTATION
1382         out->m[0][0] = in[0];
1383         out->m[1][0] = in[1];
1384         out->m[2][0] = in[2];
1385         out->m[3][0] = in[3];
1386         out->m[0][1] = in[4];
1387         out->m[1][1] = in[5];
1388         out->m[2][1] = in[6];
1389         out->m[3][1] = in[7];
1390         out->m[0][2] = in[8];
1391         out->m[1][2] = in[9];
1392         out->m[2][2] = in[10];
1393         out->m[3][2] = in[11];
1394         out->m[0][3] = 0;
1395         out->m[1][3] = 0;
1396         out->m[2][3] = 0;
1397         out->m[3][3] = 1;
1398 #else
1399         out->m[0][0] = in[0];
1400         out->m[0][1] = in[1];
1401         out->m[0][2] = in[2];
1402         out->m[0][3] = in[3];
1403         out->m[1][0] = in[4];
1404         out->m[1][1] = in[5];
1405         out->m[1][2] = in[6];
1406         out->m[1][3] = in[7];
1407         out->m[2][0] = in[8];
1408         out->m[2][1] = in[9];
1409         out->m[2][2] = in[10];
1410         out->m[2][3] = in[11];
1411         out->m[3][0] = 0;
1412         out->m[3][1] = 0;
1413         out->m[3][2] = 0;
1414         out->m[3][3] = 1;
1415 #endif
1416 }
1417
1418 void Matrix4x4_FromOriginQuat(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z, double w)
1419 {
1420 #ifdef MATRIX4x4_OPENGLORIENTATION
1421         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;
1422         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;
1423         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;
1424         m->m[0][3]=  0          ;m->m[1][3]=  0          ;m->m[2][3]=  0          ;m->m[3][3]=1;
1425 #else
1426         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;
1427         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;
1428         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;
1429         m->m[3][0]=  0          ;m->m[3][1]=  0          ;m->m[3][2]=  0          ;m->m[3][3]=1;
1430 #endif
1431 }
1432
1433 // LordHavoc: I got this code from:
1434 //http://www.doom3world.org/phpbb2/viewtopic.php?t=2884
1435 void Matrix4x4_FromDoom3Joint(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z)
1436 {
1437         double w = 1.0 - (x*x+y*y+z*z);
1438         w = w > 0.0 ? -sqrt(w) : 0.0;
1439 #ifdef MATRIX4x4_OPENGLORIENTATION
1440         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;
1441         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;
1442         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;
1443         m->m[0][3]=  0          ;m->m[1][3]=  0          ;m->m[2][3]=  0          ;m->m[3][3]=1;
1444 #else
1445         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;
1446         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;
1447         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;
1448         m->m[3][0]=  0          ;m->m[3][1]=  0          ;m->m[3][2]=  0          ;m->m[3][3]=1;
1449 #endif
1450 }
1451
1452 void Matrix4x4_Blend (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2, double blend)
1453 {
1454         double iblend = 1 - blend;
1455         out->m[0][0] = in1->m[0][0] * iblend + in2->m[0][0] * blend;
1456         out->m[0][1] = in1->m[0][1] * iblend + in2->m[0][1] * blend;
1457         out->m[0][2] = in1->m[0][2] * iblend + in2->m[0][2] * blend;
1458         out->m[0][3] = in1->m[0][3] * iblend + in2->m[0][3] * blend;
1459         out->m[1][0] = in1->m[1][0] * iblend + in2->m[1][0] * blend;
1460         out->m[1][1] = in1->m[1][1] * iblend + in2->m[1][1] * blend;
1461         out->m[1][2] = in1->m[1][2] * iblend + in2->m[1][2] * blend;
1462         out->m[1][3] = in1->m[1][3] * iblend + in2->m[1][3] * blend;
1463         out->m[2][0] = in1->m[2][0] * iblend + in2->m[2][0] * blend;
1464         out->m[2][1] = in1->m[2][1] * iblend + in2->m[2][1] * blend;
1465         out->m[2][2] = in1->m[2][2] * iblend + in2->m[2][2] * blend;
1466         out->m[2][3] = in1->m[2][3] * iblend + in2->m[2][3] * blend;
1467         out->m[3][0] = in1->m[3][0] * iblend + in2->m[3][0] * blend;
1468         out->m[3][1] = in1->m[3][1] * iblend + in2->m[3][1] * blend;
1469         out->m[3][2] = in1->m[3][2] * iblend + in2->m[3][2] * blend;
1470         out->m[3][3] = in1->m[3][3] * iblend + in2->m[3][3] * blend;
1471 }
1472
1473
1474 void Matrix4x4_Transform (const matrix4x4_t *in, const float v[3], float out[3])
1475 {
1476 #ifdef MATRIX4x4_OPENGLORIENTATION
1477         out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0] + in->m[3][0];
1478         out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1] + in->m[3][1];
1479         out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2] + in->m[3][2];
1480 #else
1481         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
1482         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
1483         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
1484 #endif
1485 }
1486
1487 void Matrix4x4_Transform4 (const matrix4x4_t *in, const float v[4], float out[4])
1488 {
1489 #ifdef MATRIX4x4_OPENGLORIENTATION
1490         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];
1491         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];
1492         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];
1493         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];
1494 #else
1495         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];
1496         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];
1497         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];
1498         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];
1499 #endif
1500 }
1501
1502 void Matrix4x4_Transform3x3 (const matrix4x4_t *in, const float v[3], float out[3])
1503 {
1504 #ifdef MATRIX4x4_OPENGLORIENTATION
1505         out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0];
1506         out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1];
1507         out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2];
1508 #else
1509         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2];
1510         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2];
1511         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2];
1512 #endif
1513 }
1514
1515 void Matrix4x4_TransformPositivePlane(const matrix4x4_t *in, float x, float y, float z, float d, float *o)
1516 {
1517 #ifdef MATRIX4x4_OPENGLORIENTATION
1518         o[0] = x * in->m[0][0] + y * in->m[1][0] + z * in->m[2][0];
1519         o[1] = x * in->m[0][1] + y * in->m[1][1] + z * in->m[2][1];
1520         o[2] = x * in->m[0][2] + y * in->m[1][2] + z * in->m[2][2];
1521         o[3] = d + (x * in->m[3][0] + y * in->m[3][1] + z * in->m[3][2]);
1522 #else
1523         o[0] = x * in->m[0][0] + y * in->m[0][1] + z * in->m[0][2];
1524         o[1] = x * in->m[1][0] + y * in->m[1][1] + z * in->m[1][2];
1525         o[2] = x * in->m[2][0] + y * in->m[2][1] + z * in->m[2][2];
1526         o[3] = d + (x * in->m[0][3] + y * in->m[1][3] + z * in->m[2][3]);
1527 #endif
1528 }
1529
1530 void Matrix4x4_TransformStandardPlane(const matrix4x4_t *in, float x, float y, float z, float d, float *o)
1531 {
1532 #ifdef MATRIX4x4_OPENGLORIENTATION
1533         o[0] = x * in->m[0][0] + y * in->m[1][0] + z * in->m[2][0];
1534         o[1] = x * in->m[0][1] + y * in->m[1][1] + z * in->m[2][1];
1535         o[2] = x * in->m[0][2] + y * in->m[1][2] + z * in->m[2][2];
1536         o[3] = d - (x * in->m[3][0] + y * in->m[3][1] + z * in->m[3][2]);
1537 #else
1538         o[0] = x * in->m[0][0] + y * in->m[0][1] + z * in->m[0][2];
1539         o[1] = x * in->m[1][0] + y * in->m[1][1] + z * in->m[1][2];
1540         o[2] = x * in->m[2][0] + y * in->m[2][1] + z * in->m[2][2];
1541         o[3] = d - (x * in->m[0][3] + y * in->m[1][3] + z * in->m[2][3]);
1542 #endif
1543 }
1544
1545 /*
1546 void Matrix4x4_SimpleUntransform (const matrix4x4_t *in, const float v[3], float out[3])
1547 {
1548         double t[3];
1549 #ifdef MATRIX4x4_OPENGLORIENTATION
1550         t[0] = v[0] - in->m[3][0];
1551         t[1] = v[1] - in->m[3][1];
1552         t[2] = v[2] - in->m[3][2];
1553         out[0] = t[0] * in->m[0][0] + t[1] * in->m[0][1] + t[2] * in->m[0][2];
1554         out[1] = t[0] * in->m[1][0] + t[1] * in->m[1][1] + t[2] * in->m[1][2];
1555         out[2] = t[0] * in->m[2][0] + t[1] * in->m[2][1] + t[2] * in->m[2][2];
1556 #else
1557         t[0] = v[0] - in->m[0][3];
1558         t[1] = v[1] - in->m[1][3];
1559         t[2] = v[2] - in->m[2][3];
1560         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
1561         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
1562         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
1563 #endif
1564 }
1565 */
1566
1567 // FIXME: optimize
1568 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, double x, double y, double z)
1569 {
1570         matrix4x4_t base, temp;
1571         base = *out;
1572         Matrix4x4_CreateTranslate(&temp, x, y, z);
1573         Matrix4x4_Concat(out, &base, &temp);
1574 }
1575
1576 // FIXME: optimize
1577 void Matrix4x4_ConcatRotate (matrix4x4_t *out, double angle, double x, double y, double z)
1578 {
1579         matrix4x4_t base, temp;
1580         base = *out;
1581         Matrix4x4_CreateRotate(&temp, angle, x, y, z);
1582         Matrix4x4_Concat(out, &base, &temp);
1583 }
1584
1585 // FIXME: optimize
1586 void Matrix4x4_ConcatScale (matrix4x4_t *out, double x)
1587 {
1588         matrix4x4_t base, temp;
1589         base = *out;
1590         Matrix4x4_CreateScale(&temp, x);
1591         Matrix4x4_Concat(out, &base, &temp);
1592 }
1593
1594 // FIXME: optimize
1595 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, double x, double y, double z)
1596 {
1597         matrix4x4_t base, temp;
1598         base = *out;
1599         Matrix4x4_CreateScale3(&temp, x, y, z);
1600         Matrix4x4_Concat(out, &base, &temp);
1601 }
1602
1603 void Matrix4x4_OriginFromMatrix (const matrix4x4_t *in, float *out)
1604 {
1605 #ifdef MATRIX4x4_OPENGLORIENTATION
1606         out[0] = in->m[3][0];
1607         out[1] = in->m[3][1];
1608         out[2] = in->m[3][2];
1609 #else
1610         out[0] = in->m[0][3];
1611         out[1] = in->m[1][3];
1612         out[2] = in->m[2][3];
1613 #endif
1614 }
1615
1616 double Matrix4x4_ScaleFromMatrix (const matrix4x4_t *in)
1617 {
1618         // we only support uniform scaling, so assume the first row is enough
1619         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]);
1620 }
1621
1622 void Matrix4x4_SetOrigin (matrix4x4_t *out, double x, double y, double z)
1623 {
1624 #ifdef MATRIX4x4_OPENGLORIENTATION
1625         out->m[3][0] = x;
1626         out->m[3][1] = y;
1627         out->m[3][2] = z;
1628 #else
1629         out->m[0][3] = x;
1630         out->m[1][3] = y;
1631         out->m[2][3] = z;
1632 #endif
1633 }
1634
1635 void Matrix4x4_AdjustOrigin (matrix4x4_t *out, double x, double y, double z)
1636 {
1637 #ifdef MATRIX4x4_OPENGLORIENTATION
1638         out->m[3][0] += x;
1639         out->m[3][1] += y;
1640         out->m[3][2] += z;
1641 #else
1642         out->m[0][3] += x;
1643         out->m[1][3] += y;
1644         out->m[2][3] += z;
1645 #endif
1646 }
1647
1648 void Matrix4x4_Scale (matrix4x4_t *out, double rotatescale, double originscale)
1649 {
1650         out->m[0][0] *= rotatescale;
1651         out->m[0][1] *= rotatescale;
1652         out->m[0][2] *= rotatescale;
1653         out->m[1][0] *= rotatescale;
1654         out->m[1][1] *= rotatescale;
1655         out->m[1][2] *= rotatescale;
1656         out->m[2][0] *= rotatescale;
1657         out->m[2][1] *= rotatescale;
1658         out->m[2][2] *= rotatescale;
1659 #ifdef MATRIX4x4_OPENGLORIENTATION
1660         out->m[3][0] *= originscale;
1661         out->m[3][1] *= originscale;
1662         out->m[3][2] *= originscale;
1663 #else
1664         out->m[0][3] *= originscale;
1665         out->m[1][3] *= originscale;
1666         out->m[2][3] *= originscale;
1667 #endif
1668 }