 |
TAPs 0.7.7.3
|
|
TAPs 0.7.7.3
|
00001 /****************************************************************************** 00002 TAPsGenMath.cpp 00003 00004 Under TAPs namespace 00005 Defines a GenMath class for general math. 00006 All member functions and data members are static. 00007 00008 SUKITTI PUNAK (01/20/2006) 00009 UPDATE (02/01/2006) 00010 ******************************************************************************/ 00011 #include "TAPsGenMath.hpp" 00012 // Using Inclusion Model (i.e. definitions are included in declarations) 00013 // (this name.cpp is included in name.hpp) 00014 // Each friend is defined directly inside its declaration. 00015 00016 BEGIN_NAMESPACE_TAPs 00017 //============================================================================= 00018 // Root Finding Fns 00019 //============================================================================= 00020 //----------------------------------------------------------------------------- 00021 // Root of second degree polynomial 00022 //----------------------------------------------------------------------------- 00023 // FN: rootsOfQuadraticPolynomial() ************************************** 00024 // DESC: Find all three roots of a cubic polynomial in this form; 00025 // a*x^2 + b*x + c = 0 00026 //The formula is adapted from "Mathematical Handbook of Formulas and Tables" 00027 // I/P: a, b, and c 00028 // O/P: r1 and r2 00029 // (r1 and r2 may be real or complex conjugate of one another) 00030 template <typename T> 00031 void GenMath<T>::RootsOfQuadraticPolynomial ( 00032 T a, T b, T c, // I/P: a*x^2 + b*x + c = 0 00033 std::complex<T> &r1, // O/P: root number one 00034 std::complex<T> &r2 ) // O/P: root number two 00035 { 00036 //-------------------------------------------------------------------- 00037 // If a, b, c are real and if D = b^2 - 4ac is the discriminant, 00038 // then the roots are 00039 // (i) real and unequal if D > 0 00040 // (ii) real and equal if D = 0 00041 // (iii) complex conjugate if D < 0 00042 //-------------------------------------------------------------------- 00043 T D = b*b - 4*a*c; // discriminant 00044 if ( D == 0 ) { // the roots are real and equal 00045 r1 = r2 = -b / (2*a); 00046 } 00047 if ( D > 0 ) { // the roots are real and unequal 00048 T Dh = sqrt(D); 00049 r1 = ( -b - Dh ) / (2*a); 00050 r2 = ( -b + Dh ) / (2*a); 00051 } 00052 else { // the roots are complex conjugate 00053 r1 = std::complex<T>( -b/(2*a), sqrt(-D)/(2*a) ); 00054 r2 = conj(r1); 00055 } 00056 //-------------------------------------------------------------------- 00057 /* 00058 // DEBUG: 00059 std::cout.precision( 20 ); 00060 std::cout << "D = " << D << "\n"; 00061 std::cout << "Root#1: " << r1 << " Root#2: " << r2 << "\n"; 00062 std::cout.precision( 6 ); 00063 //*/ 00064 //-------------------------------------------------------------------- 00065 } 00066 //----------------------------------------------------------------------------- 00067 00068 //----------------------------------------------------------------------------- 00069 // Root of third degree polynomial 00070 //----------------------------------------------------------------------------- 00071 // FN: rootsOfCubicPolynomial() ********************************************** 00072 // DESC: Find all three roots of a cubic polynomial in this form; 00073 // a*x^3 + b*x^2 + c*x + d = 0 00074 //The formula is adapted from "Mathematical Handbook of Formulas and Tables" 00075 // I/P: a, b, c, and d 00076 // O/P: r1, r2, and r3 ( r1 is the real root) 00077 // (r2 and r3 may be real or complex conjugate of one another) 00078 template <typename T> 00079 void GenMath<T>::RootsOfCubicPolynomial ( 00080 T a, T b, T c, T d, // I/P: a*x^3 + b*x^2 + c*x + d = 0 00081 std::complex<T> &r1, // O/P: root number one 00082 std::complex<T> &r2, // O/P: root number two 00083 std::complex<T> &r3 ) // O/P: root number three 00084 { 00085 T a1, a2, a3; 00086 T Q, R, D; 00087 std::complex<T> SS, TT; 00088 T zeta; 00089 //-------------------------------------------------------------------- 00090 a1 = b/a; a2 = c/a; a3 = d/a; 00091 Q = (3.0*a2 - a1*a1) / 9.0; 00092 R = (9.0*a1*a2 - 27.0*a3 - 2.0*a1*a1*a1) / 54.0; 00093 D = Q*Q*Q + R*R; 00094 //-------------------------------------------------------------------- 00095 // If a1, a2, a3 are real and if D = Q^3 + R^2 is the discriminant, then 00096 // (i) one root is real and two complex conjugate if D > 0 00097 // (ii) all roots are real and at least two are equal if D = 0 00098 // (iii) all roots are real and unequal if D < 0. 00099 //-------------------------------------------------------------------- 00100 if ( D >= 0 ) { 00101 complex<T> Dh = sqrt(D); 00102 T div = 1.0/3.0; 00103 SS = pow(R + Dh, div); 00104 TT = pow(R - Dh, div); 00105 r1 = SS + TT - a1/3.0; 00106 r2 = -(SS + TT)/2.0 - a1/3.0; 00107 std::complex<T> i( 0, sqrt(1.0) ); 00108 r2 += i*sqrt(3.0)*(SS - TT)/2.0; 00109 r3 = conj(r2); 00110 } 00111 //-------------------------------------------------------------------- 00112 // All roots are real and unequal if D < 0. 00113 // The formula is simplified by use of trigonometry 00114 else { 00115 zeta = acos( R / sqrt(-(Q*Q*Q)) ); 00116 r1 = 2 * sqrt(-Q) * cos(zeta/3.0) - a1/3.0; 00117 r2 = 2 * sqrt(-Q) * cos(zeta/3.0 + 2.0/3.0*Math<T>::PI) - a1/3.0; 00118 r3 = 2 * sqrt(-Q) * cos(zeta/3.0 + 4.0/3.0*Math<T>::PI) - a1/3.0; 00119 } 00120 //-------------------------------------------------------------------- 00121 /* 00122 // DEBUG: 00123 std::cout.precision( 20 ); 00124 std::cout << "D = " << D << "\n"; 00125 std::cout << "Root#1: " << r1 << " Root#2: " << r2 << " Root#3: " << r3 << "\n"; 00126 std::cout.precision( 6 ); 00127 //*/ 00128 //-------------------------------------------------------------------- 00129 } // END FN: rootsOfCubicPolynomial() 00130 //----------------------------------------------------------------------------- 00131 //============================================================================= 00132 00133 //============================================================================= 00134 // Matrix Fns 00135 //============================================================================= 00136 //----------------------------------------------------------------------------- 00137 // FN: eigenValsAndVecsOf3by3SymMatrixUsingQRFactor() ********************* 00138 // DESC: Find eigen vectors of a 3x3 symmetric matrix which has all real 00139 // eigen values 00140 // I/P: (T) m11, m12, m13, m22, m23, m33 00141 // O/P: (T) lamda1, lamda2, lamda3 00142 // O/P: (T) v1[3], v2[3], v3[3] 00143 template <typename T> 00144 void GenMath<T>::EigenValsAndVecsOf3by3SymMatrixUsingQRFactor ( 00145 T m11, T m12, T m13, // I/P: 00146 T m22, T m23, // I/P: 00147 T m33, // I/P: 00148 T &l1, T &l2, T &l3, // O/P: eigenvalues 00149 T v1[3], T v2[3], T v3[3], // O/P: eigenvectors 00150 T errThreshold, // I/P: error threshold 00151 int times_limit ) // I/P: iteration limit 00152 { 00153 //T errThreshold = 5E-3; // error tolerant value 00154 int iterationTimes = 1; 00155 //---------------------------------------------------------------- 00156 Matrixp<T> IT( 3, 3 ); // 3x3 matrix 00157 Matrixp<T> EG( 3, 3 ); // 3x3 matrix 00158 IT(0,0) = m11; IT(0,1) = m12; IT(0,2) = m13; 00159 IT(1,0) = m12; IT(1,1) = m22; IT(1,2) = m23; 00160 IT(2,0) = m13; IT(2,1) = m23; IT(2,2) = m33; 00161 T oldEigVal[3]; 00162 l1 = l2 = l3 = 0; 00163 //---------------------------------------------------------------- 00164 do { 00165 oldEigVal[0] = l1; 00166 oldEigVal[1] = l2; 00167 oldEigVal[2] = l3; 00168 ++iterationTimes; 00169 EigenValsByQRFactorization( IT, iterationTimes, &EG ); 00170 l1 = EG( 0, 0 ); 00171 l2 = EG( 1, 1 ); 00172 l3 = EG( 2, 2 ); 00173 // If all eigen values are equal, then eigen vectors can be 00174 // any vectors that are orthogonal to one another. 00175 // Also in this case, discriminant == 0. 00176 //if ( fabs(m12) < errThreshold && fabs(m13) < errThreshold && fabs(m23) < errThreshold ) { 00177 const T ratio = 10000.0; 00178 if ( fabs(l1 - l2) < fabs(l1/ratio) ) { 00179 if ( fabs(l1 - l3) < fabs(l1/ratio) ) { // CASE: l1 == l2 == l3 00180 v1[0] = 1; v1[1] = 0; v1[2] = 0; 00181 v2[0] = 0; v2[1] = 1; v2[2] = 0; 00182 v3[0] = 0; v3[1] = 0; v3[2] = 1; 00183 //cout << "CASE: l1 == l2 == l3\n"; 00184 } 00185 else { // CASE: l1 == l2 != l3 00186 FindAnEigenVector( m11, m12, m13, m22, m23, m33, l3, v3 ); 00187 FindOrthogonalVectors( v3, v1, v2 ); 00188 //cout << "CASE: l1 == l2 != l3\n"; 00189 } 00190 } 00191 else if ( fabs(l2 - l3) < fabs(l2/ratio) ) { // CASE: l1 != l2 == l3 00192 FindAnEigenVector( m11, m12, m13, m22, m23, m33, l1, v1 ); 00193 FindOrthogonalVectors( v1, v2, v3 ); 00194 //cout << "CASE: l1 != l2 == l3\n"; 00195 } 00196 else if ( fabs(l1 - l3) < fabs(l1/ratio) ) { // CASE: l1 == l3 != l2 00197 FindAnEigenVector( m11, m12, m13, m22, m23, m33, l2, v2 ); 00198 FindOrthogonalVectors( v2, v1, v3 ); 00199 //cout << "CASE: l1 == l3 != l2\n"; 00200 } 00201 else { // CASE: l1 != l2 != l3 00202 //if ( fabs(l1) > fabs(l2) && fabs(l1) > fabs(l3) ) {} 00203 FindAnEigenVector( m11, m12, m13, m22, m23, m33, l1, v1 ); 00204 //FindOrthogonalVectors( v1, v2, v3 ); 00205 FindAnEigenVector( m11, m12, m13, m22, m23, m33, l2, v2 ); 00206 FindAnEigenVector( m11, m12, m13, m22, m23, m33, l3, v3 ); 00207 //cout << "CASE: l1 != l2 != l3\n"; 00208 } 00209 //std::cout << "v1*v2 = " << DotProduct3(v1,v2) << "\n"; 00210 //std::cout << "v2*v3 = " << DotProduct3(v2,v3) << "\n"; 00211 //std::cout << "v1*v3 = " << DotProduct3(v1,v3) << "\n"; 00212 } while ( fabs( oldEigVal[0] - l1 ) > errThreshold 00213 || fabs( oldEigVal[1] - l2 ) > errThreshold 00214 || fabs( oldEigVal[2] - l3 ) > errThreshold 00215 || fabs(DotProduct3(v1,v2) + DotProduct3(v2,v3) + DotProduct3(v1,v3)) 00216 > errThreshold && iterationTimes <= times_limit 00217 ); 00218 } // END FN: eigenValsAndVecsOf3by3SymMatrixUsingQRFactor() 00219 //----------------------------------------------------------------------------- 00220 // Member Fn: EigenValsByQRFactorization() ---------------------------- 00221 // Desc: Find all eigen values of a real square matrix only. 00222 // I/P: A is a real square matrix. 00223 // I/P: (default = 6), times is the number of iterations. 00224 // O/P: (default = NULL), ptrO is a pointer to a matrix with will be the same 00225 // order as I/P Matrix, A. Its diagonal are the approximated eigen values. 00226 // Return: A column vector (n-by-1 matrix) contains all approximated eigenvalues. 00227 template <typename T> 00228 Matrixp<T> GenMath<T>::EigenValsByQRFactorization ( 00229 const Matrixp<T> &A, // I/P 00230 int times, // Number of iterations 00231 Matrixp<T> *ptrO ) // O/P 00232 { 00233 //-------------------------------------------------------------------- 00234 // only square matrix 00235 if ( !A.IsSquare() ) { 00236 std::cout << "ERROR: EigenValsByQRFactorization( ... )\n"; 00237 std::cout << "I/P Matrix: " << A; 00238 std::cout << " is NOT a square matrix!" << std::endl; 00239 return Matrixp<T>(0,0); 00240 } 00241 //-------------------------------------------------------------------- 00242 // Given a real n-by-n matrix A: 00243 // Define A_(1) = A 00244 // For k = 1 to times 00245 // Factor A_(k) = Q_(k)R_(k) (so that Q_(k)'A_(k) = R_(k)) 00246 // Define A_(k+1) = R_(k)Q_(k) (so that A_(k+1) = Q_(k)'A_(k)Q_(k)) 00247 // end. 00248 T errThreshold = 1.0; 00249 int limit = 10000; 00250 int n = A.GetNumOfRows(); 00251 Matrixp<T> M( A ), Q( n, n ), R( n, n ); 00252 if ( times < 0 ) { 00253 errThreshold = pow( 10.0, times ); 00254 times = limit; 00255 } 00256 //-------------------------------------------------------------------- 00257 for ( int k = 0; k < times; k++ ) { 00258 QRFactorization( M, Q, R ); 00259 M = R*Q; 00260 //------------------------------------------------------------ 00261 if ( 1.0 > errThreshold ) { 00262 bool isEnough = true; 00263 for ( int r = n/2; r < n; r++ ) { 00264 for ( int c = 0; c <= n/2; c++ ) { 00265 if ( r > c ) { 00266 if ( fabs( M( r, c ) ) > errThreshold ) { 00267 isEnough = false; 00268 break; 00269 } 00270 } 00271 } 00272 } 00273 if ( isEnough ) { 00274 cout << "Stopped at Iteration# " << k << "\n"; 00275 break; 00276 } 00277 } 00278 } 00279 //-------------------------------------------------------------------- 00280 if ( ptrO != NULL ) *ptrO = M; 00281 //-------------------------------------------------------------------- 00282 Matrixp<T> outMatrix( n, 1 ); 00283 for ( int i = 0; i < n; i++ ) { 00284 outMatrix( i, 0 ) = M( i, i ); 00285 } 00286 //-------------------------------------------------------------------- 00287 return M; 00288 } // END Member Fn: EigenValsByQRFactorization() 00289 //----------------------------------------------------------------------------- 00290 // FN: FindAnEigenVector() 00291 // DESC: Find an eigen vector of a 3x3 symmetric matrix 00292 template <typename T> 00293 void GenMath<T>::FindAnEigenVector( 00294 T &m11, T &m12, T &m13, 00295 T &m22, T &m23, 00296 T &m33, // I/P: 3x3 Symmetric Matrix 00297 T l1, // I/P: eigen value 00298 T v1[3] ) // O/P: eigen vector 00299 { 00300 T a1, b1 = m12, c1 = m13; 00301 T a2 = m12, b2, c2 = m23; 00302 T a3 = m13, b3 = m23, c3; 00303 a1 = m11 - l1; 00304 b2 = m22 - l1; 00305 c3 = m33 - l1; 00306 T divider = b2*a1 - b1*a2 - b3*a1 + b1*a3; 00307 //-------------------------------------------------------------------- 00308 //if ( divider != 0 ) { 00309 if ( fabs(divider) > 1E-40 ) { 00310 v1[0] = (-b2*c1 + b1*c2 + b3*c1 - b1*c3) / divider; 00311 v1[1] = (a2*c1 - a1*c2 - a3*c1 + a1*c3) /divider; 00312 v1[2] = 1; 00313 //------------------------------------------------------------ 00314 // make the vector a unit vector 00315 T vecLen = sqrt( v1[0]*v1[0] + v1[1]*v1[1] + v1[2]*v1[2] ); 00316 v1[0] /= vecLen; 00317 v1[1] /= vecLen; 00318 v1[2] /= vecLen; 00319 } 00320 //-------------------------------------------------------------------- 00321 else { // divider is near ZERO 00322 assert( false ); 00323 v1[0] = 0; 00324 v1[1] = 0; 00325 v1[2] = 0; 00326 } 00327 } // END FN: FindAnEigenVector() 00328 //----------------------------------------------------------------------------- 00329 // FN: FindOrthogonalVectors() 00330 // DESC: Find other two vectors (oB and oC) that are orthogonal to iA 00331 template <typename T> 00332 void GenMath<T>::FindOrthogonalVectors( const T iA[3], T oB[3], T oC[3] ) 00333 { 00334 Matrixp<T> A( 3, 1 ), B( 3, 1 ), C( 3, 1 ); 00335 //-------------------------------------------------------------------- 00336 for ( int i = 0; i < 3; i++ ) { 00337 A( i, 0 ) = iA[i]; 00338 } 00339 //-------------------------------------------------------------------- 00340 // Alpha for x, Beta for y, and Zeta for z 00341 T angleZeta; 00342 if ( iA[0] != 0 ) angleZeta = atan( iA[1] / iA[0] ); 00343 else angleZeta = ( iA[1] > 0 ? 1.0 : -1.0 ) * Math<T>::PI / 2.0; 00344 //-------------------------------------------------------------------- 00345 T angleBeta = asin( iA[2] / sqrt( iA[0]*iA[0] + iA[1]*iA[1] + iA[2]*iA[2] ) ); 00346 //T angleAlpha = 90.0/180.0*PI; 00347 Matrixp<T> Rz( 3, 3 ); 00348 Rz( 0, 0 ) = cos( angleZeta ); Rz( 0, 1 ) = sin( angleZeta ); 00349 Rz( 1, 0 ) = -sin( angleZeta ); Rz( 1, 1 ) = cos( angleZeta ); 00350 Rz( 2, 2 ) = 1; 00351 Matrixp<T> Ry( 3, 3 ); 00352 Ry( 0, 0 ) = cos( angleBeta ); Ry( 0, 2 ) = -sin( angleBeta ); 00353 Ry( 1, 1 ) = 1; 00354 Ry( 2, 0 ) = sin( angleBeta ); Ry( 2, 2 ) = cos( angleBeta ); 00355 Matrixp<T> Ryy( 3, 3 ); 00356 Ryy( 0, 0 ) = cos( Math<T>::PI/2.0 ); Ryy( 0, 2 ) = -sin( Math<T>::PI/2.0 ); 00357 Ryy( 1, 1 ) = 1; 00358 Ryy( 2, 0 ) = sin( Math<T>::PI/2.0 ); Ryy( 2, 2 ) = cos( Math<T>::PI/2.0 ); 00359 //-------------------------------------------------------------------- 00360 /* 00361 Matrixp<T> Rx( 3, 3 ); 00362 Rx( 0, 0 ) = 1; 00363 Rx( 1, 1 ) = cos( angleAlpha ); Rx( 1, 2 ) = sin( angleAlpha ); 00364 Rx( 2, 1 ) = -sin( angleAlpha ); Rx( 2, 2 ) = cos( angleAlpha ); 00365 */ 00366 //-------------------------------------------------------------------- 00367 Matrixp<T> Rym( 3, 3 ); 00368 Rym( 0, 0 ) = cos( -angleBeta ); Rym( 0, 2 ) = -sin( -angleBeta ); 00369 Rym( 1, 1 ) = 1; 00370 Rym( 2, 0 ) = sin( -angleBeta ); Rym( 2, 2 ) = cos( -angleBeta ); 00371 Matrixp<T> Rzm( 3, 3 ); 00372 Rzm( 0, 0 ) = cos( -angleZeta ); Rzm( 0, 1 ) = sin( -angleZeta ); 00373 Rzm( 1, 0 ) = -sin( -angleZeta ); Rzm( 1, 1 ) = cos( -angleZeta ); 00374 Rzm( 2, 2 ) = 1; 00375 //-------------------------------------------------------------------- 00376 B = Rzm * Rym * Ryy * Ry * Rz * A; 00377 //-------------------------------------------------------------------- 00378 oB[0] = B( 0, 0 ); 00379 oB[1] = B( 1, 0 ); 00380 oB[2] = B( 2, 0 ); 00381 //-------------------------------------------------------------------- 00382 CrossProduct3( iA, oB, oC ); 00383 } // END FN: FindOrthogonalVectors() 00384 //----------------------------------------------------------------------------- 00385 // Member Fn: QRFactorization() 00386 // Desc: Find a QR Factorization of a real square matrix only. 00387 template <typename T> 00388 bool GenMath<T>::QRFactorization( 00389 const Matrixp<T> &A, // I/P 00390 Matrixp<T> &Q, Matrixp<T> &R ) // O/P 00391 { 00392 //-------------------------------------------------------------------- 00393 // Only Square Matrix 00394 if ( !A.IsSquare() ) { 00395 std::cout << "ERROR: QRFactorization( ... )\n"; 00396 std::cout << "I/P Matrix: " << A; 00397 std::cout << " is NOT a square matrix!" << std::endl; 00398 return false; 00399 } 00400 //-------------------------------------------------------------------- 00401 // Q and A have different dimensions 00402 if ( Q.GetNumOfRows() != A.GetNumOfRows() 00403 || Q.GetNumOfCols() != A.GetNumOfCols() ) { 00404 std::cout << "ERROR: QRFactorization( ... )\n"; 00405 std::cout << "O/P Q Matrix: ";// << Q; 00406 std::cout << " does NOT have the same dimension as the I/P matrix!" 00407 << std::endl; 00408 return false; 00409 } 00410 //-------------------------------------------------------------------- 00411 // R and A have different dimensions 00412 if ( R.GetNumOfRows() != A.GetNumOfRows() 00413 || R.GetNumOfCols() != A.GetNumOfCols() ) { 00414 std::cout << "ERROR: QRFactorization( ... )\n"; 00415 std::cout << "O/P R Matrix: ";// << R; 00416 std::cout << " does NOT have the same dimension as the I/P matrix!" 00417 << std::endl; 00418 return false; 00419 } 00420 //-------------------------------------------------------------------- 00421 int n = A.GetNumOfRows(); 00422 int k; 00423 //-------------------------------------------------------------------- 00424 Matrixp<T> *arrayH = new Matrixp<T>[ n-1 ]; 00425 //-------------------------------------------------------------------- 00426 // For k = 1,...,n-1 00427 for ( k = 0; k < n-1; k++ ) { 00428 // Find H_(k) in order to reduce positions k+1,...,n, 00429 // in the k_th column of R_(k-1) to zero 00430 HouseHolderMatrix( A, arrayH, &R ); 00431 } 00432 //-------------------------------------------------------------------- 00433 // Define Q = I; 00434 // For k = n-1,...,1 00435 // Q = H_(k) * Q 00436 Q.MakeIdentity(); 00437 for ( k = n-2; k >= 0; k-- ) { 00438 Q = arrayH[k]*Q; 00439 } 00440 //-------------------------------------------------------------------- 00441 delete [] arrayH; 00442 //-------------------------------------------------------------------- 00443 return true; 00444 } // END Member Fn: QRFactorization() 00445 //----------------------------------------------------------------------------- 00446 // Member Fn: HouseHolderMatrix() 00447 // Desc: Find a HouseHolder matrix of a real square matrix only. 00448 template <typename T> 00449 Matrixp<T> GenMath<T>::HouseHolderMatrix( 00450 const Matrixp<T> &A, // I/P 00451 Matrixp<T> *ptrH, // O/P 00452 Matrixp<T> *ptrR ) // O/P 00453 { 00454 //-------------------------------------------------------------------- 00455 // only for square matrix 00456 if ( !A.IsSquare() ) { 00457 std::cout << "ERROR: HouseHolderMatrix( ... )\n"; 00458 std::cout << "I/P Matrix: " << A; 00459 std::cout << " is NOT a square matrix!" << std::endl; 00460 return Matrixp<T>(0,0); 00461 } 00462 //-------------------------------------------------------------------- 00463 if ( ptrR != NULL ) { 00464 if ( ptrR->GetNumOfRows() != A.GetNumOfRows() 00465 || ptrR->GetNumOfCols() != A.GetNumOfCols() ) { 00466 std::cout << "ERROR: HouseHolderMatrix( ... )\n"; 00467 std::cout << "(2nd) O/P Matrix: " << *ptrR; 00468 std::cout << " Its dimension is NOT agree with the I/P Matrix!" 00469 << std::endl; 00470 return Matrixp<T>(0,0); 00471 } 00472 } 00473 //-------------------------------------------------------------------- 00474 Matrixp<T> B( A ); 00475 Matrixp<T> H( A ); 00476 //-------------------------------------------------------------------- 00477 int i, k; 00478 double g, s; 00479 int n = H.GetNumOfRows(); 00480 Matrixp<T> W( n, 1 ); 00481 Matrixp<T> U( n, 1 ); 00482 //-------------------------------------------------------------------- 00483 // Create Identity matrix 00484 Matrixp<T> I( n, n ); 00485 I.MakeIdentity(); 00486 //-------------------------------------------------------------------- 00487 for ( k = 0; k < n-1; k++ ) { 00488 //------------------------------------------------------------ 00489 // Step 1: Set W_i = 0 for i = 0, ... , k-2 00490 for ( i = 0; i < k; i++ ) { 00491 W( i, 0 ) = 0.0; 00492 } 00493 //------------------------------------------------------------ 00494 // Step 2.1: Find g 00495 g = 0; 00496 for ( i = k; i < n; ++i ) { 00497 g += B( i, k ) * B( i, k ); 00498 } 00499 g = sqrt(g); 00500 //------------------------------------------------------------ 00501 // Step 2.2: Find s 00502 s = sqrt( 2 * g * ( g + fabs(B( k, k )) ) ); 00503 //------------------------------------------------------------ 00504 // Step 3.1: Set W_k 00505 W( k, 0 ) = ( B( k, k ) + (B( k, k ) >= 0.0 ? +1.0 : -1.0)*g ) / s; 00506 //------------------------------------------------------------ 00507 // Step 3.2: Set W_i 00508 for ( i = k+1; i < n; ++i ) { 00509 W( i, 0 ) = B( i, k ) / s; 00510 } 00511 //------------------------------------------------------------ 00512 U = 2 * B.GetTranspose() * W; 00513 //------------------------------------------------------------ 00514 H = I - ( 2 * W * W.GetTranspose() ); 00515 if ( ptrH != NULL ) ptrH[k] = H; 00516 B = H*B; 00517 } 00518 //-------------------------------------------------------------------- 00519 if ( ptrR != NULL ) *ptrR = B; 00520 //-------------------------------------------------------------------- 00521 return H; 00522 } // END Member Fn: HouseHolderMatrix() 00523 //----------------------------------------------------------------------------- 00524 //============================================================================= 00525 00526 //============================================================================= 00527 // Math Fns for array data, e.g. T b[]. For example dot product, etc. 00528 //============================================================================= 00529 //----------------------------------------------------------------------------- 00530 // FN: CrossProduct3() 00531 // DESC: Find C = A^B 00532 template <typename T> 00533 void GenMath<T>::CrossProduct3( const T A[3], const T B[3], T C[3] ) 00534 { 00535 C[0] = A[1]*B[2] - A[2]*B[1]; 00536 C[1] = A[2]*B[0] - A[0]*B[2]; 00537 C[2] = A[0]*B[1] - A[1]*B[0]; 00538 } // END FN: CrossProduct3() 00539 //----------------------------------------------------------------------------- 00540 // FN: DotProduct3() 00541 // DESC: Return T = A[3]*B[3] 00542 template <typename T> 00543 T GenMath<T>::DotProduct3( const T A[3], const T B[3] ) 00544 { 00545 return A[0]*B[0] + A[1]*B[1] + A[2]*B[2]; 00546 } // END FN: DotProduct3() 00547 00548 //----------------------------------------------------------------------------- 00549 //============================================================================= 00550 /* 00551 // Explicit Instantiation 00552 template class TAPs::GenMath<float>; 00553 template class TAPs::GenMath<double>; 00554 template class TAPs::GenMath<long double>; 00555 //*/ 00556 //============================================================================= 00557 END_NAMESPACE_TAPs 00558 //345678901234567890123456789012345678901234567890123456789012345678901234567890 00559 //--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----8
1.7.4 
