00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #ifdef GNUPRAGMA
00013 #pragma implementation
00014 #endif
00015
00016 #include "CLHEP/Vector/Rotation.h"
00017
00018 #include <cmath>
00019
00020 namespace CLHEP {
00021
00022
00023
00024 bool HepRotation::setCols
00025 ( const Hep3Vector & u1, const Hep3Vector & u2, const Hep3Vector & u3,
00026 double u1u2,
00027 Hep3Vector & v1, Hep3Vector & v2, Hep3Vector & v3 ) const {
00028
00029 if ( (1-std::fabs(u1u2)) <= Hep4RotationInterface::tolerance ) {
00030 std::cerr << "HepRotation::setCols() - "
00031 << "All three cols supplied for a Rotation are parallel --"
00032 << "\n an arbitrary rotation will be returned" << std::endl;
00033 setArbitrarily (u1, v1, v2, v3);
00034 return true;
00035 }
00036
00037 v1 = u1;
00038 v2 = Hep3Vector(u2 - u1u2 * u1).unit();
00039 v3 = v1.cross(v2);
00040 if ( v3.dot(u3) >= 0 ) {
00041 return true;
00042 } else {
00043 return false;
00044 }
00045
00046 }
00047
00048 void HepRotation::setArbitrarily (const Hep3Vector & ccolX,
00049 Hep3Vector & v1, Hep3Vector & v2, Hep3Vector & v3) const {
00050
00051
00052
00053
00054 v1 = ccolX.unit();
00055 v2 = v1.cross(Hep3Vector(0,0,1));
00056 if (v2.mag2() != 0) {
00057 v2 = v2.unit();
00058 } else {
00059 v2 = Hep3Vector(1,0,0);
00060 }
00061 v3 = v1.cross(v2);
00062
00063 return;
00064
00065 }
00066
00067
00068
00069
00070
00071
00072
00073 HepRotation & HepRotation::set( const Hep3Vector & ccolX,
00074 const Hep3Vector & ccolY,
00075 const Hep3Vector & ccolZ ) {
00076 Hep3Vector ucolX = ccolX.unit();
00077 Hep3Vector ucolY = ccolY.unit();
00078 Hep3Vector ucolZ = ccolZ.unit();
00079
00080 double u1u2 = ucolX.dot(ucolY);
00081 double f12 = std::fabs(u1u2);
00082 if ( f12 > Hep4RotationInterface::tolerance ) {
00083 std::cerr << "HepRotation::set() - "
00084 << "col's X and Y supplied for Rotation are not close to orthogonal"
00085 << std::endl;
00086 }
00087 double u1u3 = ucolX.dot(ucolZ);
00088 double f13 = std::fabs(u1u3);
00089 if ( f13 > Hep4RotationInterface::tolerance ) {
00090 std::cerr << "HepRotation::set() - "
00091 << "col's X and Z supplied for Rotation are not close to orthogonal"
00092 << std::endl;
00093 }
00094 double u2u3 = ucolY.dot(ucolZ);
00095 double f23 = std::fabs(u2u3);
00096 if ( f23 > Hep4RotationInterface::tolerance ) {
00097 std::cerr << "HepRotation::set() - "
00098 << "col's Y and Z supplied for Rotation are not close to orthogonal"
00099 << std::endl;
00100 }
00101
00102 Hep3Vector v1, v2, v3;
00103 bool isRotation;
00104 if ( (f12 <= f13) && (f12 <= f23) ) {
00105 isRotation = setCols ( ucolX, ucolY, ucolZ, u1u2, v1, v2, v3 );
00106 if ( !isRotation ) {
00107 std::cerr << "HepRotation::set() - "
00108 << "col's X Y and Z supplied form closer to a reflection than a Rotation "
00109 << "\n col Z is set to col X cross col Y" << std::endl;
00110 }
00111 } else if ( f13 <= f23 ) {
00112 isRotation = setCols ( ucolZ, ucolX, ucolY, u1u3, v3, v1, v2 );
00113 if ( !isRotation ) {
00114 std::cerr << "HepRotation::set() - "
00115 << "col's X Y and Z supplied form closer to a reflection than a Rotation "
00116 << "\n col Y is set to col Z cross col X" << std::endl;
00117 }
00118 } else {
00119 isRotation = setCols ( ucolY, ucolZ, ucolX, u2u3, v2, v3, v1 );
00120 if ( !isRotation ) {
00121 std::cerr << "HepRotation::set() - "
00122 << "col's X Y and Z supplied form closer to a reflection than a Rotation "
00123 << "\n col X is set to col Y cross col Z" << std::endl;
00124 }
00125 }
00126
00127 rxx = v1.x(); ryx = v1.y(); rzx = v1.z();
00128 rxy = v2.x(); ryy = v2.y(); rzy = v2.z();
00129 rxz = v3.x(); ryz = v3.y(); rzz = v3.z();
00130
00131 return *this;
00132
00133 }
00134
00135 HepRotation::HepRotation ( const Hep3Vector & ccolX,
00136 const Hep3Vector & ccolY,
00137 const Hep3Vector & ccolZ )
00138 {
00139 set (ccolX, ccolY, ccolZ);
00140 }
00141
00142 HepRotation & HepRotation::setRows( const Hep3Vector & rrowX,
00143 const Hep3Vector & rrowY,
00144 const Hep3Vector & rrowZ ) {
00145 set (rrowX, rrowY, rrowZ);
00146 invert();
00147 return *this;
00148 }
00149
00150
00151
00152 void HepRotation::rectify() {
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163 double det = rxx * ryy * rzz +
00164 rxy * ryz * rzx +
00165 rxz * ryx * rzy -
00166 rxx * ryz * rzy -
00167 rxy * ryx * rzz -
00168 rxz * ryy * rzx ;
00169 if (det <= 0) {
00170 std::cerr << "HepRotation::rectify() - "
00171 << "Attempt to rectify a Rotation with determinant <= 0" << std::endl;
00172 return;
00173 }
00174 double di = 1.0 / det;
00175
00176
00177 double xx1 = (ryy * rzz - ryz * rzy) * di;
00178 double xy1 = (rzy * rxz - rzz * rxy) * di;
00179 double xz1 = (rxy * ryz - rxz * ryy) * di;
00180 double yx1 = (ryz * rzx - ryx * rzz) * di;
00181 double yy1 = (rzz * rxx - rzx * rxz) * di;
00182 double yz1 = (rxz * ryx - rxx * ryz) * di;
00183 double zx1 = (ryx * rzy - ryy * rzx) * di;
00184 double zy1 = (rzx * rxy - rzy * rxx) * di;
00185 double zz1 = (rxx * ryy - rxy * ryx) * di;
00186
00187
00188 rxx = .5*(rxx + xx1);
00189 rxy = .5*(rxy + yx1);
00190 rxz = .5*(rxz + zx1);
00191 ryx = .5*(ryx + xy1);
00192 ryy = .5*(ryy + yy1);
00193 ryz = .5*(ryz + zy1);
00194 rzx = .5*(rzx + xz1);
00195 rzy = .5*(rzy + yz1);
00196 rzz = .5*(rzz + zz1);
00197
00198
00199 double del = delta();
00200 Hep3Vector u = axis();
00201 u = u.unit();
00202
00203 set(u, del);
00204
00205 }
00206
00207 }
00208