/*===========================================================================*\ * * * OpenFlipper * * Copyright (C) 2001-2011 by Computer Graphics Group, RWTH Aachen * * www.openflipper.org * * * *---------------------------------------------------------------------------* * This file is part of OpenFlipper. * * * * OpenFlipper is free software: you can redistribute it and/or modify * * it under the terms of the GNU Lesser General Public License as * * published by the Free Software Foundation, either version 3 of * * the License, or (at your option) any later version with the * * following exceptions: * * * * If other files instantiate templates or use macros * * or inline functions from this file, or you compile this file and * * link it with other files to produce an executable, this file does * * not by itself cause the resulting executable to be covered by the * * GNU Lesser General Public License. This exception does not however * * invalidate any other reasons why the executable file might be * * covered by the GNU Lesser General Public License. * * * * OpenFlipper is distributed in the hope that it will be useful, * * but WITHOUT ANY WARRANTY; without even the implied warranty of * * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * * GNU Lesser General Public License for more details. * * * * You should have received a copy of the GNU LesserGeneral Public * * License along with OpenFlipper. If not, * * see . * * * \*===========================================================================*/ /*===========================================================================*\ * * * $Revision: 13072 $ * * $Author: lim $ * * $Date: 2011-12-16 14:15:07 +0100 (Fr, 16 Dez 2011) $ * * * \*===========================================================================*/ //============================================================================= // // CLASS Matrix4x4T - IMPLEMENTATION // //============================================================================= #define ACG_MATRIX4X4_C //== INCLUDES ================================================================= #include "Matrix4x4T.hh" #include "NumLimitsT.hh" //== IMPLEMENTATION ========================================================== namespace ACG { #define MAT(m,r,c) ((m)[(r)+((c)<<2)]) #define M(r,w) (MAT(mat_,r,w)) //----------------------------------------------------------------------------- template Matrix4x4T Matrix4x4T:: operator* (const Matrix4x4T& _rhs) const { #define RHS(row,col) MAT(_rhs.mat_, row,col) #define TMP(row,col) MAT(tmp.mat_, row,col) Matrix4x4T tmp; Scalar mi0, mi1, mi2, mi3; int i; for (i = 0; i < 4; i++) { mi0=M(i,0); mi1=M(i,1); mi2=M(i,2); mi3=M(i,3); TMP(i,0) = mi0*RHS(0,0) + mi1*RHS(1,0) + mi2*RHS(2,0) + mi3*RHS(3,0); TMP(i,1) = mi0*RHS(0,1) + mi1*RHS(1,1) + mi2*RHS(2,1) + mi3*RHS(3,1); TMP(i,2) = mi0*RHS(0,2) + mi1*RHS(1,2) + mi2*RHS(2,2) + mi3*RHS(3,2); TMP(i,3) = mi0*RHS(0,3) + mi1*RHS(1,3) + mi2*RHS(2,3) + mi3*RHS(3,3); } return tmp; #undef RHS #undef TMP } //----------------------------------------------------------------------------- template Matrix4x4T& Matrix4x4T:: operator*= (const Matrix4x4T& _rhs) { #define RHS(row,col) MAT(_rhs.mat_, row,col) int i; Scalar mi0, mi1, mi2, mi3; for (i = 0; i < 4; i++) { mi0=M(i,0); mi1=M(i,1); mi2=M(i,2); mi3=M(i,3); M(i,0) = mi0 * RHS(0,0) + mi1 * RHS(1,0) + mi2 * RHS(2,0) + mi3 * RHS(3,0); M(i,1) = mi0 * RHS(0,1) + mi1 * RHS(1,1) + mi2 * RHS(2,1) + mi3 * RHS(3,1); M(i,2) = mi0 * RHS(0,2) + mi1 * RHS(1,2) + mi2 * RHS(2,2) + mi3 * RHS(3,2); M(i,3) = mi0 * RHS(0,3) + mi1 * RHS(1,3) + mi2 * RHS(2,3) + mi3 * RHS(3,3); } return *this; #undef RHS } //----------------------------------------------------------------------------- template Matrix4x4T& Matrix4x4T:: leftMult(const Matrix4x4T& _rhs) { #define RHS(row,col) MAT(_rhs.mat_, row,col) int i; Scalar m0i, m1i, m2i, m3i; for(i=0;i<4;i++) { m0i = M(0,i); m1i = M(1,i); m2i = M(2,i); m3i = M(3,i); M(0,i) = RHS(0,0)*m0i + RHS(0,1)*m1i + RHS(0,2)*m2i + RHS(0,3)*m3i; M(1,i) = RHS(1,0)*m0i + RHS(1,1)*m1i + RHS(1,2)*m2i + RHS(1,3)*m3i; M(2,i) = RHS(2,0)*m0i + RHS(2,1)*m1i + RHS(2,2)*m2i + RHS(2,3)*m3i; M(3,i) = RHS(3,0)*m0i + RHS(3,1)*m1i + RHS(3,2)*m2i + RHS(3,3)*m3i; } return *this; #undef RHS } //----------------------------------------------------------------------------- template template VectorT Matrix4x4T:: operator*(const VectorT& _v) const { return VectorT ( M(0,0)*_v[0] + M(0,1)*_v[1] + M(0,2)*_v[2] + M(0,3)*_v[3], M(1,0)*_v[0] + M(1,1)*_v[1] + M(1,2)*_v[2] + M(1,3)*_v[3], M(2,0)*_v[0] + M(2,1)*_v[1] + M(2,2)*_v[2] + M(2,3)*_v[3], M(3,0)*_v[0] + M(3,1)*_v[1] + M(3,2)*_v[2] + M(3,3)*_v[3]); } //----------------------------------------------------------------------------- template Matrix4x4T Matrix4x4T::operator*(const Scalar& scalar) { for (int i = 0; i < 4; ++i) { for (int j = 0; j < 4; ++j) { M(i,j) *= scalar; } } return *this; } //----------------------------------------------------------------------------- template template VectorT Matrix4x4T:: transform_point(const VectorT& _v) const { Scalar x = M(0,0)*_v[0] + M(0,1)*_v[1] + M(0,2)*_v[2] + M(0,3); Scalar y = M(1,0)*_v[0] + M(1,1)*_v[1] + M(1,2)*_v[2] + M(1,3); Scalar z = M(2,0)*_v[0] + M(2,1)*_v[1] + M(2,2)*_v[2] + M(2,3); Scalar w = M(3,0)*_v[0] + M(3,1)*_v[1] + M(3,2)*_v[2] + M(3,3); if (w) { w = 1.0 / w; return VectorT(x*w, y*w, z*w); } else return VectorT(x, y, z); } //----------------------------------------------------------------------------- template template VectorT Matrix4x4T:: transform_vector(const VectorT& _v) const { Scalar x = M(0,0)*_v[0] + M(0,1)*_v[1] + M(0,2)*_v[2]; Scalar y = M(1,0)*_v[0] + M(1,1)*_v[1] + M(1,2)*_v[2]; Scalar z = M(2,0)*_v[0] + M(2,1)*_v[1] + M(2,2)*_v[2]; return VectorT(x, y, z); } //----------------------------------------------------------------------------- template void Matrix4x4T:: clear() { Scalar* m = mat_; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m = 0.0; } //----------------------------------------------------------------------------- template void Matrix4x4T:: identity() { Scalar* m = mat_; *m++ = 1.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 1.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 1.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m = 1.0; } //----------------------------------------------------------------------------- template void Matrix4x4T:: transpose() { Scalar tmp; for( int i=0; i<4; i++ ) { for( int j=i+1; j<4; j++ ) { tmp = MAT(mat_,i,j); MAT(mat_,i,j) = MAT(mat_,j,i); MAT(mat_,j,i) = tmp; } } } //----------------------------------------------------------------------------- /* * Compute inverse of 4x4 transformation matrix. * Taken from Mesa3.1 * Code contributed by Jacques Leroy jle@star.be */ template bool Matrix4x4T:: invert() { #define SWAP_ROWS(a, b) { Scalar *_tmp = a; (a)=(b); (b)=_tmp; } Scalar wtmp[4][8]; Scalar m0, m1, m2, m3, s; Scalar *r0, *r1, *r2, *r3; r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3]; r0[0] = M(0,0); r0[1] = M(0,1); r0[2] = M(0,2); r0[3] = M(0,3); r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0; r1[0] = M(1,0); r1[1] = M(1,1); r1[2] = M(1,2); r1[3] = M(1,3); r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0; r2[0] = M(2,0); r2[1] = M(2,1); r2[2] = M(2,2); r2[3] = M(2,3); r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0; r3[0] = M(3,0); r3[1] = M(3,1); r3[2] = M(3,2); r3[3] = M(3,3); r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0; /* choose pivot - or die */ if (fabs(r3[0])>fabs(r2[0])) SWAP_ROWS(r3, r2); if (fabs(r2[0])>fabs(r1[0])) SWAP_ROWS(r2, r1); if (fabs(r1[0])>fabs(r0[0])) SWAP_ROWS(r1, r0); if (0.0 == r0[0]) return false; /* eliminate first variable */ m1 = r1[0]/r0[0]; m2 = r2[0]/r0[0]; m3 = r3[0]/r0[0]; s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s; s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s; s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s; s = r0[4]; if (s != 0.0) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; } s = r0[5]; if (s != 0.0) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; } s = r0[6]; if (s != 0.0) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; } s = r0[7]; if (s != 0.0) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; } /* choose pivot - or die */ if (fabs(r3[1])>fabs(r2[1])) SWAP_ROWS(r3, r2); if (fabs(r2[1])>fabs(r1[1])) SWAP_ROWS(r2, r1); if (0.0 == r1[1]) return false; /* eliminate second variable */ m2 = r2[1]/r1[1]; m3 = r3[1]/r1[1]; r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2]; r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3]; s = r1[4]; if (0.0 != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; } s = r1[5]; if (0.0 != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; } s = r1[6]; if (0.0 != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; } s = r1[7]; if (0.0 != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; } /* choose pivot - or die */ if (fabs(r3[2])>fabs(r2[2])) SWAP_ROWS(r3, r2); if (0.0 == r2[2]) return false; /* eliminate third variable */ m3 = r3[2]/r2[2]; r3[3] -= m3 * r2[3]; r3[4] -= m3 * r2[4]; r3[5] -= m3 * r2[5]; r3[6] -= m3 * r2[6]; r3[7] -= m3 * r2[7]; /* last check */ if (0.0 == r3[3]) return false; s = 1.0/r3[3]; /* now back substitute row 3 */ r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s; m2 = r2[3]; /* now back substitute row 2 */ s = 1.0/r2[2]; r2[4] = s * (r2[4] - r3[4] * m2); r2[5] = s * (r2[5] - r3[5] * m2); r2[6] = s * (r2[6] - r3[6] * m2); r2[7] = s * (r2[7] - r3[7] * m2); m1 = r1[3]; r1[4] -= r3[4] * m1; r1[5] -= r3[5] * m1; r1[6] -= r3[6] * m1; r1[7] -= r3[7] * m1; m0 = r0[3]; r0[4] -= r3[4] * m0; r0[5] -= r3[5] * m0; r0[6] -= r3[6] * m0; r0[7] -= r3[7] * m0; m1 = r1[2]; /* now back substitute row 1 */ s = 1.0/r1[1]; r1[4] = s * (r1[4] - r2[4] * m1); r1[5] = s * (r1[5] - r2[5] * m1); r1[6] = s * (r1[6] - r2[6] * m1); r1[7] = s * (r1[7] - r2[7] * m1); m0 = r0[2]; r0[4] -= r2[4] * m0; r0[5] -= r2[5] * m0; r0[6] -= r2[6] * m0; r0[7] -= r2[7] * m0; m0 = r0[1]; /* now back substitute row 0 */ s = 1.0/r0[0]; r0[4] = s * (r0[4] - r1[4] * m0); r0[5] = s * (r0[5] - r1[5] * m0); r0[6] = s * (r0[6] - r1[6] * m0); r0[7] = s * (r0[7] - r1[7] * m0); M(0,0) = r0[4]; M(0,1) = r0[5]; M(0,2) = r0[6]; M(0,3) = r0[7]; M(1,0) = r1[4]; M(1,1) = r1[5]; M(1,2) = r1[6]; M(1,3) = r1[7]; M(2,0) = r2[4]; M(2,1) = r2[5]; M(2,2) = r2[6]; M(2,3) = r2[7]; M(3,0) = r3[4]; M(3,1) = r3[5]; M(3,2) = r3[6]; M(3,3) = r3[7]; return true; #undef SWAP_ROWS } //----------------------------------------------------------------------------- #undef MAT #undef M //============================================================================= } // namespace ACG //=============================================================================