matrixlib.c

   1
   2 #include "quakedef.h"
   3 #include "matrixlib.h"
   4 #include <math.h>
   5
   6 void Matrix4x4_Copy (matrix4x4_t *out, matrix4x4_t *in)
   7 {
   8         *out = *in;
   9 }
  10
  11 void Matrix4x4_CopyRotateOnly (matrix4x4_t *out, 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, 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, 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         float scale;
  94         scale = 3.0 / (in1->m[0][0] * in1->m[0][0]
  95                      + in1->m[0][1] * in1->m[0][1]
  96                      + in1->m[0][2] * in1->m[0][2]
  97                      + in1->m[1][0] * in1->m[1][0]
  98                      + in1->m[1][1] * in1->m[1][1]
  99                      + in1->m[1][2] * in1->m[1][2]
 100                      + in1->m[2][0] * in1->m[2][0]
 101                      + in1->m[2][1] * in1->m[2][1]
 102                      + in1->m[2][2] * in1->m[2][2]);
 103         out->m[0][0] = in1->m[0][0] * scale;
 104         out->m[0][1] = in1->m[1][0] * scale;
 105         out->m[0][2] = in1->m[2][0] * scale;
 106         out->m[0][3] = in1->m[3][0];
 107         out->m[1][0] = in1->m[0][1] * scale;
 108         out->m[1][1] = in1->m[1][1] * scale;
 109         out->m[1][2] = in1->m[2][1] * scale;
 110         out->m[1][3] = in1->m[3][1];
 111         out->m[2][0] = in1->m[0][2] * scale;
 112         out->m[2][1] = in1->m[1][2] * scale;
 113         out->m[2][2] = in1->m[2][2] * scale;
 114         out->m[2][3] = in1->m[3][2];
 115         out->m[3][0] = in1->m[0][3];
 116         out->m[3][1] = in1->m[1][3];
 117         out->m[3][2] = in1->m[2][3];
 118         out->m[3][3] = in1->m[3][3];
 119 }
 120
 121 void Matrix4x4_CreateIdentity (matrix4x4_t *out)
 122 {
 123         out->m[0][0]=1.0f;
 124         out->m[0][1]=0.0f;
 125         out->m[0][2]=0.0f;
 126         out->m[0][3]=0.0f;
 127         out->m[1][0]=0.0f;
 128         out->m[1][1]=1.0f;
 129         out->m[1][2]=0.0f;
 130         out->m[1][3]=0.0f;
 131         out->m[2][0]=0.0f;
 132         out->m[2][1]=0.0f;
 133         out->m[2][2]=1.0f;
 134         out->m[2][3]=0.0f;
 135         out->m[3][0]=0.0f;
 136         out->m[3][1]=0.0f;
 137         out->m[3][2]=0.0f;
 138         out->m[3][3]=1.0f;
 139 }
 140
 141 void Matrix4x4_CreateTranslate (matrix4x4_t *out, float x, float y, float z)
 142 {
 143         out->m[0][0]=1.0f;
 144         out->m[0][1]=0.0f;
 145         out->m[0][2]=0.0f;
 146         out->m[0][3]=x;
 147         out->m[1][0]=0.0f;
 148         out->m[1][1]=1.0f;
 149         out->m[1][2]=0.0f;
 150         out->m[1][3]=y;
 151         out->m[2][0]=0.0f;
 152         out->m[2][1]=0.0f;
 153         out->m[2][2]=1.0f;
 154         out->m[2][3]=z;
 155         out->m[3][0]=0.0f;
 156         out->m[3][1]=0.0f;
 157         out->m[3][2]=0.0f;
 158         out->m[3][3]=1.0f;
 159 }
 160
 161 void Matrix4x4_CreateRotate (matrix4x4_t *out, float angle, float x, float y, float z)
 162 {
 163         float len, c, s;
 164
 165         len = x*x+y*y+z*z;
 166         if (len != 0.0f)
 167                 len = 1.0f / sqrt(len);
 168         x *= len;
 169         y *= len;
 170         z *= len;
 171
 172         angle *= M_PI / 180.0;
 173         c = cos(angle);
 174         s = sin(angle);
 175
 176         out->m[0][0]=x * x + c * (1 - x * x);
 177         out->m[0][1]=x * y * (1 - c) + z * s;
 178         out->m[0][2]=z * x * (1 - c) - y * s;
 179         out->m[0][3]=0.0f;
 180         out->m[1][0]=x * y * (1 - c) - z * s;
 181         out->m[1][1]=y * y + c * (1 - y * y);
 182         out->m[1][2]=y * z * (1 - c) + x * s;
 183         out->m[1][3]=0.0f;
 184         out->m[2][0]=z * x * (1 - c) + y * s;
 185         out->m[2][1]=y * z * (1 - c) - x * s;
 186         out->m[2][2]=z * z + c * (1 - z * z);
 187         out->m[2][3]=0.0f;
 188         out->m[3][0]=0.0f;
 189         out->m[3][1]=0.0f;
 190         out->m[3][2]=0.0f;
 191         out->m[3][3]=1.0f;
 192 }
 193
 194 void Matrix4x4_CreateScale (matrix4x4_t *out, float x)
 195 {
 196         out->m[0][0]=x;
 197         out->m[0][1]=0.0f;
 198         out->m[0][2]=0.0f;
 199         out->m[0][3]=0.0f;
 200         out->m[1][0]=0.0f;
 201         out->m[1][1]=x;
 202         out->m[1][2]=0.0f;
 203         out->m[1][3]=0.0f;
 204         out->m[2][0]=0.0f;
 205         out->m[2][1]=0.0f;
 206         out->m[2][2]=x;
 207         out->m[2][3]=0.0f;
 208         out->m[3][0]=0.0f;
 209         out->m[3][1]=0.0f;
 210         out->m[3][2]=0.0f;
 211         out->m[3][3]=1.0f;
 212 }
 213
 214 void Matrix4x4_CreateScale3 (matrix4x4_t *out, float x, float y, float z)
 215 {
 216         out->m[0][0]=x;
 217         out->m[0][1]=0.0f;
 218         out->m[0][2]=0.0f;
 219         out->m[0][3]=0.0f;
 220         out->m[1][0]=0.0f;
 221         out->m[1][1]=y;
 222         out->m[1][2]=0.0f;
 223         out->m[1][3]=0.0f;
 224         out->m[2][0]=0.0f;
 225         out->m[2][1]=0.0f;
 226         out->m[2][2]=z;
 227         out->m[2][3]=0.0f;
 228         out->m[3][0]=0.0f;
 229         out->m[3][1]=0.0f;
 230         out->m[3][2]=0.0f;
 231         out->m[3][3]=1.0f;
 232 }
 233
 234 void Matrix4x4_ToVectors(const matrix4x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
 235 {
 236         vx[0] = in->m[0][0];
 237         vx[1] = in->m[1][0];
 238         vx[2] = in->m[2][0];
 239         vy[0] = in->m[0][1];
 240         vy[1] = in->m[1][1];
 241         vy[2] = in->m[2][1];
 242         vz[0] = in->m[0][2];
 243         vz[1] = in->m[1][2];
 244         vz[2] = in->m[2][2];
 245         t[0] = in->m[0][3];
 246         t[1] = in->m[1][3];
 247         t[2] = in->m[2][3];
 248 }
 249
 250 void Matrix4x4_FromVectors(matrix4x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
 251 {
 252         out->m[0][0] = vx[0];
 253         out->m[0][1] = vy[0];
 254         out->m[0][2] = vz[0];
 255         out->m[0][3] = t[0];
 256         out->m[1][0] = vx[1];
 257         out->m[1][1] = vy[1];
 258         out->m[1][2] = vz[1];
 259         out->m[1][3] = t[1];
 260         out->m[2][0] = vx[2];
 261         out->m[2][1] = vy[2];
 262         out->m[2][2] = vz[2];
 263         out->m[2][3] = t[2];
 264         out->m[3][0] = 0.0f;
 265         out->m[3][1] = 0.0f;
 266         out->m[3][2] = 0.0f;
 267         out->m[3][3] = 1.0f;
 268 }
 269
 270 void Matrix4x4_Transform (const matrix4x4_t *in, const float v[3], float out[3])
 271 {
 272         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
 273         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
 274         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
 275 }
 276
 277 void Matrix4x4_Transform4 (const matrix4x4_t *in, const float v[4], float out[4])
 278 {
 279         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];
 280         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];
 281         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];
 282         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];
 283 }
 284
 285 void Matrix4x4_SimpleUntransform (const matrix4x4_t *in, const float v[3], float out[3])
 286 {
 287         float t[3];
 288         t[0] = v[0] - in->m[0][3];
 289         t[1] = v[1] - in->m[1][3];
 290         t[2] = v[2] - in->m[2][3];
 291         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
 292         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
 293         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
 294 }
 295
 296 // FIXME: optimize
 297 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, float x, float y, float z)
 298 {
 299         matrix4x4_t base, temp;
 300         Matrix4x4_Copy(out, &base);
 301         Matrix4x4_CreateTranslate(&temp, x, y, z);
 302         Matrix4x4_Concat(out, &base, &temp);
 303 }
 304
 305 // FIXME: optimize
 306 void Matrix4x4_ConcatRotate (matrix4x4_t *out, float angle, float x, float y, float z)
 307 {
 308         matrix4x4_t base, temp;
 309         Matrix4x4_Copy(out, &base);
 310         Matrix4x4_CreateRotate(&temp, angle, x, y, z);
 311         Matrix4x4_Concat(out, &base, &temp);
 312 }
 313
 314 // FIXME: optimize
 315 void Matrix4x4_ConcatScale (matrix4x4_t *out, float x)
 316 {
 317         matrix4x4_t base, temp;
 318         Matrix4x4_Copy(out, &base);
 319         Matrix4x4_CreateScale(&temp, x);
 320         Matrix4x4_Concat(out, &base, &temp);
 321 }
 322
 323 // FIXME: optimize
 324 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, float x, float y, float z)
 325 {
 326         matrix4x4_t base, temp;
 327         Matrix4x4_Copy(out, &base);
 328         Matrix4x4_CreateScale3(&temp, x, y, z);
 329         Matrix4x4_Concat(out, &base, &temp);
 330 }
 331
 332
 333
 334
 335
 336
 337
 338
 339 void Matrix3x4_Copy (matrix3x4_t *out, matrix3x4_t *in)
 340 {
 341         *out = *in;
 342 }
 343
 344 void Matrix3x4_CopyRotateOnly (matrix3x4_t *out, matrix3x4_t *in)
 345 {
 346         out->m[0][0] = in->m[0][0];
 347         out->m[0][1] = in->m[0][1];
 348         out->m[0][2] = in->m[0][2];
 349         out->m[0][3] = 0.0f;
 350         out->m[1][0] = in->m[1][0];
 351         out->m[1][1] = in->m[1][1];
 352         out->m[1][2] = in->m[1][2];
 353         out->m[1][3] = 0.0f;
 354         out->m[2][0] = in->m[2][0];
 355         out->m[2][1] = in->m[2][1];
 356         out->m[2][2] = in->m[2][2];
 357         out->m[2][3] = 0.0f;
 358 }
 359
 360 void Matrix3x4_CopyTranslateOnly (matrix3x4_t *out, matrix3x4_t *in)
 361 {
 362         out->m[0][0] = 0.0f;
 363         out->m[0][1] = 0.0f;
 364         out->m[0][2] = 0.0f;
 365         out->m[0][3] = in->m[0][3];
 366         out->m[1][0] = 0.0f;
 367         out->m[1][1] = 0.0f;
 368         out->m[1][2] = 0.0f;
 369         out->m[1][3] = in->m[0][3];
 370         out->m[2][0] = 0.0f;
 371         out->m[2][1] = 0.0f;
 372         out->m[2][2] = 0.0f;
 373         out->m[2][3] = in->m[0][3];
 374 }
 375
 376 void Matrix3x4_FromMatrix4x4 (matrix3x4_t *out, matrix4x4_t *in)
 377 {
 378         out->m[0][0] = in->m[0][0];
 379         out->m[0][1] = in->m[0][1];
 380         out->m[0][2] = in->m[0][2];
 381         out->m[0][3] = in->m[0][3];
 382         out->m[1][0] = in->m[1][0];
 383         out->m[1][1] = in->m[1][1];
 384         out->m[1][2] = in->m[1][2];
 385         out->m[1][3] = in->m[1][3];
 386         out->m[2][0] = in->m[2][0];
 387         out->m[2][1] = in->m[2][1];
 388         out->m[2][2] = in->m[2][2];
 389         out->m[2][3] = in->m[2][3];
 390 }
 391
 392 void Matrix3x4_Concat (matrix3x4_t *out, const matrix3x4_t *in1, const matrix3x4_t *in2)
 393 {
 394         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];
 395         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];
 396         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];
 397         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];
 398         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];
 399         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];
 400         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];
 401         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];
 402         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];
 403         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];
 404         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];
 405         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];
 406 }
 407
 408 void Matrix3x4_Transpose3x3 (matrix3x4_t *out, const matrix3x4_t *in1)
 409 {
 410         float scale;
 411         scale = 3.0 / (in1->m[0][0] * in1->m[0][0]
 412                      + in1->m[0][1] * in1->m[0][1]
 413                      + in1->m[0][2] * in1->m[0][2]
 414                      + in1->m[1][0] * in1->m[1][0]
 415                      + in1->m[1][1] * in1->m[1][1]
 416                      + in1->m[1][2] * in1->m[1][2]
 417                      + in1->m[2][0] * in1->m[2][0]
 418                      + in1->m[2][1] * in1->m[2][1]
 419                      + in1->m[2][2] * in1->m[2][2]);
 420         out->m[0][0] = in1->m[0][0] * scale;
 421         out->m[0][1] = in1->m[1][0] * scale;
 422         out->m[0][2] = in1->m[2][0] * scale;
 423         out->m[0][3] = 0.0f;
 424         out->m[1][0] = in1->m[0][1] * scale;
 425         out->m[1][1] = in1->m[1][1] * scale;
 426         out->m[1][2] = in1->m[2][1] * scale;
 427         out->m[1][3] = 0.0f;
 428         out->m[2][0] = in1->m[0][2] * scale;
 429         out->m[2][1] = in1->m[1][2] * scale;
 430         out->m[2][2] = in1->m[2][2] * scale;
 431         out->m[2][3] = 0.0f;
 432 }
 433
 434 void Matrix3x4_CreateIdentity (matrix3x4_t *out)
 435 {
 436         out->m[0][0]=1.0f;
 437         out->m[0][1]=0.0f;
 438         out->m[0][2]=0.0f;
 439         out->m[0][3]=0.0f;
 440         out->m[1][0]=0.0f;
 441         out->m[1][1]=1.0f;
 442         out->m[1][2]=0.0f;
 443         out->m[1][3]=0.0f;
 444         out->m[2][0]=0.0f;
 445         out->m[2][1]=0.0f;
 446         out->m[2][2]=1.0f;
 447         out->m[2][3]=0.0f;
 448 }
 449
 450 void Matrix3x4_CreateTranslate (matrix3x4_t *out, float x, float y, float z)
 451 {
 452         out->m[0][0]=1.0f;
 453         out->m[0][1]=0.0f;
 454         out->m[0][2]=0.0f;
 455         out->m[0][3]=x;
 456         out->m[1][0]=0.0f;
 457         out->m[1][1]=1.0f;
 458         out->m[1][2]=0.0f;
 459         out->m[1][3]=y;
 460         out->m[2][0]=0.0f;
 461         out->m[2][1]=0.0f;
 462         out->m[2][2]=1.0f;
 463         out->m[2][3]=z;
 464 }
 465
 466 void Matrix3x4_CreateRotate (matrix3x4_t *out, float angle, float x, float y, float z)
 467 {
 468         float len, c, s;
 469
 470         len = x*x+y*y+z*z;
 471         if (len != 0.0f)
 472                 len = 1.0f / sqrt(len);
 473         x *= len;
 474         y *= len;
 475         z *= len;
 476
 477         angle *= M_PI / 180.0;
 478         c = cos(angle);
 479         s = sin(angle);
 480
 481         out->m[0][0]=x * x + c * (1 - x * x);
 482         out->m[0][1]=x * y * (1 - c) + z * s;
 483         out->m[0][2]=z * x * (1 - c) - y * s;
 484         out->m[0][3]=0.0f;
 485         out->m[1][0]=x * y * (1 - c) - z * s;
 486         out->m[1][1]=y * y + c * (1 - y * y);
 487         out->m[1][2]=y * z * (1 - c) + x * s;
 488         out->m[1][3]=0.0f;
 489         out->m[2][0]=z * x * (1 - c) + y * s;
 490         out->m[2][1]=y * z * (1 - c) - x * s;
 491         out->m[2][2]=z * z + c * (1 - z * z);
 492         out->m[2][3]=0.0f;
 493 }
 494
 495 void Matrix3x4_CreateScale (matrix3x4_t *out, float x)
 496 {
 497         out->m[0][0]=x;
 498         out->m[0][1]=0.0f;
 499         out->m[0][2]=0.0f;
 500         out->m[0][3]=0.0f;
 501         out->m[1][0]=0.0f;
 502         out->m[1][1]=x;
 503         out->m[1][2]=0.0f;
 504         out->m[1][3]=0.0f;
 505         out->m[2][0]=0.0f;
 506         out->m[2][1]=0.0f;
 507         out->m[2][2]=x;
 508         out->m[2][3]=0.0f;
 509 }
 510
 511 void Matrix3x4_CreateScale3 (matrix3x4_t *out, float x, float y, float z)
 512 {
 513         out->m[0][0]=x;
 514         out->m[0][1]=0.0f;
 515         out->m[0][2]=0.0f;
 516         out->m[0][3]=0.0f;
 517         out->m[1][0]=0.0f;
 518         out->m[1][1]=y;
 519         out->m[1][2]=0.0f;
 520         out->m[1][3]=0.0f;
 521         out->m[2][0]=0.0f;
 522         out->m[2][1]=0.0f;
 523         out->m[2][2]=z;
 524         out->m[2][3]=0.0f;
 525 }
 526
 527 void Matrix3x4_ToVectors(const matrix3x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
 528 {
 529         vx[0] = in->m[0][0];
 530         vx[1] = in->m[1][0];
 531         vx[2] = in->m[2][0];
 532         vy[0] = in->m[0][1];
 533         vy[1] = in->m[1][1];
 534         vy[2] = in->m[2][1];
 535         vz[0] = in->m[0][2];
 536         vz[1] = in->m[1][2];
 537         vz[2] = in->m[2][2];
 538         t[0] = in->m[0][3];
 539         t[1] = in->m[1][3];
 540         t[2] = in->m[2][3];
 541 }
 542
 543 void Matrix3x4_FromVectors(matrix3x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
 544 {
 545         out->m[0][0] = vx[0];
 546         out->m[0][1] = vy[0];
 547         out->m[0][2] = vz[0];
 548         out->m[0][3] = t[0];
 549         out->m[1][0] = vx[1];
 550         out->m[1][1] = vy[1];
 551         out->m[1][2] = vz[1];
 552         out->m[1][3] = t[1];
 553         out->m[2][0] = vx[2];
 554         out->m[2][1] = vy[2];
 555         out->m[2][2] = vz[2];
 556         out->m[2][3] = t[2];
 557 }
 558
 559 void Matrix3x4_Transform (const matrix3x4_t *in, const float v[3], float out[3])
 560 {
 561         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
 562         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
 563         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
 564 }
 565
 566 void Matrix3x4_SimpleUntransform (const matrix3x4_t *in, const float v[3], float out[3])
 567 {
 568         float t[3];
 569         t[0] = v[0] - in->m[0][3];
 570         t[1] = v[1] - in->m[1][3];
 571         t[2] = v[2] - in->m[2][3];
 572         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
 573         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
 574         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
 575 }
 576
 577 // FIXME: optimize
 578 void Matrix3x4_ConcatTranslate (matrix3x4_t *out, float x, float y, float z)
 579 {
 580         matrix3x4_t base, temp;
 581         Matrix3x4_Copy(out, &base);
 582         Matrix3x4_CreateTranslate(&temp, x, y, z);
 583         Matrix3x4_Concat(out, &base, &temp);
 584 }
 585
 586 // FIXME: optimize
 587 void Matrix3x4_ConcatRotate (matrix3x4_t *out, float angle, float x, float y, float z)
 588 {
 589         matrix3x4_t base, temp;
 590         Matrix3x4_Copy(out, &base);
 591         Matrix3x4_CreateRotate(&temp, angle, x, y, z);
 592         Matrix3x4_Concat(out, &base, &temp);
 593 }
 594
 595 // FIXME: optimize
 596 void Matrix3x4_ConcatScale (matrix3x4_t *out, float x)
 597 {
 598         matrix3x4_t base, temp;
 599         Matrix3x4_Copy(out, &base);
 600         Matrix3x4_CreateScale(&temp, x);
 601         Matrix3x4_Concat(out, &base, &temp);
 602 }
 603
 604 // FIXME: optimize
 605 void Matrix3x4_ConcatScale3 (matrix3x4_t *out, float x, float y, float z)
 606 {
 607         matrix3x4_t base, temp;
 608         Matrix3x4_Copy(out, &base);
 609         Matrix3x4_CreateScale3(&temp, x, y, z);
 610         Matrix3x4_Concat(out, &base, &temp);
 611 }