matrixlib.c

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