12 #pragma implementation
15 #include "CLHEP/Vector/Rotation.h"
16 #include "CLHEP/Units/PhysicalConstants.h"
24 if (std::abs(x) <= 1.0)
return std::acos(x);
28 double HepRotation::operator() (
int i,
int j)
const {
30 if (j == 0) {
return xx(); }
31 if (j == 1) {
return xy(); }
32 if (j == 2) {
return xz(); }
34 if (j == 0) {
return yx(); }
35 if (j == 1) {
return yy(); }
36 if (j == 2) {
return yz(); }
38 if (j == 0) {
return zx(); }
39 if (j == 1) {
return zy(); }
40 if (j == 2) {
return zz(); }
42 std::cerr <<
"HepRotation subscripting: bad indices "
43 <<
"(" << i <<
"," << j <<
")" << std::endl;
47 HepRotation & HepRotation::rotate(
double a,
const Hep3Vector& aaxis) {
49 double ll = aaxis.mag();
51 std::cerr <<
"HepRotation::rotate() - "
52 <<
"HepRotation: zero axis" << std::endl;
54 double sa = std::sin(a), ca = std::cos(a);
55 double dx = aaxis.x()/ll, dy = aaxis.y()/ll, dz = aaxis.z()/ll;
57 ca+(1-ca)*dx*dx, (1-ca)*dx*dy-sa*dz, (1-ca)*dx*dz+sa*dy,
58 (1-ca)*dy*dx+sa*dz, ca+(1-ca)*dy*dy, (1-ca)*dy*dz-sa*dx,
59 (1-ca)*dz*dx-sa*dy, (1-ca)*dz*dy+sa*dx, ca+(1-ca)*dz*dz );
66 HepRotation & HepRotation::rotateX(
double a) {
67 double c1 = std::cos(a);
68 double s1 = std::sin(a);
69 double x1 = ryx, y1 = ryy, z1 = ryz;
79 HepRotation & HepRotation::rotateY(
double a){
80 double c1 = std::cos(a);
81 double s1 = std::sin(a);
82 double x1 = rzx, y1 = rzy, z1 = rzz;
92 HepRotation & HepRotation::rotateZ(
double a) {
93 double c1 = std::cos(a);
94 double s1 = std::sin(a);
95 double x1 = rxx, y1 = rxy, z1 = rxz;
100 ryy = s1*y1 + c1*ryy;
101 ryz = s1*z1 + c1*ryz;
105 HepRotation & HepRotation::rotateAxes(
const Hep3Vector &newX,
106 const Hep3Vector &newY,
107 const Hep3Vector &newZ) {
109 Hep3Vector
w = newX.cross(newY);
111 if (std::abs(newZ.x()-w.x()) > del ||
112 std::abs(newZ.y()-w.y()) > del ||
113 std::abs(newZ.z()-w.z()) > del ||
114 std::abs(newX.mag2()-1.) > del ||
115 std::abs(newY.mag2()-1.) > del ||
116 std::abs(newZ.mag2()-1.) > del ||
117 std::abs(newX.dot(newY)) > del ||
118 std::abs(newY.dot(newZ)) > del ||
119 std::abs(newZ.dot(newX)) > del) {
120 std::cerr <<
"HepRotation::rotateAxes: bad axis vectors" << std::endl;
123 return transform(HepRotation(newX.x(), newY.x(), newZ.x(),
124 newX.y(), newY.y(), newZ.y(),
125 newX.z(), newY.z(), newZ.z()));
129 double HepRotation::phiX()
const {
130 return (yx() == 0.0 && xx() == 0.0) ? 0.0 : std::atan2(yx(),xx());
133 double HepRotation::phiY()
const {
134 return (yy() == 0.0 && xy() == 0.0) ? 0.0 : std::atan2(yy(),xy());
137 double HepRotation::phiZ()
const {
138 return (yz() == 0.0 && xz() == 0.0) ? 0.0 : std::atan2(yz(),xz());
141 double HepRotation::thetaX()
const {
145 double HepRotation::thetaY()
const {
149 double HepRotation::thetaZ()
const {
153 void HepRotation::getAngleAxis(
double &
angle, Hep3Vector &aaxis)
const {
154 double cosa = 0.5*(xx()+yy()+zz()-1);
155 double cosa1 = 1-cosa;
158 aaxis = Hep3Vector(0,0,1);
160 double x=0, y=0,
z=0;
161 if (xx() > cosa) x = std::sqrt((xx()-cosa)/cosa1);
162 if (yy() > cosa) y = std::sqrt((yy()-cosa)/cosa1);
163 if (zz() > cosa)
z = std::sqrt((zz()-cosa)/cosa1);
164 if (zy() < yz()) x = -
x;
165 if (xz() < zx()) y = -y;
166 if (yx() < xy())
z = -
z;
167 angle = (cosa < -1.) ? std::acos(-1.) :
std::acos(cosa);
168 aaxis = Hep3Vector(x,y,
z);
172 bool HepRotation::isIdentity()
const {
173 return (rxx == 1.0 && rxy == 0.0 && rxz == 0.0 &&
174 ryx == 0.0 && ryy == 1.0 && ryz == 0.0 &&
175 rzx == 0.0 && rzy == 0.0 && rzz == 1.0) ?
true :
false;
178 int HepRotation::compare (
const HepRotation & r )
const {
179 if (rzz<r.rzz)
return -1;
else if (rzz>r.rzz)
return 1;
180 else if (rzy<r.rzy)
return -1;
else if (rzy>r.rzy)
return 1;
181 else if (rzx<r.rzx)
return -1;
else if (rzx>r.rzx)
return 1;
182 else if (ryz<r.ryz)
return -1;
else if (ryz>r.ryz)
return 1;
183 else if (ryy<r.ryy)
return -1;
else if (ryy>r.ryy)
return 1;
184 else if (ryx<r.ryx)
return -1;
else if (ryx>r.ryx)
return 1;
185 else if (rxz<r.rxz)
return -1;
else if (rxz>r.rxz)
return 1;
186 else if (rxy<r.rxy)
return -1;
else if (rxy>r.rxy)
return 1;
187 else if (rxx<r.rxx)
return -1;
else if (rxx>r.rxx)
return 1;
192 const HepRotation HepRotation::IDENTITY;
const G4double w[NPOINTSGL]
static G4double angle[DIM]
const G4double x[NPOINTSGL]
static double safe_acos(double x)