 |
TAPs 0.7.7.3
|
|
TAPs 0.7.7.3
|
00001 /****************************************************************************** 00002 SP_Mathematics.cpp 00003 00004 Include all mathematics constants and functions 00005 00006 SUKITTI PUNAK (07/14/2007) 00007 UPDATE (07/14/2007) 00008 ******************************************************************************/ 00009 #include "SP_Mathematics.h" 00010 // Using Inclusion Model (i.e. definitions are included in declarations) 00011 // (this name.cpp is included in name.hpp) 00012 // Each friend is defined directly inside its declaration. 00013 //----------------------------------------------------------------------------- 00014 //============================================================================= 00015 //----------------------------------------------------------------------------- 00016 namespace SP_Maths { 00017 //---------------------------------------------------------------------------------------- 00018 // FN: CrossProduct() 00019 // DESC: Find C = A^B 00020 void CrossProduct( const double A[3], const double B[3], double C[3] ) 00021 { 00022 C[0] = A[1]*B[2] - A[2]*B[1]; 00023 C[1] = A[2]*B[0] - A[0]*B[2]; 00024 C[2] = A[0]*B[1] - A[1]*B[0]; 00025 } // END FN: CrossProduct() 00026 00027 //---------------------------------------------------------------------------------------- 00028 // FN: DotProduct() 00029 // DESC: Return double = A*B 00030 double DotProduct( const double A[3], const double B[3] ) 00031 { 00032 return A[0]*B[0] + A[1]*B[1] + A[2]*B[2]; 00033 } // END FN: DotProduct() 00034 00035 //---------------------------------------------------------------------------------------- 00036 // FN: FindAnEigenVector() 00037 // DESC: Find an eigen vector of a 3x3 symmetric matrix 00038 void FindAnEigenVector( double &m11, double &m12, double &m13, 00039 double &m22, double &m23, 00040 double &m33, // I/P: 3x3 Symmetric Matrix 00041 double l1, // O/P: eigen value 00042 double v1[3] // O/P: eigen vector 00043 ) 00044 { 00045 double a1, b1 = m12, c1 = m13; 00046 double a2 = m12, b2, c2 = m23; 00047 double a3 = m13, b3 = m23, c3; 00048 00049 a1 = m11 - l1; 00050 b2 = m22 - l1; 00051 c3 = m33 - l1; 00052 double divider = b2*a1 - b1*a2 - b3*a1 + b1*a3; 00053 //if ( divider != 0 ) { 00054 if ( fabs(divider) > 1E-40 ) { 00055 v1[0] = (-b2*c1 + b1*c2 + b3*c1 - b1*c3) / divider; 00056 v1[1] = (a2*c1 - a1*c2 - a3*c1 + a1*c3) /divider; 00057 v1[2] = 1; 00058 00059 // make the vector a unit vector 00060 double vecLen = sqrt( v1[0]*v1[0] + v1[1]*v1[1] + v1[2]*v1[2] ); 00061 v1[0] /= vecLen; 00062 v1[1] /= vecLen; 00063 v1[2] /= vecLen; 00064 } 00065 else { 00066 v1[0] = 0; 00067 v1[1] = 0; 00068 v1[2] = 0; 00069 } 00070 } // END FN: FindAnEigenVector() 00071 00072 //---------------------------------------------------------------------------------------- 00073 // FN: FindOrthogonalVectors() 00074 // DESC: Find other two vectors (oB and oC) that are orthogonal to iA 00075 void FindOrthogonalVectors( const double iA[3], double oB[3], double oC[3] ) 00076 { 00077 SPtMatrix< double > A( 3, 1 ), B( 3, 1 ), C( 3, 1 ); 00078 for ( int i = 0; i < 3; i++ ) { 00079 A.Set( i, 0, iA[i] ); 00080 } 00081 00082 // alpha for x, beta for y, and zeta for z 00083 00084 double angleZeta; 00085 if ( iA[0] != 0 ) angleZeta = atan( iA[1] / iA[0] ); 00086 else angleZeta = ( iA[1] > 0 ? 1.0 : -1.0 ) * PI / 2.0; 00087 double angleBeta = asin( iA[2] / sqrt( iA[0]*iA[0] + iA[1]*iA[1] + iA[2]*iA[2] ) ); 00088 //double angleAlpha = 90.0/180.0*PI; 00089 SPtMatrix< double > Rz( 3, 3 ); 00090 Rz.Set( 0, 0, cos( angleZeta ) ); Rz.Set( 0, 1, sin( angleZeta ) ); 00091 Rz.Set( 1, 0, -sin( angleZeta ) ); Rz.Set( 1, 1, cos( angleZeta ) ); 00092 Rz.Set( 2, 2, 1 ); 00093 SPtMatrix< double > Ry( 3, 3 ); 00094 Ry.Set( 0, 0, cos( angleBeta ) ); Ry.Set( 0, 2, -sin( angleBeta ) ); 00095 Ry.Set( 1, 1, 1 ); 00096 Ry.Set( 2, 0, sin( angleBeta ) ); Ry.Set( 2, 2, cos( angleBeta ) ); 00097 00098 SPtMatrix< double > Ryy( 3, 3 ); 00099 Ryy.Set( 0, 0, cos( PI/2.0 ) ); Ryy.Set( 0, 2, -sin( PI/2.0 ) ); 00100 Ryy.Set( 1, 1, 1 ); 00101 Ryy.Set( 2, 0, sin( PI/2.0 ) ); Ryy.Set( 2, 2, cos( PI/2.0 ) ); 00102 00103 /* 00104 SPtMatrix< double > Rx( 3, 3 ); 00105 Rx.Set( 0, 0, 1 ); 00106 Rx.Set( 1, 1, cos( angleAlpha ) ); Rx.Set( 1, 2, sin( angleAlpha ) ); 00107 Rx.Set( 2, 1, -sin( angleAlpha ) ); Rx.Set( 2, 2, cos( angleAlpha ) ); 00108 */ 00109 00110 SPtMatrix< double > Rym( 3, 3 ); 00111 Rym.Set( 0, 0, cos( -angleBeta ) ); Rym.Set( 0, 2, -sin( -angleBeta ) ); 00112 Rym.Set( 1, 1, 1 ); 00113 Rym.Set( 2, 0, sin( -angleBeta ) ); Rym.Set( 2, 2, cos( -angleBeta ) ); 00114 SPtMatrix< double > Rzm( 3, 3 ); 00115 Rzm.Set( 0, 0, cos( -angleZeta ) ); Rzm.Set( 0, 1, sin( -angleZeta ) ); 00116 Rzm.Set( 1, 0, -sin( -angleZeta ) ); Rzm.Set( 1, 1, cos( -angleZeta ) ); 00117 Rzm.Set( 2, 2, 1 ); 00118 00119 //B = Rz * Ry * Ryy * Rym * Rzm * A; 00120 B = Rzm * Rym * Ryy * Ry * Rz * A; 00121 00122 //B.DisplayElements(); 00123 //(A.GetTranspose()*B).DisplayElements(); 00124 00125 oB[0] = B.Get( 0, 0 ); 00126 oB[1] = B.Get( 1, 0 ); 00127 oB[2] = B.Get( 2, 0 ); 00128 00129 cout << "\niA*oB = " << ( iA[0]*oB[0] + iA[1]*oB[1] + iA[2]*oB[2] ) << "\n"; 00130 CrossProduct( iA, oB, oC ); 00131 } // END FN: FindOrthogonalVectors() 00132 00133 //---------------------------------------------------------------------------------------- 00134 void rootsOfCubicPolynomialCodeGeneratedByMaple( 00135 double a, double b, double c, double d , 00136 std::complex<double> &r1, std::complex<double> &r2, std::complex<double> &r3 ) 00137 { 00138 std::complex<double> a1; 00139 std::complex<double> a2; 00140 std::complex<double> a3; 00141 00142 a1 = b/a; 00143 a2 = c/a; 00144 a3 = d/a; 00145 00146 std::complex<double> div = 1.0/3.0; 00147 00148 // common part for all roots 00149 std::complex<double> t1 = a2*a1; 00150 std::complex<double> t4 = a1*a1; 00151 std::complex<double> t5 = t4*a1; 00152 std::complex<double> t7 = a2*a2; 00153 std::complex<double> t14 = a3*a3; 00154 std::complex<double> t19 = sqrt(12.0*t7*a2-3.0*t7*t4-54.0*t1*a3+81.0*t14+12.0*a3*t5); 00155 std::complex<double> t22 = pow(36.0*t1-108.0*a3-8.0*t5+12.0*t19,div); 00156 00157 // Root# 1 00158 r1 = t22/6.0-6.0*(a2/3.0-t4/9.0)/t22-a1/3.0; 00159 00160 // common part for root# 2 and 3 00161 std::complex<double> t28 = (a2/3.0-t4/9.0)/t22; 00162 std::complex<double> t31 = sqrt(3.0); 00163 00164 // Root# 2 00165 //r2 = -t22/12.0+3.0*t28-a1/3.0+(1.0/2.0*sqrt(-1.0))*t31*(t22/6.0+6.0*t28); 00166 std::complex<double> st1 = t31*(t22/6.0+6.0*t28); 00167 std::complex<double> st2 = (sqrt(-1.0)); 00168 cout << st1 << ", " << st2 << endl; 00169 r2 = -t22/12.0+3.0*t28-a1/3.0+(1.0/2.0*sqrt(-1.0))*t31*(t22/6.0+6.0*t28); 00170 00171 // Root# 3 00172 //r3 = -t22/12.0+3.0*t28-a1/3.0+(-1.0/2.0*sqrt(-1.0))*t31*(t22/6.0+6.0*t28); 00173 r2 = -t22/12.0+3.0*t28-a1/3.0+(1.0/2.0*sqrt(-1.0))*t31*(t22/6.0+6.0*t28); 00174 00175 00176 // DEBUG: 00177 cout.precision( 10 ); 00178 // cout << "*x^3" << (a1>0 ? " + " : " - ") << fabs(a1) << "*x^2"<< (a1>0 ? " + " : " - ") 00179 // << fabs(a2) << "*x" << (a1>0 ? " + " : " - ") << fabs(a3) << "\n"; 00180 cout << "Root#1: " << r1 << " Root#2: " << r2 << " Root#3: " << r3 << "\n"; 00181 cout.precision( 6 ); 00182 00183 } // END FN: rootsOfCubicPolynomial() 00184 00185 //---------------------------------------------------------------------------------------- 00186 // FN: rootsOfCubicPolynomial() ************************************************************* 00187 // DESC: Find all three roots of a cubic polynomial in this form; 00188 // a*x^3 + b*x^2 + c*x + d = 0; 00189 // The formula is adapted from "Mathematical Handbook of Formulas and Tables" 00190 // I/P: a, b, c, and d 00191 // O/P: r1, r2, and r3 ( r1 is the real root) 00192 // r2 and r3 are only the real parts of the other two roots 00193 double rootsOfCubicPolynomial( 00194 double a, double b, double c, double d , 00195 //double &r1, double &r2, double &r3 ) 00196 std::complex<double> &r1, std::complex<double> &r2, std::complex<double> &r3 ) 00197 { 00198 double a1, a2, a3; 00199 double Q, R, D; 00200 std::complex<double> S, T; 00201 double zeta; 00202 00203 a1 = b/a; 00204 a2 = c/a; 00205 a3 = d/a; 00206 00207 //Q = (3.0*a2 - a1*a1) / 9.0; 00208 //R = (9.0*a1*a2 - 27.0*a3 - 2.0*a1*a1*a1) / 54.0; 00209 // Q^3 + R^2 can factor 9*9*9 (or 27*27) out 00210 // So Q = 3*a2 - a1^2 00211 // and R = (f1 + f2 + f3) / 2 00212 00213 Q = 3.0*a2 - a1*a1; 00214 double Rpos = 0, Rneg = 0; 00215 double f[3]; 00216 f[0] = 9.0*a1*a2; 00217 f[1] = -27.0*a3; 00218 f[2] = -2.0*a1*a1*a1; 00219 for ( int i = 0; i < 3; i++ ) { 00220 ( f[i] >= 0 ) ? ( Rpos += f[i] ) : ( Rneg += f[i] ); 00221 //if ( f[i] >= 0 ) Rpos += f[i]; 00222 //else Rneg += f[i]; 00223 } 00224 R = (Rpos + Rneg) / 2.0; 00225 00226 00227 00228 D = ( Q*Q*Q + R*R ) / 729.0; 00229 Q /= 9.0; 00230 R /= 27.0; 00231 00232 // If a1, a2, a3 are real and if D = Q^3 + R^2 is the discriminant, then 00233 // (i) one root is real and two complex conjugate if D > 0 00234 // (ii) all roots are real and at least two are equal if D = 0 00235 // (iii) all roots are real and unequal if D < 0. 00236 00237 if ( D >= 0 ) { 00238 std::complex<double> Dh = sqrt(D); 00239 00240 S = pow(R + Dh, 1.0/3.0); 00241 T = pow(R - Dh, 1.0/3.0); 00242 00243 r1 = S + T - a1/3.0; 00244 std::complex<double> pureComplex( 0, sqrt(3.0) ); 00245 //r2 = -(S + T)/2.0 - a1/3.0 + i*sqrt(3.0)*(S - T)/2.0; 00246 //r3 = -(S + T)/2.0 - a1/3.0 - i*sqrt(3.0)*(S - T)/2.0; 00247 r2 = - (S + T)/2.0 - a1/3.0 + pureComplex*(S - T)/2.0; 00248 r3 = - (S + T)/2.0 - a1/3.0 - pureComplex*(S - T)/2.0; 00249 } 00250 00251 // All roots are real and unequal if D < 0. 00252 //if ( D < 0 ) { 00253 else { 00254 //cout << "D < 0" << endl; 00255 zeta = acos( R / sqrt(-(Q*Q*Q)) ); 00256 r1 = 2 * sqrt(-Q) * cos(zeta/3.0) - a1/3.0; 00257 r2 = 2 * sqrt(-Q) * cos(zeta/3.0 + 2.0/3.0*PI) - a1/3.0; 00258 r3 = 2 * sqrt(-Q) * cos(zeta/3.0 + 4.0/3.0*PI) - a1/3.0; 00259 } 00260 00261 // DEBUG: 00262 cout.precision( 10 ); 00263 cout << "*x^3" << (a1>0 ? " + " : " - ") << fabs(a1) << "*x^2"<< (a1>0 ? " + " : " - ") 00264 << fabs(a2) << "*x" << (a1>0 ? " + " : " - ") << fabs(a3) << "\n"; 00265 cout << "Q = " << Q << "\n"; 00266 cout << "R = " << R << "\n"; 00267 cout << "Q^3 = " << Q*Q*Q << "\n"; 00268 cout << "R^2 = " << R*R << "\n"; 00269 cout << "D = " << D << "\n"; 00270 cout << "Root#1: " << r1 << " Root#2: " << r2 << " Root#3: " << r3 << "\n"; 00271 cout.precision( 6 ); 00272 00273 00274 std::complex<double> cr1, cr2, cr3; 00275 rootsOfCubicPolynomialCodeGeneratedByMaple( a, b, c, d, cr1, cr2, cr3 ); 00276 00277 00278 return D; // return discriminant 00279 } // END FN: rootsOfCubicPolynomial() 00280 00281 //---------------------------------------------------------------------------------------- 00282 // Overloaded Function 00283 double rootsOfCubicPolynomial( double a, double b, double c, double d , 00284 double &r1, double &r2, double &r3 ) 00285 { 00286 std::complex< double > cr1, cr2, cr3; 00287 double discriminant = rootsOfCubicPolynomial( a, b, c, d, cr1, cr2, cr3 ); 00288 r1 = cr1.real(); 00289 r2 = cr2.real(); 00290 r3 = cr3.real(); 00291 00292 return discriminant; 00293 } 00294 00295 //---------------------------------------------------------------------------------------- 00296 // MARK: FIX IT!!!!!!!!!!!!!!!! 00297 // FN: rootOfCubicPolynomial() ************************************************************** 00298 // DESC: Find all three roots of a cubic polynomial in this form; 00299 // a*x^3 + b*x^2 + c*x + d = 0; 00300 // The formula is adapted from "Mathematical Handbook of Formulas and Tables" 00301 // I/P: a, b, c, d, and rootNo 00302 // O/P: a double value of the rootNo (1, 2, or 3) 00303 // r1 is the real root 00304 // r2 and r3 are only the real parts of the other two roots 00305 double rootOfCubicPolynomial( double a, double b, double c, double d , int rootNo ) 00306 { 00307 double a1, a2, a3; 00308 double Q, R, D; 00309 double S, T; // FIX IT HERE! 00310 double zeta; 00311 double rootValue; 00312 00313 a1 = b/a; 00314 a2 = c/a; 00315 a3 = d/a; 00316 Q = (3.0*a2 - a1*a1) / 9.0; 00317 R = (9.0*a1*a2 - 27.0*a3 - 2.0*a1*a1*a1) / 54.0; 00318 D = Q*Q*Q + R*R; 00319 00320 // If a1, a2, a3 are real and if D = Q^3 + R^2 is the discriminant, then 00321 // (i) one root is real and two complex conjugate if D > 0 00322 // (ii) all roots are real and at least two are equal if D = 0 00323 // (iii) all roots are real and unequal if D < 0. 00324 00325 if ( D >= 0 ) { 00326 double Dh = sqrt(D); 00327 S = pow(R + Dh, 1.0/3.0); // FIX IT HERE! 00328 T = R - Dh; 00329 if ( T >= 0 ) T = pow( T, 1.0/3.0 ); 00330 else T = -pow( -T, 1.0/3.0 ); 00331 00332 switch ( rootNo ) { 00333 // first root 00334 case 1: 00335 rootValue = S + T - a1/3.0; 00336 return rootValue; 00337 break; 00338 00339 // second root 00340 case 2: 00341 rootValue = -(S + T)/2.0 - a1/3.0; // + i*sqrt(3.0)*(S - T)/2.0; 00342 return rootValue; 00343 break; 00344 00345 // third root 00346 case 3: 00347 rootValue = -(S + T)/2.0 - a1/3.0; // - i*sqrt(3.0)*(S - T)/2.0; 00348 return rootValue; 00349 break; 00350 00351 // default 00352 default: 00353 cerr << "ERROR: Incorrect Root# Option!" << endl; 00354 return -777; 00355 break; 00356 } 00357 } 00358 00359 // All roots are real and unequal if D < 0. 00360 //if ( D < 0 ) { 00361 else { 00362 zeta = acos( R / sqrt(-(Q*Q*Q)) ); 00363 switch ( rootNo ) { 00364 // first root 00365 case 1: 00366 rootValue = 2 * sqrt(-Q) * cos(zeta/3.0) - a1/3.0; 00367 return rootValue; 00368 break; 00369 00370 // second root 00371 case 2: 00372 rootValue = 2 * sqrt(-Q) * cos(zeta/3.0 + 2.0/3.0*PI) - a1/3.0; 00373 return rootValue; 00374 break; 00375 00376 // third root 00377 case 3: 00378 rootValue = 2 * sqrt(-Q) * cos(zeta/3.0 + 4.0/3.0*PI) - a1/3.0; 00379 return rootValue; 00380 break; 00381 00382 // default 00383 default: 00384 cerr << "ERROR: Incorrect Root# Option!" << endl; 00385 return -777; 00386 break; 00387 } 00388 } 00389 } // END FN: rootOfCubicPolynomial() 00390 00391 //---------------------------------------------------------------------------------------- 00392 // FN: eigenValsOf3by3SymMatrix() ******************************************************* 00393 // DESC: Find eigen values of a 3x3 symmetric matrix 00394 // I/P: (double) m11, m12, m13, m22, m23, m33 00395 // O/P: (double) lamda1, lamda2, lamda3 00396 double eigenValsOf3by3SymMatrix( double m11, double m12, double m13, 00397 double m22, double m23, 00398 double m33, 00399 double &lamda1, double &lamda2, double &lamda3 ) 00400 { 00401 double errThreshold = 1E-6; // error tolerant value 00402 double b, c, d = 0; 00403 double f[5]; 00404 00405 // coefficients of the characteristic polynomial of the 3x3 symmetric matrix 00406 b = - m11 - m22 - m33; 00407 c = (m11*m22 + m22*m33 + m33*m11) - (m12*m12 + m23*m23 + m13*m13); 00408 //d = m11*m23*m23 + m22*m13*m13 + m33*m12*m12 - m11*m22*m33 - 2*m12*m23*m13; 00409 00410 f[0] = m11*m23*m23; 00411 f[1] = m22*m13*m13; 00412 f[2] = m33*m12*m12; 00413 f[3] = -m11*m22*m33; 00414 f[4] = -2*m12*m23*m13; 00415 double dNeg = 0; 00416 for ( int i = 0; i < 5; i++ ) { 00417 if ( f[i] >= 0 ) d += f[i]; 00418 else dNeg += f[i]; 00419 } 00420 d += dNeg; 00421 00422 // the roots of the polynomial function are the eigen values of the matrix 00423 if ( fabs(m12) < errThreshold && fabs(m13) < errThreshold && fabs(m23) < errThreshold ) { 00424 double difThreshold = fabs( m11 * 1E-06 ); 00425 if ( fabs(m11-m22) < difThreshold && fabs(m11-m33) < difThreshold ) { // all roots are real and at least two are equal if discriminant = 0. 00426 lamda1 = lamda2 = lamda3 = ( m11 + m22 + m33 ) / 3.0; 00427 return 0; // return zero discriminant value 00428 } 00429 else { // all roots are real and unequal if discriminant < 0. 00430 lamda1 = m11; 00431 lamda2 = m22; 00432 lamda3 = m33; 00433 return -1; // return negative discriminant value 00434 } 00435 } 00436 00437 // Find roots using formulas from "Handbook of Mathematic" 00438 double discriminant = rootsOfCubicPolynomial( 1, b, c, d, lamda1, lamda2, lamda3 ); 00439 00440 return static_cast<double>(discriminant); 00441 } // END FN: eigenValsOf3by3SymMatrix() 00442 00443 //---------------------------------------------------------------------------------------- 00444 // FN: eigenValsOf3by3SymMatrixSmith1961() *********************************************** 00445 // DESC: Find eigen values of a 3x3 symmetric matrix 00446 // I/P: (double) m11, m12, m13, m22, m23, m33 00447 // O/P: (double) lamda1, lamda2, lamda3 00448 double eigenValsOf3by3SymMatrixSmith1961( double m11, double m12, double m13, 00449 double m22, double m23, 00450 double m33, 00451 double &lamda1, double &lamda2, double &lamda3 ) 00452 { 00453 00454 double errThreshold = 1E-10; 00455 00456 // 3m = trace(A) 00457 // 2q = det(A - mI) 00458 // 6p = the sum of squares of elements of (A - mI) 00459 // discriminant = p^3 - q^2 00460 // phi = 1/3*arctan( sqrt(discriminant)/q ) 00461 // root1 = m + 2*sqrt(p)*cos(phi) 00462 // root2 = m - sqrt(p)*(cos(phi) + sqrt(3)*sin(phi)) 00463 // root3 = m - sqrt(p)*(cos(phi) - sqrt(3)*sin(phi)) 00464 double m, p, q, phi, discriminant; 00465 00466 00467 SPtMatrix3x3< double > IT( m11, m12, m13, 00468 m12, m22, m23, 00469 m13, m23, m33 ); 00470 SPtMatrix3x3< double > I( 1,0,0, 0,1,0, 0,0,1 ); 00471 00472 00473 m = ( m11 + m22 + m33 ) / 3.0; 00474 00475 SPtMatrix3x3< double > B = IT - m*I; 00476 00477 q = ( B.Get(0,0) * ( B.Get(1,1)*B.Get(2,2) - B.Get(2,1)*B.Get(1,2) ) 00478 - B.Get(1,0) * ( B.Get(0,1)*B.Get(2,2) - B.Get(2,1)*B.Get(0,2) ) 00479 + B.Get(2,0) * ( B.Get(0,1)*B.Get(1,2) - B.Get(1,1)*B.Get(0,2) ) 00480 ) / 2.0; 00481 00482 p = 0; 00483 for ( int r = 0; r < 3; r++ ) { 00484 for ( int c = 0; c < 3; c++ ) { 00485 p += B.Get(r,c)*B.Get(r,c); 00486 } 00487 } 00488 p /= 6.0; 00489 00490 discriminant = p*p*p - q*q; 00491 if ( discriminant >= 0 ) { 00492 phi = atan( sqrt(discriminant)/q ) / 3.0; 00493 00494 lamda1 = m + 2*sqrt(p)*cos(phi); 00495 lamda2 = m - sqrt((double)p)*(cos(phi) + sqrt((double)3)*sin(phi)); 00496 lamda3 = m - sqrt((double)p)*(cos(phi) - sqrt((double)3)*sin(phi)); 00497 } 00498 else { 00499 cout << "Discriminant (Smith's Method (1961) < 0: discriminant = " << discriminant << "\n"; 00500 } 00501 00502 // the roots of the polynomial function are the eigen values of the matrix 00503 if ( fabs(m12) < errThreshold && fabs(m13) < errThreshold && fabs(m23) < errThreshold ) { 00504 double difThreshold = fabs( m11 * 1E-06 ); 00505 if ( fabs(m11-m22) < difThreshold && fabs(m11-m33) < difThreshold ) { // all roots are real and at least two are equal if discriminant = 0. 00506 lamda1 = lamda2 = lamda3 = ( m11 + m22 + m33 ) / 3.0; 00507 return 0; // return zero discriminant value 00508 } 00509 else { // all roots are real and unequal if discriminant < 0. 00510 lamda1 = m11; 00511 lamda2 = m22; 00512 lamda3 = m33; 00513 return -1; // return negative discriminant value 00514 } 00515 } 00516 00517 return static_cast<double>(discriminant); 00518 } // END FN: eigenValsOf3by3SymMatrixSmith1961() 00519 00520 //---------------------------------------------------------------------------------------- 00521 // FN: eigenValsAndVecsOf3by3SymMatrix() **************************************************** 00522 // DESC: Find eigen vectors of a 3x3 symmetric matrix which has all real eigen values 00523 // I/P: (double) m11, m12, m13, m22, m23, m33 00524 // O/P: (double) lamda1, lamda2, lamda3 00525 void eigenValsAndVecsOf3by3SymMatrix( double m11, double m12, double m13, 00526 double m22, double m23, 00527 double m33, 00528 double &l1, double &l2, double &l3, 00529 double v1[3], double v2[3], double v3[3] ) 00530 { 00531 double errThreshold = 1E-05; // error tolerant value 00532 00533 double discriminant = eigenValsOf3by3SymMatrix( m11, m12, m13, m22, m23, m33, l1, l2, l3 ); 00534 00535 // Smith's Method (1961) 00536 //double discriminant = eigenValsOf3by3SymMatrixSmith1961( m11, m12, m13, m22, m23, m33, ll1, ll2, ll3 ); 00537 00538 //cout << "discriminant = " << discriminant << endl; 00539 //cout << "l1: " << l1 << " l2: " << l2 << " l3: " << l3 << endl; 00540 00541 // ******************************************************************** 00542 // ASSUME all eigen values are real! 00543 00544 // If discriminant > 0, then one root is real and two complex conjugate. 00545 /* 00546 if ( discriminant > errThreshold ) { 00547 cerr << "ERR: Discriminant > 0, i.e. one eigen value is real and two complex conjugate!" << "\n"; 00548 cerr << "CONT: The function is not intended for this kind of matrix!" << endl; 00549 return; 00550 } 00551 */ 00552 00553 00554 00555 // If all eigen values are equal, then eigen vectors can be 00556 // any vectors that are orthogonal to one another. 00557 // Also in this case, discriminant == 0. 00558 //if ( fabs(m12) < errThreshold && fabs(m13) < errThreshold && fabs(m23) < errThreshold ) { 00559 const double ratio = 100000.0; 00560 if ( fabs(l1 - l2) < fabs(l1/ratio) ) { 00561 if ( fabs(l1 - l3) < fabs(l1/ratio) ) { // CASE: l1 == l2 == l3 00562 v1[0] = 1; v1[1] = 0; v1[2] = 0; 00563 v2[0] = 0; v2[1] = 1; v2[2] = 0; 00564 v3[0] = 0; v3[1] = 0; v3[2] = 1; 00565 cout << "CASE: l1 == l2 == l3\n"; 00566 } 00567 else { // CASE: l1 == l2 != l3 00568 FindAnEigenVector( m11, m12, m13, m22, m23, m33, l3, v3 ); 00569 FindOrthogonalVectors( v3, v1, v2 ); 00570 cout << "CASE: l1 == l2 != l3\n"; 00571 } 00572 } 00573 else if ( fabs(l2 - l3) < fabs(l2/ratio) ) { // CASE: l1 != l2 == l3 00574 FindAnEigenVector( m11, m12, m13, m22, m23, m33, l1, v1 ); 00575 FindOrthogonalVectors( v1, v2, v3 ); 00576 cout << "CASE: l1 != l2 == l3\n"; 00577 } 00578 else if ( fabs(l1 - l3) < fabs(l1/ratio) ) { // CASE: l1 == l3 != l2 00579 FindAnEigenVector( m11, m12, m13, m22, m23, m33, l2, v2 ); 00580 FindOrthogonalVectors( v2, v1, v3 ); 00581 cout << "CASE: l1 == l3 != l2\n"; 00582 } 00583 else { // CASE: l1 != l2 != l3 00584 //if ( fabs(l1) > fabs(l2) && fabs(l1) > fabs(l3) ) {} 00585 FindAnEigenVector( m11, m12, m13, m22, m23, m33, l1, v1 ); 00586 //FindOrthogonalVectors( v1, v2, v3 ); 00587 FindAnEigenVector( m11, m12, m13, m22, m23, m33, l2, v2 ); 00588 FindAnEigenVector( m11, m12, m13, m22, m23, m33, l3, v3 ); 00589 cout << "CASE: l1 != l2 != l3\n"; 00590 } 00591 00592 cout << "v1*v2 = " << DotProduct(v1,v2) << "\n"; 00593 cout << "v2*v3 = " << DotProduct(v2,v3) << "\n"; 00594 cout << "v1*v3 = " << DotProduct(v1,v3) << "\n"; 00595 } // END FN: eigenValsAndVecsOf3by3SymMatrix() 00596 00597 //---------------------------------------------------------------------------------------- 00598 // FN: eigenValsAndVecsOf3by3SymMatrixUsingQRFactor() *************************************** 00599 // DESC: Find eigen vectors of a 3x3 symmetric matrix which has all real eigen values 00600 // I/P: (double) m11, m12, m13, m22, m23, m33 00601 // O/P: (double) lamda1, lamda2, lamda3 00602 void eigenValsAndVecsOf3by3SymMatrixUsingQRFactor( double m11, double m12, double m13, 00603 double m22, double m23, 00604 double m33, 00605 double &l1, double &l2, double &l3, 00606 double v1[3], double v2[3], double v3[3] ) 00607 { 00608 //double errThreshold = 5E-3; // error tolerant value 00609 static const int TIMES_LIMIT = 256; 00610 int iterationTimes = 1; 00611 00612 SPtMatrix< double > IT( 3, 3 ); 00613 SPtMatrix< double > EG( 3, 3 ); 00614 IT.Set( 0, 0, m11 ); IT.Set( 0, 1, m12 ); IT.Set( 0, 2, m13 ); 00615 IT.Set( 1, 0, m12 ); IT.Set( 1, 1, m22 ); IT.Set( 1, 2, m23 ); 00616 IT.Set( 2, 0, m13 ); IT.Set( 2, 1, m23 ); IT.Set( 2, 2, m33 ); 00617 00618 //cout << "IT is \n"; 00619 //IT.DisplayElements(); 00620 do { 00621 iterationTimes *= 2; 00622 00623 MatrixOperations::EigenValsByQRFactorization( IT, iterationTimes, &EG ); 00624 00625 cout << "QR Factorization iterates " << iterationTimes << " times.\n"; 00626 00627 //cout << "IT is \n"; 00628 //IT.DisplayElements(); 00629 //cout << "EG is \n"; 00630 //EG.DisplayElements(); 00631 00632 l1 = EG.Get( 0, 0 ); 00633 l2 = EG.Get( 1, 1 ); 00634 l3 = EG.Get( 2, 2 ); 00635 00636 // If all eigen values are equal, then eigen vectors can be 00637 // any vectors that are orthogonal to one another. 00638 // Also in this case, discriminant == 0. 00639 //if ( fabs(m12) < errThreshold && fabs(m13) < errThreshold && fabs(m23) < errThreshold ) { 00640 const double ratio = 10000.0; 00641 if ( fabs(l1 - l2) < fabs(l1/ratio) ) { 00642 if ( fabs(l1 - l3) < fabs(l1/ratio) ) { // CASE: l1 == l2 == l3 00643 v1[0] = 1; v1[1] = 0; v1[2] = 0; 00644 v2[0] = 0; v2[1] = 1; v2[2] = 0; 00645 v3[0] = 0; v3[1] = 0; v3[2] = 1; 00646 cout << "CASE: l1 == l2 == l3\n"; 00647 } 00648 else { // CASE: l1 == l2 != l3 00649 FindAnEigenVector( m11, m12, m13, m22, m23, m33, l3, v3 ); 00650 FindOrthogonalVectors( v3, v1, v2 ); 00651 cout << "CASE: l1 == l2 != l3\n"; 00652 } 00653 } 00654 else if ( fabs(l2 - l3) < fabs(l2/ratio) ) { // CASE: l1 != l2 == l3 00655 FindAnEigenVector( m11, m12, m13, m22, m23, m33, l1, v1 ); 00656 FindOrthogonalVectors( v1, v2, v3 ); 00657 cout << "CASE: l1 != l2 == l3\n"; 00658 } 00659 else if ( fabs(l1 - l3) < fabs(l1/ratio) ) { // CASE: l1 == l3 != l2 00660 FindAnEigenVector( m11, m12, m13, m22, m23, m33, l2, v2 ); 00661 FindOrthogonalVectors( v2, v1, v3 ); 00662 cout << "CASE: l1 == l3 != l2\n"; 00663 } 00664 else { // CASE: l1 != l2 != l3 00665 //if ( fabs(l1) > fabs(l2) && fabs(l1) > fabs(l3) ) {} 00666 FindAnEigenVector( m11, m12, m13, m22, m23, m33, l1, v1 ); 00667 //FindOrthogonalVectors( v1, v2, v3 ); 00668 FindAnEigenVector( m11, m12, m13, m22, m23, m33, l2, v2 ); 00669 FindAnEigenVector( m11, m12, m13, m22, m23, m33, l3, v3 ); 00670 cout << "CASE: l1 != l2 != l3\n"; 00671 } 00672 00673 cout << "v1*v2 = " << DotProduct(v1,v2) << "\n"; 00674 cout << "v2*v3 = " << DotProduct(v2,v3) << "\n"; 00675 cout << "v1*v3 = " << DotProduct(v1,v3) << "\n"; 00676 } while ( fabs(DotProduct(v1,v2) + DotProduct(v2,v3) + DotProduct(v1,v3)) > 1E-06 && iterationTimes <= TIMES_LIMIT ); 00677 } // END FN: eigenValsAndVecsOf3by3SymMatrixUsingQRFactor() 00678 00679 //---------------------------------------------------------------------------------------- 00680 // FN: RootFindingByNewtonRaphsonMethod() *************************************************** 00681 // ADAPTED FROM: "Numerical Recipes in C: The Art of Scientific Computing" (1988) 00682 // by W. H. Press, B. P. Flannery, S. A. Teukolsky, and W. T. Vetterling 00683 // DESC: Using the Newton-Raphson method to find the root of a function known to lie in the 00684 // interval (x1,x2). The root rtnewt will be refined until its accuracy is known within 00685 // +/-xacc. funcd is a user-supplied routine that provides both the funciton value and 00686 // the first derivative of the function at the point x. 00687 double RootFindingByNewtonRaphsonMethod( 00688 void (*funcd)( double x, double *fx, double *dfx ), // A user-supplied routine that provides both the function value and the first derivative of the function at the point x. 00689 double x1, double x2, // The root of a function must lies in the interval (x1,x2). 00690 double xacc ) // The root will be refined until its accuracy is known within +/-xacc. 00691 { 00692 const int ITERATION_LIMIT = 200; 00693 double x, fx, dx = 0, dfx; 00694 00695 x = 0.5 * (x1 + x2); // initial guess is the middle of the interval (x1,x2) 00696 for ( int i = 1; i < ITERATION_LIMIT; i++ ) { 00697 (*funcd)( x, &fx, &dfx ); 00698 if ( fx != 0 && dfx != 0) { 00699 dx = fx/dfx; 00700 x -= dx; 00701 } 00702 if ( (x1-x)*(x-x2) < 0.0 ) { 00703 cerr << "From RootFindingByNewtonRaphsonMethod():\n" 00704 << " The root lies outside the provided interval (x1,x2)!" << endl; 00705 return -777; 00706 } 00707 if ( fabs(dx) < xacc ) return x; // the convergence value is the root 00708 } 00709 00710 cerr << "From RootFindingByNewtonRaphsonMethod():\n" 00711 << " Maximum number of iterations exceeded!" << endl; 00712 return -777; 00713 } // END FN: RootFindingByNewtonRaphsonMethod() 00714 //---------------------------------------------------------------------------------------- 00715 } // END: namespace SP_Maths 00716 //----------------------------------------------------------------------------- 00717 //============================================================================= 00718 //----------------------------------------------------------------------------- 00719 //34567890123456789012345678901234567890123456789012345678901234567890123456789 00720 //--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----
1.7.4 
