| Geant4 Cross Reference |
1 // -*- C++ -*-
2 // ---------------------------------------------------------------------------
3 //
4 // This file is a part of the CLHEP - a Class Library for High Energy Physics.
5 //
6 // This is the implementation of those methods of the HepRotation class which
7 // were introduced when ZOOM PhysicsVectors was merged in, and which involve
8 // the angle/axis representation of a Rotation.
9 //
10
11 #include "CLHEP/Vector/Rotation.h"
12 #include "CLHEP/Units/PhysicalConstants.h"
13
14 #include <iostream>
15 #include <cmath>
16
17 namespace CLHEP {
18
19 // ---------- Constructors and Assignment:
20
21 // axis and angle
22
23 HepRotation & HepRotation::set( const Hep3Vector & aaxis, double ddelta ) {
24
25 double sinDelta = std::sin(ddelta), cosDelta = std::cos(ddelta);
26 double oneMinusCosDelta = 1.0 - cosDelta;
27
28 Hep3Vector u = aaxis.unit();
29
30 double uX = u.getX();
31 double uY = u.getY();
32 double uZ = u.getZ();
33
34 rxx = oneMinusCosDelta * uX * uX + cosDelta;
35 rxy = oneMinusCosDelta * uX * uY - sinDelta * uZ;
36 rxz = oneMinusCosDelta * uX * uZ + sinDelta * uY;
37
38 ryx = oneMinusCosDelta * uY * uX + sinDelta * uZ;
39 ryy = oneMinusCosDelta * uY * uY + cosDelta;
40 ryz = oneMinusCosDelta * uY * uZ - sinDelta * uX;
41
42 rzx = oneMinusCosDelta * uZ * uX - sinDelta * uY;
43 rzy = oneMinusCosDelta * uZ * uY + sinDelta * uX;
44 rzz = oneMinusCosDelta * uZ * uZ + cosDelta;
45
46 return *this;
47
48 } // HepRotation::set(axis, delta)
49
50 HepRotation::HepRotation ( const Hep3Vector & aaxis, double ddelta )
51 {
52 set( aaxis, ddelta );
53 }
54 HepRotation & HepRotation::set( const HepAxisAngle & ax ) {
55 return set ( ax.axis(), ax.delta() );
56 }
57 HepRotation::HepRotation ( const HepAxisAngle & ax )
58 {
59 set ( ax.axis(), ax.delta() );
60 }
61
62 double HepRotation::delta() const {
63
64 double cosdelta = (rxx + ryy + rzz - 1.0) / 2.0;
65 if (cosdelta > 1.0) {
66 return 0;
67 } else if (cosdelta < -1.0) {
68 return CLHEP::pi;
69 } else {
70 return std::acos( cosdelta ); // Already safe due to the cosdelta > 1 check
71 }
72
73 } // delta()
74
75 Hep3Vector HepRotation::axis () const {
76
77 const double eps = 1e-15;
78
79 double Ux = rzy - ryz;
80 double Uy = rxz - rzx;
81 double Uz = ryx - rxy;
82 if (std::abs(Ux) < eps && std::abs(Uy) < eps && std::abs(Uz) < eps) {
83
84 double cosdelta = (rxx + ryy + rzz - 1.0) / 2.0;
85 if (cosdelta > 0.0) return Hep3Vector(0,0,1); // angle = 0, any axis is good
86
87 double mxx = (rxx + 1)/2;
88 double myy = (ryy + 1)/2;
89 double mzz = (rzz + 1)/2;
90 double mxy = (rxy + ryx)/4;
91 double mxz = (rxz + rzx)/4;
92 double myz = (ryz + rzy)/4;
93 double x, y, z;
94
95 if (mxx > ryy && mxx > rzz) {
96 x = std::sqrt(mxx);
97 if (rzy - ryz < 0) x = -x;
98 y = mxy/x;
99 z = mxz/x;
100 return Hep3Vector( x, y, z ).unit();
101 } else if (myy > mzz) {
102 y = std::sqrt(myy);
103 if (rxz - rzx < 0) y = -y;
104 x = mxy/y;
105 z = myz/y;
106 return Hep3Vector( x, y, z ).unit();
107 } else {
108 z = std::sqrt(mzz);
109 if (ryx - rxy < 0) z = -z;
110 x = mxz/z;
111 y = myz/z;
112 return Hep3Vector( x, y, z ).unit();
113 }
114 } else {
115 return Hep3Vector( Ux, Uy, Uz ).unit();
116 }
117
118 } // axis()
119
120 HepAxisAngle HepRotation::axisAngle() const {
121
122 return HepAxisAngle (axis(), delta());
123
124 } // axisAngle()
125
126
127 void HepRotation::setAxis (const Hep3Vector & aaxis) {
128 set ( aaxis, delta() );
129 }
130
131 void HepRotation::setDelta (double ddelta) {
132 set ( axis(), ddelta );
133 }
134
135 } // namespace CLHEP
136