15 #pragma implementation 18 #include "CLHEP/Vector/defs.h" 19 #include "CLHEP/Vector/Rotation.h" 20 #include "CLHEP/Vector/EulerAngles.h" 21 #include "CLHEP/Units/PhysicalConstants.h" 28 static inline double safe_acos (
double x) {
29 if (std::abs(x) <= 1.0)
return std::acos(x);
30 return ( (x>0) ? 0 : CLHEP::pi );
39 register double sinPhi = std::sin( phi1 ), cosPhi = std::cos( phi1 );
40 register double sinTheta = std::sin( theta1 ), cosTheta = std::cos( theta1 );
41 register double sinPsi = std::sin( psi1 ), cosPsi = std::cos( psi1 );
43 rxx = cosPsi * cosPhi - cosTheta * sinPhi * sinPsi;
44 rxy = cosPsi * sinPhi + cosTheta * cosPhi * sinPsi;
45 rxz = sinPsi * sinTheta;
47 ryx = - sinPsi * cosPhi - cosTheta * sinPhi * cosPsi;
48 ryy = - sinPsi * sinPhi + cosTheta * cosPhi * cosPsi;
49 ryz = cosPsi * sinTheta;
51 rzx = sinTheta * sinPhi;
52 rzy = - sinTheta * cosPhi;
61 set (phi1, theta1, psi1);
78 "HepRotation::phi() finds | rzz | > 1 "));
81 const double sinTheta = std::sqrt( s2 );
89 const double cscTheta = 1/sinTheta;
90 double cosabsphi = -
rzy * cscTheta;
91 if ( std::fabs(cosabsphi) > 1 ) {
93 "HepRotation::phi() finds | cos phi | > 1 "));
96 const double absPhi = std::acos ( cosabsphi );
102 return (
rzy < 0) ? 0 : CLHEP::pi;
109 return safe_acos(
rzz );
116 if ( std::fabs(
rzz) > 1 ) {
118 "HepRotation::psi() finds | rzz | > 1"));
121 sinTheta = std::sqrt( 1.0 -
rzz*
rzz );
124 if (sinTheta < .01) {
130 const double cscTheta = 1/sinTheta;
131 double cosabspsi =
ryz * cscTheta;
132 if ( std::fabs(cosabspsi) > 1 ) {
134 "HepRotation::psi() finds | cos psi | > 1"));
137 const double absPsi = std::acos ( cosabspsi );
140 }
else if (
rxz < 0) {
143 return (
ryz > 0) ? 0 : CLHEP::pi;
152 void correctByPi (
double& psi1,
double& phi1 ) {
166 void correctPsiPhi (
double rxz,
double rzx,
double ryz,
double rzy,
167 double& psi1,
double& phi1 ) {
175 double maxw = std::abs(w[0]);
177 for (
int i = 1; i < 4; ++i) {
178 if (std::abs(w[i]) > maxw) {
179 maxw = std::abs(w[i]);
187 if (w[0] > 0 && psi1 < 0) correctByPi ( psi1, phi1 );
188 if (w[0] < 0 && psi1 > 0) correctByPi ( psi1, phi1 );
191 if (w[1] > 0 && phi1 < 0) correctByPi ( psi1, phi1 );
192 if (w[1] < 0 && phi1 > 0) correctByPi ( psi1, phi1 );
195 if (w[2] > 0 && std::abs(psi1) > CLHEP::halfpi) correctByPi ( psi1, phi1 );
196 if (w[2] < 0 && std::abs(psi1) < CLHEP::halfpi) correctByPi ( psi1, phi1 );
199 if (w[3] > 0 && std::abs(phi1) > CLHEP::halfpi) correctByPi ( psi1, phi1 );
200 if (w[3] < 0 && std::abs(phi1) < CLHEP::halfpi) correctByPi ( psi1, phi1 );
210 double phi1, theta1, psi1;
211 double psiPlusPhi, psiMinusPhi;
213 theta1 = safe_acos(
rzz );
215 if (
rzz > 1 ||
rzz < -1) {
217 "HepRotation::eulerAngles() finds | rzz | > 1 "));
220 double cosTheta =
rzz;
221 if (cosTheta > 1) cosTheta = 1;
222 if (cosTheta < -1) cosTheta = -1;
228 }
else if (cosTheta >= 0) {
236 psiMinusPhi = std::atan2 ( s1, c );
238 }
else if (cosTheta > -1) {
246 psiPlusPhi = std::atan2 ( s1, c );
255 psi1 = .5 * (psiPlusPhi + psiMinusPhi);
256 phi1 = .5 * (psiPlusPhi - psiMinusPhi);
260 correctPsiPhi ( rxz, rzx, ryz, rzy, psi1, phi1 );
273 set (
phi(), theta1,
psi() );
HepRotation & set(const Hep3Vector &axis, double delta)
void setTheta(double theta)
HepEulerAngles eulerAngles() const