Matrix4x4_Impl.h

00001 /*************************************************************************************************
00002  *
00003  * Modeling and animation (TNM079) 2007
00004  * Code base for lab assignments. Copyright:
00005  *   Gunnar Johansson (gunnar.johansson@itn.liu.se)
00006  *   Ken Museth (ken.museth@itn.liu.se)
00007  *   Michael Bang Nielsen (bang@daimi.au.dk)
00008  *   Ola Nilsson (ola.nilsson@itn.liu.se)
00009  *   Andreas Söderström (andreas.soderstrom@itn.liu.se)
00010  *
00011  *************************************************************************************************/
00012 #ifndef _matrix4x4_impl_h
00013 #define _matrix4x4_impl_h
00014 
00015 #include <cmath>
00016 #include "Vector4.h"
00017 #include <algorithm>
00018 
00019 template <typename Real> Matrix4x4<Real> Matrix4x4<Real>::identityMatrix = Matrix4x4<Real>(); 
00020 
00021 template <typename Real>
00022 Matrix4x4<Real>::Matrix4x4()
00023 {
00024   m[0][0] = 1; m[0][1] = 0; m[0][2] = 0; m[0][3] = 0;
00025   m[1][0] = 0; m[1][1] = 1; m[1][2] = 0; m[1][3] = 0;
00026   m[2][0] = 0; m[2][1] = 0; m[2][2] = 1; m[2][3] = 0;
00027   m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
00028 }
00029 
00030 
00031 template <typename Real>
00032 Matrix4x4<Real>::Matrix4x4(const Real m[4][4])
00033 {
00034   this->m[0][0] = m[0][0]; this->m[0][1] = m[0][1]; this->m[0][2] = m[0][2]; this->m[0][3] = m[0][3];
00035   this->m[1][0] = m[1][0]; this->m[1][1] = m[1][1]; this->m[1][2] = m[1][2]; this->m[1][3] = m[1][3];
00036   this->m[2][0] = m[2][0]; this->m[2][1] = m[2][1]; this->m[2][2] = m[2][2]; this->m[2][3] = m[2][3];
00037   this->m[3][0] = m[3][0]; this->m[3][1] = m[3][1]; this->m[3][2] = m[3][2]; this->m[3][3] = m[3][3];
00038 }
00039 
00040 
00041 template <typename Real>
00042 Matrix4x4<Real> Matrix4x4<Real>::identity()
00043 {
00044   return identityMatrix;
00045 }
00046 
00047 
00048 template <typename Real>
00049 Vector4<Real> Matrix4x4<Real>::operator*(const Vector4<Real>& vec4) const
00050 {
00051   Vector4<Real> res;
00052 
00053   res[0] = vec4[0] * m[0][0] + vec4[1] * m[0][1] + vec4[2] * m[0][2] + vec4[3] * m[0][3];
00054   res[1] = vec4[0] * m[1][0] + vec4[1] * m[1][1] + vec4[2] * m[1][2] + vec4[3] * m[1][3];
00055   res[2] = vec4[0] * m[2][0] + vec4[1] * m[2][1] + vec4[2] * m[2][2] + vec4[3] * m[2][3];
00056   res[3] = vec4[0] * m[3][0] + vec4[1] * m[3][1] + vec4[2] * m[3][2] + vec4[3] * m[3][3];
00057 
00058   return res;
00059 }
00060 
00061 
00062 template <typename Real>
00063 Matrix4x4<Real> Matrix4x4<Real>::operator*(const Matrix4x4& m2) const
00064 {
00065   Matrix4x4 res;
00066   unsigned int i, j, k;
00067 
00068   for (i=0; i<4; i++)
00069     {
00070       for (j=0; j<4; j++)
00071         {
00072           res.m[i][j] = Real(0.0);
00073           for (k=0; k<4; k++)
00074             {
00075               res.m[i][j] += m[i][k] * m2.m[k][j];
00076             }
00077         }
00078     }
00079 
00080   return res;
00081 }
00082 
00083 
00084 // Input: i (row), j (column)
00085 template <typename Real>
00086 Real &Matrix4x4<Real>::operator()(unsigned int i, unsigned int j)
00087 {
00088   return m[i][j];
00089 }
00090 
00091 
00092 template <typename Real>
00093 Matrix4x4<Real> Matrix4x4<Real>::inverse() const
00094 {
00095   Matrix4x4 a = Matrix4x4(m);  // the inverse
00096   Matrix4x4 b = identity();
00097                 
00098 
00100   // Taken from Numerical Recipes in C++;
00101   // void NR::gaussj(Mat_IO_DP &a, Mat_IO_DP &b)
00102 
00103   int i,icol,irow,j,k,l,ll;
00104   Real big,dum,pivinv,temp;
00105 
00106   //int n=a.nrows();
00107   int n = 4;
00108   //int m=b.ncols();
00109   int m = 4;
00110   //Vec_INT indxc(n),indxr(n),ipiv(n);
00111   Vector4<int> indxc, indxr, ipiv;
00112   for (j=0;j<n;j++) ipiv[j]=0;
00113   for (i=0;i<n;i++) {
00114     big=0.0;
00115     for (j=0;j<n;j++)
00116       if (ipiv[j] != 1)
00117         for (k=0;k<n;k++) {
00118           if (ipiv[k] == 0) {
00119             if (fabs(a[j][k]) >= big) {
00120               big=fabs(a[j][k]);
00121               irow=j;
00122               icol=k;
00123             }
00124           }
00125         }
00126     ++(ipiv[icol]);
00127     if (irow != icol) {
00128       for (l=0;l<n;l++) std::swap(a[irow][l],a[icol][l]);
00129       for (l=0;l<m;l++) std::swap(b[irow][l],b[icol][l]);
00130     }
00131     indxr[i]=irow;
00132     indxc[i]=icol;
00133     //if (a[icol][icol] == 0.0) nrerror("gaussj: Singular Matrix");
00134     pivinv=1.0/a[icol][icol];
00135     a[icol][icol]=1.0;
00136     for (l=0;l<n;l++) a[icol][l] *= pivinv;
00137     for (l=0;l<m;l++) b[icol][l] *= pivinv;
00138     for (ll=0;ll<n;ll++)
00139       if (ll != icol) {
00140         dum=a[ll][icol];
00141         a[ll][icol]=0.0;
00142         for (l=0;l<n;l++) a[ll][l] -= a[icol][l]*dum;
00143         for (l=0;l<m;l++) b[ll][l] -= b[icol][l]*dum;
00144       }
00145   }
00146   for (l=n-1;l>=0;l--) {
00147     if (indxr[l] != indxc[l])
00148       for (k=0;k<n;k++)
00149         std::swap(a[k][indxr[l]],a[k][indxc[l]]);
00150   }
00152 
00153 
00154   return a;
00155 }
00156 
00157 
00158 
00159 // Static methods
00160 
00161 template <typename Real>
00162 Matrix4x4<Real> Matrix4x4<Real>::scale(Real sx, Real sy, Real sz)
00163 {
00164   Real m[4][4];
00165 
00166   m[0][0] = sx; m[0][1] = 0; m[0][2] = 0; m[0][3] = 0;
00167   m[1][0] = 0; m[1][1] = sy; m[1][2] = 0; m[1][3] = 0;
00168   m[2][0] = 0; m[2][1] = 0; m[2][2] = sz; m[2][3] = 0;
00169   m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
00170 
00171   return Matrix4x4(m);
00172 }
00173 
00174 
00175 template <typename Real>
00176 Matrix4x4<Real> Matrix4x4<Real>::scale(Real scale)
00177 {
00178   Real m[4][4];
00179 
00180   m[0][0] = scale; m[0][1] = 0; m[0][2] = 0; m[0][3] = 0;
00181   m[1][0] = 0; m[1][1] = scale; m[1][2] = 0; m[1][3] = 0;
00182   m[2][0] = 0; m[2][1] = 0; m[2][2] = scale; m[2][3] = 0;
00183   m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
00184 
00185   return Matrix4x4(m);
00186 }
00187 
00188 template <typename Real>
00189 Matrix4x4<Real> Matrix4x4<Real>::rotationXYZ(Real rx, Real ry, Real rz)
00190 {
00191   Real m[4][4];
00192 
00193   m[0][0] = cos(ry) * cos(rz); 
00194   m[0][1] = cos(ry) * sin(rz); 
00195   m[0][2] = -sin(ry); 
00196   m[0][3] = 0;
00197   m[1][0] = sin(rx) * sin(ry) * cos(rz) - cos(rx) * sin(rz); 
00198   m[1][1] = sin(rx) * sin(ry) * sin(rz) + cos(rx) * cos(rz); 
00199   m[1][2] = cos(ry) * sin(rx); 
00200   m[1][3] = 0;
00201   m[2][0] = cos(rx) * sin(ry) * cos(rz) + sin(rx) * sin(rz);
00202   m[2][1] = cos(rx) * sin(ry) * sin(rz) - sin(rx) * cos(rz); 
00203   m[2][2] = cos(ry) * cos(rx); 
00204   m[2][3] = 0;
00205   m[3][0] = 0; 
00206   m[3][1] = 0; 
00207   m[3][2] = 0; 
00208   m[3][3] = 1;
00209 
00210   return Matrix4x4(m);
00211 }
00212 
00213 template <typename Real>
00214 Matrix4x4<Real> Matrix4x4<Real>::translation(Real tx, Real ty, Real tz)
00215 {
00216   Real m[4][4];
00217 
00218   m[0][0] = 1; m[0][1] = 0; m[0][2] = 0; m[0][3] = tx;
00219   m[1][0] = 0; m[1][1] = 1; m[1][2] = 0; m[1][3] = ty;
00220   m[2][0] = 0; m[2][1] = 0; m[2][2] = 1; m[2][3] = tz;
00221   m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
00222 
00223   return Matrix4x4(m);
00224 }
00225 
00226 template <typename Real>
00227 void Matrix4x4<Real>::toGLMatrix(float glMatrix[16]) const
00228 {
00229   int glIndex = 0;
00230   for (int i = 0; i < 4; i++){
00231     for (int j = 0; j < 4; j++){
00232       glMatrix[glIndex] = m[j][i]; 
00233       glIndex++;
00234     }
00235   }     
00236 }
00237 
00238 #endif

Generated on Fri Jul 20 23:57:42 2007 for HalfEdge by  doxygen 1.5.1