CLHEP Home Page CLHEP Documentation CLHEP Bug Reports
|
|
CLHEP VERSION Reference Documentation
CLHEP Home Page CLHEP Documentation CLHEP Bug Reports |
00001 // eulerTest.cc 00002 00003 // Test extreme cases for Euler angles -- 00004 // We perturb the matrix slightly before taking Euler angles. 00005 // A test will have failed if the Rotation resulting from taking euler angles 00006 // and forming a rotation from them, is ever significantly different than the 00007 // origninal rotation. 00008 00009 00010 #include "CLHEP/Vector/Rotation.h" 00011 #include "CLHEP/Vector/EulerAngles.h" 00012 #include "CLHEP/Units/PhysicalConstants.h" 00013 00014 #include <iostream> 00015 #include <math.h> 00016 00017 using std::cout; 00018 using namespace CLHEP; 00019 00020 const int Nperturbs = 24; 00021 00022 void perturb ( int i, const HepRotation & r, HepRotation & rp, double & del ); 00023 bool compareR ( const HepRotation & r1, const HepRotation & r2, double tol ); 00024 00025 bool test (double phi, double theta, double psi) { 00026 HepRotation r (phi, theta, psi); 00027 HepRotation rp; 00028 HepRotation rpe; 00029 HepEulerAngles e; 00030 bool retval = true; 00031 double del; 00032 cout << "\n\n -------------------------------- \n\n"; 00033 for ( int i = 0; i < Nperturbs; ++i ) { 00034 perturb ( i, r, rp, del ); 00035 e = rp.eulerAngles(); 00036 cout << "(" << phi <<", " << theta << ", " << psi << ") --> "<< e << "\n"; 00037 cout << " e.phi() = " << e.phi() 00038 << " rp.phi() = " << rp.phi() 00039 << " difference is " << (e.phi() - rp.phi())/del << " * del\n"; 00040 if ( std::fabs (e.phi() - rp.phi()) > 200*del ) { 00041 cout << "phi discrepancy in " << rp << "\n"; 00042 cout << " e.phi() = " << e.phi() 00043 << " rp.phi() = " << rp.phi() 00044 << " difference is " << e.phi() - rp.phi() << "\n"; 00045 if (del < 1.0e-4) cout << "??????????\n"; 00046 retval = false; 00047 } 00048 if ( std::fabs (e.theta() - rp.theta()) > 200*del ) { 00049 cout << "theta discrepancy in " << rp << "\n"; 00050 cout << " e.theta() = " << e.theta() 00051 << " rp.theta() = " << rp.theta() 00052 << " difference is " << e.theta() - rp.theta() << "\n"; 00053 if (del < 1.0e-4) cout << "??????????\n"; 00054 retval = false; 00055 } 00056 cout << " e.psi() = " << e.psi() 00057 << " rp.psi() = " << rp.psi() 00058 << " difference is " << (e.psi() - rp.psi())/del << " * del\n"; 00059 if ( std::fabs (e.psi() - rp.psi()) > 200*del ) { 00060 cout << "psi discrepancy in " << rp << "\n"; 00061 cout << " e.psi() = " << e.psi() 00062 << " rp.psi() = " << rp.psi() 00063 << " difference is " << e.psi() - rp.psi() << "\n"; 00064 if (del < 1.0e-4) cout << "??????????\n"; 00065 retval = false; 00066 } 00067 rpe.set(e); 00068 retval &= compareR (rpe, rp, del); 00069 } 00070 return retval; 00071 } 00072 00073 int main () { 00074 00075 bool res = true; 00076 double PI = CLHEP::pi; 00077 00078 // Some cases not in the potentially unstable region: 00079 res &= test ( .05, PI/5, .1 ); 00080 res &= test ( .4, PI/7, -.35 ); 00081 res &= test ( PI/3, PI/6, -PI/3 ); 00082 res &= test ( PI/5, PI/2, -PI/2.5 ); 00083 res &= test ( -PI/5, PI/2, PI/3 ); 00084 res &= test ( -4*PI/5, PI/2, PI/2.5 ); 00085 res &= test ( 4*PI/5, PI/2, 2*PI/3 ); 00086 res &= test ( -3*PI/4, PI/2, -PI/3 ); 00087 res &= test ( 5*PI/6, PI/2, -4*PI/5 ); 00088 res &= test ( 5*PI/6, PI/2, -PI/3 ); 00089 00090 // Specialized cases 00091 res &= test ( .05, 0, .1 ); 00092 res &= test ( .2, 0, 1.1 ); 00093 res &= test ( -.4, 0, .4 ); 00094 res &= test ( -2.4, 0, 2.0 ); 00095 res &= test ( -2.4, 0, -2.0 ); 00096 res &= test ( -2.2, 0, 1.8 ); 00097 res &= test ( -2.2, 0, -1.8 ); 00098 res &= test ( .05, PI, .1 ); 00099 res &= test ( .2, PI, 1.1 ); 00100 res &= test ( -.4, PI, .4 ); 00101 res &= test ( -2.4, PI, 2.0 ); 00102 res &= test ( -2.4, PI, -2.0 ); 00103 res &= test ( -2.2, PI, 1.8 ); 00104 res &= test ( -2.2, PI, -1.8 ); 00105 00106 // Cases near zero 00107 res &= test ( .1, .0000000004, .5 ); 00108 res &= test ( -1.2, .0000000004, .5 ); 00109 res &= test ( .7, .0000000004, -.6 ); 00110 res &= test ( 1.5, .0000000004, -1.1 ); 00111 res &= test ( 1.4, .0000000004, -1.5 ); 00112 res &= test ( -.1, .0000000000028, .5 ); 00113 res &= test ( -1.2, .0000000000028, -.5 ); 00114 res &= test ( .7, .0000000000028, -.6 ); 00115 res &= test ( -1.5, .0000000000028, -1.1 ); 00116 res &= test ( 1.4, .0000000000028, 1.5 ); 00117 00118 // Cases near PI 00119 double nearPI = PI - .00000002; 00120 res &= test ( .1, nearPI, .5 ); 00121 res &= test ( -1.2, nearPI, .5 ); 00122 res &= test ( .7, nearPI, -.6 ); 00123 res &= test ( 1.5, nearPI, -1.1 ); 00124 res &= test ( 1.4, nearPI, -1.5 ); 00125 res &= test ( 2.4, nearPI, -1.6 ); 00126 res &= test ( 2.3, nearPI, 1.9 ); 00127 res &= test ( -2.8, nearPI, .6 ); 00128 res &= test ( -.4, nearPI, -3.1 ); 00129 nearPI = PI - .000000000009; 00130 res &= test ( .1, nearPI, -.5 ); 00131 res &= test ( 1.2, nearPI, .5 ); 00132 res &= test ( .7, nearPI, -.6 ); 00133 res &= test ( 1.5, nearPI, 1.1 ); 00134 res &= test ( -1.4, nearPI, -1.5 ); 00135 res &= test ( 2.1, nearPI, -1.2 ); 00136 res &= test ( 2.9, nearPI, .9 ); 00137 res &= test ( -2.8, nearPI, 1.6 ); 00138 res &= test ( -.4, nearPI, -3.0 ); 00139 00140 if (!res) return -1; 00141 return 0; 00142 } 00143 00144 bool compareR ( const HepRotation & r1, const HepRotation & r2, double tol ) { 00145 HepRep3x3 m1 = r1.rep3x3(); 00146 HepRep3x3 m2 = r2.rep3x3(); 00147 double flaw = 0; 00148 flaw = max (flaw, (m1.xx_ - m2.xx_)); 00149 flaw = max (flaw, (m1.xy_ - m2.xy_)); 00150 flaw = max (flaw, (m1.xz_ - m2.xz_)); 00151 flaw = max (flaw, (m1.yx_ - m2.yx_)); 00152 flaw = max (flaw, (m1.yy_ - m2.yy_)); 00153 flaw = max (flaw, (m1.yz_ - m2.yz_)); 00154 flaw = max (flaw, (m1.zx_ - m2.zx_)); 00155 flaw = max (flaw, (m1.zy_ - m2.zy_)); 00156 flaw = max (flaw, (m1.zz_ - m2.zz_)); 00157 if (flaw > 20*std::sqrt(tol)) { 00158 cout << "???????? comparison flaw at level of " << flaw << "\n" 00159 << r1 << r2; 00160 } 00161 cout << "flaw size is " << flaw << " (" << flaw/std::sqrt(tol) << ")\n\n"; 00162 return (flaw <= tol); 00163 } 00164 00165 void perturb ( int i, const HepRotation & r, HepRotation & rp, double & del ) { 00166 00167 HepRep3x3 p0 ( 1, 3, -2, 00168 -1, -2, 4, 00169 2, -1, -1 ); 00170 00171 HepRep3x3 p1 ( 1, -1, -2, 00172 1, 3, -1, 00173 2, -1, -3 ); 00174 00175 HepRep3x3 p2 ( 5, 1, -5, 00176 -3, -2, 3, 00177 -1, 4, -1 ); 00178 00179 HepRep3x3 p3 ( -2, -2, 1, 00180 -1, -2, -4, 00181 4, 2, -2 ); 00182 00183 HepRep3x3 p[4]; 00184 p[0] = p0; 00185 p[1] = p1; 00186 p[2] = p2; 00187 p[3] = p3; 00188 00189 int cycle = i/4; 00190 double q; 00191 switch (cycle){ 00192 case 0: q = 1.0e-14; break; 00193 case 1: q = 1.0e-12; break; 00194 case 2: q = 1.0e-10; break; 00195 case 3: q = 1.0e-8; break; 00196 case 4: q = 1.0e-6; break; 00197 case 5: q = 1.0e-4; break; 00198 } 00199 HepRep3x3 d = p[i%4]; 00200 HepRep3x3 m = r.rep3x3(); 00201 if ((m.zz_ + q*d.zz_) < -1) { 00202 d.zz_ = -d.zz_; 00203 } 00204 cout << "i = " << i << " q is " << q << "\n"; 00205 rp.set (HepRep3x3 ( m.xx_ + q*d.xx_ , m.xy_ + q*d.xy_ , m.xz_ + q*d.xz_ , 00206 m.yx_ + q*d.yx_ , m.yy_ + q*d.yy_ , m.yz_ + q*d.yz_ , 00207 m.zx_ + q*d.zx_ , m.zy_ + q*d.zy_ , m.zz_ + q*d.zz_ ) ); 00208 del = q; 00209 }
1.4.7