Matrix4x4T.cc

00001 /*===========================================================================*\
00002  *                                                                           *
00003  *                              OpenFlipper                                  *
00004  *      Copyright (C) 2001-2009 by Computer Graphics Group, RWTH Aachen      *
00005  *                           www.openflipper.org                             *
00006  *                                                                           *
00007  *---------------------------------------------------------------------------*
00008  *  This file is part of OpenFlipper.                                        *
00009  *                                                                           *
00010  *  OpenFlipper is free software: you can redistribute it and/or modify      *
00011  *  it under the terms of the GNU Lesser General Public License as           *
00012  *  published by the Free Software Foundation, either version 3 of           *
00013  *  the License, or (at your option) any later version with the              *
00014  *  following exceptions:                                                    *
00015  *                                                                           *
00016  *  If other files instantiate templates or use macros                       *
00017  *  or inline functions from this file, or you compile this file and         *
00018  *  link it with other files to produce an executable, this file does        *
00019  *  not by itself cause the resulting executable to be covered by the        *
00020  *  GNU Lesser General Public License. This exception does not however       *
00021  *  invalidate any other reasons why the executable file might be            *
00022  *  covered by the GNU Lesser General Public License.                        *
00023  *                                                                           *
00024  *  OpenFlipper is distributed in the hope that it will be useful,           *
00025  *  but WITHOUT ANY WARRANTY; without even the implied warranty of           *
00026  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the            *
00027  *  GNU Lesser General Public License for more details.                      *
00028  *                                                                           *
00029  *  You should have received a copy of the GNU LesserGeneral Public          *
00030  *  License along with OpenFlipper. If not,                                  *
00031  *  see <http://www.gnu.org/licenses/>.                                      *
00032  *                                                                           *
00033 \*===========================================================================*/
00034 
00035 /*===========================================================================*\
00036  *                                                                           *
00037  *   $Revision: 6743 $                                                       *
00038  *   $Author: moebius $                                                      *
00039  *   $Date: 2009-08-05 11:03:10 +0200 (Mi, 05. Aug 2009) $                   *
00040  *                                                                           *
00041 \*===========================================================================*/
00042 
00043 
00044 
00045 //=============================================================================
00046 //
00047 //  CLASS Matrix4x4T - IMPLEMENTATION
00048 //
00049 //=============================================================================
00050 
00051 
00052 #define ACG_MATRIX4X4_C
00053 
00054 
00055 //== INCLUDES =================================================================
00056 
00057 
00058 #include "Matrix4x4T.hh"
00059 #include "../Utils/NumLimitsT.hh"
00060 
00061 
00062 //== IMPLEMENTATION ========================================================== 
00063 
00064 
00065 namespace ACG {
00066 
00067 
00068 #define MAT(m,r,c)  ((m)[(r)+((c)<<2)])
00069 #define M(r,w)      (MAT(mat_,r,w))
00070 
00071 
00072 //-----------------------------------------------------------------------------
00073 
00074 
00075 template <typename Scalar> 
00076 Matrix4x4T<Scalar>
00077 Matrix4x4T<Scalar>::
00078 operator* (const Matrix4x4T<Scalar>& _rhs) const
00079 {
00080 #define RHS(row,col)  MAT(_rhs.mat_, row,col)
00081 #define TMP(row,col)  MAT(tmp.mat_, row,col)
00082 
00083   Matrix4x4T<Scalar> tmp;
00084   Scalar mi0, mi1, mi2, mi3;
00085   int i;
00086 
00087   for (i = 0; i < 4; i++) {
00088     mi0=M(i,0);  mi1=M(i,1);  mi2=M(i,2);  mi3=M(i,3);
00089     TMP(i,0) = mi0*RHS(0,0) + mi1*RHS(1,0) + mi2*RHS(2,0) + mi3*RHS(3,0);
00090     TMP(i,1) = mi0*RHS(0,1) + mi1*RHS(1,1) + mi2*RHS(2,1) + mi3*RHS(3,1);
00091     TMP(i,2) = mi0*RHS(0,2) + mi1*RHS(1,2) + mi2*RHS(2,2) + mi3*RHS(3,2);
00092     TMP(i,3) = mi0*RHS(0,3) + mi1*RHS(1,3) + mi2*RHS(2,3) + mi3*RHS(3,3);
00093   }
00094 
00095   return tmp;
00096 
00097 #undef RHS
00098 #undef TMP
00099 }
00100 
00101 
00102 //-----------------------------------------------------------------------------
00103 
00104 
00105 template <typename Scalar> 
00106 Matrix4x4T<Scalar>&
00107 Matrix4x4T<Scalar>::
00108 operator*= (const Matrix4x4T<Scalar>& _rhs)
00109 {
00110 #define RHS(row,col)  MAT(_rhs.mat_, row,col)
00111 
00112   int i;
00113   Scalar mi0, mi1, mi2, mi3;
00114 
00115   for (i = 0; i < 4; i++) 
00116   {
00117     mi0=M(i,0);  mi1=M(i,1);  mi2=M(i,2);  mi3=M(i,3);
00118     M(i,0) = mi0 * RHS(0,0) + mi1 * RHS(1,0) + mi2 * RHS(2,0) + mi3 * RHS(3,0);
00119     M(i,1) = mi0 * RHS(0,1) + mi1 * RHS(1,1) + mi2 * RHS(2,1) + mi3 * RHS(3,1);
00120     M(i,2) = mi0 * RHS(0,2) + mi1 * RHS(1,2) + mi2 * RHS(2,2) + mi3 * RHS(3,2);
00121     M(i,3) = mi0 * RHS(0,3) + mi1 * RHS(1,3) + mi2 * RHS(2,3) + mi3 * RHS(3,3);
00122   }
00123 
00124   return *this;
00125 
00126 #undef RHS
00127 }
00128 
00129 
00130 //-----------------------------------------------------------------------------
00131 
00132 
00133 template <typename Scalar> 
00134 Matrix4x4T<Scalar>&
00135 Matrix4x4T<Scalar>::
00136 leftMult(const Matrix4x4T<Scalar>& _rhs)
00137 {
00138 #define RHS(row,col)  MAT(_rhs.mat_, row,col)
00139   int i;
00140   Scalar m0i, m1i, m2i, m3i; 
00141   for(i=0;i<4;i++)
00142   {
00143     m0i = M(0,i);  m1i = M(1,i);  m2i = M(2,i);  m3i = M(3,i); 
00144     M(0,i) = RHS(0,0)*m0i + RHS(0,1)*m1i + RHS(0,2)*m2i + RHS(0,3)*m3i;
00145     M(1,i) = RHS(1,0)*m0i + RHS(1,1)*m1i + RHS(1,2)*m2i + RHS(1,3)*m3i;
00146     M(2,i) = RHS(2,0)*m0i + RHS(2,1)*m1i + RHS(2,2)*m2i + RHS(2,3)*m3i;
00147     M(3,i) = RHS(3,0)*m0i + RHS(3,1)*m1i + RHS(3,2)*m2i + RHS(3,3)*m3i;
00148   }
00149   return *this;
00150 #undef RHS
00151 }  
00152 
00153 
00154 //-----------------------------------------------------------------------------
00155 
00156 
00157 template <typename Scalar> 
00158 template <typename T>
00159 VectorT<T,4>
00160 Matrix4x4T<Scalar>::
00161 operator*(const VectorT<T,4>& _v) const
00162 {
00163   return VectorT<T,4> (
00164     M(0,0)*_v[0] + M(0,1)*_v[1] + M(0,2)*_v[2] + M(0,3)*_v[3],
00165     M(1,0)*_v[0] + M(1,1)*_v[1] + M(1,2)*_v[2] + M(1,3)*_v[3],
00166     M(2,0)*_v[0] + M(2,1)*_v[1] + M(2,2)*_v[2] + M(2,3)*_v[3],
00167     M(3,0)*_v[0] + M(3,1)*_v[1] + M(3,2)*_v[2] + M(3,3)*_v[3]);
00168 }
00169 
00170 
00171 //-----------------------------------------------------------------------------
00172 
00173 
00174 template <typename Scalar> 
00175 template <typename T>
00176 VectorT<T,3>
00177 Matrix4x4T<Scalar>::
00178 transform_point(const VectorT<T,3>& _v) const
00179 {
00180   Scalar x = M(0,0)*_v[0] + M(0,1)*_v[1] + M(0,2)*_v[2] + M(0,3);
00181   Scalar y = M(1,0)*_v[0] + M(1,1)*_v[1] + M(1,2)*_v[2] + M(1,3);
00182   Scalar z = M(2,0)*_v[0] + M(2,1)*_v[1] + M(2,2)*_v[2] + M(2,3);
00183   Scalar w = M(3,0)*_v[0] + M(3,1)*_v[1] + M(3,2)*_v[2] + M(3,3);
00184 
00185   if (w)
00186   {
00187     w = 1.0 / w;
00188     return VectorT<T,3>(x*w, y*w, z*w);
00189   } 
00190   else return VectorT<T,3>(x, y, z);
00191 }
00192 
00193 
00194 //-----------------------------------------------------------------------------
00195 
00196 
00197 template <typename Scalar> 
00198 template <typename T>
00199 VectorT<T,3>
00200 Matrix4x4T<Scalar>::
00201 transform_vector(const VectorT<T,3>& _v) const
00202 {
00203   Scalar x = M(0,0)*_v[0] + M(0,1)*_v[1] + M(0,2)*_v[2];
00204   Scalar y = M(1,0)*_v[0] + M(1,1)*_v[1] + M(1,2)*_v[2];
00205   Scalar z = M(2,0)*_v[0] + M(2,1)*_v[1] + M(2,2)*_v[2];
00206   return VectorT<T,3>(x, y, z);
00207 }
00208 
00209 
00210 //-----------------------------------------------------------------------------
00211 
00212 
00213 template <typename Scalar> 
00214 void
00215 Matrix4x4T<Scalar>::
00216 clear()
00217 {
00218   Scalar* m = mat_;
00219   *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; 
00220   *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; 
00221   *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; 
00222   *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m   = 0.0; 
00223 }
00224 
00225 
00226 //-----------------------------------------------------------------------------
00227 
00228 
00229 template <typename Scalar> 
00230 void
00231 Matrix4x4T<Scalar>::
00232 identity()
00233 {
00234   Scalar* m = mat_;
00235   *m++ = 1.0; *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; 
00236   *m++ = 0.0; *m++ = 1.0; *m++ = 0.0; *m++ = 0.0; 
00237   *m++ = 0.0; *m++ = 0.0; *m++ = 1.0; *m++ = 0.0; 
00238   *m++ = 0.0; *m++ = 0.0; *m++ = 0.0; *m   = 1.0; 
00239 }
00240 
00241 
00242 //-----------------------------------------------------------------------------
00243 
00244 
00245 template <typename Scalar> 
00246 void
00247 Matrix4x4T<Scalar>::
00248 transpose()
00249 {
00250   Scalar tmp;
00251   for( int i=0; i<4; i++ ) 
00252   {
00253     for( int j=i+1; j<4; j++ ) 
00254     {
00255       tmp           = MAT(mat_,i,j);
00256       MAT(mat_,i,j) = MAT(mat_,j,i);
00257       MAT(mat_,j,i) = tmp;
00258     }
00259   }
00260 }
00261   
00262 
00263 //-----------------------------------------------------------------------------
00264 
00265 
00266 /*
00267  * Compute inverse of 4x4 transformation matrix.
00268  * Taken from Mesa3.1
00269  * Code contributed by Jacques Leroy jle@star.be */
00270 template <typename Scalar> 
00271 bool
00272 Matrix4x4T<Scalar>::
00273 invert()
00274 {
00275 #define SWAP_ROWS(a, b) { Scalar *_tmp = a; (a)=(b); (b)=_tmp; }
00276 
00277   Scalar wtmp[4][8];
00278   Scalar m0, m1, m2, m3, s;
00279   Scalar *r0, *r1, *r2, *r3;
00280   
00281   r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];
00282   
00283   r0[0] = M(0,0); r0[1] = M(0,1);
00284   r0[2] = M(0,2); r0[3] = M(0,3);
00285   r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0;
00286     
00287   r1[0] = M(1,0); r1[1] = M(1,1);
00288   r1[2] = M(1,2); r1[3] = M(1,3);
00289   r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0;
00290   
00291   r2[0] = M(2,0); r2[1] = M(2,1);
00292   r2[2] = M(2,2); r2[3] = M(2,3);
00293   r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0;
00294   
00295   r3[0] = M(3,0); r3[1] = M(3,1);
00296   r3[2] = M(3,2); r3[3] = M(3,3);
00297   r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0;
00298   
00299 
00300   /* choose pivot - or die */
00301   if (fabs(r3[0])>fabs(r2[0])) SWAP_ROWS(r3, r2);
00302   if (fabs(r2[0])>fabs(r1[0])) SWAP_ROWS(r2, r1);
00303   if (fabs(r1[0])>fabs(r0[0])) SWAP_ROWS(r1, r0);
00304   if (0.0 == r0[0])  return false;
00305 
00306   
00307   /* eliminate first variable     */
00308   m1 = r1[0]/r0[0]; m2 = r2[0]/r0[0]; m3 = r3[0]/r0[0];
00309   s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s;
00310   s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s;
00311   s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s;
00312   s = r0[4];
00313   if (s != 0.0) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; }
00314   s = r0[5];
00315   if (s != 0.0) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; }
00316   s = r0[6];
00317   if (s != 0.0) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; }
00318   s = r0[7];
00319   if (s != 0.0) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; }
00320   
00321 
00322   /* choose pivot - or die */
00323   if (fabs(r3[1])>fabs(r2[1])) SWAP_ROWS(r3, r2);
00324   if (fabs(r2[1])>fabs(r1[1])) SWAP_ROWS(r2, r1);
00325   if (0.0 == r1[1])  return false;
00326   
00327 
00328   /* eliminate second variable */
00329   m2 = r2[1]/r1[1]; m3 = r3[1]/r1[1];
00330   r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2];
00331   r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3];
00332   s = r1[4]; if (0.0 != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; }
00333   s = r1[5]; if (0.0 != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; }
00334   s = r1[6]; if (0.0 != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; }
00335   s = r1[7]; if (0.0 != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; }
00336   
00337 
00338   /* choose pivot - or die */
00339   if (fabs(r3[2])>fabs(r2[2])) SWAP_ROWS(r3, r2);
00340   if (0.0 == r2[2])  return false;
00341   
00342   /* eliminate third variable */
00343   m3 = r3[2]/r2[2];
00344   r3[3] -= m3 * r2[3]; 
00345   r3[4] -= m3 * r2[4];
00346   r3[5] -= m3 * r2[5]; 
00347   r3[6] -= m3 * r2[6];
00348   r3[7] -= m3 * r2[7];
00349   
00350   /* last check */
00351   if (0.0 == r3[3]) return false;
00352   
00353   s = 1.0/r3[3];              /* now back substitute row 3 */
00354   r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s;
00355   
00356   m2 = r2[3];                 /* now back substitute row 2 */
00357   s  = 1.0/r2[2];
00358   r2[4] = s * (r2[4] - r3[4] * m2); r2[5] = s * (r2[5] - r3[5] * m2);
00359   r2[6] = s * (r2[6] - r3[6] * m2); r2[7] = s * (r2[7] - r3[7] * m2);
00360   m1 = r1[3];
00361   r1[4] -= r3[4] * m1; r1[5] -= r3[5] * m1;
00362   r1[6] -= r3[6] * m1; r1[7] -= r3[7] * m1;
00363   m0 = r0[3];
00364   r0[4] -= r3[4] * m0; r0[5] -= r3[5] * m0;
00365   r0[6] -= r3[6] * m0; r0[7] -= r3[7] * m0;
00366   
00367   m1 = r1[2];                 /* now back substitute row 1 */
00368   s  = 1.0/r1[1];
00369   r1[4] = s * (r1[4] - r2[4] * m1); r1[5] = s * (r1[5] - r2[5] * m1);
00370   r1[6] = s * (r1[6] - r2[6] * m1); r1[7] = s * (r1[7] - r2[7] * m1);
00371   m0 = r0[2];
00372   r0[4] -= r2[4] * m0; r0[5] -= r2[5] * m0;
00373   r0[6] -= r2[6] * m0; r0[7] -= r2[7] * m0;
00374   
00375   m0 = r0[1];                 /* now back substitute row 0 */
00376   s  = 1.0/r0[0];
00377   r0[4] = s * (r0[4] - r1[4] * m0); r0[5] = s * (r0[5] - r1[5] * m0);
00378   r0[6] = s * (r0[6] - r1[6] * m0); r0[7] = s * (r0[7] - r1[7] * m0);
00379   
00380   M(0,0) = r0[4]; M(0,1) = r0[5];
00381   M(0,2) = r0[6]; M(0,3) = r0[7];
00382   M(1,0) = r1[4]; M(1,1) = r1[5];
00383   M(1,2) = r1[6]; M(1,3) = r1[7];
00384   M(2,0) = r2[4]; M(2,1) = r2[5];
00385   M(2,2) = r2[6]; M(2,3) = r2[7];
00386   M(3,0) = r3[4]; M(3,1) = r3[5];
00387   M(3,2) = r3[6]; M(3,3) = r3[7]; 
00388   
00389   return true;
00390 #undef SWAP_ROWS
00391 }
00392 
00393 
00394 //-----------------------------------------------------------------------------
00395 
00396 
00397 #undef MAT
00398 #undef M
00399 
00400 
00401 //=============================================================================
00402 } // namespace ACG
00403 //=============================================================================
acg pic Project OpenFlipper, ©  Computer Graphics Group, RWTH Aachen. Documentation generated using doxygen .