5 const matrix4x4_t identitymatrix =
15 void Matrix4x4_Copy (matrix4x4_t *out, const matrix4x4_t *in)
20 void Matrix4x4_CopyRotateOnly (matrix4x4_t *out, const matrix4x4_t *in)
22 out->m[0][0] = in->m[0][0];
23 out->m[0][1] = in->m[0][1];
24 out->m[0][2] = in->m[0][2];
26 out->m[1][0] = in->m[1][0];
27 out->m[1][1] = in->m[1][1];
28 out->m[1][2] = in->m[1][2];
30 out->m[2][0] = in->m[2][0];
31 out->m[2][1] = in->m[2][1];
32 out->m[2][2] = in->m[2][2];
40 void Matrix4x4_CopyTranslateOnly (matrix4x4_t *out, const matrix4x4_t *in)
45 out->m[0][3] = in->m[0][3];
49 out->m[1][3] = in->m[1][3];
53 out->m[2][3] = in->m[2][3];
60 void Matrix4x4_Concat (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2)
62 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];
63 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];
64 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];
65 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];
66 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];
67 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];
68 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];
69 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];
70 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];
71 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];
72 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];
73 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];
74 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];
75 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];
76 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];
77 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];
80 void Matrix4x4_Transpose (matrix4x4_t *out, const matrix4x4_t *in1)
82 out->m[0][0] = in1->m[0][0];
83 out->m[0][1] = in1->m[1][0];
84 out->m[0][2] = in1->m[2][0];
85 out->m[0][3] = in1->m[3][0];
86 out->m[1][0] = in1->m[0][1];
87 out->m[1][1] = in1->m[1][1];
88 out->m[1][2] = in1->m[2][1];
89 out->m[1][3] = in1->m[3][1];
90 out->m[2][0] = in1->m[0][2];
91 out->m[2][1] = in1->m[1][2];
92 out->m[2][2] = in1->m[2][2];
93 out->m[2][3] = in1->m[3][2];
94 out->m[3][0] = in1->m[0][3];
95 out->m[3][1] = in1->m[1][3];
96 out->m[3][2] = in1->m[2][3];
97 out->m[3][3] = in1->m[3][3];
100 void Matrix4x4_Transpose3x3 (matrix4x4_t *out, const matrix4x4_t *in1)
102 out->m[0][0] = in1->m[0][0];
103 out->m[0][1] = in1->m[1][0];
104 out->m[0][2] = in1->m[2][0];
105 out->m[1][0] = in1->m[0][1];
106 out->m[1][1] = in1->m[1][1];
107 out->m[1][2] = in1->m[2][1];
108 out->m[2][0] = in1->m[0][2];
109 out->m[2][1] = in1->m[1][2];
110 out->m[2][2] = in1->m[2][2];
112 out->m[0][3] = in1->m[0][3];
113 out->m[1][3] = in1->m[1][3];
114 out->m[2][3] = in1->m[2][3];
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];
121 void Matrix4x4_Invert_Simple (matrix4x4_t *out, const matrix4x4_t *in1)
123 // we only support uniform scaling, so assume the first row is enough
124 // (note the lack of sqrt here, because we're trying to undo the scaling,
125 // this means multiplying by the inverse scale twice - squaring it, which
126 // makes the sqrt a waste of time)
128 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]);
130 double scale = 3.0 / sqrt
131 (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]
132 + in1->m[1][0] * in1->m[1][0] + in1->m[1][1] * in1->m[1][1] + in1->m[1][2] * in1->m[1][2]
133 + in1->m[2][0] * in1->m[2][0] + in1->m[2][1] * in1->m[2][1] + in1->m[2][2] * in1->m[2][2]);
137 // invert the rotation by transposing and multiplying by the squared
138 // recipricol of the input matrix scale as described above
139 out->m[0][0] = (float)(in1->m[0][0] * scale);
140 out->m[0][1] = (float)(in1->m[1][0] * scale);
141 out->m[0][2] = (float)(in1->m[2][0] * scale);
142 out->m[1][0] = (float)(in1->m[0][1] * scale);
143 out->m[1][1] = (float)(in1->m[1][1] * scale);
144 out->m[1][2] = (float)(in1->m[2][1] * scale);
145 out->m[2][0] = (float)(in1->m[0][2] * scale);
146 out->m[2][1] = (float)(in1->m[1][2] * scale);
147 out->m[2][2] = (float)(in1->m[2][2] * scale);
149 // invert the translate
150 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]);
151 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]);
152 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]);
154 // don't know if there's anything worth doing here
161 void Matrix4x4_CreateIdentity (matrix4x4_t *out)
181 void Matrix4x4_CreateTranslate (matrix4x4_t *out, float x, float y, float z)
201 void Matrix4x4_CreateRotate (matrix4x4_t *out, float angle, float x, float y, float z)
207 len = 1.0f / (float)sqrt(len);
212 angle *= (float)(-M_PI / 180.0);
213 c = (float)cos(angle);
214 s = (float)sin(angle);
216 out->m[0][0]=x * x + c * (1 - x * x);
217 out->m[0][1]=x * y * (1 - c) + z * s;
218 out->m[0][2]=z * x * (1 - c) - y * s;
220 out->m[1][0]=x * y * (1 - c) - z * s;
221 out->m[1][1]=y * y + c * (1 - y * y);
222 out->m[1][2]=y * z * (1 - c) + x * s;
224 out->m[2][0]=z * x * (1 - c) + y * s;
225 out->m[2][1]=y * z * (1 - c) - x * s;
226 out->m[2][2]=z * z + c * (1 - z * z);
234 void Matrix4x4_CreateScale (matrix4x4_t *out, float x)
254 void Matrix4x4_CreateScale3 (matrix4x4_t *out, float x, float y, float z)
274 void Matrix4x4_CreateFromQuakeEntity(matrix4x4_t *out, float x, float y, float z, float pitch, float yaw, float roll, float scale)
276 double angle, sr, sp, sy, cr, cp, cy;
280 angle = yaw * (M_PI*2 / 360);
283 angle = pitch * (M_PI*2 / 360);
286 angle = roll * (M_PI*2 / 360);
289 out->m[0][0] = (float)((cp*cy) * scale);
290 out->m[0][1] = (float)((sr*sp*cy+cr*-sy) * scale);
291 out->m[0][2] = (float)((cr*sp*cy+-sr*-sy) * scale);
293 out->m[1][0] = (float)((cp*sy) * scale);
294 out->m[1][1] = (float)((sr*sp*sy+cr*cy) * scale);
295 out->m[1][2] = (float)((cr*sp*sy+-sr*cy) * scale);
297 out->m[2][0] = (float)((-sp) * scale);
298 out->m[2][1] = (float)((sr*cp) * scale);
299 out->m[2][2] = (float)((cr*cp) * scale);
308 angle = yaw * (M_PI*2 / 360);
311 angle = pitch * (M_PI*2 / 360);
314 out->m[0][0] = (float)((cp*cy) * scale);
315 out->m[0][1] = (float)((-sy) * scale);
316 out->m[0][2] = (float)((sp*cy) * scale);
318 out->m[1][0] = (float)((cp*sy) * scale);
319 out->m[1][1] = (float)((cy) * scale);
320 out->m[1][2] = (float)((sp*sy) * scale);
322 out->m[2][0] = (float)((-sp) * scale);
324 out->m[2][2] = (float)((cp) * scale);
333 angle = yaw * (M_PI*2 / 360);
336 out->m[0][0] = (float)((cy) * scale);
337 out->m[0][1] = (float)((-sy) * scale);
340 out->m[1][0] = (float)((sy) * scale);
341 out->m[1][1] = (float)((cy) * scale);
346 out->m[2][2] = scale;
355 out->m[0][0] = scale;
360 out->m[1][1] = scale;
365 out->m[2][2] = scale;
374 void Matrix4x4_ToVectors(const matrix4x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
390 void Matrix4x4_FromVectors(matrix4x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
392 out->m[0][0] = vx[0];
393 out->m[0][1] = vy[0];
394 out->m[0][2] = vz[0];
396 out->m[1][0] = vx[1];
397 out->m[1][1] = vy[1];
398 out->m[1][2] = vz[1];
400 out->m[2][0] = vx[2];
401 out->m[2][1] = vy[2];
402 out->m[2][2] = vz[2];
410 void Matrix4x4_Blend (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2, float blend)
412 float iblend = 1 - blend;
413 out->m[0][0] = in1->m[0][0] * iblend + in2->m[0][0] * blend;
414 out->m[0][1] = in1->m[0][1] * iblend + in2->m[0][1] * blend;
415 out->m[0][2] = in1->m[0][2] * iblend + in2->m[0][2] * blend;
416 out->m[0][3] = in1->m[0][3] * iblend + in2->m[0][3] * blend;
417 out->m[1][0] = in1->m[1][0] * iblend + in2->m[1][0] * blend;
418 out->m[1][1] = in1->m[1][1] * iblend + in2->m[1][1] * blend;
419 out->m[1][2] = in1->m[1][2] * iblend + in2->m[1][2] * blend;
420 out->m[1][3] = in1->m[1][3] * iblend + in2->m[1][3] * blend;
421 out->m[2][0] = in1->m[2][0] * iblend + in2->m[2][0] * blend;
422 out->m[2][1] = in1->m[2][1] * iblend + in2->m[2][1] * blend;
423 out->m[2][2] = in1->m[2][2] * iblend + in2->m[2][2] * blend;
424 out->m[2][3] = in1->m[2][3] * iblend + in2->m[2][3] * blend;
425 out->m[3][0] = in1->m[3][0] * iblend + in2->m[3][0] * blend;
426 out->m[3][1] = in1->m[3][1] * iblend + in2->m[3][1] * blend;
427 out->m[3][2] = in1->m[3][2] * iblend + in2->m[3][2] * blend;
428 out->m[3][3] = in1->m[3][3] * iblend + in2->m[3][3] * blend;
432 void Matrix4x4_Transform (const matrix4x4_t *in, const float v[3], float out[3])
434 out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
435 out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
436 out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
439 void Matrix4x4_Transform4 (const matrix4x4_t *in, const float v[4], float out[4])
441 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];
442 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];
443 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];
444 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];
447 void Matrix4x4_Transform3x3 (const matrix4x4_t *in, const float v[3], float out[3])
449 out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2];
450 out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2];
451 out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2];
455 void Matrix4x4_SimpleUntransform (const matrix4x4_t *in, const float v[3], float out[3])
458 t[0] = v[0] - in->m[0][3];
459 t[1] = v[1] - in->m[1][3];
460 t[2] = v[2] - in->m[2][3];
461 out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
462 out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
463 out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
468 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, float x, float y, float z)
470 matrix4x4_t base, temp;
472 Matrix4x4_CreateTranslate(&temp, x, y, z);
473 Matrix4x4_Concat(out, &base, &temp);
477 void Matrix4x4_ConcatRotate (matrix4x4_t *out, float angle, float x, float y, float z)
479 matrix4x4_t base, temp;
481 Matrix4x4_CreateRotate(&temp, angle, x, y, z);
482 Matrix4x4_Concat(out, &base, &temp);
486 void Matrix4x4_ConcatScale (matrix4x4_t *out, float x)
488 matrix4x4_t base, temp;
490 Matrix4x4_CreateScale(&temp, x);
491 Matrix4x4_Concat(out, &base, &temp);
495 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, float x, float y, float z)
497 matrix4x4_t base, temp;
499 Matrix4x4_CreateScale3(&temp, x, y, z);
500 Matrix4x4_Concat(out, &base, &temp);
503 void Matrix4x4_OriginFromMatrix (const matrix4x4_t *in, float *out)
505 out[0] = in->m[0][3];
506 out[1] = in->m[1][3];
507 out[2] = in->m[2][3];
510 float Matrix4x4_ScaleFromMatrix (const matrix4x4_t *in)
512 // we only support uniform scaling, so assume the first row is enough
513 return (float)sqrt(in->m[0][0] * in->m[0][0] + in->m[0][1] * in->m[0][1] + in->m[0][2] * in->m[0][2]);