matrixlib.c

   1
   2 #include "quakedef.h"
   3 #include "matrixlib.h"
   4 #include <math.h>
   5
   6 void Matrix4x4_Copy (matrix4x4_t *out, const matrix4x4_t *in)
   7 {
   8         *out = *in;
   9 }
  10
  11 void Matrix4x4_CopyRotateOnly (matrix4x4_t *out, const matrix4x4_t *in)
  12 {
  13         out->m[0][0] = in->m[0][0];
  14         out->m[0][1] = in->m[0][1];
  15         out->m[0][2] = in->m[0][2];
  16         out->m[0][3] = 0.0f;
  17         out->m[1][0] = in->m[1][0];
  18         out->m[1][1] = in->m[1][1];
  19         out->m[1][2] = in->m[1][2];
  20         out->m[1][3] = 0.0f;
  21         out->m[2][0] = in->m[2][0];
  22         out->m[2][1] = in->m[2][1];
  23         out->m[2][2] = in->m[2][2];
  24         out->m[2][3] = 0.0f;
  25         out->m[3][0] = 0.0f;
  26         out->m[3][1] = 0.0f;
  27         out->m[3][2] = 0.0f;
  28         out->m[3][3] = 1.0f;
  29 }
  30
  31 void Matrix4x4_CopyTranslateOnly (matrix4x4_t *out, const matrix4x4_t *in)
  32 {
  33         out->m[0][0] = 0.0f;
  34         out->m[0][1] = 0.0f;
  35         out->m[0][2] = 0.0f;
  36         out->m[0][3] = in->m[0][3];
  37         out->m[1][0] = 0.0f;
  38         out->m[1][1] = 0.0f;
  39         out->m[1][2] = 0.0f;
  40         out->m[1][3] = in->m[0][3];
  41         out->m[2][0] = 0.0f;
  42         out->m[2][1] = 0.0f;
  43         out->m[2][2] = 0.0f;
  44         out->m[2][3] = in->m[0][3];
  45         out->m[3][0] = 0.0f;
  46         out->m[3][1] = 0.0f;
  47         out->m[3][2] = 0.0f;
  48         out->m[3][3] = 1.0f;
  49 }
  50
  51 void Matrix4x4_FromMatrix3x4 (matrix4x4_t *out, const matrix3x4_t *in)
  52 {
  53         out->m[0][0] = in->m[0][0];
  54         out->m[0][1] = in->m[0][1];
  55         out->m[0][2] = in->m[0][2];
  56         out->m[0][3] = in->m[0][3];
  57         out->m[1][0] = in->m[1][0];
  58         out->m[1][1] = in->m[1][1];
  59         out->m[1][2] = in->m[1][2];
  60         out->m[1][3] = in->m[1][3];
  61         out->m[2][0] = in->m[2][0];
  62         out->m[2][1] = in->m[2][1];
  63         out->m[2][2] = in->m[2][2];
  64         out->m[2][3] = in->m[2][3];
  65         out->m[3][0] = 0.0f;
  66         out->m[3][1] = 0.0f;
  67         out->m[3][2] = 0.0f;
  68         out->m[3][3] = 1.0f;
  69 }
  70
  71 void Matrix4x4_Concat (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2)
  72 {
  73         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];
  74         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];
  75         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];
  76         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];
  77         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];
  78         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];
  79         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];
  80         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];
  81         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];
  82         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];
  83         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];
  84         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];
  85         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];
  86         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];
  87         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];
  88         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];
  89 }
  90
  91 void Matrix4x4_Transpose (matrix4x4_t *out, const matrix4x4_t *in1)
  92 {
  93         out->m[0][0] = in1->m[0][0];
  94         out->m[0][1] = in1->m[1][0];
  95         out->m[0][2] = in1->m[2][0];
  96         out->m[0][3] = in1->m[3][0];
  97         out->m[1][0] = in1->m[0][1];
  98         out->m[1][1] = in1->m[1][1];
  99         out->m[1][2] = in1->m[2][1];
 100         out->m[1][3] = in1->m[3][1];
 101         out->m[2][0] = in1->m[0][2];
 102         out->m[2][1] = in1->m[1][2];
 103         out->m[2][2] = in1->m[2][2];
 104         out->m[2][3] = in1->m[3][2];
 105         out->m[3][0] = in1->m[0][3];
 106         out->m[3][1] = in1->m[1][3];
 107         out->m[3][2] = in1->m[2][3];
 108         out->m[3][3] = in1->m[3][3];
 109 }
 110
 111 void Matrix4x4_Transpose3x3 (matrix4x4_t *out, const matrix4x4_t *in1)
 112 {
 113         out->m[0][0] = in1->m[0][0];
 114         out->m[0][1] = in1->m[1][0];
 115         out->m[0][2] = in1->m[2][0];
 116         out->m[1][0] = in1->m[0][1];
 117         out->m[1][1] = in1->m[1][1];
 118         out->m[1][2] = in1->m[2][1];
 119         out->m[2][0] = in1->m[0][2];
 120         out->m[2][1] = in1->m[1][2];
 121         out->m[2][2] = in1->m[2][2];
 122
 123         out->m[0][3] = in1->m[0][3];
 124         out->m[1][3] = in1->m[1][3];
 125         out->m[2][3] = in1->m[2][3];
 126         out->m[3][0] = in1->m[0][3];
 127         out->m[3][1] = in1->m[1][3];
 128         out->m[3][2] = in1->m[2][3];
 129         out->m[3][3] = in1->m[3][3];
 130 }
 131
 132 void Matrix4x4_Invert_Simple (matrix4x4_t *out, const matrix4x4_t *in1)
 133 {
 134         // we only support uniform scaling, so assume the first row is enough
 135         // (note the lack of sqrt here, because we're trying to undo the scaling,
 136         // this means multiplying by the inverse scale twice - squaring it, which
 137         // makes the sqrt a waste of time)
 138         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]);
 139
 140         // invert the rotation by transposing and multiplying by the squared
 141         // recipricol of the input matrix scale as described above
 142         out->m[0][0] = in1->m[0][0] * scale;
 143         out->m[0][1] = in1->m[1][0] * scale;
 144         out->m[0][2] = in1->m[2][0] * scale;
 145         out->m[1][0] = in1->m[0][1] * scale;
 146         out->m[1][1] = in1->m[1][1] * scale;
 147         out->m[1][2] = in1->m[2][1] * scale;
 148         out->m[2][0] = in1->m[0][2] * scale;
 149         out->m[2][1] = in1->m[1][2] * scale;
 150         out->m[2][2] = in1->m[2][2] * scale;
 151
 152         // invert the translate
 153         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]);
 154         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]);
 155         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]);
 156
 157         // don't know if there's anything worth doing here
 158         out->m[3][0] = 0;
 159         out->m[3][1] = 0;
 160         out->m[3][2] = 0;
 161         out->m[3][3] = 1;
 162 }
 163
 164 void Matrix4x4_CreateIdentity (matrix4x4_t *out)
 165 {
 166         out->m[0][0]=1.0f;
 167         out->m[0][1]=0.0f;
 168         out->m[0][2]=0.0f;
 169         out->m[0][3]=0.0f;
 170         out->m[1][0]=0.0f;
 171         out->m[1][1]=1.0f;
 172         out->m[1][2]=0.0f;
 173         out->m[1][3]=0.0f;
 174         out->m[2][0]=0.0f;
 175         out->m[2][1]=0.0f;
 176         out->m[2][2]=1.0f;
 177         out->m[2][3]=0.0f;
 178         out->m[3][0]=0.0f;
 179         out->m[3][1]=0.0f;
 180         out->m[3][2]=0.0f;
 181         out->m[3][3]=1.0f;
 182 }
 183
 184 void Matrix4x4_CreateTranslate (matrix4x4_t *out, float x, float y, float z)
 185 {
 186         out->m[0][0]=1.0f;
 187         out->m[0][1]=0.0f;
 188         out->m[0][2]=0.0f;
 189         out->m[0][3]=x;
 190         out->m[1][0]=0.0f;
 191         out->m[1][1]=1.0f;
 192         out->m[1][2]=0.0f;
 193         out->m[1][3]=y;
 194         out->m[2][0]=0.0f;
 195         out->m[2][1]=0.0f;
 196         out->m[2][2]=1.0f;
 197         out->m[2][3]=z;
 198         out->m[3][0]=0.0f;
 199         out->m[3][1]=0.0f;
 200         out->m[3][2]=0.0f;
 201         out->m[3][3]=1.0f;
 202 }
 203
 204 void Matrix4x4_CreateRotate (matrix4x4_t *out, float angle, float x, float y, float z)
 205 {
 206         float len, c, s;
 207
 208         len = x*x+y*y+z*z;
 209         if (len != 0.0f)
 210                 len = 1.0f / sqrt(len);
 211         x *= len;
 212         y *= len;
 213         z *= len;
 214
 215         angle *= M_PI / 180.0;
 216         c = cos(angle);
 217         s = sin(angle);
 218
 219         out->m[0][0]=x * x + c * (1 - x * x);
 220         out->m[0][1]=x * y * (1 - c) + z * s;
 221         out->m[0][2]=z * x * (1 - c) - y * s;
 222         out->m[0][3]=0.0f;
 223         out->m[1][0]=x * y * (1 - c) - z * s;
 224         out->m[1][1]=y * y + c * (1 - y * y);
 225         out->m[1][2]=y * z * (1 - c) + x * s;
 226         out->m[1][3]=0.0f;
 227         out->m[2][0]=z * x * (1 - c) + y * s;
 228         out->m[2][1]=y * z * (1 - c) - x * s;
 229         out->m[2][2]=z * z + c * (1 - z * z);
 230         out->m[2][3]=0.0f;
 231         out->m[3][0]=0.0f;
 232         out->m[3][1]=0.0f;
 233         out->m[3][2]=0.0f;
 234         out->m[3][3]=1.0f;
 235 }
 236
 237 void Matrix4x4_CreateScale (matrix4x4_t *out, float x)
 238 {
 239         out->m[0][0]=x;
 240         out->m[0][1]=0.0f;
 241         out->m[0][2]=0.0f;
 242         out->m[0][3]=0.0f;
 243         out->m[1][0]=0.0f;
 244         out->m[1][1]=x;
 245         out->m[1][2]=0.0f;
 246         out->m[1][3]=0.0f;
 247         out->m[2][0]=0.0f;
 248         out->m[2][1]=0.0f;
 249         out->m[2][2]=x;
 250         out->m[2][3]=0.0f;
 251         out->m[3][0]=0.0f;
 252         out->m[3][1]=0.0f;
 253         out->m[3][2]=0.0f;
 254         out->m[3][3]=1.0f;
 255 }
 256
 257 void Matrix4x4_CreateScale3 (matrix4x4_t *out, float x, float y, float z)
 258 {
 259         out->m[0][0]=x;
 260         out->m[0][1]=0.0f;
 261         out->m[0][2]=0.0f;
 262         out->m[0][3]=0.0f;
 263         out->m[1][0]=0.0f;
 264         out->m[1][1]=y;
 265         out->m[1][2]=0.0f;
 266         out->m[1][3]=0.0f;
 267         out->m[2][0]=0.0f;
 268         out->m[2][1]=0.0f;
 269         out->m[2][2]=z;
 270         out->m[2][3]=0.0f;
 271         out->m[3][0]=0.0f;
 272         out->m[3][1]=0.0f;
 273         out->m[3][2]=0.0f;
 274         out->m[3][3]=1.0f;
 275 }
 276
 277 void Matrix4x4_CreateFromQuakeEntity(matrix4x4_t *out, float x, float y, float z, float pitch, float yaw, float roll, float scale)
 278 {
 279         double angle, sr, sp, sy, cr, cp, cy;
 280
 281         angle = yaw * (M_PI*2 / 360);
 282         sy = sin(angle);
 283         cy = cos(angle);
 284         angle = pitch * (M_PI*2 / 360);
 285         sp = sin(angle);
 286         cp = cos(angle);
 287         angle = roll * (M_PI*2 / 360);
 288         sr = sin(angle);
 289         cr = cos(angle);
 290         out->m[0][0] = cp*cy * scale;
 291         out->m[0][1] = sr*sp*cy+cr*-sy * scale;
 292         out->m[0][2] = cr*sp*cy+-sr*-sy * scale;
 293         out->m[0][3] = x;
 294         out->m[1][0] = cp*sy * scale;
 295         out->m[1][1] = sr*sp*sy+cr*cy * scale;
 296         out->m[1][2] = cr*sp*sy+-sr*cy * scale;
 297         out->m[1][3] = y;
 298         out->m[2][0] = -sp * scale;
 299         out->m[2][1] = sr*cp * scale;
 300         out->m[2][2] = cr*cp * scale;
 301         out->m[2][3] = z;
 302         out->m[3][0] = 0;
 303         out->m[3][1] = 0;
 304         out->m[3][2] = 0;
 305         out->m[3][3] = 1;
 306 }
 307
 308 void Matrix4x4_ToVectors(const matrix4x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
 309 {
 310         vx[0] = in->m[0][0];
 311         vx[1] = in->m[1][0];
 312         vx[2] = in->m[2][0];
 313         vy[0] = in->m[0][1];
 314         vy[1] = in->m[1][1];
 315         vy[2] = in->m[2][1];
 316         vz[0] = in->m[0][2];
 317         vz[1] = in->m[1][2];
 318         vz[2] = in->m[2][2];
 319         t[0] = in->m[0][3];
 320         t[1] = in->m[1][3];
 321         t[2] = in->m[2][3];
 322 }
 323
 324 void Matrix4x4_FromVectors(matrix4x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
 325 {
 326         out->m[0][0] = vx[0];
 327         out->m[0][1] = vy[0];
 328         out->m[0][2] = vz[0];
 329         out->m[0][3] = t[0];
 330         out->m[1][0] = vx[1];
 331         out->m[1][1] = vy[1];
 332         out->m[1][2] = vz[1];
 333         out->m[1][3] = t[1];
 334         out->m[2][0] = vx[2];
 335         out->m[2][1] = vy[2];
 336         out->m[2][2] = vz[2];
 337         out->m[2][3] = t[2];
 338         out->m[3][0] = 0.0f;
 339         out->m[3][1] = 0.0f;
 340         out->m[3][2] = 0.0f;
 341         out->m[3][3] = 1.0f;
 342 }
 343
 344 void Matrix4x4_Transform (const matrix4x4_t *in, const float v[3], float out[3])
 345 {
 346         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
 347         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
 348         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
 349 }
 350
 351 void Matrix4x4_Transform4 (const matrix4x4_t *in, const float v[4], float out[4])
 352 {
 353         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];
 354         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];
 355         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];
 356         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];
 357 }
 358
 359 /*
 360 void Matrix4x4_SimpleUntransform (const matrix4x4_t *in, const float v[3], float out[3])
 361 {
 362         float t[3];
 363         t[0] = v[0] - in->m[0][3];
 364         t[1] = v[1] - in->m[1][3];
 365         t[2] = v[2] - in->m[2][3];
 366         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
 367         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
 368         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
 369 }
 370 */
 371
 372 // FIXME: optimize
 373 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, float x, float y, float z)
 374 {
 375         matrix4x4_t base, temp;
 376         Matrix4x4_Copy(out, &base);
 377         Matrix4x4_CreateTranslate(&temp, x, y, z);
 378         Matrix4x4_Concat(out, &base, &temp);
 379 }
 380
 381 // FIXME: optimize
 382 void Matrix4x4_ConcatRotate (matrix4x4_t *out, float angle, float x, float y, float z)
 383 {
 384         matrix4x4_t base, temp;
 385         Matrix4x4_Copy(out, &base);
 386         Matrix4x4_CreateRotate(&temp, angle, x, y, z);
 387         Matrix4x4_Concat(out, &base, &temp);
 388 }
 389
 390 // FIXME: optimize
 391 void Matrix4x4_ConcatScale (matrix4x4_t *out, float x)
 392 {
 393         matrix4x4_t base, temp;
 394         Matrix4x4_Copy(out, &base);
 395         Matrix4x4_CreateScale(&temp, x);
 396         Matrix4x4_Concat(out, &base, &temp);
 397 }
 398
 399 // FIXME: optimize
 400 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, float x, float y, float z)
 401 {
 402         matrix4x4_t base, temp;
 403         Matrix4x4_Copy(out, &base);
 404         Matrix4x4_CreateScale3(&temp, x, y, z);
 405         Matrix4x4_Concat(out, &base, &temp);
 406 }
 407
 408
 409
 410
 411
 412
 413
 414
 415 void Matrix3x4_Copy (matrix3x4_t *out, const matrix3x4_t *in)
 416 {
 417         *out = *in;
 418 }
 419
 420 void Matrix3x4_CopyRotateOnly (matrix3x4_t *out, const matrix3x4_t *in)
 421 {
 422         out->m[0][0] = in->m[0][0];
 423         out->m[0][1] = in->m[0][1];
 424         out->m[0][2] = in->m[0][2];
 425         out->m[0][3] = 0.0f;
 426         out->m[1][0] = in->m[1][0];
 427         out->m[1][1] = in->m[1][1];
 428         out->m[1][2] = in->m[1][2];
 429         out->m[1][3] = 0.0f;
 430         out->m[2][0] = in->m[2][0];
 431         out->m[2][1] = in->m[2][1];
 432         out->m[2][2] = in->m[2][2];
 433         out->m[2][3] = 0.0f;
 434 }
 435
 436 void Matrix3x4_CopyTranslateOnly (matrix3x4_t *out, const matrix3x4_t *in)
 437 {
 438         out->m[0][0] = 0.0f;
 439         out->m[0][1] = 0.0f;
 440         out->m[0][2] = 0.0f;
 441         out->m[0][3] = in->m[0][3];
 442         out->m[1][0] = 0.0f;
 443         out->m[1][1] = 0.0f;
 444         out->m[1][2] = 0.0f;
 445         out->m[1][3] = in->m[0][3];
 446         out->m[2][0] = 0.0f;
 447         out->m[2][1] = 0.0f;
 448         out->m[2][2] = 0.0f;
 449         out->m[2][3] = in->m[0][3];
 450 }
 451
 452 void Matrix3x4_FromMatrix4x4 (matrix3x4_t *out, const matrix4x4_t *in)
 453 {
 454         out->m[0][0] = in->m[0][0];
 455         out->m[0][1] = in->m[0][1];
 456         out->m[0][2] = in->m[0][2];
 457         out->m[0][3] = in->m[0][3];
 458         out->m[1][0] = in->m[1][0];
 459         out->m[1][1] = in->m[1][1];
 460         out->m[1][2] = in->m[1][2];
 461         out->m[1][3] = in->m[1][3];
 462         out->m[2][0] = in->m[2][0];
 463         out->m[2][1] = in->m[2][1];
 464         out->m[2][2] = in->m[2][2];
 465         out->m[2][3] = in->m[2][3];
 466 }
 467
 468 void Matrix3x4_Concat (matrix3x4_t *out, const matrix3x4_t *in1, const matrix3x4_t *in2)
 469 {
 470         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];
 471         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];
 472         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];
 473         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];
 474         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];
 475         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];
 476         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];
 477         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];
 478         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];
 479         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];
 480         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];
 481         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];
 482 }
 483
 484 void Matrix3x4_Transpose3x3 (matrix3x4_t *out, const matrix3x4_t *in1)
 485 {
 486         out->m[0][0] = in1->m[0][0];
 487         out->m[0][1] = in1->m[1][0];
 488         out->m[0][2] = in1->m[2][0];
 489         out->m[0][3] = 0.0f;
 490         out->m[1][0] = in1->m[0][1];
 491         out->m[1][1] = in1->m[1][1];
 492         out->m[1][2] = in1->m[2][1];
 493         out->m[1][3] = 0.0f;
 494         out->m[2][0] = in1->m[0][2];
 495         out->m[2][1] = in1->m[1][2];
 496         out->m[2][2] = in1->m[2][2];
 497         out->m[2][3] = 0.0f;
 498 }
 499
 500 void Matrix3x4_Invert_Simple (matrix3x4_t *out, const matrix3x4_t *in1)
 501 {
 502         // we only support uniform scaling, so assume the first row is enough
 503         // (note the lack of sqrt here, because we're trying to undo the scaling,
 504         // this means multiplying by the inverse scale twice - squaring it, which
 505         // makes the sqrt a waste of time)
 506         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]);
 507
 508         // invert the rotation by transposing and multiplying by the squared
 509         // recipricol of the input matrix scale as described above
 510         out->m[0][0] = in1->m[0][0] * scale;
 511         out->m[0][1] = in1->m[1][0] * scale;
 512         out->m[0][2] = in1->m[2][0] * scale;
 513         out->m[1][0] = in1->m[0][1] * scale;
 514         out->m[1][1] = in1->m[1][1] * scale;
 515         out->m[1][2] = in1->m[2][1] * scale;
 516         out->m[2][0] = in1->m[0][2] * scale;
 517         out->m[2][1] = in1->m[1][2] * scale;
 518         out->m[2][2] = in1->m[2][2] * scale;
 519
 520         // invert the translate
 521         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]);
 522         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]);
 523         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]);
 524 }
 525
 526
 527 void Matrix3x4_CreateIdentity (matrix3x4_t *out)
 528 {
 529         out->m[0][0]=1.0f;
 530         out->m[0][1]=0.0f;
 531         out->m[0][2]=0.0f;
 532         out->m[0][3]=0.0f;
 533         out->m[1][0]=0.0f;
 534         out->m[1][1]=1.0f;
 535         out->m[1][2]=0.0f;
 536         out->m[1][3]=0.0f;
 537         out->m[2][0]=0.0f;
 538         out->m[2][1]=0.0f;
 539         out->m[2][2]=1.0f;
 540         out->m[2][3]=0.0f;
 541 }
 542
 543 void Matrix3x4_CreateTranslate (matrix3x4_t *out, float x, float y, float z)
 544 {
 545         out->m[0][0]=1.0f;
 546         out->m[0][1]=0.0f;
 547         out->m[0][2]=0.0f;
 548         out->m[0][3]=x;
 549         out->m[1][0]=0.0f;
 550         out->m[1][1]=1.0f;
 551         out->m[1][2]=0.0f;
 552         out->m[1][3]=y;
 553         out->m[2][0]=0.0f;
 554         out->m[2][1]=0.0f;
 555         out->m[2][2]=1.0f;
 556         out->m[2][3]=z;
 557 }
 558
 559 void Matrix3x4_CreateRotate (matrix3x4_t *out, float angle, float x, float y, float z)
 560 {
 561         float len, c, s;
 562
 563         len = x*x+y*y+z*z;
 564         if (len != 0.0f)
 565                 len = 1.0f / sqrt(len);
 566         x *= len;
 567         y *= len;
 568         z *= len;
 569
 570         angle *= M_PI / 180.0;
 571         c = cos(angle);
 572         s = sin(angle);
 573
 574         out->m[0][0]=x * x + c * (1 - x * x);
 575         out->m[0][1]=x * y * (1 - c) + z * s;
 576         out->m[0][2]=z * x * (1 - c) - y * s;
 577         out->m[0][3]=0.0f;
 578         out->m[1][0]=x * y * (1 - c) - z * s;
 579         out->m[1][1]=y * y + c * (1 - y * y);
 580         out->m[1][2]=y * z * (1 - c) + x * s;
 581         out->m[1][3]=0.0f;
 582         out->m[2][0]=z * x * (1 - c) + y * s;
 583         out->m[2][1]=y * z * (1 - c) - x * s;
 584         out->m[2][2]=z * z + c * (1 - z * z);
 585         out->m[2][3]=0.0f;
 586 }
 587
 588 void Matrix3x4_CreateScale (matrix3x4_t *out, float x)
 589 {
 590         out->m[0][0]=x;
 591         out->m[0][1]=0.0f;
 592         out->m[0][2]=0.0f;
 593         out->m[0][3]=0.0f;
 594         out->m[1][0]=0.0f;
 595         out->m[1][1]=x;
 596         out->m[1][2]=0.0f;
 597         out->m[1][3]=0.0f;
 598         out->m[2][0]=0.0f;
 599         out->m[2][1]=0.0f;
 600         out->m[2][2]=x;
 601         out->m[2][3]=0.0f;
 602 }
 603
 604 void Matrix3x4_CreateScale3 (matrix3x4_t *out, float x, float y, float z)
 605 {
 606         out->m[0][0]=x;
 607         out->m[0][1]=0.0f;
 608         out->m[0][2]=0.0f;
 609         out->m[0][3]=0.0f;
 610         out->m[1][0]=0.0f;
 611         out->m[1][1]=y;
 612         out->m[1][2]=0.0f;
 613         out->m[1][3]=0.0f;
 614         out->m[2][0]=0.0f;
 615         out->m[2][1]=0.0f;
 616         out->m[2][2]=z;
 617         out->m[2][3]=0.0f;
 618 }
 619
 620 void Matrix3x4_CreateFromQuakeEntity(matrix3x4_t *out, float x, float y, float z, float pitch, float yaw, float roll, float scale)
 621 {
 622         double angle, sr, sp, sy, cr, cp, cy;
 623
 624         angle = yaw * (M_PI*2 / 360);
 625         sy = sin(angle);
 626         cy = cos(angle);
 627         angle = pitch * (M_PI*2 / 360);
 628         sp = sin(angle);
 629         cp = cos(angle);
 630         angle = roll * (M_PI*2 / 360);
 631         sr = sin(angle);
 632         cr = cos(angle);
 633         out->m[0][0] = cp*cy * scale;
 634         out->m[0][1] = sr*sp*cy+cr*-sy * scale;
 635         out->m[0][2] = cr*sp*cy+-sr*-sy * scale;
 636         out->m[0][3] = x;
 637         out->m[1][0] = cp*sy * scale;
 638         out->m[1][1] = sr*sp*sy+cr*cy * scale;
 639         out->m[1][2] = cr*sp*sy+-sr*cy * scale;
 640         out->m[1][3] = y;
 641         out->m[2][0] = -sp * scale;
 642         out->m[2][1] = sr*cp * scale;
 643         out->m[2][2] = cr*cp * scale;
 644         out->m[2][3] = z;
 645 }
 646
 647 void Matrix3x4_ToVectors(const matrix3x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
 648 {
 649         vx[0] = in->m[0][0];
 650         vx[1] = in->m[1][0];
 651         vx[2] = in->m[2][0];
 652         vy[0] = in->m[0][1];
 653         vy[1] = in->m[1][1];
 654         vy[2] = in->m[2][1];
 655         vz[0] = in->m[0][2];
 656         vz[1] = in->m[1][2];
 657         vz[2] = in->m[2][2];
 658         t[0] = in->m[0][3];
 659         t[1] = in->m[1][3];
 660         t[2] = in->m[2][3];
 661 }
 662
 663 void Matrix3x4_FromVectors(matrix3x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
 664 {
 665         out->m[0][0] = vx[0];
 666         out->m[0][1] = vy[0];
 667         out->m[0][2] = vz[0];
 668         out->m[0][3] = t[0];
 669         out->m[1][0] = vx[1];
 670         out->m[1][1] = vy[1];
 671         out->m[1][2] = vz[1];
 672         out->m[1][3] = t[1];
 673         out->m[2][0] = vx[2];
 674         out->m[2][1] = vy[2];
 675         out->m[2][2] = vz[2];
 676         out->m[2][3] = t[2];
 677 }
 678
 679 void Matrix3x4_Transform (const matrix3x4_t *in, const float v[3], float out[3])
 680 {
 681         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
 682         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
 683         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
 684 }
 685
 686 void Matrix3x4_SimpleUntransform (const matrix3x4_t *in, const float v[3], float out[3])
 687 {
 688         float t[3];
 689         t[0] = v[0] - in->m[0][3];
 690         t[1] = v[1] - in->m[1][3];
 691         t[2] = v[2] - in->m[2][3];
 692         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
 693         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
 694         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
 695 }
 696
 697 // FIXME: optimize
 698 void Matrix3x4_ConcatTranslate (matrix3x4_t *out, float x, float y, float z)
 699 {
 700         matrix3x4_t base, temp;
 701         Matrix3x4_Copy(out, &base);
 702         Matrix3x4_CreateTranslate(&temp, x, y, z);
 703         Matrix3x4_Concat(out, &base, &temp);
 704 }
 705
 706 // FIXME: optimize
 707 void Matrix3x4_ConcatRotate (matrix3x4_t *out, float angle, float x, float y, float z)
 708 {
 709         matrix3x4_t base, temp;
 710         Matrix3x4_Copy(out, &base);
 711         Matrix3x4_CreateRotate(&temp, angle, x, y, z);
 712         Matrix3x4_Concat(out, &base, &temp);
 713 }
 714
 715 // FIXME: optimize
 716 void Matrix3x4_ConcatScale (matrix3x4_t *out, float x)
 717 {
 718         matrix3x4_t base, temp;
 719         Matrix3x4_Copy(out, &base);
 720         Matrix3x4_CreateScale(&temp, x);
 721         Matrix3x4_Concat(out, &base, &temp);
 722 }
 723
 724 // FIXME: optimize
 725 void Matrix3x4_ConcatScale3 (matrix3x4_t *out, float x, float y, float z)
 726 {
 727         matrix3x4_t base, temp;
 728         Matrix3x4_Copy(out, &base);
 729         Matrix3x4_CreateScale3(&temp, x, y, z);
 730         Matrix3x4_Concat(out, &base, &temp);
 731 }