15 #pragma implementation
27 if (std::abs(x) <= 1.0)
return std::acos(x);
37 double sinPhi = std::sin( phi1 ), cosPhi = std::cos( phi1 );
38 double sinTheta = std::sin( theta1 ), cosTheta = std::cos( theta1 );
39 double sinPsi = std::sin( psi1 ), cosPsi = std::cos( psi1 );
41 rxx = cosPsi * cosPhi - cosTheta * sinPhi * sinPsi;
42 rxy = cosPsi * sinPhi + cosTheta * cosPhi * sinPsi;
43 rxz = sinPsi * sinTheta;
45 ryx = - sinPsi * cosPhi - cosTheta * sinPhi * cosPsi;
46 ryy = - sinPsi * sinPhi + cosTheta * cosPhi * cosPsi;
47 ryz = cosPsi * sinTheta;
49 rzx = sinTheta * sinPhi;
50 rzy = - sinTheta * cosPhi;
59 set (phi1, theta1, psi1);
74 std::cerr <<
"HepRotation::phi() - "
75 <<
"HepRotation::phi() finds | rzz | > 1 " << std::endl;
78 const double sinTheta = std::sqrt( s2 );
86 const double cscTheta = 1/sinTheta;
87 double cosabsphi = -
rzy * cscTheta;
88 if ( std::fabs(cosabsphi) > 1 ) {
89 std::cerr <<
"HepRotation::phi() - "
90 <<
"HepRotation::phi() finds | cos phi | > 1 " << std::endl;
93 const double absPhi = std::acos ( cosabsphi );
113 if ( std::fabs(
rzz) > 1 ) {
114 std::cerr <<
"HepRotation::psi() - "
115 <<
"HepRotation::psi() finds | rzz | > 1" << std::endl;
118 sinTheta = std::sqrt( 1.0 -
rzz*
rzz );
121 if (sinTheta < .01) {
127 const double cscTheta = 1/sinTheta;
128 double cosabspsi =
ryz * cscTheta;
129 if ( std::fabs(cosabspsi) > 1 ) {
130 std::cerr <<
"HepRotation::psi() - "
131 <<
"HepRotation::psi() finds | cos psi | > 1" << std::endl;
134 const double absPsi = std::acos ( cosabspsi );
137 }
else if (
rxz < 0) {
163 double& psi1,
double& phi1 ) {
168 w[0] = rxz; w[1] = rzx; w[2] = ryz; w[3] = -rzy;
171 double maxw = std::abs(w[0]);
173 for (
int i = 1; i < 4; ++i) {
174 if (std::abs(w[i]) > maxw) {
175 maxw = std::abs(w[i]);
183 if (w[0] > 0 && psi1 < 0)
correctByPi ( psi1, phi1 );
184 if (w[0] < 0 && psi1 > 0)
correctByPi ( psi1, phi1 );
187 if (w[1] > 0 && phi1 < 0)
correctByPi ( psi1, phi1 );
188 if (w[1] < 0 && phi1 > 0)
correctByPi ( psi1, phi1 );
205 double phi1, theta1, psi1;
206 double psiPlusPhi, psiMinusPhi;
215 double cosTheta =
rzz;
216 if (cosTheta > 1) cosTheta = 1;
217 if (cosTheta < -1) cosTheta = -1;
223 }
else if (cosTheta >= 0) {
231 psiMinusPhi = std::atan2 ( s1, c1 );
233 }
else if (cosTheta > -1) {
241 psiPlusPhi = std::atan2 ( s1, c1 );
250 psi1 = .5 * (psiPlusPhi + psiMinusPhi);
251 phi1 = .5 * (psiPlusPhi - psiMinusPhi);
HepEulerAngles eulerAngles() const
HepRotation & set(const Hep3Vector &axis, double delta)
static double safe_acos(double x)
static void correctPsiPhi(double rxz, double rzx, double ryz, double rzy, double &psi1, double &phi1)
static void correctByPi(double &psi1, double &phi1)
void setTheta(double theta)
static constexpr double halfpi
static constexpr double pi