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_Reflect (matrix4x4_t *out, double normalx, double normaly, double normalz, double dist, double axisscale)
 369 {
 370         int i;
 371         double d;
 372         double p[4], p2[4];
 373         p[0] = normalx;
 374         p[1] = normaly;
 375         p[2] = normalz;
 376         p[3] = -dist;
 377         p2[0] = p[0] * axisscale;
 378         p2[1] = p[1] * axisscale;
 379         p2[2] = p[2] * axisscale;
 380         p2[3] = 0;
 381         for (i = 0;i < 4;i++)
 382         {
 383 #ifdef MATRIX4x4_OPENGLORIENTATION
 384                 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];
 385                 out->m[i][0] += p2[0] * d;
 386                 out->m[i][1] += p2[1] * d;
 387                 out->m[i][2] += p2[2] * d;
 388 #else
 389                 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];
 390                 out->m[0][i] += p2[0] * d;
 391                 out->m[1][i] += p2[1] * d;
 392                 out->m[2][i] += p2[2] * d;
 393 #endif
 394         }
 395 }
 396
 397 void Matrix4x4_CreateIdentity (matrix4x4_t *out)
 398 {
 399         out->m[0][0]=1.0f;
 400         out->m[0][1]=0.0f;
 401         out->m[0][2]=0.0f;
 402         out->m[0][3]=0.0f;
 403         out->m[1][0]=0.0f;
 404         out->m[1][1]=1.0f;
 405         out->m[1][2]=0.0f;
 406         out->m[1][3]=0.0f;
 407         out->m[2][0]=0.0f;
 408         out->m[2][1]=0.0f;
 409         out->m[2][2]=1.0f;
 410         out->m[2][3]=0.0f;
 411         out->m[3][0]=0.0f;
 412         out->m[3][1]=0.0f;
 413         out->m[3][2]=0.0f;
 414         out->m[3][3]=1.0f;
 415 }
 416
 417 void Matrix4x4_CreateTranslate (matrix4x4_t *out, double x, double y, double z)
 418 {
 419 #ifdef MATRIX4x4_OPENGLORIENTATION
 420         out->m[0][0]=1.0f;
 421         out->m[1][0]=0.0f;
 422         out->m[2][0]=0.0f;
 423         out->m[3][0]=x;
 424         out->m[0][1]=0.0f;
 425         out->m[1][1]=1.0f;
 426         out->m[2][1]=0.0f;
 427         out->m[3][1]=y;
 428         out->m[0][2]=0.0f;
 429         out->m[1][2]=0.0f;
 430         out->m[2][2]=1.0f;
 431         out->m[3][2]=z;
 432         out->m[0][3]=0.0f;
 433         out->m[1][3]=0.0f;
 434         out->m[2][3]=0.0f;
 435         out->m[3][3]=1.0f;
 436 #else
 437         out->m[0][0]=1.0f;
 438         out->m[0][1]=0.0f;
 439         out->m[0][2]=0.0f;
 440         out->m[0][3]=x;
 441         out->m[1][0]=0.0f;
 442         out->m[1][1]=1.0f;
 443         out->m[1][2]=0.0f;
 444         out->m[1][3]=y;
 445         out->m[2][0]=0.0f;
 446         out->m[2][1]=0.0f;
 447         out->m[2][2]=1.0f;
 448         out->m[2][3]=z;
 449         out->m[3][0]=0.0f;
 450         out->m[3][1]=0.0f;
 451         out->m[3][2]=0.0f;
 452         out->m[3][3]=1.0f;
 453 #endif
 454 }
 455
 456 void Matrix4x4_CreateRotate (matrix4x4_t *out, double angle, double x, double y, double z)
 457 {
 458         double len, c, s;
 459
 460         len = x*x+y*y+z*z;
 461         if (len != 0.0f)
 462                 len = 1.0f / sqrt(len);
 463         x *= len;
 464         y *= len;
 465         z *= len;
 466
 467         angle *= (-M_PI / 180.0);
 468         c = cos(angle);
 469         s = sin(angle);
 470
 471 #ifdef MATRIX4x4_OPENGLORIENTATION
 472         out->m[0][0]=x * x + c * (1 - x * x);
 473         out->m[1][0]=x * y * (1 - c) + z * s;
 474         out->m[2][0]=z * x * (1 - c) - y * s;
 475         out->m[3][0]=0.0f;
 476         out->m[0][1]=x * y * (1 - c) - z * s;
 477         out->m[1][1]=y * y + c * (1 - y * y);
 478         out->m[2][1]=y * z * (1 - c) + x * s;
 479         out->m[3][1]=0.0f;
 480         out->m[0][2]=z * x * (1 - c) + y * s;
 481         out->m[1][2]=y * z * (1 - c) - x * s;
 482         out->m[2][2]=z * z + c * (1 - z * z);
 483         out->m[3][2]=0.0f;
 484         out->m[0][3]=0.0f;
 485         out->m[1][3]=0.0f;
 486         out->m[2][3]=0.0f;
 487         out->m[3][3]=1.0f;
 488 #else
 489         out->m[0][0]=x * x + c * (1 - x * x);
 490         out->m[0][1]=x * y * (1 - c) + z * s;
 491         out->m[0][2]=z * x * (1 - c) - y * s;
 492         out->m[0][3]=0.0f;
 493         out->m[1][0]=x * y * (1 - c) - z * s;
 494         out->m[1][1]=y * y + c * (1 - y * y);
 495         out->m[1][2]=y * z * (1 - c) + x * s;
 496         out->m[1][3]=0.0f;
 497         out->m[2][0]=z * x * (1 - c) + y * s;
 498         out->m[2][1]=y * z * (1 - c) - x * s;
 499         out->m[2][2]=z * z + c * (1 - z * z);
 500         out->m[2][3]=0.0f;
 501         out->m[3][0]=0.0f;
 502         out->m[3][1]=0.0f;
 503         out->m[3][2]=0.0f;
 504         out->m[3][3]=1.0f;
 505 #endif
 506 }
 507
 508 void Matrix4x4_CreateScale (matrix4x4_t *out, double x)
 509 {
 510         out->m[0][0]=x;
 511         out->m[0][1]=0.0f;
 512         out->m[0][2]=0.0f;
 513         out->m[0][3]=0.0f;
 514         out->m[1][0]=0.0f;
 515         out->m[1][1]=x;
 516         out->m[1][2]=0.0f;
 517         out->m[1][3]=0.0f;
 518         out->m[2][0]=0.0f;
 519         out->m[2][1]=0.0f;
 520         out->m[2][2]=x;
 521         out->m[2][3]=0.0f;
 522         out->m[3][0]=0.0f;
 523         out->m[3][1]=0.0f;
 524         out->m[3][2]=0.0f;
 525         out->m[3][3]=1.0f;
 526 }
 527
 528 void Matrix4x4_CreateScale3 (matrix4x4_t *out, double x, double y, double z)
 529 {
 530         out->m[0][0]=x;
 531         out->m[0][1]=0.0f;
 532         out->m[0][2]=0.0f;
 533         out->m[0][3]=0.0f;
 534         out->m[1][0]=0.0f;
 535         out->m[1][1]=y;
 536         out->m[1][2]=0.0f;
 537         out->m[1][3]=0.0f;
 538         out->m[2][0]=0.0f;
 539         out->m[2][1]=0.0f;
 540         out->m[2][2]=z;
 541         out->m[2][3]=0.0f;
 542         out->m[3][0]=0.0f;
 543         out->m[3][1]=0.0f;
 544         out->m[3][2]=0.0f;
 545         out->m[3][3]=1.0f;
 546 }
 547
 548 void Matrix4x4_CreateFromQuakeEntity(matrix4x4_t *out, double x, double y, double z, double pitch, double yaw, double roll, double scale)
 549 {
 550         double angle, sr, sp, sy, cr, cp, cy;
 551
 552         if (roll)
 553         {
 554                 angle = yaw * (M_PI*2 / 360);
 555                 sy = sin(angle);
 556                 cy = cos(angle);
 557                 angle = pitch * (M_PI*2 / 360);
 558                 sp = sin(angle);
 559                 cp = cos(angle);
 560                 angle = roll * (M_PI*2 / 360);
 561                 sr = sin(angle);
 562                 cr = cos(angle);
 563 #ifdef MATRIX4x4_OPENGLORIENTATION
 564                 out->m[0][0] = (cp*cy) * scale;
 565                 out->m[1][0] = (sr*sp*cy+cr*-sy) * scale;
 566                 out->m[2][0] = (cr*sp*cy+-sr*-sy) * scale;
 567                 out->m[3][0] = x;
 568                 out->m[0][1] = (cp*sy) * scale;
 569                 out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
 570                 out->m[2][1] = (cr*sp*sy+-sr*cy) * scale;
 571                 out->m[3][1] = y;
 572                 out->m[0][2] = (-sp) * scale;
 573                 out->m[1][2] = (sr*cp) * scale;
 574                 out->m[2][2] = (cr*cp) * scale;
 575                 out->m[3][2] = z;
 576                 out->m[0][3] = 0;
 577                 out->m[1][3] = 0;
 578                 out->m[2][3] = 0;
 579                 out->m[3][3] = 1;
 580 #else
 581                 out->m[0][0] = (cp*cy) * scale;
 582                 out->m[0][1] = (sr*sp*cy+cr*-sy) * scale;
 583                 out->m[0][2] = (cr*sp*cy+-sr*-sy) * scale;
 584                 out->m[0][3] = x;
 585                 out->m[1][0] = (cp*sy) * scale;
 586                 out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
 587                 out->m[1][2] = (cr*sp*sy+-sr*cy) * scale;
 588                 out->m[1][3] = y;
 589                 out->m[2][0] = (-sp) * scale;
 590                 out->m[2][1] = (sr*cp) * scale;
 591                 out->m[2][2] = (cr*cp) * scale;
 592                 out->m[2][3] = z;
 593                 out->m[3][0] = 0;
 594                 out->m[3][1] = 0;
 595                 out->m[3][2] = 0;
 596                 out->m[3][3] = 1;
 597 #endif
 598         }
 599         else if (pitch)
 600         {
 601                 angle = yaw * (M_PI*2 / 360);
 602                 sy = sin(angle);
 603                 cy = cos(angle);
 604                 angle = pitch * (M_PI*2 / 360);
 605                 sp = sin(angle);
 606                 cp = cos(angle);
 607 #ifdef MATRIX4x4_OPENGLORIENTATION
 608                 out->m[0][0] = (cp*cy) * scale;
 609                 out->m[1][0] = (-sy) * scale;
 610                 out->m[2][0] = (sp*cy) * scale;
 611                 out->m[3][0] = x;
 612                 out->m[0][1] = (cp*sy) * scale;
 613                 out->m[1][1] = (cy) * scale;
 614                 out->m[2][1] = (sp*sy) * scale;
 615                 out->m[3][1] = y;
 616                 out->m[0][2] = (-sp) * scale;
 617                 out->m[1][2] = 0;
 618                 out->m[2][2] = (cp) * scale;
 619                 out->m[3][2] = z;
 620                 out->m[0][3] = 0;
 621                 out->m[1][3] = 0;
 622                 out->m[2][3] = 0;
 623                 out->m[3][3] = 1;
 624 #else
 625                 out->m[0][0] = (cp*cy) * scale;
 626                 out->m[0][1] = (-sy) * scale;
 627                 out->m[0][2] = (sp*cy) * scale;
 628                 out->m[0][3] = x;
 629                 out->m[1][0] = (cp*sy) * scale;
 630                 out->m[1][1] = (cy) * scale;
 631                 out->m[1][2] = (sp*sy) * scale;
 632                 out->m[1][3] = y;
 633                 out->m[2][0] = (-sp) * scale;
 634                 out->m[2][1] = 0;
 635                 out->m[2][2] = (cp) * scale;
 636                 out->m[2][3] = z;
 637                 out->m[3][0] = 0;
 638                 out->m[3][1] = 0;
 639                 out->m[3][2] = 0;
 640                 out->m[3][3] = 1;
 641 #endif
 642         }
 643         else if (yaw)
 644         {
 645                 angle = yaw * (M_PI*2 / 360);
 646                 sy = sin(angle);
 647                 cy = cos(angle);
 648 #ifdef MATRIX4x4_OPENGLORIENTATION
 649                 out->m[0][0] = (cy) * scale;
 650                 out->m[1][0] = (-sy) * scale;
 651                 out->m[2][0] = 0;
 652                 out->m[3][0] = x;
 653                 out->m[0][1] = (sy) * scale;
 654                 out->m[1][1] = (cy) * scale;
 655                 out->m[2][1] = 0;
 656                 out->m[3][1] = y;
 657                 out->m[0][2] = 0;
 658                 out->m[1][2] = 0;
 659                 out->m[2][2] = scale;
 660                 out->m[3][2] = z;
 661                 out->m[0][3] = 0;
 662                 out->m[1][3] = 0;
 663                 out->m[2][3] = 0;
 664                 out->m[3][3] = 1;
 665 #else
 666                 out->m[0][0] = (cy) * scale;
 667                 out->m[0][1] = (-sy) * scale;
 668                 out->m[0][2] = 0;
 669                 out->m[0][3] = x;
 670                 out->m[1][0] = (sy) * scale;
 671                 out->m[1][1] = (cy) * scale;
 672                 out->m[1][2] = 0;
 673                 out->m[1][3] = y;
 674                 out->m[2][0] = 0;
 675                 out->m[2][1] = 0;
 676                 out->m[2][2] = scale;
 677                 out->m[2][3] = z;
 678                 out->m[3][0] = 0;
 679                 out->m[3][1] = 0;
 680                 out->m[3][2] = 0;
 681                 out->m[3][3] = 1;
 682 #endif
 683         }
 684         else
 685         {
 686 #ifdef MATRIX4x4_OPENGLORIENTATION
 687                 out->m[0][0] = scale;
 688                 out->m[1][0] = 0;
 689                 out->m[2][0] = 0;
 690                 out->m[3][0] = x;
 691                 out->m[0][1] = 0;
 692                 out->m[1][1] = scale;
 693                 out->m[2][1] = 0;
 694                 out->m[3][1] = y;
 695                 out->m[0][2] = 0;
 696                 out->m[1][2] = 0;
 697                 out->m[2][2] = scale;
 698                 out->m[3][2] = z;
 699                 out->m[0][3] = 0;
 700                 out->m[1][3] = 0;
 701                 out->m[2][3] = 0;
 702                 out->m[3][3] = 1;
 703 #else
 704                 out->m[0][0] = scale;
 705                 out->m[0][1] = 0;
 706                 out->m[0][2] = 0;
 707                 out->m[0][3] = x;
 708                 out->m[1][0] = 0;
 709                 out->m[1][1] = scale;
 710                 out->m[1][2] = 0;
 711                 out->m[1][3] = y;
 712                 out->m[2][0] = 0;
 713                 out->m[2][1] = 0;
 714                 out->m[2][2] = scale;
 715                 out->m[2][3] = z;
 716                 out->m[3][0] = 0;
 717                 out->m[3][1] = 0;
 718                 out->m[3][2] = 0;
 719                 out->m[3][3] = 1;
 720 #endif
 721         }
 722 }
 723
 724 void Matrix4x4_ToVectors(const matrix4x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
 725 {
 726 #ifdef MATRIX4x4_OPENGLORIENTATION
 727         vx[0] = in->m[0][0];
 728         vx[1] = in->m[0][1];
 729         vx[2] = in->m[0][2];
 730         vy[0] = in->m[1][0];
 731         vy[1] = in->m[1][1];
 732         vy[2] = in->m[1][2];
 733         vz[0] = in->m[2][0];
 734         vz[1] = in->m[2][1];
 735         vz[2] = in->m[2][2];
 736         t [0] = in->m[3][0];
 737         t [1] = in->m[3][1];
 738         t [2] = in->m[3][2];
 739 #else
 740         vx[0] = in->m[0][0];
 741         vx[1] = in->m[1][0];
 742         vx[2] = in->m[2][0];
 743         vy[0] = in->m[0][1];
 744         vy[1] = in->m[1][1];
 745         vy[2] = in->m[2][1];
 746         vz[0] = in->m[0][2];
 747         vz[1] = in->m[1][2];
 748         vz[2] = in->m[2][2];
 749         t [0] = in->m[0][3];
 750         t [1] = in->m[1][3];
 751         t [2] = in->m[2][3];
 752 #endif
 753 }
 754
 755 void Matrix4x4_FromVectors(matrix4x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
 756 {
 757 #ifdef MATRIX4x4_OPENGLORIENTATION
 758         out->m[0][0] = vx[0];
 759         out->m[1][0] = vy[0];
 760         out->m[2][0] = vz[0];
 761         out->m[3][0] = t[0];
 762         out->m[0][1] = vx[1];
 763         out->m[1][1] = vy[1];
 764         out->m[2][1] = vz[1];
 765         out->m[3][1] = t[1];
 766         out->m[0][2] = vx[2];
 767         out->m[1][2] = vy[2];
 768         out->m[2][2] = vz[2];
 769         out->m[3][2] = t[2];
 770         out->m[0][3] = 0.0f;
 771         out->m[1][3] = 0.0f;
 772         out->m[2][3] = 0.0f;
 773         out->m[3][3] = 1.0f;
 774 #else
 775         out->m[0][0] = vx[0];
 776         out->m[0][1] = vy[0];
 777         out->m[0][2] = vz[0];
 778         out->m[0][3] = t[0];
 779         out->m[1][0] = vx[1];
 780         out->m[1][1] = vy[1];
 781         out->m[1][2] = vz[1];
 782         out->m[1][3] = t[1];
 783         out->m[2][0] = vx[2];
 784         out->m[2][1] = vy[2];
 785         out->m[2][2] = vz[2];
 786         out->m[2][3] = t[2];
 787         out->m[3][0] = 0.0f;
 788         out->m[3][1] = 0.0f;
 789         out->m[3][2] = 0.0f;
 790         out->m[3][3] = 1.0f;
 791 #endif
 792 }
 793
 794 void Matrix4x4_ToArrayDoubleGL(const matrix4x4_t *in, double out[16])
 795 {
 796 #ifdef MATRIX4x4_OPENGLORIENTATION
 797         out[ 0] = in->m[0][0];
 798         out[ 1] = in->m[0][1];
 799         out[ 2] = in->m[0][2];
 800         out[ 3] = in->m[0][3];
 801         out[ 4] = in->m[1][0];
 802         out[ 5] = in->m[1][1];
 803         out[ 6] = in->m[1][2];
 804         out[ 7] = in->m[1][3];
 805         out[ 8] = in->m[2][0];
 806         out[ 9] = in->m[2][1];
 807         out[10] = in->m[2][2];
 808         out[11] = in->m[2][3];
 809         out[12] = in->m[3][0];
 810         out[13] = in->m[3][1];
 811         out[14] = in->m[3][2];
 812         out[15] = in->m[3][3];
 813 #else
 814         out[ 0] = in->m[0][0];
 815         out[ 1] = in->m[1][0];
 816         out[ 2] = in->m[2][0];
 817         out[ 3] = in->m[3][0];
 818         out[ 4] = in->m[0][1];
 819         out[ 5] = in->m[1][1];
 820         out[ 6] = in->m[2][1];
 821         out[ 7] = in->m[3][1];
 822         out[ 8] = in->m[0][2];
 823         out[ 9] = in->m[1][2];
 824         out[10] = in->m[2][2];
 825         out[11] = in->m[3][2];
 826         out[12] = in->m[0][3];
 827         out[13] = in->m[1][3];
 828         out[14] = in->m[2][3];
 829         out[15] = in->m[3][3];
 830 #endif
 831 }
 832
 833 void Matrix4x4_FromArrayDoubleGL (matrix4x4_t *out, const double in[16])
 834 {
 835 #ifdef MATRIX4x4_OPENGLORIENTATION
 836         out->m[0][0] = in[0];
 837         out->m[0][1] = in[1];
 838         out->m[0][2] = in[2];
 839         out->m[0][3] = in[3];
 840         out->m[1][0] = in[4];
 841         out->m[1][1] = in[5];
 842         out->m[1][2] = in[6];
 843         out->m[1][3] = in[7];
 844         out->m[2][0] = in[8];
 845         out->m[2][1] = in[9];
 846         out->m[2][2] = in[10];
 847         out->m[2][3] = in[11];
 848         out->m[3][0] = in[12];
 849         out->m[3][1] = in[13];
 850         out->m[3][2] = in[14];
 851         out->m[3][3] = in[15];
 852 #else
 853         out->m[0][0] = in[0];
 854         out->m[1][0] = in[1];
 855         out->m[2][0] = in[2];
 856         out->m[3][0] = in[3];
 857         out->m[0][1] = in[4];
 858         out->m[1][1] = in[5];
 859         out->m[2][1] = in[6];
 860         out->m[3][1] = in[7];
 861         out->m[0][2] = in[8];
 862         out->m[1][2] = in[9];
 863         out->m[2][2] = in[10];
 864         out->m[3][2] = in[11];
 865         out->m[0][3] = in[12];
 866         out->m[1][3] = in[13];
 867         out->m[2][3] = in[14];
 868         out->m[3][3] = in[15];
 869 #endif
 870 }
 871
 872 void Matrix4x4_ToArrayDoubleD3D(const matrix4x4_t *in, double out[16])
 873 {
 874 #ifdef MATRIX4x4_OPENGLORIENTATION
 875         out[ 0] = in->m[0][0];
 876         out[ 1] = in->m[1][0];
 877         out[ 2] = in->m[2][0];
 878         out[ 3] = in->m[3][0];
 879         out[ 4] = in->m[0][1];
 880         out[ 5] = in->m[1][1];
 881         out[ 6] = in->m[2][1];
 882         out[ 7] = in->m[3][1];
 883         out[ 8] = in->m[0][2];
 884         out[ 9] = in->m[1][2];
 885         out[10] = in->m[2][2];
 886         out[11] = in->m[3][2];
 887         out[12] = in->m[0][3];
 888         out[13] = in->m[1][3];
 889         out[14] = in->m[2][3];
 890         out[15] = in->m[3][3];
 891 #else
 892         out[ 0] = in->m[0][0];
 893         out[ 1] = in->m[0][1];
 894         out[ 2] = in->m[0][2];
 895         out[ 3] = in->m[0][3];
 896         out[ 4] = in->m[1][0];
 897         out[ 5] = in->m[1][1];
 898         out[ 6] = in->m[1][2];
 899         out[ 7] = in->m[1][3];
 900         out[ 8] = in->m[2][0];
 901         out[ 9] = in->m[2][1];
 902         out[10] = in->m[2][2];
 903         out[11] = in->m[2][3];
 904         out[12] = in->m[3][0];
 905         out[13] = in->m[3][1];
 906         out[14] = in->m[3][2];
 907         out[15] = in->m[3][3];
 908 #endif
 909 }
 910
 911 void Matrix4x4_FromArrayDoubleD3D (matrix4x4_t *out, const double in[16])
 912 {
 913 #ifdef MATRIX4x4_OPENGLORIENTATION
 914         out->m[0][0] = in[0];
 915         out->m[1][0] = in[1];
 916         out->m[2][0] = in[2];
 917         out->m[3][0] = in[3];
 918         out->m[0][1] = in[4];
 919         out->m[1][1] = in[5];
 920         out->m[2][1] = in[6];
 921         out->m[3][1] = in[7];
 922         out->m[0][2] = in[8];
 923         out->m[1][2] = in[9];
 924         out->m[2][2] = in[10];
 925         out->m[3][2] = in[11];
 926         out->m[0][3] = in[12];
 927         out->m[1][3] = in[13];
 928         out->m[2][3] = in[14];
 929         out->m[3][3] = in[15];
 930 #else
 931         out->m[0][0] = in[0];
 932         out->m[0][1] = in[1];
 933         out->m[0][2] = in[2];
 934         out->m[0][3] = in[3];
 935         out->m[1][0] = in[4];
 936         out->m[1][1] = in[5];
 937         out->m[1][2] = in[6];
 938         out->m[1][3] = in[7];
 939         out->m[2][0] = in[8];
 940         out->m[2][1] = in[9];
 941         out->m[2][2] = in[10];
 942         out->m[2][3] = in[11];
 943         out->m[3][0] = in[12];
 944         out->m[3][1] = in[13];
 945         out->m[3][2] = in[14];
 946         out->m[3][3] = in[15];
 947 #endif
 948 }
 949
 950 void Matrix4x4_ToArray12FloatGL(const matrix4x4_t *in, float out[12])
 951 {
 952 #ifdef MATRIX4x4_OPENGLORIENTATION
 953         out[ 0] = in->m[0][0];
 954         out[ 1] = in->m[0][1];
 955         out[ 2] = in->m[0][2];
 956         out[ 3] = in->m[1][0];
 957         out[ 4] = in->m[1][1];
 958         out[ 5] = in->m[1][2];
 959         out[ 6] = in->m[2][0];
 960         out[ 7] = in->m[2][1];
 961         out[ 8] = in->m[2][2];
 962         out[ 9] = in->m[3][0];
 963         out[10] = in->m[3][1];
 964         out[11] = in->m[3][2];
 965 #else
 966         out[ 0] = in->m[0][0];
 967         out[ 1] = in->m[1][0];
 968         out[ 2] = in->m[2][0];
 969         out[ 3] = in->m[0][1];
 970         out[ 4] = in->m[1][1];
 971         out[ 5] = in->m[2][1];
 972         out[ 6] = in->m[0][2];
 973         out[ 7] = in->m[1][2];
 974         out[ 8] = in->m[2][2];
 975         out[ 9] = in->m[0][3];
 976         out[10] = in->m[1][3];
 977         out[11] = in->m[2][3];
 978 #endif
 979 }
 980
 981 void Matrix4x4_FromArray12FloatGL(matrix4x4_t *out, const float in[12])
 982 {
 983 #ifdef MATRIX4x4_OPENGLORIENTATION
 984         out->m[0][0] = in[0];
 985         out->m[0][1] = in[1];
 986         out->m[0][2] = in[2];
 987         out->m[0][3] = 0;
 988         out->m[1][0] = in[3];
 989         out->m[1][1] = in[4];
 990         out->m[1][2] = in[5];
 991         out->m[1][3] = 0;
 992         out->m[2][0] = in[6];
 993         out->m[2][1] = in[7];
 994         out->m[2][2] = in[8];
 995         out->m[2][3] = 0;
 996         out->m[3][0] = in[9];
 997         out->m[3][1] = in[10];
 998         out->m[3][2] = in[11];
 999         out->m[3][3] = 1;
1000 #else
1001         out->m[0][0] = in[0];
1002         out->m[1][0] = in[1];
1003         out->m[2][0] = in[2];
1004         out->m[3][0] = 0;
1005         out->m[0][1] = in[3];
1006         out->m[1][1] = in[4];
1007         out->m[2][1] = in[5];
1008         out->m[3][1] = 0;
1009         out->m[0][2] = in[6];
1010         out->m[1][2] = in[7];
1011         out->m[2][2] = in[8];
1012         out->m[3][2] = 0;
1013         out->m[0][3] = in[9];
1014         out->m[1][3] = in[10];
1015         out->m[2][3] = in[11];
1016         out->m[3][3] = 1;
1017 #endif
1018 }
1019
1020 void Matrix4x4_ToArray12FloatD3D(const matrix4x4_t *in, float out[12])
1021 {
1022 #ifdef MATRIX4x4_OPENGLORIENTATION
1023         out[ 0] = in->m[0][0];
1024         out[ 1] = in->m[1][0];
1025         out[ 2] = in->m[2][0];
1026         out[ 3] = in->m[3][0];
1027         out[ 4] = in->m[0][1];
1028         out[ 5] = in->m[1][1];
1029         out[ 6] = in->m[2][1];
1030         out[ 7] = in->m[3][1];
1031         out[ 8] = in->m[0][2];
1032         out[ 9] = in->m[1][2];
1033         out[10] = in->m[2][2];
1034         out[11] = in->m[3][2];
1035 #else
1036         out[ 0] = in->m[0][0];
1037         out[ 1] = in->m[0][1];
1038         out[ 2] = in->m[0][2];
1039         out[ 3] = in->m[0][3];
1040         out[ 4] = in->m[1][0];
1041         out[ 5] = in->m[1][1];
1042         out[ 6] = in->m[1][2];
1043         out[ 7] = in->m[1][3];
1044         out[ 8] = in->m[2][0];
1045         out[ 9] = in->m[2][1];
1046         out[10] = in->m[2][2];
1047         out[11] = in->m[2][3];
1048 #endif
1049 }
1050
1051 void Matrix4x4_FromArray12FloatD3D(matrix4x4_t *out, const float in[12])
1052 {
1053 #ifdef MATRIX4x4_OPENGLORIENTATION
1054         out->m[0][0] = in[0];
1055         out->m[1][0] = in[1];
1056         out->m[2][0] = in[2];
1057         out->m[3][0] = in[3];
1058         out->m[0][1] = in[4];
1059         out->m[1][1] = in[5];
1060         out->m[2][1] = in[6];
1061         out->m[3][1] = in[7];
1062         out->m[0][2] = in[8];
1063         out->m[1][2] = in[9];
1064         out->m[2][2] = in[10];
1065         out->m[3][2] = in[11];
1066         out->m[0][3] = 0;
1067         out->m[1][3] = 0;
1068         out->m[2][3] = 0;
1069         out->m[3][3] = 1;
1070 #else
1071         out->m[0][0] = in[0];
1072         out->m[0][1] = in[1];
1073         out->m[0][2] = in[2];
1074         out->m[0][3] = in[3];
1075         out->m[1][0] = in[4];
1076         out->m[1][1] = in[5];
1077         out->m[1][2] = in[6];
1078         out->m[1][3] = in[7];
1079         out->m[2][0] = in[8];
1080         out->m[2][1] = in[9];
1081         out->m[2][2] = in[10];
1082         out->m[2][3] = in[11];
1083         out->m[3][0] = 0;
1084         out->m[3][1] = 0;
1085         out->m[3][2] = 0;
1086         out->m[3][3] = 1;
1087 #endif
1088 }
1089
1090 void Matrix4x4_FromOriginQuat(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z, double w)
1091 {
1092 #ifdef MATRIX4x4_OPENGLORIENTATION
1093         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;
1094         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;
1095         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;
1096         m->m[0][3]=  0          ;m->m[1][3]=  0          ;m->m[2][3]=  0          ;m->m[3][3]=1;
1097 #else
1098         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;
1099         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;
1100         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;
1101         m->m[3][0]=  0          ;m->m[3][1]=  0          ;m->m[3][2]=  0          ;m->m[3][3]=1;
1102 #endif
1103 }
1104
1105 // LordHavoc: I got this code from:
1106 //http://www.doom3world.org/phpbb2/viewtopic.php?t=2884
1107 void Matrix4x4_FromDoom3Joint(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z)
1108 {
1109         double w = 1.0 - (x*x+y*y+z*z);
1110         w = w > 0.0 ? -sqrt(w) : 0.0;
1111 #ifdef MATRIX4x4_OPENGLORIENTATION
1112         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;
1113         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;
1114         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;
1115         m->m[0][3]=  0          ;m->m[1][3]=  0          ;m->m[2][3]=  0          ;m->m[3][3]=1;
1116 #else
1117         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;
1118         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;
1119         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;
1120         m->m[3][0]=  0          ;m->m[3][1]=  0          ;m->m[3][2]=  0          ;m->m[3][3]=1;
1121 #endif
1122 }
1123
1124 void Matrix4x4_Blend (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2, double blend)
1125 {
1126         double iblend = 1 - blend;
1127         out->m[0][0] = in1->m[0][0] * iblend + in2->m[0][0] * blend;
1128         out->m[0][1] = in1->m[0][1] * iblend + in2->m[0][1] * blend;
1129         out->m[0][2] = in1->m[0][2] * iblend + in2->m[0][2] * blend;
1130         out->m[0][3] = in1->m[0][3] * iblend + in2->m[0][3] * blend;
1131         out->m[1][0] = in1->m[1][0] * iblend + in2->m[1][0] * blend;
1132         out->m[1][1] = in1->m[1][1] * iblend + in2->m[1][1] * blend;
1133         out->m[1][2] = in1->m[1][2] * iblend + in2->m[1][2] * blend;
1134         out->m[1][3] = in1->m[1][3] * iblend + in2->m[1][3] * blend;
1135         out->m[2][0] = in1->m[2][0] * iblend + in2->m[2][0] * blend;
1136         out->m[2][1] = in1->m[2][1] * iblend + in2->m[2][1] * blend;
1137         out->m[2][2] = in1->m[2][2] * iblend + in2->m[2][2] * blend;
1138         out->m[2][3] = in1->m[2][3] * iblend + in2->m[2][3] * blend;
1139         out->m[3][0] = in1->m[3][0] * iblend + in2->m[3][0] * blend;
1140         out->m[3][1] = in1->m[3][1] * iblend + in2->m[3][1] * blend;
1141         out->m[3][2] = in1->m[3][2] * iblend + in2->m[3][2] * blend;
1142         out->m[3][3] = in1->m[3][3] * iblend + in2->m[3][3] * blend;
1143 }
1144
1145
1146 void Matrix4x4_Transform (const matrix4x4_t *in, const float v[3], float out[3])
1147 {
1148 #ifdef MATRIX4x4_OPENGLORIENTATION
1149         out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0] + in->m[3][0];
1150         out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1] + in->m[3][1];
1151         out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2] + in->m[3][2];
1152 #else
1153         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
1154         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
1155         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
1156 #endif
1157 }
1158
1159 void Matrix4x4_Transform4 (const matrix4x4_t *in, const float v[4], float out[4])
1160 {
1161 #ifdef MATRIX4x4_OPENGLORIENTATION
1162         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];
1163         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];
1164         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];
1165         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];
1166 #else
1167         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];
1168         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];
1169         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];
1170         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];
1171 #endif
1172 }
1173
1174 void Matrix4x4_Transform3x3 (const matrix4x4_t *in, const float v[3], float out[3])
1175 {
1176 #ifdef MATRIX4x4_OPENGLORIENTATION
1177         out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0];
1178         out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1];
1179         out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2];
1180 #else
1181         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2];
1182         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2];
1183         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2];
1184 #endif
1185 }
1186
1187 /*
1188 void Matrix4x4_SimpleUntransform (const matrix4x4_t *in, const float v[3], float out[3])
1189 {
1190         double t[3];
1191 #ifdef MATRIX4x4_OPENGLORIENTATION
1192         t[0] = v[0] - in->m[3][0];
1193         t[1] = v[1] - in->m[3][1];
1194         t[2] = v[2] - in->m[3][2];
1195         out[0] = t[0] * in->m[0][0] + t[1] * in->m[0][1] + t[2] * in->m[0][2];
1196         out[1] = t[0] * in->m[1][0] + t[1] * in->m[1][1] + t[2] * in->m[1][2];
1197         out[2] = t[0] * in->m[2][0] + t[1] * in->m[2][1] + t[2] * in->m[2][2];
1198 #else
1199         t[0] = v[0] - in->m[0][3];
1200         t[1] = v[1] - in->m[1][3];
1201         t[2] = v[2] - in->m[2][3];
1202         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
1203         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
1204         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
1205 #endif
1206 }
1207 */
1208
1209 // FIXME: optimize
1210 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, double x, double y, double z)
1211 {
1212         matrix4x4_t base, temp;
1213         base = *out;
1214         Matrix4x4_CreateTranslate(&temp, x, y, z);
1215         Matrix4x4_Concat(out, &base, &temp);
1216 }
1217
1218 // FIXME: optimize
1219 void Matrix4x4_ConcatRotate (matrix4x4_t *out, double angle, double x, double y, double z)
1220 {
1221         matrix4x4_t base, temp;
1222         base = *out;
1223         Matrix4x4_CreateRotate(&temp, angle, x, y, z);
1224         Matrix4x4_Concat(out, &base, &temp);
1225 }
1226
1227 // FIXME: optimize
1228 void Matrix4x4_ConcatScale (matrix4x4_t *out, double x)
1229 {
1230         matrix4x4_t base, temp;
1231         base = *out;
1232         Matrix4x4_CreateScale(&temp, x);
1233         Matrix4x4_Concat(out, &base, &temp);
1234 }
1235
1236 // FIXME: optimize
1237 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, double x, double y, double z)
1238 {
1239         matrix4x4_t base, temp;
1240         base = *out;
1241         Matrix4x4_CreateScale3(&temp, x, y, z);
1242         Matrix4x4_Concat(out, &base, &temp);
1243 }
1244
1245 void Matrix4x4_OriginFromMatrix (const matrix4x4_t *in, float *out)
1246 {
1247 #ifdef MATRIX4x4_OPENGLORIENTATION
1248         out[0] = in->m[3][0];
1249         out[1] = in->m[3][1];
1250         out[2] = in->m[3][2];
1251 #else
1252         out[0] = in->m[0][3];
1253         out[1] = in->m[1][3];
1254         out[2] = in->m[2][3];
1255 #endif
1256 }
1257
1258 double Matrix4x4_ScaleFromMatrix (const matrix4x4_t *in)
1259 {
1260         // we only support uniform scaling, so assume the first row is enough
1261         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]);
1262 }
1263
1264 void Matrix4x4_SetOrigin (matrix4x4_t *out, double x, double y, double z)
1265 {
1266 #ifdef MATRIX4x4_OPENGLORIENTATION
1267         out->m[3][0] = x;
1268         out->m[3][1] = y;
1269         out->m[3][2] = z;
1270 #else
1271         out->m[0][3] = x;
1272         out->m[1][3] = y;
1273         out->m[2][3] = z;
1274 #endif
1275 }
1276
1277 void Matrix4x4_AdjustOrigin (matrix4x4_t *out, double x, double y, double z)
1278 {
1279 #ifdef MATRIX4x4_OPENGLORIENTATION
1280         out->m[3][0] += x;
1281         out->m[3][1] += y;
1282         out->m[3][2] += z;
1283 #else
1284         out->m[0][3] += x;
1285         out->m[1][3] += y;
1286         out->m[2][3] += z;
1287 #endif
1288 }
1289
1290 void Matrix4x4_Scale (matrix4x4_t *out, double rotatescale, double originscale)
1291 {
1292         out->m[0][0] *= rotatescale;
1293         out->m[0][1] *= rotatescale;
1294         out->m[0][2] *= rotatescale;
1295         out->m[1][0] *= rotatescale;
1296         out->m[1][1] *= rotatescale;
1297         out->m[1][2] *= rotatescale;
1298         out->m[2][0] *= rotatescale;
1299         out->m[2][1] *= rotatescale;
1300         out->m[2][2] *= rotatescale;
1301 #ifdef MATRIX4x4_OPENGLORIENTATION
1302         out->m[3][0] *= originscale;
1303         out->m[3][1] *= originscale;
1304         out->m[3][2] *= originscale;
1305 #else
1306         out->m[0][3] *= originscale;
1307         out->m[1][3] *= originscale;
1308         out->m[2][3] *= originscale;
1309 #endif
1310 }