2 This file is part of XRay, a photorealistic 3D rendering library.
3 Copyright (C) 2005 John Tsiombikas
5 XRay is free software; you can redistribute it and/or modify
6 it under the terms of the GNU General Public License as published by
7 the Free Software Foundation; either version 2 of the License, or
8 (at your option) any later version.
10 XRay is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU General Public License for more details.
15 You should have received a copy of the GNU General Public License
16 along with XRay; if not, write to the Free Software
17 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
22 * @author John Tsiombikas
32 #include "quaternion.h"
36 // ----------- Matrix3x3 --------------
38 Matrix3x3 Matrix3x3::identity_matrix = Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1);
40 Matrix3x3::Matrix3x3() {
41 *this = identity_matrix;
44 Matrix3x3::Matrix3x3( scalar_t m11, scalar_t m12, scalar_t m13,
45 scalar_t m21, scalar_t m22, scalar_t m23,
46 scalar_t m31, scalar_t m32, scalar_t m33) {
47 m[0][0] = m11; m[0][1] = m12; m[0][2] = m13;
48 m[1][0] = m21; m[1][1] = m22; m[1][2] = m23;
49 m[2][0] = m31; m[2][1] = m32; m[2][2] = m33;
52 Matrix3x3::Matrix3x3(const Matrix4x4 &mat4x4) {
53 for(int i=0; i<3; i++) {
54 for(int j=0; j<3; j++) {
55 m[i][j] = mat4x4[i][j];
60 Matrix3x3 operator +(const Matrix3x3 &m1, const Matrix3x3 &m2) {
62 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
63 scalar_t *dest = res.m[0];
65 for(int i=0; i<9; i++) {
66 *dest++ = *op1++ + *op2++;
71 Matrix3x3 operator -(const Matrix3x3 &m1, const Matrix3x3 &m2) {
73 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
74 scalar_t *dest = res.m[0];
76 for(int i=0; i<9; i++) {
77 *dest++ = *op1++ - *op2++;
82 Matrix3x3 operator *(const Matrix3x3 &m1, const Matrix3x3 &m2) {
84 for(int i=0; i<3; i++) {
85 for(int j=0; j<3; j++) {
86 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];
92 void operator +=(Matrix3x3 &m1, const Matrix3x3 &m2) {
93 scalar_t *op1 = m1.m[0];
94 const scalar_t *op2 = m2.m[0];
96 for(int i=0; i<9; i++) {
101 void operator -=(Matrix3x3 &m1, const Matrix3x3 &m2) {
102 scalar_t *op1 = m1.m[0];
103 const scalar_t *op2 = m2.m[0];
105 for(int i=0; i<9; i++) {
110 void operator *=(Matrix3x3 &m1, const Matrix3x3 &m2) {
112 for(int i=0; i<3; i++) {
113 for(int j=0; j<3; j++) {
114 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];
117 memcpy(m1.m, res.m, 9 * sizeof(scalar_t));
120 Matrix3x3 operator *(const Matrix3x3 &mat, scalar_t scalar) {
122 const scalar_t *mptr = mat.m[0];
123 scalar_t *dptr = res.m[0];
125 for(int i=0; i<9; i++) {
126 *dptr++ = *mptr++ * scalar;
131 Matrix3x3 operator *(scalar_t scalar, const Matrix3x3 &mat) {
133 const scalar_t *mptr = mat.m[0];
134 scalar_t *dptr = res.m[0];
136 for(int i=0; i<9; i++) {
137 *dptr++ = *mptr++ * scalar;
142 void operator *=(Matrix3x3 &mat, scalar_t scalar) {
143 scalar_t *mptr = mat.m[0];
145 for(int i=0; i<9; i++) {
150 void Matrix3x3::translate(const Vector2 &trans) {
151 Matrix3x3 tmat(1, 0, trans.x, 0, 1, trans.y, 0, 0, 1);
155 void Matrix3x3::set_translation(const Vector2 &trans) {
156 *this = Matrix3x3(1, 0, trans.x, 0, 1, trans.y, 0, 0, 1);
159 void Matrix3x3::rotate(scalar_t angle) {
160 scalar_t cos_a = cos(angle);
161 scalar_t sin_a = sin(angle);
162 Matrix3x3 rmat( cos_a, -sin_a, 0,
168 void Matrix3x3::set_rotation(scalar_t angle) {
169 scalar_t cos_a = cos(angle);
170 scalar_t sin_a = sin(angle);
171 *this = Matrix3x3(cos_a, -sin_a, 0, sin_a, cos_a, 0, 0, 0, 1);
174 void Matrix3x3::rotate(const Vector3 &euler_angles) {
175 Matrix3x3 xrot, yrot, zrot;
177 xrot = Matrix3x3( 1, 0, 0,
178 0, cos(euler_angles.x), -sin(euler_angles.x),
179 0, sin(euler_angles.x), cos(euler_angles.x));
181 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),
183 -sin(euler_angles.y), 0, cos(euler_angles.y));
185 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,
186 sin(euler_angles.z), cos(euler_angles.z), 0,
189 *this *= xrot * yrot * zrot;
192 void Matrix3x3::set_rotation(const Vector3 &euler_angles) {
193 Matrix3x3 xrot, yrot, zrot;
195 xrot = Matrix3x3( 1, 0, 0,
196 0, cos(euler_angles.x), -sin(euler_angles.x),
197 0, sin(euler_angles.x), cos(euler_angles.x));
199 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),
201 -sin(euler_angles.y), 0, cos(euler_angles.y));
203 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,
204 sin(euler_angles.z), cos(euler_angles.z), 0,
207 *this = xrot * yrot * zrot;
210 void Matrix3x3::rotate(const Vector3 &axis, scalar_t angle) {
211 scalar_t sina = (scalar_t)sin(angle);
212 scalar_t cosa = (scalar_t)cos(angle);
213 scalar_t invcosa = 1-cosa;
214 scalar_t nxsq = axis.x * axis.x;
215 scalar_t nysq = axis.y * axis.y;
216 scalar_t nzsq = axis.z * axis.z;
219 xform.m[0][0] = nxsq + (1-nxsq) * cosa;
220 xform.m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
221 xform.m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
222 xform.m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
223 xform.m[1][1] = nysq + (1-nysq) * cosa;
224 xform.m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
225 xform.m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
226 xform.m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
227 xform.m[2][2] = nzsq + (1-nzsq) * cosa;
232 void Matrix3x3::set_rotation(const Vector3 &axis, scalar_t angle) {
233 scalar_t sina = (scalar_t)sin(angle);
234 scalar_t cosa = (scalar_t)cos(angle);
235 scalar_t invcosa = 1-cosa;
236 scalar_t nxsq = axis.x * axis.x;
237 scalar_t nysq = axis.y * axis.y;
238 scalar_t nzsq = axis.z * axis.z;
241 m[0][0] = nxsq + (1-nxsq) * cosa;
242 m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
243 m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
244 m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
245 m[1][1] = nysq + (1-nysq) * cosa;
246 m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
247 m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
248 m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
249 m[2][2] = nzsq + (1-nzsq) * cosa;
252 void Matrix3x3::scale(const Vector3 &scale_vec) {
253 Matrix3x3 smat( scale_vec.x, 0, 0,
259 void Matrix3x3::set_scaling(const Vector3 &scale_vec) {
260 *this = Matrix3x3( scale_vec.x, 0, 0,
265 void Matrix3x3::set_column_vector(const Vector3 &vec, unsigned int col_index) {
266 m[0][col_index] = vec.x;
267 m[1][col_index] = vec.y;
268 m[2][col_index] = vec.z;
271 void Matrix3x3::set_row_vector(const Vector3 &vec, unsigned int row_index) {
272 m[row_index][0] = vec.x;
273 m[row_index][1] = vec.y;
274 m[row_index][2] = vec.z;
277 Vector3 Matrix3x3::get_column_vector(unsigned int col_index) const {
278 return Vector3(m[0][col_index], m[1][col_index], m[2][col_index]);
281 Vector3 Matrix3x3::get_row_vector(unsigned int row_index) const {
282 return Vector3(m[row_index][0], m[row_index][1], m[row_index][2]);
285 void Matrix3x3::transpose() {
286 Matrix3x3 tmp = *this;
287 for(int i=0; i<3; i++) {
288 for(int j=0; j<3; j++) {
294 Matrix3x3 Matrix3x3::transposed() const {
296 for(int i=0; i<3; i++) {
297 for(int j=0; j<3; j++) {
304 scalar_t Matrix3x3::determinant() const {
305 return m[0][0] * (m[1][1]*m[2][2] - m[1][2]*m[2][1]) -
306 m[0][1] * (m[1][0]*m[2][2] - m[1][2]*m[2][0]) +
307 m[0][2] * (m[1][0]*m[2][1] - m[1][1]*m[2][0]);
310 Matrix3x3 Matrix3x3::inverse() const {
311 // TODO: implement 3x3 inverse
315 ostream &operator <<(ostream &out, const Matrix3x3 &mat) {
316 for(int i=0; i<3; i++) {
318 sprintf(str, "[ %12.5f %12.5f %12.5f ]\n", (float)mat.m[i][0], (float)mat.m[i][1], (float)mat.m[i][2]);
326 ////////////////////// Matrix4x4 implementation ///////////////////////
328 Matrix4x4 Matrix4x4::identity_matrix = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
330 Matrix4x4::Matrix4x4() {
331 *this = identity_matrix;
334 Matrix4x4::Matrix4x4( scalar_t m11, scalar_t m12, scalar_t m13, scalar_t m14,
335 scalar_t m21, scalar_t m22, scalar_t m23, scalar_t m24,
336 scalar_t m31, scalar_t m32, scalar_t m33, scalar_t m34,
337 scalar_t m41, scalar_t m42, scalar_t m43, scalar_t m44) {
338 m[0][0] = m11; m[0][1] = m12; m[0][2] = m13; m[0][3] = m14;
339 m[1][0] = m21; m[1][1] = m22; m[1][2] = m23; m[1][3] = m24;
340 m[2][0] = m31; m[2][1] = m32; m[2][2] = m33; m[2][3] = m34;
341 m[3][0] = m41; m[3][1] = m42; m[3][2] = m43; m[3][3] = m44;
346 Matrix4x4::Matrix4x4(const Matrix3x3 &mat3x3) {
348 for(int i=0; i<3; i++) {
349 for(int j=0; j<3; j++) {
350 m[i][j] = mat3x3[i][j];
357 Matrix4x4::~Matrix4x4() {
358 if(glmatrix) delete [] glmatrix;
361 Matrix4x4 operator +(const Matrix4x4 &m1, const Matrix4x4 &m2) {
363 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
364 scalar_t *dest = res.m[0];
366 for(int i=0; i<16; i++) {
367 *dest++ = *op1++ + *op2++;
372 Matrix4x4 operator -(const Matrix4x4 &m1, const Matrix4x4 &m2) {
374 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
375 scalar_t *dest = res.m[0];
377 for(int i=0; i<16; i++) {
378 *dest++ = *op1++ - *op2++;
383 Matrix4x4 operator *(const Matrix4x4 &m1, const Matrix4x4 &m2) {
386 for(int i=0; i<4; i++) {
387 for(int j=0; j<4; j++) {
388 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];
395 void operator +=(Matrix4x4 &m1, const Matrix4x4 &m2) {
396 scalar_t *op1 = m1.m[0];
397 const scalar_t *op2 = m2.m[0];
399 for(int i=0; i<16; i++) {
404 void operator -=(Matrix4x4 &m1, const Matrix4x4 &m2) {
405 scalar_t *op1 = m1.m[0];
406 const scalar_t *op2 = m2.m[0];
408 for(int i=0; i<16; i++) {
413 void operator *=(Matrix4x4 &m1, const Matrix4x4 &m2) {
415 for(int i=0; i<4; i++) {
416 for(int j=0; j<4; j++) {
417 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];
420 memcpy(m1.m, res.m, 16 * sizeof(scalar_t));
423 Matrix4x4 operator *(const Matrix4x4 &mat, scalar_t scalar) {
425 const scalar_t *mptr = mat.m[0];
426 scalar_t *dptr = res.m[0];
428 for(int i=0; i<16; i++) {
429 *dptr++ = *mptr++ * scalar;
434 Matrix4x4 operator *(scalar_t scalar, const Matrix4x4 &mat) {
436 const scalar_t *mptr = mat.m[0];
437 scalar_t *dptr = res.m[0];
439 for(int i=0; i<16; i++) {
440 *dptr++ = *mptr++ * scalar;
445 void operator *=(Matrix4x4 &mat, scalar_t scalar) {
446 scalar_t *mptr = mat.m[0];
448 for(int i=0; i<16; i++) {
453 void Matrix4x4::translate(const Vector3 &trans) {
454 Matrix4x4 tmat(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
458 void Matrix4x4::set_translation(const Vector3 &trans) {
459 *this = Matrix4x4(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
462 void Matrix4x4::rotate(const Vector3 &euler_angles) {
463 Matrix3x3 xrot, yrot, zrot;
465 xrot = Matrix3x3( 1, 0, 0,
466 0, cos(euler_angles.x), -sin(euler_angles.x),
467 0, sin(euler_angles.x), cos(euler_angles.x));
469 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),
471 -sin(euler_angles.y), 0, cos(euler_angles.y));
473 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,
474 sin(euler_angles.z), cos(euler_angles.z), 0,
477 *this *= Matrix4x4(xrot * yrot * zrot);
480 void Matrix4x4::set_rotation(const Vector3 &euler_angles) {
481 Matrix3x3 xrot, yrot, zrot;
483 xrot = Matrix3x3( 1, 0, 0,
484 0, cos(euler_angles.x), -sin(euler_angles.x),
485 0, sin(euler_angles.x), cos(euler_angles.x));
487 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),
489 -sin(euler_angles.y), 0, cos(euler_angles.y));
491 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,
492 sin(euler_angles.z), cos(euler_angles.z), 0,
495 *this = Matrix4x4(xrot * yrot * zrot);
498 void Matrix4x4::rotate(const Vector3 &axis, scalar_t angle) {
499 scalar_t sina = (scalar_t)sin(angle);
500 scalar_t cosa = (scalar_t)cos(angle);
501 scalar_t invcosa = 1-cosa;
502 scalar_t nxsq = axis.x * axis.x;
503 scalar_t nysq = axis.y * axis.y;
504 scalar_t nzsq = axis.z * axis.z;
507 xform[0][0] = nxsq + (1-nxsq) * cosa;
508 xform[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
509 xform[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
510 xform[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
511 xform[1][1] = nysq + (1-nysq) * cosa;
512 xform[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
513 xform[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
514 xform[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
515 xform[2][2] = nzsq + (1-nzsq) * cosa;
517 *this *= Matrix4x4(xform);
520 void Matrix4x4::set_rotation(const Vector3 &axis, scalar_t angle) {
521 scalar_t sina = (scalar_t)sin(angle);
522 scalar_t cosa = (scalar_t)cos(angle);
523 scalar_t invcosa = 1-cosa;
524 scalar_t nxsq = axis.x * axis.x;
525 scalar_t nysq = axis.y * axis.y;
526 scalar_t nzsq = axis.z * axis.z;
529 m[0][0] = nxsq + (1-nxsq) * cosa;
530 m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
531 m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
532 m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
533 m[1][1] = nysq + (1-nysq) * cosa;
534 m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
535 m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
536 m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
537 m[2][2] = nzsq + (1-nzsq) * cosa;
540 void Matrix4x4::scale(const Vector4 &scale_vec) {
541 Matrix4x4 smat( scale_vec.x, 0, 0, 0,
542 0, scale_vec.y, 0, 0,
543 0, 0, scale_vec.z, 0,
544 0, 0, 0, scale_vec.w);
548 void Matrix4x4::set_scaling(const Vector4 &scale_vec) {
549 *this = Matrix4x4( scale_vec.x, 0, 0, 0,
550 0, scale_vec.y, 0, 0,
551 0, 0, scale_vec.z, 0,
552 0, 0, 0, scale_vec.w);
555 void Matrix4x4::set_column_vector(const Vector4 &vec, unsigned int col_index) {
556 m[0][col_index] = vec.x;
557 m[1][col_index] = vec.y;
558 m[2][col_index] = vec.z;
559 m[3][col_index] = vec.w;
562 void Matrix4x4::set_row_vector(const Vector4 &vec, unsigned int row_index) {
563 m[row_index][0] = vec.x;
564 m[row_index][1] = vec.y;
565 m[row_index][2] = vec.z;
566 m[row_index][3] = vec.w;
569 Vector4 Matrix4x4::get_column_vector(unsigned int col_index) const {
570 return Vector4(m[0][col_index], m[1][col_index], m[2][col_index], m[3][col_index]);
573 Vector4 Matrix4x4::get_row_vector(unsigned int row_index) const {
574 return Vector4(m[row_index][0], m[row_index][1], m[row_index][2], m[row_index][3]);
577 void Matrix4x4::transpose() {
578 Matrix4x4 tmp = *this;
579 for(int i=0; i<4; i++) {
580 for(int j=0; j<4; j++) {
586 Matrix4x4 Matrix4x4::transposed() const {
588 for(int i=0; i<4; i++) {
589 for(int j=0; j<4; j++) {
596 scalar_t Matrix4x4::determinant() const {
598 scalar_t det11 = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
599 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
600 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
602 scalar_t det12 = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
603 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
604 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
606 scalar_t det13 = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
607 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
608 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
610 scalar_t det14 = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
611 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
612 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
614 return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
618 Matrix4x4 Matrix4x4::adjoint() const {
622 coef.m[0][0] = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
623 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
624 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
625 coef.m[0][1] = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
626 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
627 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
628 coef.m[0][2] = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
629 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
630 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
631 coef.m[0][3] = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
632 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
633 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
635 coef.m[1][0] = (m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
636 (m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
637 (m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
638 coef.m[1][1] = (m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
639 (m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
640 (m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
641 coef.m[1][2] = (m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
642 (m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
643 (m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
644 coef.m[1][3] = (m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
645 (m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
646 (m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
648 coef.m[2][0] = (m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
649 (m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
650 (m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
651 coef.m[2][1] = (m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
652 (m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
653 (m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
654 coef.m[2][2] = (m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
655 (m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
656 (m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
657 coef.m[2][3] = (m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
658 (m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
659 (m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
661 coef.m[3][0] = (m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
662 (m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
663 (m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
664 coef.m[3][1] = (m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
665 (m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
666 (m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
667 coef.m[3][2] = (m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
668 (m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
669 (m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
670 coef.m[3][3] = (m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
671 (m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
672 (m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
676 for(int i=0; i<4; i++) {
677 for(int j=0; j<4; j++) {
678 coef.m[i][j] = j%2 ? -coef.m[i][j] : coef.m[i][j];
679 if(i%2) coef.m[i][j] = -coef.m[i][j];
686 Matrix4x4 Matrix4x4::inverse() const {
688 Matrix4x4 AdjMat = adjoint();
690 return AdjMat * (1.0f / determinant());
693 const scalar_t *Matrix4x4::opengl_matrix() const {
694 return (const scalar_t*)m;
697 ostream &operator <<(ostream &out, const Matrix4x4 &mat) {
698 for(int i=0; i<4; i++) {
700 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]);