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