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 #endif
 146         scale *= scale;
 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         Matrix4x4_Copy(out, &base);
 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         Matrix4x4_Copy(out, &base);
 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         Matrix4x4_Copy(out, &base);
 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         Matrix4x4_Copy(out, &base);
 412         Matrix4x4_CreateScale3(&temp, x, y, z);
 413         Matrix4x4_Concat(out, &base, &temp);
 414 }
 415
 416
 417
 418
 419
 420
 421
 422
 423 void Matrix3x4_Copy (matrix3x4_t *out, const matrix3x4_t *in)
 424 {
 425         *out = *in;
 426 }
 427
 428 void Matrix3x4_CopyRotateOnly (matrix3x4_t *out, const matrix3x4_t *in)
 429 {
 430         out->m[0][0] = in->m[0][0];
 431         out->m[0][1] = in->m[0][1];
 432         out->m[0][2] = in->m[0][2];
 433         out->m[0][3] = 0.0f;
 434         out->m[1][0] = in->m[1][0];
 435         out->m[1][1] = in->m[1][1];
 436         out->m[1][2] = in->m[1][2];
 437         out->m[1][3] = 0.0f;
 438         out->m[2][0] = in->m[2][0];
 439         out->m[2][1] = in->m[2][1];
 440         out->m[2][2] = in->m[2][2];
 441         out->m[2][3] = 0.0f;
 442 }
 443
 444 void Matrix3x4_CopyTranslateOnly (matrix3x4_t *out, const matrix3x4_t *in)
 445 {
 446         out->m[0][0] = 0.0f;
 447         out->m[0][1] = 0.0f;
 448         out->m[0][2] = 0.0f;
 449         out->m[0][3] = in->m[0][3];
 450         out->m[1][0] = 0.0f;
 451         out->m[1][1] = 0.0f;
 452         out->m[1][2] = 0.0f;
 453         out->m[1][3] = in->m[0][3];
 454         out->m[2][0] = 0.0f;
 455         out->m[2][1] = 0.0f;
 456         out->m[2][2] = 0.0f;
 457         out->m[2][3] = in->m[0][3];
 458 }
 459
 460 void Matrix3x4_FromMatrix4x4 (matrix3x4_t *out, const matrix4x4_t *in)
 461 {
 462         out->m[0][0] = in->m[0][0];
 463         out->m[0][1] = in->m[0][1];
 464         out->m[0][2] = in->m[0][2];
 465         out->m[0][3] = in->m[0][3];
 466         out->m[1][0] = in->m[1][0];
 467         out->m[1][1] = in->m[1][1];
 468         out->m[1][2] = in->m[1][2];
 469         out->m[1][3] = in->m[1][3];
 470         out->m[2][0] = in->m[2][0];
 471         out->m[2][1] = in->m[2][1];
 472         out->m[2][2] = in->m[2][2];
 473         out->m[2][3] = in->m[2][3];
 474 }
 475
 476 void Matrix3x4_Concat (matrix3x4_t *out, const matrix3x4_t *in1, const matrix3x4_t *in2)
 477 {
 478         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];
 479         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];
 480         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];
 481         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];
 482         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];
 483         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];
 484         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];
 485         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];
 486         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];
 487         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];
 488         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];
 489         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];
 490 }
 491
 492 void Matrix3x4_Transpose3x3 (matrix3x4_t *out, const matrix3x4_t *in1)
 493 {
 494         out->m[0][0] = in1->m[0][0];
 495         out->m[0][1] = in1->m[1][0];
 496         out->m[0][2] = in1->m[2][0];
 497         out->m[0][3] = 0.0f;
 498         out->m[1][0] = in1->m[0][1];
 499         out->m[1][1] = in1->m[1][1];
 500         out->m[1][2] = in1->m[2][1];
 501         out->m[1][3] = 0.0f;
 502         out->m[2][0] = in1->m[0][2];
 503         out->m[2][1] = in1->m[1][2];
 504         out->m[2][2] = in1->m[2][2];
 505         out->m[2][3] = 0.0f;
 506 }
 507
 508 void Matrix3x4_Invert_Simple (matrix3x4_t *out, const matrix3x4_t *in1)
 509 {
 510         // we only support uniform scaling, so assume the first row is enough
 511         // (note the lack of sqrt here, because we're trying to undo the scaling,
 512         // this means multiplying by the inverse scale twice - squaring it, which
 513         // makes the sqrt a waste of time)
 514 #if 1
 515         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]);
 516 #else
 517         double scale = 3.0 / sqrt
 518                  (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]
 519                 + in1->m[1][0] * in1->m[1][0] + in1->m[1][1] * in1->m[1][1] + in1->m[1][2] * in1->m[1][2]
 520                 + in1->m[2][0] * in1->m[2][0] + in1->m[2][1] * in1->m[2][1] + in1->m[2][2] * in1->m[2][2]);
 521 #endif
 522
 523         // invert the rotation by transposing and multiplying by the squared
 524         // recipricol of the input matrix scale as described above
 525         out->m[0][0] = in1->m[0][0] * scale;
 526         out->m[0][1] = in1->m[1][0] * scale;
 527         out->m[0][2] = in1->m[2][0] * scale;
 528         out->m[1][0] = in1->m[0][1] * scale;
 529         out->m[1][1] = in1->m[1][1] * scale;
 530         out->m[1][2] = in1->m[2][1] * scale;
 531         out->m[2][0] = in1->m[0][2] * scale;
 532         out->m[2][1] = in1->m[1][2] * scale;
 533         out->m[2][2] = in1->m[2][2] * scale;
 534
 535         // invert the translate
 536         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]);
 537         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]);
 538         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]);
 539 }
 540
 541
 542 void Matrix3x4_CreateIdentity (matrix3x4_t *out)
 543 {
 544         out->m[0][0]=1.0f;
 545         out->m[0][1]=0.0f;
 546         out->m[0][2]=0.0f;
 547         out->m[0][3]=0.0f;
 548         out->m[1][0]=0.0f;
 549         out->m[1][1]=1.0f;
 550         out->m[1][2]=0.0f;
 551         out->m[1][3]=0.0f;
 552         out->m[2][0]=0.0f;
 553         out->m[2][1]=0.0f;
 554         out->m[2][2]=1.0f;
 555         out->m[2][3]=0.0f;
 556 }
 557
 558 void Matrix3x4_CreateTranslate (matrix3x4_t *out, float x, float y, float z)
 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]=x;
 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]=y;
 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]=z;
 572 }
 573
 574 void Matrix3x4_CreateRotate (matrix3x4_t *out, float angle, float x, float y, float z)
 575 {
 576         float len, c, s;
 577
 578         len = x*x+y*y+z*z;
 579         if (len != 0.0f)
 580                 len = 1.0f / sqrt(len);
 581         x *= len;
 582         y *= len;
 583         z *= len;
 584
 585         angle *= M_PI / 180.0;
 586         c = cos(angle);
 587         s = sin(angle);
 588
 589         out->m[0][0]=x * x + c * (1 - x * x);
 590         out->m[0][1]=x * y * (1 - c) + z * s;
 591         out->m[0][2]=z * x * (1 - c) - y * s;
 592         out->m[0][3]=0.0f;
 593         out->m[1][0]=x * y * (1 - c) - z * s;
 594         out->m[1][1]=y * y + c * (1 - y * y);
 595         out->m[1][2]=y * z * (1 - c) + x * s;
 596         out->m[1][3]=0.0f;
 597         out->m[2][0]=z * x * (1 - c) + y * s;
 598         out->m[2][1]=y * z * (1 - c) - x * s;
 599         out->m[2][2]=z * z + c * (1 - z * z);
 600         out->m[2][3]=0.0f;
 601 }
 602
 603 void Matrix3x4_CreateScale (matrix3x4_t *out, float x)
 604 {
 605         out->m[0][0]=x;
 606         out->m[0][1]=0.0f;
 607         out->m[0][2]=0.0f;
 608         out->m[0][3]=0.0f;
 609         out->m[1][0]=0.0f;
 610         out->m[1][1]=x;
 611         out->m[1][2]=0.0f;
 612         out->m[1][3]=0.0f;
 613         out->m[2][0]=0.0f;
 614         out->m[2][1]=0.0f;
 615         out->m[2][2]=x;
 616         out->m[2][3]=0.0f;
 617 }
 618
 619 void Matrix3x4_CreateScale3 (matrix3x4_t *out, float x, float y, float z)
 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]=y;
 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]=z;
 632         out->m[2][3]=0.0f;
 633 }
 634
 635 void Matrix3x4_CreateFromQuakeEntity(matrix3x4_t *out, float x, float y, float z, float pitch, float yaw, float roll, float scale)
 636 {
 637         double angle, sr, sp, sy, cr, cp, cy;
 638
 639         angle = yaw * (M_PI*2 / 360);
 640         sy = sin(angle);
 641         cy = cos(angle);
 642         angle = pitch * (M_PI*2 / 360);
 643         sp = sin(angle);
 644         cp = cos(angle);
 645         angle = roll * (M_PI*2 / 360);
 646         sr = sin(angle);
 647         cr = cos(angle);
 648         out->m[0][0] = (cp*cy) * scale;
 649         out->m[0][1] = (sr*sp*cy+cr*-sy) * scale;
 650         out->m[0][2] = (cr*sp*cy+-sr*-sy) * scale;
 651         out->m[0][3] = x;
 652         out->m[1][0] = (cp*sy) * scale;
 653         out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
 654         out->m[1][2] = (cr*sp*sy+-sr*cy) * scale;
 655         out->m[1][3] = y;
 656         out->m[2][0] = (-sp) * scale;
 657         out->m[2][1] = (sr*cp) * scale;
 658         out->m[2][2] = (cr*cp) * scale;
 659         out->m[2][3] = z;
 660 }
 661
 662 void Matrix3x4_ToVectors(const matrix3x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
 663 {
 664         vx[0] = in->m[0][0];
 665         vx[1] = in->m[1][0];
 666         vx[2] = in->m[2][0];
 667         vy[0] = in->m[0][1];
 668         vy[1] = in->m[1][1];
 669         vy[2] = in->m[2][1];
 670         vz[0] = in->m[0][2];
 671         vz[1] = in->m[1][2];
 672         vz[2] = in->m[2][2];
 673         t[0] = in->m[0][3];
 674         t[1] = in->m[1][3];
 675         t[2] = in->m[2][3];
 676 }
 677
 678 void Matrix3x4_FromVectors(matrix3x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
 679 {
 680         out->m[0][0] = vx[0];
 681         out->m[0][1] = vy[0];
 682         out->m[0][2] = vz[0];
 683         out->m[0][3] = t[0];
 684         out->m[1][0] = vx[1];
 685         out->m[1][1] = vy[1];
 686         out->m[1][2] = vz[1];
 687         out->m[1][3] = t[1];
 688         out->m[2][0] = vx[2];
 689         out->m[2][1] = vy[2];
 690         out->m[2][2] = vz[2];
 691         out->m[2][3] = t[2];
 692 }
 693
 694 void Matrix3x4_Transform (const matrix3x4_t *in, const float v[3], float out[3])
 695 {
 696         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
 697         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
 698         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
 699 }
 700
 701 void Matrix3x4_SimpleUntransform (const matrix3x4_t *in, const float v[3], float out[3])
 702 {
 703         float t[3];
 704         t[0] = v[0] - in->m[0][3];
 705         t[1] = v[1] - in->m[1][3];
 706         t[2] = v[2] - in->m[2][3];
 707         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
 708         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
 709         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
 710 }
 711
 712 // FIXME: optimize
 713 void Matrix3x4_ConcatTranslate (matrix3x4_t *out, float x, float y, float z)
 714 {
 715         matrix3x4_t base, temp;
 716         Matrix3x4_Copy(out, &base);
 717         Matrix3x4_CreateTranslate(&temp, x, y, z);
 718         Matrix3x4_Concat(out, &base, &temp);
 719 }
 720
 721 // FIXME: optimize
 722 void Matrix3x4_ConcatRotate (matrix3x4_t *out, float angle, float x, float y, float z)
 723 {
 724         matrix3x4_t base, temp;
 725         Matrix3x4_Copy(out, &base);
 726         Matrix3x4_CreateRotate(&temp, angle, x, y, z);
 727         Matrix3x4_Concat(out, &base, &temp);
 728 }
 729
 730 // FIXME: optimize
 731 void Matrix3x4_ConcatScale (matrix3x4_t *out, float x)
 732 {
 733         matrix3x4_t base, temp;
 734         Matrix3x4_Copy(out, &base);
 735         Matrix3x4_CreateScale(&temp, x);
 736         Matrix3x4_Concat(out, &base, &temp);
 737 }
 738
 739 // FIXME: optimize
 740 void Matrix3x4_ConcatScale3 (matrix3x4_t *out, float x, float y, float z)
 741 {
 742         matrix3x4_t base, temp;
 743         Matrix3x4_Copy(out, &base);
 744         Matrix3x4_CreateScale3(&temp, x, y, z);
 745         Matrix3x4_Concat(out, &base, &temp);
 746 }