15 #pragma implementation
18 #include "CLHEP/Vector/Rotation.h"
19 #include "CLHEP/Vector/EulerAngles.h"
20 #include "CLHEP/Units/PhysicalConstants.h"
27 if (std::abs(x) <= 1.0)
return std::acos(x);
35 HepRotation & HepRotation::set(
double phi1,
double theta1,
double psi1) {
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;
57 HepRotation::HepRotation(
double phi1,
double theta1,
double psi1 )
59 set (phi1, theta1, psi1);
61 HepRotation & HepRotation::set(
const HepEulerAngles & e ) {
62 return set(e.phi(), e.theta(), e.psi());
64 HepRotation::HepRotation (
const HepEulerAngles & e )
66 set(e.phi(), e.theta(), e.psi());
70 double HepRotation::phi ()
const {
72 double s2 = 1.0 - rzz*rzz;
74 std::cerr <<
"HepRotation::phi() - "
75 <<
"HepRotation::phi() finds | rzz | > 1 " << std::endl;
78 const double sinTheta = std::sqrt( s2 );
82 HepEulerAngles ea = eulerAngles();
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 );
104 double HepRotation::theta()
const {
110 double HepRotation::psi ()
const {
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) {
123 HepEulerAngles ea = eulerAngles();
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 );
201 HepEulerAngles HepRotation::eulerAngles()
const {
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;
220 psiPlusPhi = std::atan2 ( rxy - ryx, rxx + ryy );
223 }
else if (cosTheta >= 0) {
226 psiPlusPhi = std::atan2 ( rxy - ryx, rxx + ryy );
229 double s1 = -rxy - ryx;
230 double c1 = rxx - ryy;
231 psiMinusPhi = std::atan2 ( s1, c1 );
233 }
else if (cosTheta > -1) {
236 psiMinusPhi = std::atan2 ( -rxy - ryx, rxx - ryy );
239 double s1 = rxy - ryx;
240 double c1 = rxx + ryy;
241 psiPlusPhi = std::atan2 ( s1, c1 );
245 psiMinusPhi = std::atan2 ( -rxy - ryx, rxx - ryy );
250 psi1 = .5 * (psiPlusPhi + psiMinusPhi);
251 phi1 = .5 * (psiPlusPhi - psiMinusPhi);
257 return HepEulerAngles( phi1, theta1, psi1 );
262 void HepRotation::setPhi (
double phi1) {
263 set ( phi1, theta(), psi() );
266 void HepRotation::setTheta (
double theta1) {
267 set ( phi(), theta1, psi() );
270 void HepRotation::setPsi (
double psi1) {
271 set ( phi(), theta(), psi1 );
static const double halfpi
const G4double w[NPOINTSGL]
static void correctByPi(double &psi1, double &phi1)
static void correctPsiPhi(double rxz, double rzx, double ryz, double rzy, double &psi1, double &phi1)
const G4double x[NPOINTSGL]
static double safe_acos(double x)