2 This file is part of the n3dmath2 library.
4 Copyright (c) 2004, 2005 John Tsiombikas <nuclear@siggraph.org>
6 The n3dmath2 library is free software; you can redistribute it and/or modify
7 it under the terms of the GNU General Public License as published by
8 the Free Software Foundation; either version 2 of the License, or
9 (at your option) any later version.
11 The n3dmath2 library is distributed in the hope that it will be useful,
12 but WITHOUT ANY WARRANTY; without even the implied warranty of
13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 GNU General Public License for more details.
16 You should have received a copy of the GNU General Public License
17 along with the n3dmath2 library; if not, write to the Free Software
18 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
23 #include "n3dmath2_mat.hpp"
27 // ----------- Matrix3x3 --------------
29 Matrix3x3 Matrix3x3::identity_matrix = Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1);
31 Matrix3x3::Matrix3x3() {
32 *this = identity_matrix;
35 Matrix3x3::Matrix3x3( scalar_t m11, scalar_t m12, scalar_t m13,
36 scalar_t m21, scalar_t m22, scalar_t m23,
37 scalar_t m31, scalar_t m32, scalar_t m33) {
38 m[0][0] = m11; m[0][1] = m12; m[0][2] = m13;
39 m[1][0] = m21; m[1][1] = m22; m[1][2] = m23;
40 m[2][0] = m31; m[2][1] = m32; m[2][2] = m33;
41 //memcpy(m, &m11, 9 * sizeof(scalar_t)); // args are adjacent in the stack
44 Matrix3x3::Matrix3x3(const Matrix4x4 &mat4x4) {
45 for(int i=0; i<3; i++) {
46 for(int j=0; j<3; j++) {
47 m[i][j] = mat4x4[i][j];
52 Matrix3x3 operator +(const Matrix3x3 &m1, const Matrix3x3 &m2) {
54 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
55 scalar_t *dest = res.m[0];
57 for(int i=0; i<9; i++) {
58 *dest++ = *op1++ + *op2++;
63 Matrix3x3 operator -(const Matrix3x3 &m1, const Matrix3x3 &m2) {
65 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
66 scalar_t *dest = res.m[0];
68 for(int i=0; i<9; i++) {
69 *dest++ = *op1++ - *op2++;
74 Matrix3x3 operator *(const Matrix3x3 &m1, const Matrix3x3 &m2) {
76 for(int i=0; i<3; i++) {
77 for(int j=0; j<3; j++) {
78 res.m[i][j] = m1.m[i][0] * m2.m[0][j] + m1.m[i][1] * m2.m[1][j] + m1.m[i][2] * m2.m[2][j];
84 void operator +=(Matrix3x3 &m1, const Matrix3x3 &m2) {
85 scalar_t *op1 = m1.m[0];
86 const scalar_t *op2 = m2.m[0];
88 for(int i=0; i<9; i++) {
93 void operator -=(Matrix3x3 &m1, const Matrix3x3 &m2) {
94 scalar_t *op1 = m1.m[0];
95 const scalar_t *op2 = m2.m[0];
97 for(int i=0; i<9; i++) {
102 void operator *=(Matrix3x3 &m1, const Matrix3x3 &m2) {
104 for(int i=0; i<3; i++) {
105 for(int j=0; j<3; j++) {
106 res.m[i][j] = m1.m[i][0] * m2.m[0][j] + m1.m[i][1] * m2.m[1][j] + m1.m[i][2] * m2.m[2][j];
109 memcpy(m1.m, res.m, 9 * sizeof(scalar_t));
112 Matrix3x3 operator *(const Matrix3x3 &mat, scalar_t scalar) {
114 const scalar_t *mptr = mat.m[0];
115 scalar_t *dptr = res.m[0];
117 for(int i=0; i<9; i++) {
118 *dptr++ = *mptr++ * scalar;
123 Matrix3x3 operator *(scalar_t scalar, const Matrix3x3 &mat) {
125 const scalar_t *mptr = mat.m[0];
126 scalar_t *dptr = res.m[0];
128 for(int i=0; i<9; i++) {
129 *dptr++ = *mptr++ * scalar;
134 void operator *=(Matrix3x3 &mat, scalar_t scalar) {
135 scalar_t *mptr = mat.m[0];
137 for(int i=0; i<9; i++) {
142 void Matrix3x3::translate(const Vector2 &trans) {
143 Matrix3x3 tmat(1, 0, trans.x, 0, 1, trans.y, 0, 0, 1);
147 void Matrix3x3::set_translation(const Vector2 &trans) {
148 *this = Matrix3x3(1, 0, trans.x, 0, 1, trans.y, 0, 0, 1);
151 void Matrix3x3::rotate(scalar_t angle) {
152 scalar_t cos_a = cos(angle);
153 scalar_t sin_a = sin(angle);
154 Matrix3x3 rmat( cos_a, -sin_a, 0,
160 void Matrix3x3::set_rotation(scalar_t angle) {
161 scalar_t cos_a = cos(angle);
162 scalar_t sin_a = sin(angle);
163 *this = Matrix3x3(cos_a, -sin_a, 0, sin_a, cos_a, 0, 0, 0, 1);
166 void Matrix3x3::rotate(const Vector3 &euler_angles) {
167 Matrix3x3 xrot, yrot, zrot;
169 xrot = Matrix3x3( 1, 0, 0,
170 0, cos(euler_angles.x), -sin(euler_angles.x),
171 0, sin(euler_angles.x), cos(euler_angles.x));
173 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),
175 -sin(euler_angles.y), 0, cos(euler_angles.y));
177 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,
178 sin(euler_angles.z), cos(euler_angles.z), 0,
181 *this *= xrot * yrot * zrot;
184 void Matrix3x3::set_rotation(const Vector3 &euler_angles) {
185 Matrix3x3 xrot, yrot, zrot;
187 xrot = Matrix3x3( 1, 0, 0,
188 0, cos(euler_angles.x), -sin(euler_angles.x),
189 0, sin(euler_angles.x), cos(euler_angles.x));
191 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),
193 -sin(euler_angles.y), 0, cos(euler_angles.y));
195 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,
196 sin(euler_angles.z), cos(euler_angles.z), 0,
199 *this = xrot * yrot * zrot;
202 void Matrix3x3::rotate(const Vector3 &axis, scalar_t angle) {
203 scalar_t sina = (scalar_t)sin(angle);
204 scalar_t cosa = (scalar_t)cos(angle);
205 scalar_t invcosa = 1-cosa;
206 scalar_t nxsq = axis.x * axis.x;
207 scalar_t nysq = axis.y * axis.y;
208 scalar_t nzsq = axis.z * axis.z;
211 xform.m[0][0] = nxsq + (1-nxsq) * cosa;
212 xform.m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
213 xform.m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
214 xform.m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
215 xform.m[1][1] = nysq + (1-nysq) * cosa;
216 xform.m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
217 xform.m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
218 xform.m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
219 xform.m[2][2] = nzsq + (1-nzsq) * cosa;
224 void Matrix3x3::set_rotation(const Vector3 &axis, scalar_t angle) {
225 scalar_t sina = (scalar_t)sin(angle);
226 scalar_t cosa = (scalar_t)cos(angle);
227 scalar_t invcosa = 1-cosa;
228 scalar_t nxsq = axis.x * axis.x;
229 scalar_t nysq = axis.y * axis.y;
230 scalar_t nzsq = axis.z * axis.z;
233 m[0][0] = nxsq + (1-nxsq) * cosa;
234 m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
235 m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
236 m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
237 m[1][1] = nysq + (1-nysq) * cosa;
238 m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
239 m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
240 m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
241 m[2][2] = nzsq + (1-nzsq) * cosa;
244 void Matrix3x3::scale(const Vector3 &scale_vec) {
245 Matrix3x3 smat( scale_vec.x, 0, 0,
251 void Matrix3x3::set_scaling(const Vector3 &scale_vec) {
252 *this = Matrix3x3( scale_vec.x, 0, 0,
257 void Matrix3x3::set_column_vector(const Vector3 &vec, unsigned int col_index) {
258 m[0][col_index] = vec.x;
259 m[1][col_index] = vec.y;
260 m[2][col_index] = vec.z;
263 void Matrix3x3::set_row_vector(const Vector3 &vec, unsigned int row_index) {
264 m[row_index][0] = vec.x;
265 m[row_index][1] = vec.y;
266 m[row_index][2] = vec.z;
269 Vector3 Matrix3x3::get_column_vector(unsigned int col_index) const {
270 return Vector3(m[0][col_index], m[1][col_index], m[2][col_index]);
273 Vector3 Matrix3x3::get_row_vector(unsigned int row_index) const {
274 return Vector3(m[row_index][0], m[row_index][1], m[row_index][2]);
277 void Matrix3x3::transpose() {
278 Matrix3x3 tmp = *this;
279 for(int i=0; i<3; i++) {
280 for(int j=0; j<3; j++) {
286 Matrix3x3 Matrix3x3::transposed() const {
288 for(int i=0; i<3; i++) {
289 for(int j=0; j<3; j++) {
296 scalar_t Matrix3x3::determinant() const {
297 return m[0][0] * (m[1][1]*m[2][2] - m[1][2]*m[2][1]) -
298 m[0][1] * (m[1][0]*m[2][2] - m[1][2]*m[2][0]) +
299 m[0][2] * (m[1][0]*m[2][1] - m[1][1]*m[2][0]);
302 Matrix3x3 Matrix3x3::inverse() const {
303 // TODO: implement 3x3 inverse
307 ostream &operator <<(ostream &out, const Matrix3x3 &mat) {
308 for(int i=0; i<3; i++) {
310 sprintf(str, "[ %12.5f %12.5f %12.5f ]\n", (float)mat.m[i][0], (float)mat.m[i][1], (float)mat.m[i][2]);
318 ////////////////////// Matrix4x4 implementation ///////////////////////
320 Matrix4x4 Matrix4x4::identity_matrix = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
322 Matrix4x4::Matrix4x4() {
323 *this = identity_matrix;
326 Matrix4x4::Matrix4x4( scalar_t m11, scalar_t m12, scalar_t m13, scalar_t m14,
327 scalar_t m21, scalar_t m22, scalar_t m23, scalar_t m24,
328 scalar_t m31, scalar_t m32, scalar_t m33, scalar_t m34,
329 scalar_t m41, scalar_t m42, scalar_t m43, scalar_t m44) {
330 m[0][0] = m11; m[0][1] = m12; m[0][2] = m13; m[0][3] = m14;
331 m[1][0] = m21; m[1][1] = m22; m[1][2] = m23; m[1][3] = m24;
332 m[2][0] = m31; m[2][1] = m32; m[2][2] = m33; m[2][3] = m34;
333 m[3][0] = m41; m[3][1] = m42; m[3][2] = m43; m[3][3] = m44;
334 //memcpy(m, &m11, 16 * sizeof(scalar_t)); // args are adjacent in the stack
339 Matrix4x4::Matrix4x4(const Matrix3x3 &mat3x3) {
341 for(int i=0; i<3; i++) {
342 for(int j=0; j<3; j++) {
343 m[i][j] = mat3x3[i][j];
350 Matrix4x4::~Matrix4x4() {
351 if(glmatrix) delete [] glmatrix;
354 Matrix4x4 operator +(const Matrix4x4 &m1, const Matrix4x4 &m2) {
356 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
357 scalar_t *dest = res.m[0];
359 for(int i=0; i<16; i++) {
360 *dest++ = *op1++ + *op2++;
365 Matrix4x4 operator -(const Matrix4x4 &m1, const Matrix4x4 &m2) {
367 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
368 scalar_t *dest = res.m[0];
370 for(int i=0; i<16; i++) {
371 *dest++ = *op1++ - *op2++;
376 Matrix4x4 operator *(const Matrix4x4 &m1, const Matrix4x4 &m2) {
379 for(int i=0; i<4; i++) {
380 for(int j=0; j<4; j++) {
381 res.m[i][j] = m1.m[i][0] * m2.m[0][j] + m1.m[i][1] * m2.m[1][j] + m1.m[i][2] * m2.m[2][j] + m1.m[i][3] * m2.m[3][j];
388 void operator +=(Matrix4x4 &m1, const Matrix4x4 &m2) {
389 scalar_t *op1 = m1.m[0];
390 const scalar_t *op2 = m2.m[0];
392 for(int i=0; i<16; i++) {
397 void operator -=(Matrix4x4 &m1, const Matrix4x4 &m2) {
398 scalar_t *op1 = m1.m[0];
399 const scalar_t *op2 = m2.m[0];
401 for(int i=0; i<16; i++) {
406 void operator *=(Matrix4x4 &m1, const Matrix4x4 &m2) {
409 for(int i=0; i<4; i++) {
410 for(int j=0; j<4; j++) {
411 res.m[i][j] = m1.m[i][0] * m2.m[0][j] + m1.m[i][1] * m2.m[1][j] + m1.m[i][2] * m2.m[2][j] + m1.m[i][3] * m2.m[3][j];
415 memcpy(m1.m, res.m, 16 * sizeof(scalar_t));
418 Matrix4x4 operator *(const Matrix4x4 &mat, scalar_t scalar) {
420 const scalar_t *mptr = mat.m[0];
421 scalar_t *dptr = res.m[0];
423 for(int i=0; i<16; i++) {
424 *dptr++ = *mptr++ * scalar;
429 Matrix4x4 operator *(scalar_t scalar, const Matrix4x4 &mat) {
431 const scalar_t *mptr = mat.m[0];
432 scalar_t *dptr = res.m[0];
434 for(int i=0; i<16; i++) {
435 *dptr++ = *mptr++ * scalar;
440 void operator *=(Matrix4x4 &mat, scalar_t scalar) {
441 scalar_t *mptr = mat.m[0];
443 for(int i=0; i<16; i++) {
448 void Matrix4x4::translate(const Vector3 &trans) {
449 Matrix4x4 tmat(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
453 void Matrix4x4::set_translation(const Vector3 &trans) {
454 *this = Matrix4x4(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
457 void Matrix4x4::rotate(const Vector3 &euler_angles) {
458 Matrix3x3 xrot, yrot, zrot;
460 xrot = Matrix3x3( 1, 0, 0,
461 0, cos(euler_angles.x), -sin(euler_angles.x),
462 0, sin(euler_angles.x), cos(euler_angles.x));
464 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),
466 -sin(euler_angles.y), 0, cos(euler_angles.y));
468 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,
469 sin(euler_angles.z), cos(euler_angles.z), 0,
472 *this *= Matrix4x4(xrot * yrot * zrot);
475 void Matrix4x4::set_rotation(const Vector3 &euler_angles) {
476 Matrix3x3 xrot, yrot, zrot;
478 xrot = Matrix3x3( 1, 0, 0,
479 0, cos(euler_angles.x), -sin(euler_angles.x),
480 0, sin(euler_angles.x), cos(euler_angles.x));
482 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),
484 -sin(euler_angles.y), 0, cos(euler_angles.y));
486 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,
487 sin(euler_angles.z), cos(euler_angles.z), 0,
490 *this = Matrix4x4(xrot * yrot * zrot);
493 void Matrix4x4::rotate(const Vector3 &axis, scalar_t angle) {
494 scalar_t sina = (scalar_t)sin(angle);
495 scalar_t cosa = (scalar_t)cos(angle);
496 scalar_t invcosa = 1-cosa;
497 scalar_t nxsq = axis.x * axis.x;
498 scalar_t nysq = axis.y * axis.y;
499 scalar_t nzsq = axis.z * axis.z;
502 xform[0][0] = nxsq + (1-nxsq) * cosa;
503 xform[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
504 xform[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
505 xform[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
506 xform[1][1] = nysq + (1-nysq) * cosa;
507 xform[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
508 xform[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
509 xform[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
510 xform[2][2] = nzsq + (1-nzsq) * cosa;
512 *this *= Matrix4x4(xform);
515 void Matrix4x4::set_rotation(const Vector3 &axis, scalar_t angle) {
516 scalar_t sina = (scalar_t)sin(angle);
517 scalar_t cosa = (scalar_t)cos(angle);
518 scalar_t invcosa = 1-cosa;
519 scalar_t nxsq = axis.x * axis.x;
520 scalar_t nysq = axis.y * axis.y;
521 scalar_t nzsq = axis.z * axis.z;
524 m[0][0] = nxsq + (1-nxsq) * cosa;
525 m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
526 m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
527 m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
528 m[1][1] = nysq + (1-nysq) * cosa;
529 m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
530 m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
531 m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
532 m[2][2] = nzsq + (1-nzsq) * cosa;
535 void Matrix4x4::scale(const Vector4 &scale_vec) {
536 Matrix4x4 smat( scale_vec.x, 0, 0, 0,
537 0, scale_vec.y, 0, 0,
538 0, 0, scale_vec.z, 0,
539 0, 0, 0, scale_vec.w);
543 void Matrix4x4::set_scaling(const Vector4 &scale_vec) {
544 *this = Matrix4x4( scale_vec.x, 0, 0, 0,
545 0, scale_vec.y, 0, 0,
546 0, 0, scale_vec.z, 0,
547 0, 0, 0, scale_vec.w);
550 void Matrix4x4::set_column_vector(const Vector4 &vec, unsigned int col_index) {
551 m[0][col_index] = vec.x;
552 m[1][col_index] = vec.y;
553 m[2][col_index] = vec.z;
554 m[3][col_index] = vec.w;
557 void Matrix4x4::set_row_vector(const Vector4 &vec, unsigned int row_index) {
558 m[row_index][0] = vec.x;
559 m[row_index][1] = vec.y;
560 m[row_index][2] = vec.z;
561 m[row_index][3] = vec.w;
564 Vector4 Matrix4x4::get_column_vector(unsigned int col_index) const {
565 return Vector4(m[0][col_index], m[1][col_index], m[2][col_index], m[3][col_index]);
568 Vector4 Matrix4x4::get_row_vector(unsigned int row_index) const {
569 return Vector4(m[row_index][0], m[row_index][1], m[row_index][2], m[row_index][3]);
572 void Matrix4x4::transpose() {
573 Matrix4x4 tmp = *this;
574 for(int i=0; i<4; i++) {
575 for(int j=0; j<4; j++) {
581 Matrix4x4 Matrix4x4::transposed() const {
583 for(int i=0; i<4; i++) {
584 for(int j=0; j<4; j++) {
591 scalar_t Matrix4x4::determinant() const {
593 scalar_t det11 = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
594 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
595 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
597 scalar_t det12 = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
598 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
599 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
601 scalar_t det13 = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
602 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
603 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
605 scalar_t det14 = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
606 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
607 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
609 return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
613 Matrix4x4 Matrix4x4::adjoint() const {
617 coef.m[0][0] = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
618 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
619 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
620 coef.m[0][1] = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
621 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
622 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
623 coef.m[0][2] = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
624 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
625 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
626 coef.m[0][3] = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
627 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
628 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
630 coef.m[1][0] = (m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
631 (m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
632 (m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
633 coef.m[1][1] = (m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
634 (m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
635 (m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
636 coef.m[1][2] = (m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
637 (m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
638 (m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
639 coef.m[1][3] = (m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
640 (m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
641 (m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
643 coef.m[2][0] = (m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
644 (m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
645 (m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
646 coef.m[2][1] = (m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
647 (m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
648 (m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
649 coef.m[2][2] = (m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
650 (m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
651 (m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
652 coef.m[2][3] = (m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
653 (m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
654 (m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
656 coef.m[3][0] = (m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
657 (m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
658 (m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
659 coef.m[3][1] = (m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
660 (m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
661 (m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
662 coef.m[3][2] = (m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
663 (m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
664 (m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
665 coef.m[3][3] = (m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
666 (m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
667 (m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
671 for(int i=0; i<4; i++) {
672 for(int j=0; j<4; j++) {
673 coef.m[i][j] = j%2 ? -coef.m[i][j] : coef.m[i][j];
674 if(i%2) coef.m[i][j] = -coef.m[i][j];
681 Matrix4x4 Matrix4x4::inverse() const {
683 Matrix4x4 AdjMat = adjoint();
685 return AdjMat * (1.0f / determinant());
688 const scalar_t *Matrix4x4::opengl_matrix() const {
689 return (const scalar_t*)m;
692 ostream &operator <<(ostream &out, const Matrix4x4 &mat) {
693 for(int i=0; i<4; i++) {
695 sprintf(str, "[ %12.5f %12.5f %12.5f %12.5f ]\n", (float)mat.m[i][0], (float)mat.m[i][1], (float)mat.m[i][2], (float)mat.m[i][3]);