matrixRT.cpp Source File

00001 /*=============================================================================
00002         File: matrixRT.cpp
00003      Purpose:       
00004     Revision: $Id: matrixRT.cpp,v 1.3 2003/01/13 19:41:35 philosophil Exp $
00005   Created by: Philippe Lavoie          (25 July, 1997)
00006  Modified by: 
00007 
00008  Copyright notice:
00009           Copyright (C) 1996-1997 Philippe Lavoie
00010  
00011           This library is free software; you can redistribute it and/or
00012           modify it under the terms of the GNU Library General Public
00013           License as published by the Free Software Foundation; either
00014           version 2 of the License, or (at your option) any later version.
00015  
00016           This library is distributed in the hope that it will be useful,
00017           but WITHOUT ANY WARRANTY; without even the implied warranty of
00018           MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00019           Library General Public License for more details.
00020  
00021           You should have received a copy of the GNU Library General Public
00022           License along with this library; if not, write to the Free
00023           Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
00024 =============================================================================*/
00025 
00026 #ifndef PLIB_NURBS_MATRIXRT_SOURCE
00027 #define PLIB_NURBS_MATRIXRT_SOURCE
00028 
00029 
00030 #include <matrixRT.h>
00031 
00034 namespace PLib {
00035 
00052 template <class T>
00053 MatrixRT<T>::MatrixRT(T ax, T ay, T az, T x, T y, T z): Matrix<T>(4,4) {
00054   // the following is the same as
00055   // *this = C.translate(x,y,z)*B.rotate(ax,ay,az) ;
00056   rotate(ax,ay,az) ;
00057 #ifdef COLUMN_ORDER
00058   m[12] = x ;
00059   m[13] = y ;
00060   m[14] = z ;  
00061 #else
00062   m[3] = x ;
00063   m[7] = y ;
00064   m[11] = z ;
00065 #endif
00066 }
00067 
00076 template <class T>
00077 MatrixRT<T>::MatrixRT() : Matrix<T>(4,4) {
00078   reset(0) ;
00079   diag(1.0) ;
00080 }
00081 
00092 template <class T>
00093 MatrixRT<T>::MatrixRT(const Matrix<T>& plM) : Matrix<T>(4,4) {
00094   if(plM.rows() == 4 && plM.cols() == 4)
00095     *this = plM;
00096   else{
00097 #ifdef USE_EXCEPTION
00098     throw(WrongSize2D(4,4,plM.rows(),plM.cols()));
00099 #else
00100     Error error("MatrixRT<T>::MatrixRt(const Matrix<T>&)");
00101     error << "The matrix doesn't have the size (4,4)\n" ;
00102     error.fatal();
00103 #endif
00104   }
00105 }
00106 
00107 
00118 template <class T>
00119 MatrixRT<T>::MatrixRT(T* p) : Matrix<T>(p,4,4) {
00120   // nothing to do, p should have all the proper data.
00121 }
00122 
00139 template <class T>
00140 MatrixRT<T>& MatrixRT<T>::rotate(T ax,T ay, T az){
00141   T t1,t2,t4,t6,t7,t8,t10,t13 ;
00142   t1 = cos(az);
00143   t2 = cos(ay);
00144   t4 = sin(az);
00145   t6 = sin(ay);
00146   t7 = t1*t6;
00147   t8 = sin(ax);
00148   t10 = cos(ax);
00149   t13 = t4*t6;
00150 #ifdef COLUMN_ORDER
00151   m[0] = t1*t2;
00152   m[4] = -t4*t2;
00153   m[8] = t6;
00154   m[12] = 0 ;
00155   m[1] = t7*t8+t4*t10;
00156   m[5] = -t13*t8+t1*t10;
00157   m[9] = -t2*t8;
00158   m[13] = 0 ;
00159   m[2] = -t7*t10+t4*t8;
00160   m[6] = t13*t10+t1*t8;
00161   m[10] = t2*t10;
00162   m[14] = m[3] = m[7] = m[11] = 0.0 ;
00163   m[15] = 1.0 ;
00164 #else
00165   m[0] = t1*t2;
00166   m[1] = -t4*t2;
00167   m[2] = t6;
00168   m[3] = 0 ;
00169   m[4] = t7*t8+t4*t10;
00170   m[5] = -t13*t8+t1*t10;
00171   m[6] = -t2*t8;
00172   m[7] = 0 ;
00173   m[8] = -t7*t10+t4*t8;
00174   m[9] = t13*t10+t1*t8;
00175   m[10] = t2*t10;
00176   m[11] = m[12] = m[13] = m[14] = 0 ;
00177   m[15] = 1.0 ;
00178 #endif
00179   return *this ;
00180 }
00181 
00198 template <class T>
00199 MatrixRT<T>& MatrixRT<T>::rotateXYZ(T ax,T ay, T az){
00200   T t1,t2,t4,t5,t7,t8,t9,t17 ;
00201   t1 = (T)cos((double)az);
00202   t2 = (T)cos((double)ay);
00203   t4 = (T)sin((double)az);
00204   t5 = (T)cos((double)ax);
00205   t7 = (T)sin((double)ay);
00206   t8 = t1*t7;
00207   t9 = (T)sin((double)ax);
00208   t17 = t4*t7;
00209 #ifdef COLUMN_ORDER
00210   m[0] = t1*t2;
00211   m[4] = -t4*t5+t8*t9;
00212   m[8] = t4*t9+t8*t5;
00213   m[12] = 0.0 ;
00214   m[1] = t4*t2;
00215   m[5] = t1*t5+t17*t9;
00216   m[9] = -t1*t9+t17*t5;
00217   m[13] = 0.0 ;
00218   m[2] = -t7;
00219   m[6] = t2*t9;
00220   m[10] = t2*t5;
00221   m[14] = m[3] = m[7] = m[11] = 0 ;
00222   m[15] = 1.0 ;
00223 #else
00224   m[0] = t1*t2;
00225   m[1] = -t4*t5+t8*t9;
00226   m[2] = t4*t9+t8*t5;
00227   m[3] = 0.0 ;
00228   m[4] = t4*t2;
00229   m[5] = t1*t5+t17*t9;
00230   m[6] = -t1*t9+t17*t5;
00231   m[7] = 0.0 ;
00232   m[8] = -t7;
00233   m[9] = t2*t9;
00234   m[10] = t2*t5;
00235   m[11] = m[12] = m[13] = m[14] = 0 ;
00236   m[15] = 1.0 ;
00237 #endif
00238   return *this ;
00239 }
00240 
00250 template <class T>
00251 MatrixRT<T>& MatrixRT<T>::translate(T x, T y, T z){
00252   reset(0) ;
00253   diag(1.0) ;
00254 #ifdef COLUMN_ORDER
00255   m[12] = x ;
00256   m[13] = y ;
00257   m[14] = z ;
00258 #else
00259   m[3] = x ;
00260   m[7] = y ;
00261   m[11] = z ;
00262 #endif
00263   return *this ;
00264 }
00265 
00276 template <class T>
00277 MatrixRT<T>& MatrixRT<T>::scale(T x, T y, T z){
00278   reset(0) ;
00279   m[0] = x ;
00280   m[5] = y ;
00281   m[10] = z ;
00282   m[15] = 1.0 ;
00283   return *this ;
00284 }
00285 
00296 template <class T,int N>
00297 HPoint_nD<T,N> operator*(const MatrixRT<T>& M, const HPoint_nD<T,N>& P){
00298   HPoint_nD<T,N> P2 ;
00299 
00300   P2.x() = M(0,0)*(T)P.x() + M(0,1)*(T)P.y() + M(0,2)*(T)P.z() + M(0,3)*(T)P.w() ;
00301   P2.y() = M(1,0)*(T)P.x() + M(1,1)*(T)P.y() + M(1,2)*(T)P.z() + M(1,3)*(T)P.w() ;
00302   P2.z() = M(2,0)*(T)P.x() + M(2,1)*(T)P.y() + M(2,2)*(T)P.z() + M(2,3)*(T)P.w() ;
00303   P2.w() = M(3,0)*(T)P.x() + M(3,1)*(T)P.y() + M(3,2)*(T)P.z() + M(3,3)*(T)P.w() ;
00304 
00305   return P2 ;
00306 }
00307 
00318 template <class T,int N>
00319 Point_nD<T,N> operator*(const MatrixRT<T>& M, const Point_nD<T,N>& P){
00320   Point_nD<T,N> P2 ;
00321 
00322   P2.x() = M(0,0)*(T)P.x() + M(0,1)*(T)P.y() + M(0,2)*(T)P.z() + M(0,3) ;
00323   P2.y() = M(1,0)*(T)P.x() + M(1,1)*(T)P.y() + M(1,2)*(T)P.z() + M(1,3) ;
00324   P2.z() = M(2,0)*(T)P.x() + M(2,1)*(T)P.y() + M(2,2)*(T)P.z() + M(2,3) ;
00325 
00326   return P2 ;
00327 }
00328 
00348 template <class T>
00349 MatrixRT<T> operator*(const MatrixRT<T>& M1, const MatrixRT<T>& M2){
00350   MatrixRT<T> M ;
00351   T *m1,*m2,*m ;
00352   m1 = M1.m ;
00353   m2 = M2.m ;
00354   m = M.m ;
00355 #ifdef COLUMN_ORDER
00356   m[0] = m1[0]*m2[0] + m1[4]*m2[1] + m1[8]*m2[2] + m1[12]*m2[3] ;
00357   m[4] = m1[0]*m2[4] + m1[4]*m2[5] + m1[8]*m2[6] + m1[12]*m2[7] ;
00358   m[8] = m1[0]*m2[8] + m1[4]*m2[9] + m1[8]*m2[10] + m1[12]*m2[11] ;
00359   m[12] = m1[0]*m2[12] + m1[4]*m2[13] + m1[8]*m2[14] + m1[12]*m2[15] ;
00360   
00361   m[1] = m1[1]*m2[0] + m1[5]*m2[1] + m1[9]*m2[2] + m1[13]*m2[3] ;
00362   m[5] = m1[1]*m2[4] + m1[5]*m2[5] + m1[9]*m2[6] + m1[13]*m2[7] ;
00363   m[9] = m1[1]*m2[8] + m1[5]*m2[9] + m1[9]*m2[10] + m1[13]*m2[11] ;
00364   m[13] = m1[1]*m2[12] + m1[5]*m2[13] + m1[9]*m2[14] + m1[13]*m2[15] ;
00365   
00366   m[2] = m1[2]*m2[0] + m1[6]*m2[1] + m1[10]*m2[2] + m1[14]*m2[3] ;
00367   m[6] = m1[2]*m2[4] + m1[6]*m2[5] + m1[10]*m2[6] + m1[14]*m2[7] ;
00368   m[10] = m1[2]*m2[8] + m1[6]*m2[9] + m1[10]*m2[10] + m1[14]*m2[11] ;
00369   m[14] = m1[2]*m2[12] + m1[6]*m2[13] + m1[10]*m2[14] + m1[14]*m2[15] ;
00370   
00371   m[3] = m1[3]*m2[0] + m1[7]*m2[1] + m1[11]*m2[2] + m1[15]*m2[3] ;
00372   m[7] = m1[3]*m2[4] + m1[7]*m2[5] + m1[11]*m2[6] + m1[15]*m2[7] ;
00373   m[11] = m1[3]*m2[8] + m1[7]*m2[9] + m1[11]*m2[10] + m1[15]*m2[11] ;
00374   m[15] = m1[3]*m2[12] + m1[7]*m2[13] + m1[11]*m2[14] + m1[15]*m2[15] ;
00375   
00376 #else
00377   m[0] = m1[0]*m2[0] + m1[1]*m2[4] + m1[2]*m2[8] + m1[3]*m2[12] ;
00378   m[1] = m1[0]*m2[1] + m1[1]*m2[5] + m1[2]*m2[9] + m1[3]*m2[13] ;
00379   m[2] = m1[0]*m2[2] + m1[1]*m2[6] + m1[2]*m2[10] + m1[3]*m2[14] ;
00380   m[3] = m1[0]*m2[3] + m1[1]*m2[7] + m1[2]*m2[11] + m1[3]*m2[15] ;
00381   
00382   m[4] = m1[4]*m2[0] + m1[5]*m2[4] + m1[6]*m2[8] + m1[7]*m2[12] ;
00383   m[5] = m1[4]*m2[1] + m1[5]*m2[5] + m1[6]*m2[9] + m1[7]*m2[13] ;
00384   m[6] = m1[4]*m2[2] + m1[5]*m2[6] + m1[6]*m2[10] + m1[7]*m2[14] ;
00385   m[7] = m1[4]*m2[3] + m1[5]*m2[7] + m1[6]*m2[11] + m1[7]*m2[15] ;
00386   
00387   m[8] = m1[8]*m2[0] + m1[9]*m2[4] + m1[10]*m2[8] + m1[11]*m2[12] ;
00388   m[9] = m1[8]*m2[1] + m1[9]*m2[5] + m1[10]*m2[9] + m1[11]*m2[13] ;
00389   m[10] = m1[8]*m2[2] + m1[9]*m2[6] + m1[10]*m2[10] + m1[11]*m2[14] ;
00390   m[11] = m1[8]*m2[3] + m1[9]*m2[7] + m1[10]*m2[11] + m1[11]*m2[15] ;
00391   
00392   m[12] = m1[12]*m2[0] + m1[13]*m2[4] + m1[14]*m2[8] + m1[15]*m2[12] ;
00393   m[13] = m1[12]*m2[1] + m1[13]*m2[5] + m1[14]*m2[9] + m1[15]*m2[13] ;
00394   m[14] = m1[12]*m2[2] + m1[13]*m2[6] + m1[14]*m2[10] + m1[15]*m2[14] ;
00395   m[15] = m1[12]*m2[3] + m1[13]*m2[7] + m1[14]*m2[11] + m1[15]*m2[15] ;
00396 #endif
00397   return M ;
00398 }
00399 
00400 
00414 template <class T>
00415 MatrixRT<T>& MatrixRT<T>::operator=(const Matrix<T>& M) {
00416   if(M.rows() != 4 || M.cols() != 4){
00417     Error error("MatrixRT<T>::operator=") ;
00418     error << "Trying to assign with a matrix of dimensions" << 
00419       M.rows() << ' ' << M.cols() << endl ;
00420     error.fatal() ;
00421   }
00422   T *a,*b ;
00423   a = m-1 ;
00424   b = M[0] - 1 ;
00425   for(int i=0;i<16;++i){
00426     *(++a) = *(++b) ;
00427   }
00428   return *this ;
00429 }
00430 
00439 template <class T>
00440 MatrixRT<T>& MatrixRT<T>::operator=(const MatrixRT<T>& M) {
00441   T *a,*b ;
00442   a = m-1 ;
00443   b = M.m - 1 ;
00444   for(int i=0;i<16;++i){
00445     *(++a) = *(++b) ;
00446   }
00447   return *this ;
00448 }
00449 
00450 
00451 
00452 #ifdef NO_IMPLICIT_TEMPLATES
00453 
00454 template class MatrixRT<float> ;
00455 template class MatrixRT<double> ;
00456 
00457 template Point_nD<float,3> operator*(const MatrixRT<float>& M, const Point_nD<float,3>& P) ;
00458 template Point_nD<double,3> operator*(const MatrixRT<double>& M, const Point_nD<double,3>& P) ;
00459 template HPoint_nD<float,3> operator*(const MatrixRT<float>& M, const HPoint_nD<float,3>& P) ;
00460 template HPoint_nD<double,3> operator*(const MatrixRT<double>& M, const HPoint_nD<double,3>& P) ;
00461 template MatrixRT<float> operator*(const MatrixRT<float>& M1, const MatrixRT<float>& M2) ;
00462 template MatrixRT<double> operator*(const MatrixRT<double>& M1, const MatrixRT<double>& M2) ;
00463 
00464 template Point_nD<float,2> operator*(const MatrixRT<float>& M, const Point_nD<float,2>& P) ;
00465 template Point_nD<double,2> operator*(const MatrixRT<double>& M, const Point_nD<double,2>& P) ;
00466 template HPoint_nD<float,2> operator*(const MatrixRT<float>& M, const HPoint_nD<float,2>& P) ;
00467 template HPoint_nD<double,2> operator*(const MatrixRT<double>& M, const HPoint_nD<double,2>& P) ;
00468 
00469 
00470 #else
00471 #ifndef USING_VCC 
00472 
00473 void stupidSparcMatrix(){
00474   Point_nD<float,3> a ;
00475   HPoint_nD<float,3> b ;
00476   MatrixRT<float> A ;
00477   MatrixRT<double> B ;
00478   a = A*a ;
00479   b = A*b ;
00480 //  a = B*a ;
00481 //  b = B*b ;
00482 }
00483 #endif // !USING_VCC 
00484 
00485 
00486 #endif 
00487 
00488 } // end namespace
00489 
00490 
00491 #endif // #define PLIB_NURBS_MATRIXRT_SOURCE