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