matrixlib.c

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