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 int Matrix4x4_Invert_Full (matrix4x4_t *out, const matrix4x4_t *in1)
 145 {
 146         double  *temp;
 147         double  *r[4];
 148         double  rtemp[4][8];
 149         double  m[4];
 150         double  s;
 151
 152         r[0]    = rtemp[0];
 153         r[1]    = rtemp[1];
 154         r[2]    = rtemp[2];
 155         r[3]    = rtemp[3];
 156
 157 #ifdef MATRIX4x4_OPENGLORIENTATION
 158         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];
 159         r[0][4] = 1.0;                  r[0][5] =                               r[0][6] =                               r[0][7] = 0.0;
 160
 161         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];
 162         r[1][5] = 1.0;                  r[1][4] =                               r[1][6] =                               r[1][7] = 0.0;
 163
 164         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];
 165         r[2][6] = 1.0;                  r[2][4] =                               r[2][5] =                               r[2][7] = 0.0;
 166
 167         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];
 168         r[3][7] = 1.0;                  r[3][4] =                               r[3][5] =                               r[3][6] = 0.0;
 169 #else
 170         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];
 171         r[0][4] = 1.0;                  r[0][5] =                               r[0][6] =                               r[0][7] = 0.0;
 172
 173         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];
 174         r[1][5] = 1.0;                  r[1][4] =                               r[1][6] =                               r[1][7] = 0.0;
 175
 176         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];
 177         r[2][6] = 1.0;                  r[2][4] =                               r[2][5] =                               r[2][7] = 0.0;
 178
 179         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];
 180         r[3][7] = 1.0;                  r[3][4] =                               r[3][5] =                               r[3][6] = 0.0;
 181 #endif
 182
 183         if (fabs (r[3][0]) > fabs (r[2][0])) { temp = r[3]; r[3] = r[2]; r[2] = temp; }
 184         if (fabs (r[2][0]) > fabs (r[1][0])) { temp = r[2]; r[2] = r[1]; r[1] = temp; }
 185         if (fabs (r[1][0]) > fabs (r[0][0])) { temp = r[1]; r[1] = r[0]; r[0] = temp; }
 186
 187         if (r[0][0])
 188         {
 189                 m[1]    = r[1][0] / r[0][0];
 190                 m[2]    = r[2][0] / r[0][0];
 191                 m[3]    = r[3][0] / r[0][0];
 192
 193                 s       = r[0][1]; r[1][1] -= m[1] * s; r[2][1] -= m[2] * s; r[3][1] -= m[3] * s;
 194                 s       = r[0][2]; r[1][2] -= m[1] * s; r[2][2] -= m[2] * s; r[3][2] -= m[3] * s;
 195                 s       = r[0][3]; r[1][3] -= m[1] * s; r[2][3] -= m[2] * s; r[3][3] -= m[3] * s;
 196
 197                 s       = r[0][4]; if (s) { r[1][4] -= m[1] * s; r[2][4] -= m[2] * s; r[3][4] -= m[3] * s; }
 198                 s       = r[0][5]; if (s) { r[1][5] -= m[1] * s; r[2][5] -= m[2] * s; r[3][5] -= m[3] * s; }
 199                 s       = r[0][6]; if (s) { r[1][6] -= m[1] * s; r[2][6] -= m[2] * s; r[3][6] -= m[3] * s; }
 200                 s       = r[0][7]; if (s) { r[1][7] -= m[1] * s; r[2][7] -= m[2] * s; r[3][7] -= m[3] * s; }
 201
 202                 if (fabs (r[3][1]) > fabs (r[2][1])) { temp = r[3]; r[3] = r[2]; r[2] = temp; }
 203                 if (fabs (r[2][1]) > fabs (r[1][1])) { temp = r[2]; r[2] = r[1]; r[1] = temp; }
 204
 205                 if (r[1][1])
 206                 {
 207                         m[2]            = r[2][1] / r[1][1];
 208                         m[3]            = r[3][1] / r[1][1];
 209                         r[2][2] -= m[2] * r[1][2];
 210                         r[3][2] -= m[3] * r[1][2];
 211                         r[2][3] -= m[2] * r[1][3];
 212                         r[3][3] -= m[3] * r[1][3];
 213
 214                         s       = r[1][4]; if (s) { r[2][4] -= m[2] * s; r[3][4] -= m[3] * s; }
 215                         s       = r[1][5]; if (s) { r[2][5] -= m[2] * s; r[3][5] -= m[3] * s; }
 216                         s       = r[1][6]; if (s) { r[2][6] -= m[2] * s; r[3][6] -= m[3] * s; }
 217                         s       = r[1][7]; if (s) { r[2][7] -= m[2] * s; r[3][7] -= m[3] * s; }
 218
 219                         if (fabs (r[3][2]) > fabs (r[2][2])) { temp = r[3]; r[3] = r[2]; r[2] = temp; }
 220
 221                         if (r[2][2])
 222                         {
 223                                 m[3]            = r[3][2] / r[2][2];
 224                                 r[3][3] -= m[3] * r[2][3];
 225                                 r[3][4] -= m[3] * r[2][4];
 226                                 r[3][5] -= m[3] * r[2][5];
 227                                 r[3][6] -= m[3] * r[2][6];
 228                                 r[3][7] -= m[3] * r[2][7];
 229
 230                                 if (r[3][3])
 231                                 {
 232                                         s                       = 1.0 / r[3][3];
 233                                         r[3][4] *= s;
 234                                         r[3][5] *= s;
 235                                         r[3][6] *= s;
 236                                         r[3][7] *= s;
 237
 238                                         m[2]            = r[2][3];
 239                                         s                       = 1.0 / r[2][2];
 240                                         r[2][4] = s * (r[2][4] - r[3][4] * m[2]);
 241                                         r[2][5] = s * (r[2][5] - r[3][5] * m[2]);
 242                                         r[2][6] = s * (r[2][6] - r[3][6] * m[2]);
 243                                         r[2][7] = s * (r[2][7] - r[3][7] * m[2]);
 244
 245                                         m[1]            = r[1][3];
 246                                         r[1][4] -= r[3][4] * m[1], r[1][5] -= r[3][5] * m[1];
 247                                         r[1][6] -= r[3][6] * m[1], r[1][7] -= r[3][7] * m[1];
 248
 249                                         m[0]            = r[0][3];
 250                                         r[0][4] -= r[3][4] * m[0], r[0][5] -= r[3][5] * m[0];
 251                                         r[0][6] -= r[3][6] * m[0], r[0][7] -= r[3][7] * m[0];
 252
 253                                         m[1]            = r[1][2];
 254                                         s                       = 1.0 / r[1][1];
 255                                         r[1][4] = s * (r[1][4] - r[2][4] * m[1]), r[1][5] = s * (r[1][5] - r[2][5] * m[1]);
 256                                         r[1][6] = s * (r[1][6] - r[2][6] * m[1]), r[1][7] = s * (r[1][7] - r[2][7] * m[1]);
 257
 258                                         m[0]            = r[0][2];
 259                                         r[0][4] -= r[2][4] * m[0], r[0][5] -= r[2][5] * m[0];
 260                                         r[0][6] -= r[2][6] * m[0], r[0][7] -= r[2][7] * m[0];
 261
 262                                         m[0]            = r[0][1];
 263                                         s                       = 1.0 / r[0][0];
 264                                         r[0][4] = s * (r[0][4] - r[1][4] * m[0]), r[0][5] = s * (r[0][5] - r[1][5] * m[0]);
 265                                         r[0][6] = s * (r[0][6] - r[1][6] * m[0]), r[0][7] = s * (r[0][7] - r[1][7] * m[0]);
 266
 267 #ifdef MATRIX4x4_OPENGLORIENTATION
 268                                         out->m[0][0]    = r[0][4];
 269                                         out->m[0][1]    = r[1][4];
 270                                         out->m[0][2]    = r[2][4];
 271                                         out->m[0][3]    = r[3][4];
 272                                         out->m[1][0]    = r[0][5];
 273                                         out->m[1][1]    = r[1][5];
 274                                         out->m[1][2]    = r[2][5];
 275                                         out->m[1][3]    = r[3][5];
 276                                         out->m[2][0]    = r[0][6];
 277                                         out->m[2][1]    = r[1][6];
 278                                         out->m[2][2]    = r[2][6];
 279                                         out->m[2][3]    = r[3][6];
 280                                         out->m[3][0]    = r[0][7];
 281                                         out->m[3][1]    = r[1][7];
 282                                         out->m[3][2]    = r[2][7];
 283                                         out->m[3][3]    = r[3][7];
 284 #else
 285                                         out->m[0][0]    = r[0][4];
 286                                         out->m[0][1]    = r[0][5];
 287                                         out->m[0][2]    = r[0][6];
 288                                         out->m[0][3]    = r[0][7];
 289                                         out->m[1][0]    = r[1][4];
 290                                         out->m[1][1]    = r[1][5];
 291                                         out->m[1][2]    = r[1][6];
 292                                         out->m[1][3]    = r[1][7];
 293                                         out->m[2][0]    = r[2][4];
 294                                         out->m[2][1]    = r[2][5];
 295                                         out->m[2][2]    = r[2][6];
 296                                         out->m[2][3]    = r[2][7];
 297                                         out->m[3][0]    = r[3][4];
 298                                         out->m[3][1]    = r[3][5];
 299                                         out->m[3][2]    = r[3][6];
 300                                         out->m[3][3]    = r[3][7];
 301 #endif
 302
 303                                         return 1;
 304                                 }
 305                         }
 306                 }
 307         }
 308
 309         return 0;
 310 }
 311
 312 void Matrix4x4_Invert_Simple (matrix4x4_t *out, const matrix4x4_t *in1)
 313 {
 314         // we only support uniform scaling, so assume the first row is enough
 315         // (note the lack of sqrt here, because we're trying to undo the scaling,
 316         // this means multiplying by the inverse scale twice - squaring it, which
 317         // makes the sqrt a waste of time)
 318 #if 1
 319         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]);
 320 #else
 321         double scale = 3.0 / sqrt
 322                  (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]
 323                 + in1->m[1][0] * in1->m[1][0] + in1->m[1][1] * in1->m[1][1] + in1->m[1][2] * in1->m[1][2]
 324                 + in1->m[2][0] * in1->m[2][0] + in1->m[2][1] * in1->m[2][1] + in1->m[2][2] * in1->m[2][2]);
 325         scale *= scale;
 326 #endif
 327
 328         // invert the rotation by transposing and multiplying by the squared
 329         // recipricol of the input matrix scale as described above
 330         out->m[0][0] = in1->m[0][0] * scale;
 331         out->m[0][1] = in1->m[1][0] * scale;
 332         out->m[0][2] = in1->m[2][0] * scale;
 333         out->m[1][0] = in1->m[0][1] * scale;
 334         out->m[1][1] = in1->m[1][1] * scale;
 335         out->m[1][2] = in1->m[2][1] * scale;
 336         out->m[2][0] = in1->m[0][2] * scale;
 337         out->m[2][1] = in1->m[1][2] * scale;
 338         out->m[2][2] = in1->m[2][2] * scale;
 339
 340 #ifdef MATRIX4x4_OPENGLORIENTATION
 341         // invert the translate
 342         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]);
 343         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]);
 344         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]);
 345
 346         // don't know if there's anything worth doing here
 347         out->m[0][3] = 0;
 348         out->m[1][3] = 0;
 349         out->m[2][3] = 0;
 350         out->m[3][3] = 1;
 351 #else
 352         // invert the translate
 353         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]);
 354         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]);
 355         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]);
 356
 357         // don't know if there's anything worth doing here
 358         out->m[3][0] = 0;
 359         out->m[3][1] = 0;
 360         out->m[3][2] = 0;
 361         out->m[3][3] = 1;
 362 #endif
 363 }
 364
 365 void Matrix4x4_Normalize (matrix4x4_t *out, matrix4x4_t *in1)
 366 {
 367         // scale rotation matrix vectors to a length of 1
 368         // note: this is only designed to undo uniform scaling
 369         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]);
 370         *out = *in1;
 371         Matrix4x4_Scale(out, scale, 1);
 372 }
 373
 374 void Matrix4x4_Reflect (matrix4x4_t *out, double normalx, double normaly, double normalz, double dist, double axisscale)
 375 {
 376         int i;
 377         double d;
 378         double p[4], p2[4];
 379         p[0] = normalx;
 380         p[1] = normaly;
 381         p[2] = normalz;
 382         p[3] = -dist;
 383         p2[0] = p[0] * axisscale;
 384         p2[1] = p[1] * axisscale;
 385         p2[2] = p[2] * axisscale;
 386         p2[3] = 0;
 387         for (i = 0;i < 4;i++)
 388         {
 389 #ifdef MATRIX4x4_OPENGLORIENTATION
 390                 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];
 391                 out->m[i][0] += p2[0] * d;
 392                 out->m[i][1] += p2[1] * d;
 393                 out->m[i][2] += p2[2] * d;
 394 #else
 395                 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];
 396                 out->m[0][i] += p2[0] * d;
 397                 out->m[1][i] += p2[1] * d;
 398                 out->m[2][i] += p2[2] * d;
 399 #endif
 400         }
 401 }
 402
 403 void Matrix4x4_CreateIdentity (matrix4x4_t *out)
 404 {
 405         out->m[0][0]=1.0f;
 406         out->m[0][1]=0.0f;
 407         out->m[0][2]=0.0f;
 408         out->m[0][3]=0.0f;
 409         out->m[1][0]=0.0f;
 410         out->m[1][1]=1.0f;
 411         out->m[1][2]=0.0f;
 412         out->m[1][3]=0.0f;
 413         out->m[2][0]=0.0f;
 414         out->m[2][1]=0.0f;
 415         out->m[2][2]=1.0f;
 416         out->m[2][3]=0.0f;
 417         out->m[3][0]=0.0f;
 418         out->m[3][1]=0.0f;
 419         out->m[3][2]=0.0f;
 420         out->m[3][3]=1.0f;
 421 }
 422
 423 void Matrix4x4_CreateTranslate (matrix4x4_t *out, double x, double y, double z)
 424 {
 425 #ifdef MATRIX4x4_OPENGLORIENTATION
 426         out->m[0][0]=1.0f;
 427         out->m[1][0]=0.0f;
 428         out->m[2][0]=0.0f;
 429         out->m[3][0]=x;
 430         out->m[0][1]=0.0f;
 431         out->m[1][1]=1.0f;
 432         out->m[2][1]=0.0f;
 433         out->m[3][1]=y;
 434         out->m[0][2]=0.0f;
 435         out->m[1][2]=0.0f;
 436         out->m[2][2]=1.0f;
 437         out->m[3][2]=z;
 438         out->m[0][3]=0.0f;
 439         out->m[1][3]=0.0f;
 440         out->m[2][3]=0.0f;
 441         out->m[3][3]=1.0f;
 442 #else
 443         out->m[0][0]=1.0f;
 444         out->m[0][1]=0.0f;
 445         out->m[0][2]=0.0f;
 446         out->m[0][3]=x;
 447         out->m[1][0]=0.0f;
 448         out->m[1][1]=1.0f;
 449         out->m[1][2]=0.0f;
 450         out->m[1][3]=y;
 451         out->m[2][0]=0.0f;
 452         out->m[2][1]=0.0f;
 453         out->m[2][2]=1.0f;
 454         out->m[2][3]=z;
 455         out->m[3][0]=0.0f;
 456         out->m[3][1]=0.0f;
 457         out->m[3][2]=0.0f;
 458         out->m[3][3]=1.0f;
 459 #endif
 460 }
 461
 462 void Matrix4x4_CreateRotate (matrix4x4_t *out, double angle, double x, double y, double z)
 463 {
 464         double len, c, s;
 465
 466         len = x*x+y*y+z*z;
 467         if (len != 0.0f)
 468                 len = 1.0f / sqrt(len);
 469         x *= len;
 470         y *= len;
 471         z *= len;
 472
 473         angle *= (-M_PI / 180.0);
 474         c = cos(angle);
 475         s = sin(angle);
 476
 477 #ifdef MATRIX4x4_OPENGLORIENTATION
 478         out->m[0][0]=x * x + c * (1 - x * x);
 479         out->m[1][0]=x * y * (1 - c) + z * s;
 480         out->m[2][0]=z * x * (1 - c) - y * s;
 481         out->m[3][0]=0.0f;
 482         out->m[0][1]=x * y * (1 - c) - z * s;
 483         out->m[1][1]=y * y + c * (1 - y * y);
 484         out->m[2][1]=y * z * (1 - c) + x * s;
 485         out->m[3][1]=0.0f;
 486         out->m[0][2]=z * x * (1 - c) + y * s;
 487         out->m[1][2]=y * z * (1 - c) - x * s;
 488         out->m[2][2]=z * z + c * (1 - z * z);
 489         out->m[3][2]=0.0f;
 490         out->m[0][3]=0.0f;
 491         out->m[1][3]=0.0f;
 492         out->m[2][3]=0.0f;
 493         out->m[3][3]=1.0f;
 494 #else
 495         out->m[0][0]=x * x + c * (1 - x * x);
 496         out->m[0][1]=x * y * (1 - c) + z * s;
 497         out->m[0][2]=z * x * (1 - c) - y * s;
 498         out->m[0][3]=0.0f;
 499         out->m[1][0]=x * y * (1 - c) - z * s;
 500         out->m[1][1]=y * y + c * (1 - y * y);
 501         out->m[1][2]=y * z * (1 - c) + x * s;
 502         out->m[1][3]=0.0f;
 503         out->m[2][0]=z * x * (1 - c) + y * s;
 504         out->m[2][1]=y * z * (1 - c) - x * s;
 505         out->m[2][2]=z * z + c * (1 - z * z);
 506         out->m[2][3]=0.0f;
 507         out->m[3][0]=0.0f;
 508         out->m[3][1]=0.0f;
 509         out->m[3][2]=0.0f;
 510         out->m[3][3]=1.0f;
 511 #endif
 512 }
 513
 514 void Matrix4x4_CreateScale (matrix4x4_t *out, double x)
 515 {
 516         out->m[0][0]=x;
 517         out->m[0][1]=0.0f;
 518         out->m[0][2]=0.0f;
 519         out->m[0][3]=0.0f;
 520         out->m[1][0]=0.0f;
 521         out->m[1][1]=x;
 522         out->m[1][2]=0.0f;
 523         out->m[1][3]=0.0f;
 524         out->m[2][0]=0.0f;
 525         out->m[2][1]=0.0f;
 526         out->m[2][2]=x;
 527         out->m[2][3]=0.0f;
 528         out->m[3][0]=0.0f;
 529         out->m[3][1]=0.0f;
 530         out->m[3][2]=0.0f;
 531         out->m[3][3]=1.0f;
 532 }
 533
 534 void Matrix4x4_CreateScale3 (matrix4x4_t *out, double x, double y, double z)
 535 {
 536         out->m[0][0]=x;
 537         out->m[0][1]=0.0f;
 538         out->m[0][2]=0.0f;
 539         out->m[0][3]=0.0f;
 540         out->m[1][0]=0.0f;
 541         out->m[1][1]=y;
 542         out->m[1][2]=0.0f;
 543         out->m[1][3]=0.0f;
 544         out->m[2][0]=0.0f;
 545         out->m[2][1]=0.0f;
 546         out->m[2][2]=z;
 547         out->m[2][3]=0.0f;
 548         out->m[3][0]=0.0f;
 549         out->m[3][1]=0.0f;
 550         out->m[3][2]=0.0f;
 551         out->m[3][3]=1.0f;
 552 }
 553
 554 void Matrix4x4_CreateFromQuakeEntity(matrix4x4_t *out, double x, double y, double z, double pitch, double yaw, double roll, double scale)
 555 {
 556         double angle, sr, sp, sy, cr, cp, cy;
 557
 558         if (roll)
 559         {
 560                 angle = yaw * (M_PI*2 / 360);
 561                 sy = sin(angle);
 562                 cy = cos(angle);
 563                 angle = pitch * (M_PI*2 / 360);
 564                 sp = sin(angle);
 565                 cp = cos(angle);
 566                 angle = roll * (M_PI*2 / 360);
 567                 sr = sin(angle);
 568                 cr = cos(angle);
 569 #ifdef MATRIX4x4_OPENGLORIENTATION
 570                 out->m[0][0] = (cp*cy) * scale;
 571                 out->m[1][0] = (sr*sp*cy+cr*-sy) * scale;
 572                 out->m[2][0] = (cr*sp*cy+-sr*-sy) * scale;
 573                 out->m[3][0] = x;
 574                 out->m[0][1] = (cp*sy) * scale;
 575                 out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
 576                 out->m[2][1] = (cr*sp*sy+-sr*cy) * scale;
 577                 out->m[3][1] = y;
 578                 out->m[0][2] = (-sp) * scale;
 579                 out->m[1][2] = (sr*cp) * scale;
 580                 out->m[2][2] = (cr*cp) * scale;
 581                 out->m[3][2] = z;
 582                 out->m[0][3] = 0;
 583                 out->m[1][3] = 0;
 584                 out->m[2][3] = 0;
 585                 out->m[3][3] = 1;
 586 #else
 587                 out->m[0][0] = (cp*cy) * scale;
 588                 out->m[0][1] = (sr*sp*cy+cr*-sy) * scale;
 589                 out->m[0][2] = (cr*sp*cy+-sr*-sy) * scale;
 590                 out->m[0][3] = x;
 591                 out->m[1][0] = (cp*sy) * scale;
 592                 out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
 593                 out->m[1][2] = (cr*sp*sy+-sr*cy) * scale;
 594                 out->m[1][3] = y;
 595                 out->m[2][0] = (-sp) * scale;
 596                 out->m[2][1] = (sr*cp) * scale;
 597                 out->m[2][2] = (cr*cp) * scale;
 598                 out->m[2][3] = z;
 599                 out->m[3][0] = 0;
 600                 out->m[3][1] = 0;
 601                 out->m[3][2] = 0;
 602                 out->m[3][3] = 1;
 603 #endif
 604         }
 605         else if (pitch)
 606         {
 607                 angle = yaw * (M_PI*2 / 360);
 608                 sy = sin(angle);
 609                 cy = cos(angle);
 610                 angle = pitch * (M_PI*2 / 360);
 611                 sp = sin(angle);
 612                 cp = cos(angle);
 613 #ifdef MATRIX4x4_OPENGLORIENTATION
 614                 out->m[0][0] = (cp*cy) * scale;
 615                 out->m[1][0] = (-sy) * scale;
 616                 out->m[2][0] = (sp*cy) * scale;
 617                 out->m[3][0] = x;
 618                 out->m[0][1] = (cp*sy) * scale;
 619                 out->m[1][1] = (cy) * scale;
 620                 out->m[2][1] = (sp*sy) * scale;
 621                 out->m[3][1] = y;
 622                 out->m[0][2] = (-sp) * scale;
 623                 out->m[1][2] = 0;
 624                 out->m[2][2] = (cp) * scale;
 625                 out->m[3][2] = z;
 626                 out->m[0][3] = 0;
 627                 out->m[1][3] = 0;
 628                 out->m[2][3] = 0;
 629                 out->m[3][3] = 1;
 630 #else
 631                 out->m[0][0] = (cp*cy) * scale;
 632                 out->m[0][1] = (-sy) * scale;
 633                 out->m[0][2] = (sp*cy) * scale;
 634                 out->m[0][3] = x;
 635                 out->m[1][0] = (cp*sy) * scale;
 636                 out->m[1][1] = (cy) * scale;
 637                 out->m[1][2] = (sp*sy) * scale;
 638                 out->m[1][3] = y;
 639                 out->m[2][0] = (-sp) * scale;
 640                 out->m[2][1] = 0;
 641                 out->m[2][2] = (cp) * scale;
 642                 out->m[2][3] = z;
 643                 out->m[3][0] = 0;
 644                 out->m[3][1] = 0;
 645                 out->m[3][2] = 0;
 646                 out->m[3][3] = 1;
 647 #endif
 648         }
 649         else if (yaw)
 650         {
 651                 angle = yaw * (M_PI*2 / 360);
 652                 sy = sin(angle);
 653                 cy = cos(angle);
 654 #ifdef MATRIX4x4_OPENGLORIENTATION
 655                 out->m[0][0] = (cy) * scale;
 656                 out->m[1][0] = (-sy) * scale;
 657                 out->m[2][0] = 0;
 658                 out->m[3][0] = x;
 659                 out->m[0][1] = (sy) * scale;
 660                 out->m[1][1] = (cy) * scale;
 661                 out->m[2][1] = 0;
 662                 out->m[3][1] = y;
 663                 out->m[0][2] = 0;
 664                 out->m[1][2] = 0;
 665                 out->m[2][2] = scale;
 666                 out->m[3][2] = z;
 667                 out->m[0][3] = 0;
 668                 out->m[1][3] = 0;
 669                 out->m[2][3] = 0;
 670                 out->m[3][3] = 1;
 671 #else
 672                 out->m[0][0] = (cy) * scale;
 673                 out->m[0][1] = (-sy) * scale;
 674                 out->m[0][2] = 0;
 675                 out->m[0][3] = x;
 676                 out->m[1][0] = (sy) * scale;
 677                 out->m[1][1] = (cy) * scale;
 678                 out->m[1][2] = 0;
 679                 out->m[1][3] = y;
 680                 out->m[2][0] = 0;
 681                 out->m[2][1] = 0;
 682                 out->m[2][2] = scale;
 683                 out->m[2][3] = z;
 684                 out->m[3][0] = 0;
 685                 out->m[3][1] = 0;
 686                 out->m[3][2] = 0;
 687                 out->m[3][3] = 1;
 688 #endif
 689         }
 690         else
 691         {
 692 #ifdef MATRIX4x4_OPENGLORIENTATION
 693                 out->m[0][0] = scale;
 694                 out->m[1][0] = 0;
 695                 out->m[2][0] = 0;
 696                 out->m[3][0] = x;
 697                 out->m[0][1] = 0;
 698                 out->m[1][1] = scale;
 699                 out->m[2][1] = 0;
 700                 out->m[3][1] = y;
 701                 out->m[0][2] = 0;
 702                 out->m[1][2] = 0;
 703                 out->m[2][2] = scale;
 704                 out->m[3][2] = z;
 705                 out->m[0][3] = 0;
 706                 out->m[1][3] = 0;
 707                 out->m[2][3] = 0;
 708                 out->m[3][3] = 1;
 709 #else
 710                 out->m[0][0] = scale;
 711                 out->m[0][1] = 0;
 712                 out->m[0][2] = 0;
 713                 out->m[0][3] = x;
 714                 out->m[1][0] = 0;
 715                 out->m[1][1] = scale;
 716                 out->m[1][2] = 0;
 717                 out->m[1][3] = y;
 718                 out->m[2][0] = 0;
 719                 out->m[2][1] = 0;
 720                 out->m[2][2] = scale;
 721                 out->m[2][3] = z;
 722                 out->m[3][0] = 0;
 723                 out->m[3][1] = 0;
 724                 out->m[3][2] = 0;
 725                 out->m[3][3] = 1;
 726 #endif
 727         }
 728 }
 729
 730 void Matrix4x4_ToVectors(const matrix4x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
 731 {
 732 #ifdef MATRIX4x4_OPENGLORIENTATION
 733         vx[0] = in->m[0][0];
 734         vx[1] = in->m[0][1];
 735         vx[2] = in->m[0][2];
 736         vy[0] = in->m[1][0];
 737         vy[1] = in->m[1][1];
 738         vy[2] = in->m[1][2];
 739         vz[0] = in->m[2][0];
 740         vz[1] = in->m[2][1];
 741         vz[2] = in->m[2][2];
 742         t [0] = in->m[3][0];
 743         t [1] = in->m[3][1];
 744         t [2] = in->m[3][2];
 745 #else
 746         vx[0] = in->m[0][0];
 747         vx[1] = in->m[1][0];
 748         vx[2] = in->m[2][0];
 749         vy[0] = in->m[0][1];
 750         vy[1] = in->m[1][1];
 751         vy[2] = in->m[2][1];
 752         vz[0] = in->m[0][2];
 753         vz[1] = in->m[1][2];
 754         vz[2] = in->m[2][2];
 755         t [0] = in->m[0][3];
 756         t [1] = in->m[1][3];
 757         t [2] = in->m[2][3];
 758 #endif
 759 }
 760
 761 void Matrix4x4_FromVectors(matrix4x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
 762 {
 763 #ifdef MATRIX4x4_OPENGLORIENTATION
 764         out->m[0][0] = vx[0];
 765         out->m[1][0] = vy[0];
 766         out->m[2][0] = vz[0];
 767         out->m[3][0] = t[0];
 768         out->m[0][1] = vx[1];
 769         out->m[1][1] = vy[1];
 770         out->m[2][1] = vz[1];
 771         out->m[3][1] = t[1];
 772         out->m[0][2] = vx[2];
 773         out->m[1][2] = vy[2];
 774         out->m[2][2] = vz[2];
 775         out->m[3][2] = t[2];
 776         out->m[0][3] = 0.0f;
 777         out->m[1][3] = 0.0f;
 778         out->m[2][3] = 0.0f;
 779         out->m[3][3] = 1.0f;
 780 #else
 781         out->m[0][0] = vx[0];
 782         out->m[0][1] = vy[0];
 783         out->m[0][2] = vz[0];
 784         out->m[0][3] = t[0];
 785         out->m[1][0] = vx[1];
 786         out->m[1][1] = vy[1];
 787         out->m[1][2] = vz[1];
 788         out->m[1][3] = t[1];
 789         out->m[2][0] = vx[2];
 790         out->m[2][1] = vy[2];
 791         out->m[2][2] = vz[2];
 792         out->m[2][3] = t[2];
 793         out->m[3][0] = 0.0f;
 794         out->m[3][1] = 0.0f;
 795         out->m[3][2] = 0.0f;
 796         out->m[3][3] = 1.0f;
 797 #endif
 798 }
 799
 800 void Matrix4x4_ToArrayDoubleGL(const matrix4x4_t *in, double out[16])
 801 {
 802 #ifdef MATRIX4x4_OPENGLORIENTATION
 803         out[ 0] = in->m[0][0];
 804         out[ 1] = in->m[0][1];
 805         out[ 2] = in->m[0][2];
 806         out[ 3] = in->m[0][3];
 807         out[ 4] = in->m[1][0];
 808         out[ 5] = in->m[1][1];
 809         out[ 6] = in->m[1][2];
 810         out[ 7] = in->m[1][3];
 811         out[ 8] = in->m[2][0];
 812         out[ 9] = in->m[2][1];
 813         out[10] = in->m[2][2];
 814         out[11] = in->m[2][3];
 815         out[12] = in->m[3][0];
 816         out[13] = in->m[3][1];
 817         out[14] = in->m[3][2];
 818         out[15] = in->m[3][3];
 819 #else
 820         out[ 0] = in->m[0][0];
 821         out[ 1] = in->m[1][0];
 822         out[ 2] = in->m[2][0];
 823         out[ 3] = in->m[3][0];
 824         out[ 4] = in->m[0][1];
 825         out[ 5] = in->m[1][1];
 826         out[ 6] = in->m[2][1];
 827         out[ 7] = in->m[3][1];
 828         out[ 8] = in->m[0][2];
 829         out[ 9] = in->m[1][2];
 830         out[10] = in->m[2][2];
 831         out[11] = in->m[3][2];
 832         out[12] = in->m[0][3];
 833         out[13] = in->m[1][3];
 834         out[14] = in->m[2][3];
 835         out[15] = in->m[3][3];
 836 #endif
 837 }
 838
 839 void Matrix4x4_FromArrayDoubleGL (matrix4x4_t *out, const double in[16])
 840 {
 841 #ifdef MATRIX4x4_OPENGLORIENTATION
 842         out->m[0][0] = in[0];
 843         out->m[0][1] = in[1];
 844         out->m[0][2] = in[2];
 845         out->m[0][3] = in[3];
 846         out->m[1][0] = in[4];
 847         out->m[1][1] = in[5];
 848         out->m[1][2] = in[6];
 849         out->m[1][3] = in[7];
 850         out->m[2][0] = in[8];
 851         out->m[2][1] = in[9];
 852         out->m[2][2] = in[10];
 853         out->m[2][3] = in[11];
 854         out->m[3][0] = in[12];
 855         out->m[3][1] = in[13];
 856         out->m[3][2] = in[14];
 857         out->m[3][3] = in[15];
 858 #else
 859         out->m[0][0] = in[0];
 860         out->m[1][0] = in[1];
 861         out->m[2][0] = in[2];
 862         out->m[3][0] = in[3];
 863         out->m[0][1] = in[4];
 864         out->m[1][1] = in[5];
 865         out->m[2][1] = in[6];
 866         out->m[3][1] = in[7];
 867         out->m[0][2] = in[8];
 868         out->m[1][2] = in[9];
 869         out->m[2][2] = in[10];
 870         out->m[3][2] = in[11];
 871         out->m[0][3] = in[12];
 872         out->m[1][3] = in[13];
 873         out->m[2][3] = in[14];
 874         out->m[3][3] = in[15];
 875 #endif
 876 }
 877
 878 void Matrix4x4_ToArrayDoubleD3D(const matrix4x4_t *in, double out[16])
 879 {
 880 #ifdef MATRIX4x4_OPENGLORIENTATION
 881         out[ 0] = in->m[0][0];
 882         out[ 1] = in->m[1][0];
 883         out[ 2] = in->m[2][0];
 884         out[ 3] = in->m[3][0];
 885         out[ 4] = in->m[0][1];
 886         out[ 5] = in->m[1][1];
 887         out[ 6] = in->m[2][1];
 888         out[ 7] = in->m[3][1];
 889         out[ 8] = in->m[0][2];
 890         out[ 9] = in->m[1][2];
 891         out[10] = in->m[2][2];
 892         out[11] = in->m[3][2];
 893         out[12] = in->m[0][3];
 894         out[13] = in->m[1][3];
 895         out[14] = in->m[2][3];
 896         out[15] = in->m[3][3];
 897 #else
 898         out[ 0] = in->m[0][0];
 899         out[ 1] = in->m[0][1];
 900         out[ 2] = in->m[0][2];
 901         out[ 3] = in->m[0][3];
 902         out[ 4] = in->m[1][0];
 903         out[ 5] = in->m[1][1];
 904         out[ 6] = in->m[1][2];
 905         out[ 7] = in->m[1][3];
 906         out[ 8] = in->m[2][0];
 907         out[ 9] = in->m[2][1];
 908         out[10] = in->m[2][2];
 909         out[11] = in->m[2][3];
 910         out[12] = in->m[3][0];
 911         out[13] = in->m[3][1];
 912         out[14] = in->m[3][2];
 913         out[15] = in->m[3][3];
 914 #endif
 915 }
 916
 917 void Matrix4x4_FromArrayDoubleD3D (matrix4x4_t *out, const double in[16])
 918 {
 919 #ifdef MATRIX4x4_OPENGLORIENTATION
 920         out->m[0][0] = in[0];
 921         out->m[1][0] = in[1];
 922         out->m[2][0] = in[2];
 923         out->m[3][0] = in[3];
 924         out->m[0][1] = in[4];
 925         out->m[1][1] = in[5];
 926         out->m[2][1] = in[6];
 927         out->m[3][1] = in[7];
 928         out->m[0][2] = in[8];
 929         out->m[1][2] = in[9];
 930         out->m[2][2] = in[10];
 931         out->m[3][2] = in[11];
 932         out->m[0][3] = in[12];
 933         out->m[1][3] = in[13];
 934         out->m[2][3] = in[14];
 935         out->m[3][3] = in[15];
 936 #else
 937         out->m[0][0] = in[0];
 938         out->m[0][1] = in[1];
 939         out->m[0][2] = in[2];
 940         out->m[0][3] = in[3];
 941         out->m[1][0] = in[4];
 942         out->m[1][1] = in[5];
 943         out->m[1][2] = in[6];
 944         out->m[1][3] = in[7];
 945         out->m[2][0] = in[8];
 946         out->m[2][1] = in[9];
 947         out->m[2][2] = in[10];
 948         out->m[2][3] = in[11];
 949         out->m[3][0] = in[12];
 950         out->m[3][1] = in[13];
 951         out->m[3][2] = in[14];
 952         out->m[3][3] = in[15];
 953 #endif
 954 }
 955
 956 void Matrix4x4_ToArray12FloatGL(const matrix4x4_t *in, float out[12])
 957 {
 958 #ifdef MATRIX4x4_OPENGLORIENTATION
 959         out[ 0] = in->m[0][0];
 960         out[ 1] = in->m[0][1];
 961         out[ 2] = in->m[0][2];
 962         out[ 3] = in->m[1][0];
 963         out[ 4] = in->m[1][1];
 964         out[ 5] = in->m[1][2];
 965         out[ 6] = in->m[2][0];
 966         out[ 7] = in->m[2][1];
 967         out[ 8] = in->m[2][2];
 968         out[ 9] = in->m[3][0];
 969         out[10] = in->m[3][1];
 970         out[11] = in->m[3][2];
 971 #else
 972         out[ 0] = in->m[0][0];
 973         out[ 1] = in->m[1][0];
 974         out[ 2] = in->m[2][0];
 975         out[ 3] = in->m[0][1];
 976         out[ 4] = in->m[1][1];
 977         out[ 5] = in->m[2][1];
 978         out[ 6] = in->m[0][2];
 979         out[ 7] = in->m[1][2];
 980         out[ 8] = in->m[2][2];
 981         out[ 9] = in->m[0][3];
 982         out[10] = in->m[1][3];
 983         out[11] = in->m[2][3];
 984 #endif
 985 }
 986
 987 void Matrix4x4_FromArray12FloatGL(matrix4x4_t *out, const float in[12])
 988 {
 989 #ifdef MATRIX4x4_OPENGLORIENTATION
 990         out->m[0][0] = in[0];
 991         out->m[0][1] = in[1];
 992         out->m[0][2] = in[2];
 993         out->m[0][3] = 0;
 994         out->m[1][0] = in[3];
 995         out->m[1][1] = in[4];
 996         out->m[1][2] = in[5];
 997         out->m[1][3] = 0;
 998         out->m[2][0] = in[6];
 999         out->m[2][1] = in[7];
1000         out->m[2][2] = in[8];
1001         out->m[2][3] = 0;
1002         out->m[3][0] = in[9];
1003         out->m[3][1] = in[10];
1004         out->m[3][2] = in[11];
1005         out->m[3][3] = 1;
1006 #else
1007         out->m[0][0] = in[0];
1008         out->m[1][0] = in[1];
1009         out->m[2][0] = in[2];
1010         out->m[3][0] = 0;
1011         out->m[0][1] = in[3];
1012         out->m[1][1] = in[4];
1013         out->m[2][1] = in[5];
1014         out->m[3][1] = 0;
1015         out->m[0][2] = in[6];
1016         out->m[1][2] = in[7];
1017         out->m[2][2] = in[8];
1018         out->m[3][2] = 0;
1019         out->m[0][3] = in[9];
1020         out->m[1][3] = in[10];
1021         out->m[2][3] = in[11];
1022         out->m[3][3] = 1;
1023 #endif
1024 }
1025
1026 void Matrix4x4_ToArray12FloatD3D(const matrix4x4_t *in, float out[12])
1027 {
1028 #ifdef MATRIX4x4_OPENGLORIENTATION
1029         out[ 0] = in->m[0][0];
1030         out[ 1] = in->m[1][0];
1031         out[ 2] = in->m[2][0];
1032         out[ 3] = in->m[3][0];
1033         out[ 4] = in->m[0][1];
1034         out[ 5] = in->m[1][1];
1035         out[ 6] = in->m[2][1];
1036         out[ 7] = in->m[3][1];
1037         out[ 8] = in->m[0][2];
1038         out[ 9] = in->m[1][2];
1039         out[10] = in->m[2][2];
1040         out[11] = in->m[3][2];
1041 #else
1042         out[ 0] = in->m[0][0];
1043         out[ 1] = in->m[0][1];
1044         out[ 2] = in->m[0][2];
1045         out[ 3] = in->m[0][3];
1046         out[ 4] = in->m[1][0];
1047         out[ 5] = in->m[1][1];
1048         out[ 6] = in->m[1][2];
1049         out[ 7] = in->m[1][3];
1050         out[ 8] = in->m[2][0];
1051         out[ 9] = in->m[2][1];
1052         out[10] = in->m[2][2];
1053         out[11] = in->m[2][3];
1054 #endif
1055 }
1056
1057 void Matrix4x4_FromArray12FloatD3D(matrix4x4_t *out, const float in[12])
1058 {
1059 #ifdef MATRIX4x4_OPENGLORIENTATION
1060         out->m[0][0] = in[0];
1061         out->m[1][0] = in[1];
1062         out->m[2][0] = in[2];
1063         out->m[3][0] = in[3];
1064         out->m[0][1] = in[4];
1065         out->m[1][1] = in[5];
1066         out->m[2][1] = in[6];
1067         out->m[3][1] = in[7];
1068         out->m[0][2] = in[8];
1069         out->m[1][2] = in[9];
1070         out->m[2][2] = in[10];
1071         out->m[3][2] = in[11];
1072         out->m[0][3] = 0;
1073         out->m[1][3] = 0;
1074         out->m[2][3] = 0;
1075         out->m[3][3] = 1;
1076 #else
1077         out->m[0][0] = in[0];
1078         out->m[0][1] = in[1];
1079         out->m[0][2] = in[2];
1080         out->m[0][3] = in[3];
1081         out->m[1][0] = in[4];
1082         out->m[1][1] = in[5];
1083         out->m[1][2] = in[6];
1084         out->m[1][3] = in[7];
1085         out->m[2][0] = in[8];
1086         out->m[2][1] = in[9];
1087         out->m[2][2] = in[10];
1088         out->m[2][3] = in[11];
1089         out->m[3][0] = 0;
1090         out->m[3][1] = 0;
1091         out->m[3][2] = 0;
1092         out->m[3][3] = 1;
1093 #endif
1094 }
1095
1096 void Matrix4x4_FromOriginQuat(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z, double w)
1097 {
1098 #ifdef MATRIX4x4_OPENGLORIENTATION
1099         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;
1100         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;
1101         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;
1102         m->m[0][3]=  0          ;m->m[1][3]=  0          ;m->m[2][3]=  0          ;m->m[3][3]=1;
1103 #else
1104         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;
1105         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;
1106         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;
1107         m->m[3][0]=  0          ;m->m[3][1]=  0          ;m->m[3][2]=  0          ;m->m[3][3]=1;
1108 #endif
1109 }
1110
1111 // LordHavoc: I got this code from:
1112 //http://www.doom3world.org/phpbb2/viewtopic.php?t=2884
1113 void Matrix4x4_FromDoom3Joint(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z)
1114 {
1115         double w = 1.0 - (x*x+y*y+z*z);
1116         w = w > 0.0 ? -sqrt(w) : 0.0;
1117 #ifdef MATRIX4x4_OPENGLORIENTATION
1118         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;
1119         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;
1120         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;
1121         m->m[0][3]=  0          ;m->m[1][3]=  0          ;m->m[2][3]=  0          ;m->m[3][3]=1;
1122 #else
1123         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;
1124         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;
1125         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;
1126         m->m[3][0]=  0          ;m->m[3][1]=  0          ;m->m[3][2]=  0          ;m->m[3][3]=1;
1127 #endif
1128 }
1129
1130 void Matrix4x4_Blend (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2, double blend)
1131 {
1132         double iblend = 1 - blend;
1133         out->m[0][0] = in1->m[0][0] * iblend + in2->m[0][0] * blend;
1134         out->m[0][1] = in1->m[0][1] * iblend + in2->m[0][1] * blend;
1135         out->m[0][2] = in1->m[0][2] * iblend + in2->m[0][2] * blend;
1136         out->m[0][3] = in1->m[0][3] * iblend + in2->m[0][3] * blend;
1137         out->m[1][0] = in1->m[1][0] * iblend + in2->m[1][0] * blend;
1138         out->m[1][1] = in1->m[1][1] * iblend + in2->m[1][1] * blend;
1139         out->m[1][2] = in1->m[1][2] * iblend + in2->m[1][2] * blend;
1140         out->m[1][3] = in1->m[1][3] * iblend + in2->m[1][3] * blend;
1141         out->m[2][0] = in1->m[2][0] * iblend + in2->m[2][0] * blend;
1142         out->m[2][1] = in1->m[2][1] * iblend + in2->m[2][1] * blend;
1143         out->m[2][2] = in1->m[2][2] * iblend + in2->m[2][2] * blend;
1144         out->m[2][3] = in1->m[2][3] * iblend + in2->m[2][3] * blend;
1145         out->m[3][0] = in1->m[3][0] * iblend + in2->m[3][0] * blend;
1146         out->m[3][1] = in1->m[3][1] * iblend + in2->m[3][1] * blend;
1147         out->m[3][2] = in1->m[3][2] * iblend + in2->m[3][2] * blend;
1148         out->m[3][3] = in1->m[3][3] * iblend + in2->m[3][3] * blend;
1149 }
1150
1151
1152 void Matrix4x4_Transform (const matrix4x4_t *in, const float v[3], float out[3])
1153 {
1154 #ifdef MATRIX4x4_OPENGLORIENTATION
1155         out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0] + in->m[3][0];
1156         out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1] + in->m[3][1];
1157         out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2] + in->m[3][2];
1158 #else
1159         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
1160         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
1161         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
1162 #endif
1163 }
1164
1165 void Matrix4x4_Transform4 (const matrix4x4_t *in, const float v[4], float out[4])
1166 {
1167 #ifdef MATRIX4x4_OPENGLORIENTATION
1168         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];
1169         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];
1170         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];
1171         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];
1172 #else
1173         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];
1174         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];
1175         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];
1176         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];
1177 #endif
1178 }
1179
1180 void Matrix4x4_Transform3x3 (const matrix4x4_t *in, const float v[3], float out[3])
1181 {
1182 #ifdef MATRIX4x4_OPENGLORIENTATION
1183         out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0];
1184         out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1];
1185         out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2];
1186 #else
1187         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2];
1188         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2];
1189         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2];
1190 #endif
1191 }
1192
1193 /*
1194 void Matrix4x4_SimpleUntransform (const matrix4x4_t *in, const float v[3], float out[3])
1195 {
1196         double t[3];
1197 #ifdef MATRIX4x4_OPENGLORIENTATION
1198         t[0] = v[0] - in->m[3][0];
1199         t[1] = v[1] - in->m[3][1];
1200         t[2] = v[2] - in->m[3][2];
1201         out[0] = t[0] * in->m[0][0] + t[1] * in->m[0][1] + t[2] * in->m[0][2];
1202         out[1] = t[0] * in->m[1][0] + t[1] * in->m[1][1] + t[2] * in->m[1][2];
1203         out[2] = t[0] * in->m[2][0] + t[1] * in->m[2][1] + t[2] * in->m[2][2];
1204 #else
1205         t[0] = v[0] - in->m[0][3];
1206         t[1] = v[1] - in->m[1][3];
1207         t[2] = v[2] - in->m[2][3];
1208         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
1209         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
1210         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
1211 #endif
1212 }
1213 */
1214
1215 // FIXME: optimize
1216 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, double x, double y, double z)
1217 {
1218         matrix4x4_t base, temp;
1219         base = *out;
1220         Matrix4x4_CreateTranslate(&temp, x, y, z);
1221         Matrix4x4_Concat(out, &base, &temp);
1222 }
1223
1224 // FIXME: optimize
1225 void Matrix4x4_ConcatRotate (matrix4x4_t *out, double angle, double x, double y, double z)
1226 {
1227         matrix4x4_t base, temp;
1228         base = *out;
1229         Matrix4x4_CreateRotate(&temp, angle, x, y, z);
1230         Matrix4x4_Concat(out, &base, &temp);
1231 }
1232
1233 // FIXME: optimize
1234 void Matrix4x4_ConcatScale (matrix4x4_t *out, double x)
1235 {
1236         matrix4x4_t base, temp;
1237         base = *out;
1238         Matrix4x4_CreateScale(&temp, x);
1239         Matrix4x4_Concat(out, &base, &temp);
1240 }
1241
1242 // FIXME: optimize
1243 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, double x, double y, double z)
1244 {
1245         matrix4x4_t base, temp;
1246         base = *out;
1247         Matrix4x4_CreateScale3(&temp, x, y, z);
1248         Matrix4x4_Concat(out, &base, &temp);
1249 }
1250
1251 void Matrix4x4_OriginFromMatrix (const matrix4x4_t *in, float *out)
1252 {
1253 #ifdef MATRIX4x4_OPENGLORIENTATION
1254         out[0] = in->m[3][0];
1255         out[1] = in->m[3][1];
1256         out[2] = in->m[3][2];
1257 #else
1258         out[0] = in->m[0][3];
1259         out[1] = in->m[1][3];
1260         out[2] = in->m[2][3];
1261 #endif
1262 }
1263
1264 double Matrix4x4_ScaleFromMatrix (const matrix4x4_t *in)
1265 {
1266         // we only support uniform scaling, so assume the first row is enough
1267         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]);
1268 }
1269
1270 void Matrix4x4_SetOrigin (matrix4x4_t *out, double x, double y, double z)
1271 {
1272 #ifdef MATRIX4x4_OPENGLORIENTATION
1273         out->m[3][0] = x;
1274         out->m[3][1] = y;
1275         out->m[3][2] = z;
1276 #else
1277         out->m[0][3] = x;
1278         out->m[1][3] = y;
1279         out->m[2][3] = z;
1280 #endif
1281 }
1282
1283 void Matrix4x4_AdjustOrigin (matrix4x4_t *out, double x, double y, double z)
1284 {
1285 #ifdef MATRIX4x4_OPENGLORIENTATION
1286         out->m[3][0] += x;
1287         out->m[3][1] += y;
1288         out->m[3][2] += z;
1289 #else
1290         out->m[0][3] += x;
1291         out->m[1][3] += y;
1292         out->m[2][3] += z;
1293 #endif
1294 }
1295
1296 void Matrix4x4_Scale (matrix4x4_t *out, double rotatescale, double originscale)
1297 {
1298         out->m[0][0] *= rotatescale;
1299         out->m[0][1] *= rotatescale;
1300         out->m[0][2] *= rotatescale;
1301         out->m[1][0] *= rotatescale;
1302         out->m[1][1] *= rotatescale;
1303         out->m[1][2] *= rotatescale;
1304         out->m[2][0] *= rotatescale;
1305         out->m[2][1] *= rotatescale;
1306         out->m[2][2] *= rotatescale;
1307 #ifdef MATRIX4x4_OPENGLORIENTATION
1308         out->m[3][0] *= originscale;
1309         out->m[3][1] *= originscale;
1310         out->m[3][2] *= originscale;
1311 #else
1312         out->m[0][3] *= originscale;
1313         out->m[1][3] *= originscale;
1314         out->m[2][3] *= originscale;
1315 #endif
1316 }