matrixlib.c

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