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 void Matrix4x4_Transform3x3 (const matrix4x4_t *in, const float v[3], float out[3])
 368 {
 369         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2];
 370         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2];
 371         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2];
 372 }
 373
 374 /*
 375 void Matrix4x4_SimpleUntransform (const matrix4x4_t *in, const float v[3], float out[3])
 376 {
 377         float t[3];
 378         t[0] = v[0] - in->m[0][3];
 379         t[1] = v[1] - in->m[1][3];
 380         t[2] = v[2] - in->m[2][3];
 381         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
 382         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
 383         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
 384 }
 385 */
 386
 387 // FIXME: optimize
 388 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, float x, float y, float z)
 389 {
 390         matrix4x4_t base, temp;
 391         base = *out;
 392         Matrix4x4_CreateTranslate(&temp, x, y, z);
 393         Matrix4x4_Concat(out, &base, &temp);
 394 }
 395
 396 // FIXME: optimize
 397 void Matrix4x4_ConcatRotate (matrix4x4_t *out, float angle, float x, float y, float z)
 398 {
 399         matrix4x4_t base, temp;
 400         base = *out;
 401         Matrix4x4_CreateRotate(&temp, angle, x, y, z);
 402         Matrix4x4_Concat(out, &base, &temp);
 403 }
 404
 405 // FIXME: optimize
 406 void Matrix4x4_ConcatScale (matrix4x4_t *out, float x)
 407 {
 408         matrix4x4_t base, temp;
 409         base = *out;
 410         Matrix4x4_CreateScale(&temp, x);
 411         Matrix4x4_Concat(out, &base, &temp);
 412 }
 413
 414 // FIXME: optimize
 415 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, float x, float y, float z)
 416 {
 417         matrix4x4_t base, temp;
 418         base = *out;
 419         Matrix4x4_CreateScale3(&temp, x, y, z);
 420         Matrix4x4_Concat(out, &base, &temp);
 421 }
 422
 423 void Matrix4x4_Print (const matrix4x4_t *in)
 424 {
 425         Con_Printf("%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n"
 426         , in->m[0][0], in->m[0][1], in->m[0][2], in->m[0][3]
 427         , in->m[1][0], in->m[1][1], in->m[1][2], in->m[1][3]
 428         , in->m[2][0], in->m[2][1], in->m[2][2], in->m[2][3]
 429         , in->m[3][0], in->m[3][1], in->m[3][2], in->m[3][3]);
 430 }
 431
 432
 433
 434
 435
 436
 437
 438 void Matrix3x4_Copy (matrix3x4_t *out, const matrix3x4_t *in)
 439 {
 440         *out = *in;
 441 }
 442
 443 void Matrix3x4_CopyRotateOnly (matrix3x4_t *out, const matrix3x4_t *in)
 444 {
 445         out->m[0][0] = in->m[0][0];
 446         out->m[0][1] = in->m[0][1];
 447         out->m[0][2] = in->m[0][2];
 448         out->m[0][3] = 0.0f;
 449         out->m[1][0] = in->m[1][0];
 450         out->m[1][1] = in->m[1][1];
 451         out->m[1][2] = in->m[1][2];
 452         out->m[1][3] = 0.0f;
 453         out->m[2][0] = in->m[2][0];
 454         out->m[2][1] = in->m[2][1];
 455         out->m[2][2] = in->m[2][2];
 456         out->m[2][3] = 0.0f;
 457 }
 458
 459 void Matrix3x4_CopyTranslateOnly (matrix3x4_t *out, const matrix3x4_t *in)
 460 {
 461         out->m[0][0] = 0.0f;
 462         out->m[0][1] = 0.0f;
 463         out->m[0][2] = 0.0f;
 464         out->m[0][3] = in->m[0][3];
 465         out->m[1][0] = 0.0f;
 466         out->m[1][1] = 0.0f;
 467         out->m[1][2] = 0.0f;
 468         out->m[1][3] = in->m[0][3];
 469         out->m[2][0] = 0.0f;
 470         out->m[2][1] = 0.0f;
 471         out->m[2][2] = 0.0f;
 472         out->m[2][3] = in->m[0][3];
 473 }
 474
 475 void Matrix3x4_FromMatrix4x4 (matrix3x4_t *out, const matrix4x4_t *in)
 476 {
 477         out->m[0][0] = in->m[0][0];
 478         out->m[0][1] = in->m[0][1];
 479         out->m[0][2] = in->m[0][2];
 480         out->m[0][3] = in->m[0][3];
 481         out->m[1][0] = in->m[1][0];
 482         out->m[1][1] = in->m[1][1];
 483         out->m[1][2] = in->m[1][2];
 484         out->m[1][3] = in->m[1][3];
 485         out->m[2][0] = in->m[2][0];
 486         out->m[2][1] = in->m[2][1];
 487         out->m[2][2] = in->m[2][2];
 488         out->m[2][3] = in->m[2][3];
 489 }
 490
 491 void Matrix3x4_Concat (matrix3x4_t *out, const matrix3x4_t *in1, const matrix3x4_t *in2)
 492 {
 493         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];
 494         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];
 495         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];
 496         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];
 497         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];
 498         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];
 499         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];
 500         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];
 501         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];
 502         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];
 503         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];
 504         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];
 505 }
 506
 507 void Matrix3x4_Transpose3x3 (matrix3x4_t *out, const matrix3x4_t *in1)
 508 {
 509         out->m[0][0] = in1->m[0][0];
 510         out->m[0][1] = in1->m[1][0];
 511         out->m[0][2] = in1->m[2][0];
 512         out->m[0][3] = 0.0f;
 513         out->m[1][0] = in1->m[0][1];
 514         out->m[1][1] = in1->m[1][1];
 515         out->m[1][2] = in1->m[2][1];
 516         out->m[1][3] = 0.0f;
 517         out->m[2][0] = in1->m[0][2];
 518         out->m[2][1] = in1->m[1][2];
 519         out->m[2][2] = in1->m[2][2];
 520         out->m[2][3] = 0.0f;
 521 }
 522
 523 void Matrix3x4_Invert_Simple (matrix3x4_t *out, const matrix3x4_t *in1)
 524 {
 525         // we only support uniform scaling, so assume the first row is enough
 526         // (note the lack of sqrt here, because we're trying to undo the scaling,
 527         // this means multiplying by the inverse scale twice - squaring it, which
 528         // makes the sqrt a waste of time)
 529 #if 1
 530         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]);
 531 #else
 532         double scale = 3.0 / sqrt
 533                  (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]
 534                 + in1->m[1][0] * in1->m[1][0] + in1->m[1][1] * in1->m[1][1] + in1->m[1][2] * in1->m[1][2]
 535                 + in1->m[2][0] * in1->m[2][0] + in1->m[2][1] * in1->m[2][1] + in1->m[2][2] * in1->m[2][2]);
 536         scale *= scale;
 537 #endif
 538
 539         // invert the rotation by transposing and multiplying by the squared
 540         // recipricol of the input matrix scale as described above
 541         out->m[0][0] = in1->m[0][0] * scale;
 542         out->m[0][1] = in1->m[1][0] * scale;
 543         out->m[0][2] = in1->m[2][0] * scale;
 544         out->m[1][0] = in1->m[0][1] * scale;
 545         out->m[1][1] = in1->m[1][1] * scale;
 546         out->m[1][2] = in1->m[2][1] * scale;
 547         out->m[2][0] = in1->m[0][2] * scale;
 548         out->m[2][1] = in1->m[1][2] * scale;
 549         out->m[2][2] = in1->m[2][2] * scale;
 550
 551         // invert the translate
 552         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]);
 553         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]);
 554         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]);
 555 }
 556
 557
 558 void Matrix3x4_CreateIdentity (matrix3x4_t *out)
 559 {
 560         out->m[0][0]=1.0f;
 561         out->m[0][1]=0.0f;
 562         out->m[0][2]=0.0f;
 563         out->m[0][3]=0.0f;
 564         out->m[1][0]=0.0f;
 565         out->m[1][1]=1.0f;
 566         out->m[1][2]=0.0f;
 567         out->m[1][3]=0.0f;
 568         out->m[2][0]=0.0f;
 569         out->m[2][1]=0.0f;
 570         out->m[2][2]=1.0f;
 571         out->m[2][3]=0.0f;
 572 }
 573
 574 void Matrix3x4_CreateTranslate (matrix3x4_t *out, float x, float y, float z)
 575 {
 576         out->m[0][0]=1.0f;
 577         out->m[0][1]=0.0f;
 578         out->m[0][2]=0.0f;
 579         out->m[0][3]=x;
 580         out->m[1][0]=0.0f;
 581         out->m[1][1]=1.0f;
 582         out->m[1][2]=0.0f;
 583         out->m[1][3]=y;
 584         out->m[2][0]=0.0f;
 585         out->m[2][1]=0.0f;
 586         out->m[2][2]=1.0f;
 587         out->m[2][3]=z;
 588 }
 589
 590 void Matrix3x4_CreateRotate (matrix3x4_t *out, float angle, float x, float y, float z)
 591 {
 592         float len, c, s;
 593
 594         len = x*x+y*y+z*z;
 595         if (len != 0.0f)
 596                 len = 1.0f / sqrt(len);
 597         x *= len;
 598         y *= len;
 599         z *= len;
 600
 601         angle *= M_PI / 180.0;
 602         c = cos(angle);
 603         s = sin(angle);
 604
 605         out->m[0][0]=x * x + c * (1 - x * x);
 606         out->m[0][1]=x * y * (1 - c) + z * s;
 607         out->m[0][2]=z * x * (1 - c) - y * s;
 608         out->m[0][3]=0.0f;
 609         out->m[1][0]=x * y * (1 - c) - z * s;
 610         out->m[1][1]=y * y + c * (1 - y * y);
 611         out->m[1][2]=y * z * (1 - c) + x * s;
 612         out->m[1][3]=0.0f;
 613         out->m[2][0]=z * x * (1 - c) + y * s;
 614         out->m[2][1]=y * z * (1 - c) - x * s;
 615         out->m[2][2]=z * z + c * (1 - z * z);
 616         out->m[2][3]=0.0f;
 617 }
 618
 619 void Matrix3x4_CreateScale (matrix3x4_t *out, float x)
 620 {
 621         out->m[0][0]=x;
 622         out->m[0][1]=0.0f;
 623         out->m[0][2]=0.0f;
 624         out->m[0][3]=0.0f;
 625         out->m[1][0]=0.0f;
 626         out->m[1][1]=x;
 627         out->m[1][2]=0.0f;
 628         out->m[1][3]=0.0f;
 629         out->m[2][0]=0.0f;
 630         out->m[2][1]=0.0f;
 631         out->m[2][2]=x;
 632         out->m[2][3]=0.0f;
 633 }
 634
 635 void Matrix3x4_CreateScale3 (matrix3x4_t *out, float x, float y, float z)
 636 {
 637         out->m[0][0]=x;
 638         out->m[0][1]=0.0f;
 639         out->m[0][2]=0.0f;
 640         out->m[0][3]=0.0f;
 641         out->m[1][0]=0.0f;
 642         out->m[1][1]=y;
 643         out->m[1][2]=0.0f;
 644         out->m[1][3]=0.0f;
 645         out->m[2][0]=0.0f;
 646         out->m[2][1]=0.0f;
 647         out->m[2][2]=z;
 648         out->m[2][3]=0.0f;
 649 }
 650
 651 void Matrix3x4_CreateFromQuakeEntity(matrix3x4_t *out, float x, float y, float z, float pitch, float yaw, float roll, float scale)
 652 {
 653         double angle, sr, sp, sy, cr, cp, cy;
 654
 655         angle = yaw * (M_PI*2 / 360);
 656         sy = sin(angle);
 657         cy = cos(angle);
 658         angle = pitch * (M_PI*2 / 360);
 659         sp = sin(angle);
 660         cp = cos(angle);
 661         angle = roll * (M_PI*2 / 360);
 662         sr = sin(angle);
 663         cr = cos(angle);
 664         out->m[0][0] = (cp*cy) * scale;
 665         out->m[0][1] = (sr*sp*cy+cr*-sy) * scale;
 666         out->m[0][2] = (cr*sp*cy+-sr*-sy) * scale;
 667         out->m[0][3] = x;
 668         out->m[1][0] = (cp*sy) * scale;
 669         out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
 670         out->m[1][2] = (cr*sp*sy+-sr*cy) * scale;
 671         out->m[1][3] = y;
 672         out->m[2][0] = (-sp) * scale;
 673         out->m[2][1] = (sr*cp) * scale;
 674         out->m[2][2] = (cr*cp) * scale;
 675         out->m[2][3] = z;
 676 }
 677
 678 void Matrix3x4_ToVectors(const matrix3x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
 679 {
 680         vx[0] = in->m[0][0];
 681         vx[1] = in->m[1][0];
 682         vx[2] = in->m[2][0];
 683         vy[0] = in->m[0][1];
 684         vy[1] = in->m[1][1];
 685         vy[2] = in->m[2][1];
 686         vz[0] = in->m[0][2];
 687         vz[1] = in->m[1][2];
 688         vz[2] = in->m[2][2];
 689         t[0] = in->m[0][3];
 690         t[1] = in->m[1][3];
 691         t[2] = in->m[2][3];
 692 }
 693
 694 void Matrix3x4_FromVectors(matrix3x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
 695 {
 696         out->m[0][0] = vx[0];
 697         out->m[0][1] = vy[0];
 698         out->m[0][2] = vz[0];
 699         out->m[0][3] = t[0];
 700         out->m[1][0] = vx[1];
 701         out->m[1][1] = vy[1];
 702         out->m[1][2] = vz[1];
 703         out->m[1][3] = t[1];
 704         out->m[2][0] = vx[2];
 705         out->m[2][1] = vy[2];
 706         out->m[2][2] = vz[2];
 707         out->m[2][3] = t[2];
 708 }
 709
 710 void Matrix3x4_Transform (const matrix3x4_t *in, const float v[3], float out[3])
 711 {
 712         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
 713         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
 714         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
 715 }
 716
 717 /*
 718 void Matrix3x4_SimpleUntransform (const matrix3x4_t *in, const float v[3], float out[3])
 719 {
 720         float t[3];
 721         t[0] = v[0] - in->m[0][3];
 722         t[1] = v[1] - in->m[1][3];
 723         t[2] = v[2] - in->m[2][3];
 724         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
 725         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
 726         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
 727 }
 728 */
 729
 730 void Matrix3x4_Transform3x3 (const matrix3x4_t *in, const float v[3], float out[3])
 731 {
 732         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
 733         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
 734         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
 735 }
 736
 737
 738 // FIXME: optimize
 739 void Matrix3x4_ConcatTranslate (matrix3x4_t *out, float x, float y, float z)
 740 {
 741         matrix3x4_t base, temp;
 742         base = *out;
 743         Matrix3x4_CreateTranslate(&temp, x, y, z);
 744         Matrix3x4_Concat(out, &base, &temp);
 745 }
 746
 747 // FIXME: optimize
 748 void Matrix3x4_ConcatRotate (matrix3x4_t *out, float angle, float x, float y, float z)
 749 {
 750         matrix3x4_t base, temp;
 751         base = *out;
 752         Matrix3x4_CreateRotate(&temp, angle, x, y, z);
 753         Matrix3x4_Concat(out, &base, &temp);
 754 }
 755
 756 // FIXME: optimize
 757 void Matrix3x4_ConcatScale (matrix3x4_t *out, float x)
 758 {
 759         matrix3x4_t base, temp;
 760         base = *out;
 761         Matrix3x4_CreateScale(&temp, x);
 762         Matrix3x4_Concat(out, &base, &temp);
 763 }
 764
 765 // FIXME: optimize
 766 void Matrix3x4_ConcatScale3 (matrix3x4_t *out, float x, float y, float z)
 767 {
 768         matrix3x4_t base, temp;
 769         base = *out;
 770         Matrix3x4_CreateScale3(&temp, x, y, z);
 771         Matrix3x4_Concat(out, &base, &temp);
 772 }
 773
 774 void Matrix3x4_Print (const matrix3x4_t *in)
 775 {
 776         Con_Printf("%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n"
 777         , in->m[0][0], in->m[0][1], in->m[0][2], in->m[0][3]
 778         , in->m[1][0], in->m[1][1], in->m[1][2], in->m[1][3]
 779         , in->m[2][0], in->m[2][1], in->m[2][2], in->m[2][3]);
 780 }
 781