Gabriele Cosmo
273 lines
7.5 KiB
C++
273 lines
7.5 KiB
C++
// -*- C++ -*-
|
|
// ---------------------------------------------------------------------------
|
|
//
|
|
// This file is a part of the CLHEP - a Class Library for High Energy Physics.
|
|
//
|
|
// This is the implementation of methods of the HepRotation class which
|
|
// were introduced when ZOOM PhysicsVectors was merged in, and which involve
|
|
// Euler Angles representation.
|
|
//
|
|
// Apr 28, 2003 mf Modified way of computing Euler angles to avoid flawed
|
|
// answers in the case where theta is near 0 of pi, and
|
|
// the matrix is not a perfect rotation (due to roundoff).
|
|
|
|
#include "CLHEP/Vector/Rotation.h"
|
|
#include "CLHEP/Vector/EulerAngles.h"
|
|
#include "CLHEP/Units/PhysicalConstants.h"
|
|
|
|
#include <cmath>
|
|
#include <iostream>
|
|
|
|
namespace CLHEP {
|
|
|
|
static inline double safe_acos (double x) {
|
|
if (std::abs(x) <= 1.0) return std::acos(x);
|
|
return ( (x>0) ? 0 : CLHEP::pi );
|
|
}
|
|
|
|
// ---------- Constructors and Assignment:
|
|
|
|
// Euler angles
|
|
|
|
HepRotation & HepRotation::set(double phi1, double theta1, double psi1) {
|
|
|
|
double sinPhi = std::sin( phi1 ), cosPhi = std::cos( phi1 );
|
|
double sinTheta = std::sin( theta1 ), cosTheta = std::cos( theta1 );
|
|
double sinPsi = std::sin( psi1 ), cosPsi = std::cos( psi1 );
|
|
|
|
rxx = cosPsi * cosPhi - cosTheta * sinPhi * sinPsi;
|
|
rxy = cosPsi * sinPhi + cosTheta * cosPhi * sinPsi;
|
|
rxz = sinPsi * sinTheta;
|
|
|
|
ryx = - sinPsi * cosPhi - cosTheta * sinPhi * cosPsi;
|
|
ryy = - sinPsi * sinPhi + cosTheta * cosPhi * cosPsi;
|
|
ryz = cosPsi * sinTheta;
|
|
|
|
rzx = sinTheta * sinPhi;
|
|
rzy = - sinTheta * cosPhi;
|
|
rzz = cosTheta;
|
|
|
|
return *this;
|
|
|
|
} // Rotation::set(phi, theta, psi)
|
|
|
|
HepRotation::HepRotation( double phi1, double theta1, double psi1 )
|
|
{
|
|
set (phi1, theta1, psi1);
|
|
}
|
|
HepRotation & HepRotation::set( const HepEulerAngles & e ) {
|
|
return set(e.phi(), e.theta(), e.psi());
|
|
}
|
|
HepRotation::HepRotation ( const HepEulerAngles & e )
|
|
{
|
|
set(e.phi(), e.theta(), e.psi());
|
|
}
|
|
|
|
|
|
double HepRotation::phi () const {
|
|
|
|
double s2 = 1.0 - rzz*rzz;
|
|
if (s2 < 0) {
|
|
std::cerr << "HepRotation::phi() - "
|
|
<< "HepRotation::phi() finds | rzz | > 1 " << std::endl;
|
|
s2 = 0;
|
|
}
|
|
const double sinTheta = std::sqrt( s2 );
|
|
|
|
if (sinTheta < .01) { // For theta close to 0 or PI, use the more stable
|
|
// algorithm to get all three Euler angles
|
|
HepEulerAngles ea = eulerAngles();
|
|
return ea.phi();
|
|
}
|
|
|
|
const double cscTheta = 1/sinTheta;
|
|
double cosabsphi = - rzy * cscTheta;
|
|
if ( std::fabs(cosabsphi) > 1 ) { // NaN-proofing
|
|
std::cerr << "HepRotation::phi() - "
|
|
<< "HepRotation::phi() finds | cos phi | > 1 " << std::endl;
|
|
cosabsphi = 1;
|
|
}
|
|
const double absPhi = std::acos ( cosabsphi );
|
|
if (rzx > 0) {
|
|
return absPhi;
|
|
} else if (rzx < 0) {
|
|
return -absPhi;
|
|
} else {
|
|
return (rzy < 0) ? 0 : CLHEP::pi;
|
|
}
|
|
|
|
} // phi()
|
|
|
|
double HepRotation::theta() const {
|
|
|
|
return safe_acos( rzz );
|
|
|
|
} // theta()
|
|
|
|
double HepRotation::psi () const {
|
|
|
|
double sinTheta;
|
|
if ( std::fabs(rzz) > 1 ) { // NaN-proofing
|
|
std::cerr << "HepRotation::psi() - "
|
|
<< "HepRotation::psi() finds | rzz | > 1" << std::endl;
|
|
sinTheta = 0;
|
|
} else {
|
|
sinTheta = std::sqrt( 1.0 - rzz*rzz );
|
|
}
|
|
|
|
if (sinTheta < .01) { // For theta close to 0 or PI, use the more stable
|
|
// algorithm to get all three Euler angles
|
|
HepEulerAngles ea = eulerAngles();
|
|
return ea.psi();
|
|
}
|
|
|
|
const double cscTheta = 1/sinTheta;
|
|
double cosabspsi = ryz * cscTheta;
|
|
if ( std::fabs(cosabspsi) > 1 ) { // NaN-proofing
|
|
std::cerr << "HepRotation::psi() - "
|
|
<< "HepRotation::psi() finds | cos psi | > 1" << std::endl;
|
|
cosabspsi = 1;
|
|
}
|
|
const double absPsi = std::acos ( cosabspsi );
|
|
if (rxz > 0) {
|
|
return absPsi;
|
|
} else if (rxz < 0) {
|
|
return -absPsi;
|
|
} else {
|
|
return (ryz > 0) ? 0 : CLHEP::pi;
|
|
}
|
|
|
|
} // psi()
|
|
|
|
// Helpers for eulerAngles():
|
|
|
|
static
|
|
void correctByPi ( double& psi1, double& phi1 ) {
|
|
if (psi1 > 0) {
|
|
psi1 -= CLHEP::pi;
|
|
} else {
|
|
psi1 += CLHEP::pi;
|
|
}
|
|
if (phi1 > 0) {
|
|
phi1 -= CLHEP::pi;
|
|
} else {
|
|
phi1 += CLHEP::pi;
|
|
}
|
|
}
|
|
|
|
static
|
|
void correctPsiPhi ( double rxz, double rzx, double ryz, double rzy,
|
|
double& psi1, double& phi1 ) {
|
|
|
|
// set up quatities which would be positive if sin and cosine of
|
|
// psi1 and phi1 were positive:
|
|
double w[4];
|
|
w[0] = rxz; w[1] = rzx; w[2] = ryz; w[3] = -rzy;
|
|
|
|
// find biggest relevant term, which is the best one to use in correcting.
|
|
double maxw = std::abs(w[0]);
|
|
int imax = 0;
|
|
for (int i = 1; i < 4; ++i) {
|
|
if (std::abs(w[i]) > maxw) {
|
|
maxw = std::abs(w[i]);
|
|
imax = i;
|
|
}
|
|
}
|
|
// Determine if the correction needs to be applied: The criteria are
|
|
// different depending on whether a sine or cosine was the determinor:
|
|
switch (imax) {
|
|
case 0:
|
|
if (w[0] > 0 && psi1 < 0) correctByPi ( psi1, phi1 );
|
|
if (w[0] < 0 && psi1 > 0) correctByPi ( psi1, phi1 );
|
|
break;
|
|
case 1:
|
|
if (w[1] > 0 && phi1 < 0) correctByPi ( psi1, phi1 );
|
|
if (w[1] < 0 && phi1 > 0) correctByPi ( psi1, phi1 );
|
|
break;
|
|
case 2:
|
|
if (w[2] > 0 && std::abs(psi1) > CLHEP::halfpi) correctByPi ( psi1, phi1 );
|
|
if (w[2] < 0 && std::abs(psi1) < CLHEP::halfpi) correctByPi ( psi1, phi1 );
|
|
break;
|
|
case 3:
|
|
if (w[3] > 0 && std::abs(phi1) > CLHEP::halfpi) correctByPi ( psi1, phi1 );
|
|
if (w[3] < 0 && std::abs(phi1) < CLHEP::halfpi) correctByPi ( psi1, phi1 );
|
|
break;
|
|
}
|
|
}
|
|
|
|
HepEulerAngles HepRotation::eulerAngles() const {
|
|
|
|
// Please see the mathematical justification in eulerAngleComputations.ps
|
|
|
|
double phi1, theta1, psi1;
|
|
double psiPlusPhi, psiMinusPhi;
|
|
|
|
theta1 = safe_acos( rzz );
|
|
|
|
// if (rzz > 1 || rzz < -1) {
|
|
// std::cerr << "HepRotation::eulerAngles() - "
|
|
// << "HepRotation::eulerAngles() finds | rzz | > 1 " << std::endl;
|
|
// }
|
|
|
|
double cosTheta = rzz;
|
|
if (cosTheta > 1) cosTheta = 1;
|
|
if (cosTheta < -1) cosTheta = -1;
|
|
|
|
if (cosTheta == 1) {
|
|
psiPlusPhi = std::atan2 ( rxy - ryx, rxx + ryy );
|
|
psiMinusPhi = 0;
|
|
|
|
} else if (cosTheta >= 0) {
|
|
|
|
// In this realm, the atan2 expression for psi + phi is numerically stable
|
|
psiPlusPhi = std::atan2 ( rxy - ryx, rxx + ryy );
|
|
|
|
// psi - phi is potentially more subtle, but when unstable it is moot
|
|
double s1 = -rxy - ryx; // sin (psi-phi) * (1 - cos theta)
|
|
double c1 = rxx - ryy; // cos (psi-phi) * (1 - cos theta)
|
|
psiMinusPhi = std::atan2 ( s1, c1 );
|
|
|
|
} else if (cosTheta > -1) {
|
|
|
|
// In this realm, the atan2 expression for psi - phi is numerically stable
|
|
psiMinusPhi = std::atan2 ( -rxy - ryx, rxx - ryy );
|
|
|
|
// psi + phi is potentially more subtle, but when unstable it is moot
|
|
double s1 = rxy - ryx; // sin (psi+phi) * (1 + cos theta)
|
|
double c1 = rxx + ryy; // cos (psi+phi) * (1 + cos theta)
|
|
psiPlusPhi = std::atan2 ( s1, c1 );
|
|
|
|
} else { // cosTheta == -1
|
|
|
|
psiMinusPhi = std::atan2 ( -rxy - ryx, rxx - ryy );
|
|
psiPlusPhi = 0;
|
|
|
|
}
|
|
|
|
psi1 = .5 * (psiPlusPhi + psiMinusPhi);
|
|
phi1 = .5 * (psiPlusPhi - psiMinusPhi);
|
|
|
|
// Now correct by pi if we have managed to get a value of psiPlusPhi
|
|
// or psiMinusPhi that was off by 2 pi:
|
|
correctPsiPhi ( rxz, rzx, ryz, rzy, psi1, phi1 );
|
|
|
|
return HepEulerAngles( phi1, theta1, psi1 );
|
|
|
|
} // eulerAngles()
|
|
|
|
|
|
void HepRotation::setPhi (double phi1) {
|
|
set ( phi1, theta(), psi() );
|
|
}
|
|
|
|
void HepRotation::setTheta (double theta1) {
|
|
set ( phi(), theta1, psi() );
|
|
}
|
|
|
|
void HepRotation::setPsi (double psi1) {
|
|
set ( phi(), theta(), psi1 );
|
|
}
|
|
|
|
} // namespace CLHEP
|
|
|