mat33impl.hpp Source File

00001 //============================================================================
00002 // mat33impl.hpp : 3x3 matrix template.
00003 // This is the template implementation file. It is included by mat33.hpp.
00004 //============================================================================
00005 
00006 //---------------------------------------------------------------------------
00007 // CONSTRUCTORS
00008 //---------------------------------------------------------------------------
00009 template<class Type>
00010 Mat33<Type>::Mat33()
00011 { 
00012   Identity(); 
00013 }
00014 
00015 template<class Type>
00016 Mat33<Type>::Mat33(const Type *N)
00017 {
00018   M[0]=N[0]; M[3]=N[3];  M[6]=N[6];
00019   M[1]=N[1]; M[4]=N[4];  M[7]=N[7];
00020   M[2]=N[2]; M[5]=N[5];  M[8]=N[8];
00021 }
00022 
00023 template<class Type>
00024 Mat33<Type>::Mat33(Type M0, Type M3, Type M6,
00025                    Type M1, Type M4, Type M7,
00026                    Type M2, Type M5, Type M8)
00027       
00028 {
00029   M[0]=M0; M[3]=M3; M[6]=M6;
00030   M[1]=M1; M[4]=M4; M[7]=M7;
00031   M[2]=M2; M[5]=M5; M[8]=M8; 
00032 }
00033 
00034 //---------------------------------------------------------------------------
00035 // MATRIX/MATRIX AND MATRIX/VECTOR OPERATORS
00036 //---------------------------------------------------------------------------
00037 template<class Type>
00038 Mat33<Type>& Mat33<Type>::operator = (const Mat33& A)      // ASSIGNMENT (=)
00039 {
00040   M[0]=A.M[0]; M[3]=A.M[3]; M[6]=A.M[6]; 
00041   M[1]=A.M[1]; M[4]=A.M[4]; M[7]=A.M[7]; 
00042   M[2]=A.M[2]; M[5]=A.M[5]; M[8]=A.M[8];
00043   return(*this);
00044 }
00045 
00046 template<class Type>
00047 Mat33<Type>& Mat33<Type>::operator = (const Type* a) {
00048   for (int i=0;i<9;i++) {
00049     M[i] = a[i];
00050   }
00051   return *this;
00052 }
00053 
00054 template<class Type>
00055 Mat33<Type> Mat33<Type>::operator * (const Mat33& A) const  // MULTIPLICATION (*)
00056 {
00057   Mat33<Type> NewM(
00058               M[0]*A.M[0] + M[3]*A.M[1] + M[6]*A.M[2],      // ROW 1
00059               M[0]*A.M[3] + M[3]*A.M[4] + M[6]*A.M[5],
00060               M[0]*A.M[6] + M[3]*A.M[7] + M[6]*A.M[8],
00061               
00062               M[1]*A.M[0] + M[4]*A.M[1] + M[7]*A.M[2],      // ROW 2
00063               M[1]*A.M[3] + M[4]*A.M[4] + M[7]*A.M[5],
00064               M[1]*A.M[6] + M[4]*A.M[7] + M[7]*A.M[8],
00065               
00066               M[2]*A.M[0] + M[5]*A.M[1] + M[8]*A.M[2],      // ROW 3
00067               M[2]*A.M[3] + M[5]*A.M[4] + M[8]*A.M[5],
00068               M[2]*A.M[6] + M[5]*A.M[7] + M[8]*A.M[8]);              
00069   return(NewM);
00070 }
00071 
00072 // MAT-VECTOR MULTIPLICATION (*)
00073 template<class Type>
00074 Vec3<Type> Mat33<Type>::operator * (const Vec3<Type>& V) const
00075 {
00076   Vec3<Type> NewV;
00077   NewV.x = M[0]*V.x + M[3]*V.y + M[6]*V.z;
00078   NewV.y = M[1]*V.x + M[4]*V.y + M[7]*V.z;
00079   NewV.z = M[2]*V.x + M[5]*V.y + M[8]*V.z;
00080   return(NewV);
00081 }
00082 
00083 // MAT-VECTOR PRE-MULTIPLICATON (*) (non-member)
00084 // interpreted as V^t M  
00085 template<class Type>
00086 Vec3<Type> operator *(const Vec3<Type>& V, const Mat33<Type>& A)
00087 {
00088   Vec3<Type> NewV;
00089   NewV.x = A[0]*V.x + A[1]*V.y + A[2]*V.z;
00090   NewV.y = A[3]*V.x + A[4]*V.y + A[5]*V.z;
00091   NewV.z = A[6]*V.x + A[7]*V.y + A[8]*V.z;
00092   return(NewV);
00093 }
00094 
00095 // SCALAR POST-MULTIPLICATION
00096 template<class Type>
00097 Mat33<Type> Mat33<Type>::operator * (Type a) const
00098 {
00099   Mat33<Type> NewM;
00100   for (int i = 0; i < 9; i++) 
00101     NewM[i] = M[i]*a;
00102   return(NewM);
00103 }
00104 
00105 // SCALAR PRE-MULTIPLICATION (non-member)
00106 template <class Type>
00107 Mat33<Type> operator * (Type a, const Mat44<Type>& M) 
00108 {
00109   Mat33<Type> NewM;
00110   for (int i = 0; i < 9; i++) 
00111     NewM[i] = a*M[i];
00112   return(NewM);
00113 }
00114 
00115 template <class Type>
00116 Mat33<Type> Mat33<Type>::operator / (Type a) const      // SCALAR DIVISION
00117 {
00118   Mat33<Type> NewM;
00119   Type ainv = Type(1.0)/a;
00120   for (int i = 0; i < 9; i++) 
00121     NewM[i] = M[i]*ainv;
00122   return(NewM);
00123 }
00124 
00125 template <class Type>
00126 Mat33<Type> Mat33<Type>::operator + (Mat33& N) const    // ADDITION (+)
00127 {
00128   Mat33<Type> NewM;
00129   for (int i = 0; i < 9; i++) 
00130     NewM[i] = M[i]+N.M[i];
00131   return(NewM);
00132 }
00133 template <class Type>
00134 Mat33<Type>& Mat33<Type>::operator += (Mat33& N)  // ACCUMULATE ADD (+=)
00135 {
00136   for (int i = 0; i < 9; i++) 
00137     M[i] += N.M[i];
00138   return(*this);
00139 }
00140 
00141 template <class Type>
00142 Mat33<Type>& Mat33<Type>::operator -= (Mat33& N)  // ACCUMULATE SUB (-=)
00143 {
00144   for (int i = 0; i < 9; i++) 
00145     M[i] -= N.M[i];
00146   return(*this);
00147 }
00148 
00149 template <class Type>
00150 Mat33<Type>& Mat33<Type>::operator *= (Type a)   // ACCUMULATE MULTIPLY (*=)
00151 {
00152   for (int i = 0; i < 9; i++) 
00153     M[i] *= a;
00154   return(*this);
00155 }
00156 template <class Type>
00157 Mat33<Type>& Mat33<Type>::operator /= (Type a)   // ACCUMULATE DIVIDE (/=)
00158 {
00159   Type ainv = Type(1.0)/a;
00160   for (int i = 0; i < 9; i++) 
00161     M[i] *= ainv;
00162   return(*this);
00163 }
00164 
00165 template<class Type>
00166 bool Mat33<Type>::Inverse(Mat33<Type> &inv, Type tolerance) const // MATRIX INVERSE
00167 {
00168   // Invert using cofactors.  
00169 
00170   inv[0] = M[4]*M[8] - M[7]*M[5];
00171   inv[3] = M[6]*M[5] - M[3]*M[8];
00172   inv[6] = M[3]*M[7] - M[6]*M[4];
00173   inv[1] = M[7]*M[2] - M[1]*M[8];
00174   inv[4] = M[0]*M[8] - M[6]*M[2];
00175   inv[7] = M[6]*M[1] - M[0]*M[7];
00176   inv[2] = M[1]*M[5] - M[4]*M[2];
00177   inv[5] = M[3]*M[2] - M[0]*M[5];
00178   inv[8] = M[0]*M[4] - M[3]*M[1];
00179 
00180   Type det = M[0]*inv[0] + M[3]*inv[1] + M[6]*inv[2];
00181 
00182   if (fabs(det) <= tolerance) // singular
00183     return false;
00184 
00185   Type invDet = 1.0f / det;
00186   for (int i = 0; i < 9; i++) {
00187     inv[i] *= invDet;
00188   }
00189 
00190   return true;
00191 }
00192 
00193 template<class Type>
00194 Mat33<Type>::operator const Type*() const
00195 {
00196   return M;
00197 }
00198 
00199 template<class Type>
00200 Mat33<Type>::operator Type*()
00201 {
00202   return M;
00203 }
00204 
00205 template<class Type>
00206 void Mat33<Type>::RowMajor(Type m[9])
00207 {
00208   m[0] = M[0]; m[1] = M[3]; m[2] = M[6];
00209   m[3] = M[1]; m[4] = M[4]; m[5] = M[7];
00210   m[6] = M[2]; m[7] = M[5]; m[8] = M[8];
00211 }
00212 
00213 template<class Type>
00214 Type& Mat33<Type>::operator()(int col, int row)
00215 {
00216   return M[3*col+row];
00217 }
00218 
00219 template<class Type>
00220 const Type& Mat33<Type>::operator()(int col, int row) const
00221 {
00222   return M[3*col+row];
00223 }
00224 
00225 template<class Type>
00226 void Mat33<Type>::Set(const Type* a)
00227 {
00228   for (int i=0;i<9;i++) {
00229     M[i] = a[i];
00230   }
00231 }
00232 
00233 template<class Type>
00234 void Mat33<Type>::Set(Type M0, Type M3, Type M6,
00235                       Type M1, Type M4, Type M7,
00236                       Type M2, Type M5, Type M8)
00237 {
00238   M[0]=M0; M[3]=M3; M[6]=M6;
00239   M[1]=M1; M[4]=M4; M[7]=M7;
00240   M[2]=M2; M[5]=M5; M[8]=M8; 
00241 }
00242 
00243 
00244 //---------------------------------------------------------------------------
00245 // Standard Matrix Operations
00246 //---------------------------------------------------------------------------
00247 template<class Type>
00248 void Mat33<Type>::Identity()
00249 {
00250   M[0]=M[4]=M[8]=1;
00251   M[1]=M[2]=M[3]=M[5]=M[6]=M[7]=0;
00252 }
00253 template<class Type>
00254 void Mat33<Type>::Zero()
00255 {
00256   M[0]=M[1]=M[2]=M[3]=M[4]=M[5]=M[6]=M[7]=M[8]=0;
00257 }
00258 
00259 template<class Type>
00260 void Mat33<Type>::Transpose()
00261 {
00262   SWAP(M[1],M[3]);
00263   SWAP(M[2],M[6]);
00264   SWAP(M[5],M[7]);
00265 }
00266 
00267 //---------------------------------------------------------------------------
00268 // Standard Matrix Affine Transformations
00269 //---------------------------------------------------------------------------
00270 template<class Type>
00271 void Mat33<Type>::Scale(Type Sx, Type Sy, Type Sz)
00272 {
00273   M[0]=Sx; M[3]=0;  M[6]=0;
00274   M[1]=0;  M[4]=Sy; M[7]=0;
00275   M[2]=0;  M[5]=0;  M[8]=Sz;
00276 }
00277 
00278 template<class Type>
00279 void Mat33<Type>::Scale(const Vec3<Type>& S)
00280 {
00281   M[0]=S.x; M[3]=0;   M[6]=0;  
00282   M[1]=0;   M[4]=S.y; M[7]=0;  
00283   M[2]=0;   M[5]=0;   M[8]=S.z;
00284 }
00285 
00286 template<class Type>
00287 void Mat33<Type>::invScale(Type Sx, Type Sy, Type Sz)
00288 {
00289   M[0]=1/Sx; M[3]=0;    M[6]=0;    
00290   M[1]=0;    M[4]=1/Sy; M[7]=0;    
00291   M[2]=0;    M[5]=0;    M[8]=1/Sz;
00292 }
00293 
00294 template<class Type>
00295 void Mat33<Type>::invScale(const Vec3<Type>& S)
00296 {
00297   M[0]=1/S.x; M[3]=0;     M[6]=0;     
00298   M[1]=0;     M[4]=1/S.y; M[7]=0;     
00299   M[2]=0;     M[5]=0;     M[8]=1/S.z;
00300 }
00301 
00302 template<class Type>
00303 void Mat33<Type>::Rotate(Type DegAng, const Vec3<Type>& Axis)
00304 {
00305   Type RadAng = DegAng*Mat33TORADS;
00306   Type ca=(Type)cos(RadAng),
00307         sa=(Type)sin(RadAng);
00308   if (Axis.x==1 && Axis.y==0 && Axis.z==0)  // ABOUT X-AXIS
00309   {
00310    M[0]=1; M[3]=0;  M[6]=0; 
00311    M[1]=0; M[4]=ca; M[7]=-sa;
00312    M[2]=0; M[5]=sa; M[8]=ca;
00313     
00314   }
00315   else if (Axis.x==0 && Axis.y==1 && Axis.z==0)  // ABOUT Y-AXIS
00316   {
00317    M[0]=ca;  M[3]=0; M[6]=sa; 
00318    M[1]=0;   M[4]=1; M[7]=0;  
00319    M[2]=-sa; M[5]=0; M[8]=ca;
00320   }
00321   else if (Axis.x==0 && Axis.y==0 && Axis.z==1)  // ABOUT Z-AXIS
00322   {
00323    M[0]=ca; M[3]=-sa; M[6]=0;
00324    M[1]=sa; M[4]=ca;  M[7]=0;
00325    M[2]=0;  M[5]=0;   M[8]=1;
00326   }
00327   else                                      // ARBITRARY AXIS
00328   {
00329    Type l = Axis.LengthSqr();
00330    Type x, y, z;
00331    x=Axis.x, y=Axis.y, z=Axis.z;
00332    if (l > Type(1.0001) || l < Type(0.9999) && l!=0)
00333    {
00334      // needs normalization
00335      l=Type(1.0)/sqrt(l);
00336      x*=l; y*=l; z*=l;
00337    }
00338    Type x2=x*x, y2=y*y, z2=z*z;
00339    M[0]=x2+ca*(1-x2); M[3]=(x*y)+ca*(-x*y)+sa*(-z); M[6]=(x*z)+ca*(-x*z)+sa*y;
00340    M[1]=(x*y)+ca*(-x*y)+sa*z; M[4]=y2+ca*(1-y2); M[7]=(y*z)+ca*(-y*z)+sa*(-x);
00341    M[2]=(x*z)+ca*(-x*z)+sa*(-y); M[5]=(y*z)+ca*(-y*z)+sa*x; M[8]=z2+ca*(1-z2);
00342   }
00343 }
00344 
00345 template<class Type>
00346 void Mat33<Type>::invRotate(Type DegAng, const Vec3<Type>& Axis)
00347 {
00348   Rotate(DegAng,Axis);
00349   Transpose();
00350 }
00351 
00352 template <class Type>
00353 inline void Mat33<Type>::Star(const Vec3<Type>& v)
00354 {
00355   M[0]=   0; M[3]=-v.z;  M[6]= v.y;
00356   M[1]= v.z; M[4]=   0;  M[7]=-v.x;
00357   M[2]=-v.y; M[5]= v.x; M[8]=   0;
00358 }
00359 
00360 template <class Type>
00361 inline void Mat33<Type>::OuterProduct(const Vec3<Type>& u, const Vec3<Type>& v)
00362 {
00363   M[0]=u.x*v.x; M[3]=u.x*v.y;  M[6]=u.x*v.z;
00364   M[1]=u.y*v.x; M[4]=u.y*v.y;  M[7]=u.y*v.z;
00365   M[2]=u.z*v.x; M[5]=u.z*v.y; M[8]=u.z*v.z;
00366 }
00367 
00368 template<class Type>
00369 inline Type Mat33<Type>::Trace() const
00370 {
00371   return M[0] + M[4] + M[8];
00372 }
00373 
00374 //---------------------------------------------------------------------------
00375 // Handy matrix printing routine.
00376 //---------------------------------------------------------------------------
00377 template<class Type>
00378 void Mat33<Type>::Print() const
00379 {
00380   printf("\n%f %f %f\n",M[0],M[3],M[6]);
00381   printf("%f %f %f\n",M[1],M[4],M[7]);
00382   printf("%f %f %f\n",M[2],M[5],M[8]);
00383 }
00384 
00385 //---------------------------------------------------------------------------
00386 // Copy contents of matrix into matrix array.
00387 //---------------------------------------------------------------------------
00388 template<class Type>
00389 void Mat33<Type>::CopyInto(Type *Mat) const
00390 {
00391   Mat[0]=M[0]; Mat[3]=M[3];  Mat[6]=M[6];  
00392   Mat[1]=M[1]; Mat[4]=M[4];  Mat[7]=M[7];  
00393   Mat[2]=M[2]; Mat[5]=M[5]; Mat[8]=M[8]; 
00394 }
00395