Geant4 9.6.0
Toolkit for the simulation of the passage of particles through matter
•All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros
Transform3D.cc
Go to the documentation of this file.
1// -*- C++ -*-
2// $Id:$
3// ---------------------------------------------------------------------------
4//
5// This file is a part of the CLHEP - a Class Library for High Energy Physics.
6//
7// Hep geometrical 3D Transformation library
8//
9// Author: Evgeni Chernyaev <Evgueni.Tcherniaev@cern.ch>
10//
11// History:
12// 24.09.96 E.Chernyaev - initial version
13
14#include <iostream>
15#include <cmath> // double std::abs()
16#include "CLHEP/Geometry/Transform3D.h"
17
18namespace HepGeom {
19
20 const Transform3D Transform3D::Identity = Transform3D ();
21
22 // T R A N S F O R M A T I O N -------------------------------------------
23
24 double Transform3D::operator () (int i, int j) const {
25 if (i == 0) {
26 if (j == 0) { return xx_; }
27 if (j == 1) { return xy_; }
28 if (j == 2) { return xz_; }
29 if (j == 3) { return dx_; }
30 } else if (i == 1) {
31 if (j == 0) { return yx_; }
32 if (j == 1) { return yy_; }
33 if (j == 2) { return yz_; }
34 if (j == 3) { return dy_; }
35 } else if (i == 2) {
36 if (j == 0) { return zx_; }
37 if (j == 1) { return zy_; }
38 if (j == 2) { return zz_; }
39 if (j == 3) { return dz_; }
40 } else if (i == 3) {
41 if (j == 0) { return 0.0; }
42 if (j == 1) { return 0.0; }
43 if (j == 2) { return 0.0; }
44 if (j == 3) { return 1.0; }
45 }
46 std::cerr << "Transform3D subscripting: bad indeces "
47 << "(" << i << "," << j << ")" << std::endl;
48 return 0.0;
49 }
50
51 //--------------------------------------------------------------------------
52 Transform3D Transform3D::operator*(const Transform3D & b) const {
53 return Transform3D
54 (xx_*b.xx_+xy_*b.yx_+xz_*b.zx_, xx_*b.xy_+xy_*b.yy_+xz_*b.zy_,
55 xx_*b.xz_+xy_*b.yz_+xz_*b.zz_, xx_*b.dx_+xy_*b.dy_+xz_*b.dz_+dx_,
56 yx_*b.xx_+yy_*b.yx_+yz_*b.zx_, yx_*b.xy_+yy_*b.yy_+yz_*b.zy_,
57 yx_*b.xz_+yy_*b.yz_+yz_*b.zz_, yx_*b.dx_+yy_*b.dy_+yz_*b.dz_+dy_,
58 zx_*b.xx_+zy_*b.yx_+zz_*b.zx_, zx_*b.xy_+zy_*b.yy_+zz_*b.zy_,
59 zx_*b.xz_+zy_*b.yz_+zz_*b.zz_, zx_*b.dx_+zy_*b.dy_+zz_*b.dz_+dz_);
60 }
61
62 // -------------------------------------------------------------------------
63 Transform3D::Transform3D(const Point3D<double> & fr0,
64 const Point3D<double> & fr1,
65 const Point3D<double> & fr2,
66 const Point3D<double> & to0,
67 const Point3D<double> & to1,
68 const Point3D<double> & to2)
69 /***********************************************************************
70 * *
71 * Name: Transform3D::Transform3D Date: 24.09.96 *
72 * Author: E.Chernyaev (IHEP/Protvino) Revised: *
73 * *
74 * Function: Create 3D Transformation from one coordinate system *
75 * defined by its origin "fr0" and two axes "fr0->fr1", *
76 * "fr0->fr2" to another coordinate system "to0", "to0->to1" *
77 * and "to0->to2" *
78 * *
79 ***********************************************************************/
80 {
81 Vector3D<double> x1,y1,z1, x2,y2,z2;
82 x1 = (fr1 - fr0).unit();
83 y1 = (fr2 - fr0).unit();
84 x2 = (to1 - to0).unit();
85 y2 = (to2 - to0).unit();
86
87 // C H E C K A N G L E S
88
89 double cos1, cos2;
90 cos1 = x1*y1;
91 cos2 = x2*y2;
92
93 if (std::abs(1.0-cos1) <= 0.000001 || std::abs(1.0-cos2) <= 0.000001) {
94 std::cerr << "Transform3D: zero angle between axes" << std::endl;
95 setIdentity();
96 }else{
97 if (std::abs(cos1-cos2) > 0.000001) {
98 std::cerr << "Transform3D: angles between axes are not equal"
99 << std::endl;
100 }
101
102 // F I N D R O T A T I O N M A T R I X
103
104 z1 = (x1.cross(y1)).unit();
105 y1 = z1.cross(x1);
106
107 z2 = (x2.cross(y2)).unit();
108 y2 = z2.cross(x2);
109
110 double detxx = (y1.y()*z1.z() - z1.y()*y1.z());
111 double detxy = -(y1.x()*z1.z() - z1.x()*y1.z());
112 double detxz = (y1.x()*z1.y() - z1.x()*y1.y());
113 double detyx = -(x1.y()*z1.z() - z1.y()*x1.z());
114 double detyy = (x1.x()*z1.z() - z1.x()*x1.z());
115 double detyz = -(x1.x()*z1.y() - z1.x()*x1.y());
116 double detzx = (x1.y()*y1.z() - y1.y()*x1.z());
117 double detzy = -(x1.x()*y1.z() - y1.x()*x1.z());
118 double detzz = (x1.x()*y1.y() - y1.x()*x1.y());
119
120 double txx = x2.x()*detxx + y2.x()*detyx + z2.x()*detzx;
121 double txy = x2.x()*detxy + y2.x()*detyy + z2.x()*detzy;
122 double txz = x2.x()*detxz + y2.x()*detyz + z2.x()*detzz;
123 double tyx = x2.y()*detxx + y2.y()*detyx + z2.y()*detzx;
124 double tyy = x2.y()*detxy + y2.y()*detyy + z2.y()*detzy;
125 double tyz = x2.y()*detxz + y2.y()*detyz + z2.y()*detzz;
126 double tzx = x2.z()*detxx + y2.z()*detyx + z2.z()*detzx;
127 double tzy = x2.z()*detxy + y2.z()*detyy + z2.z()*detzy;
128 double tzz = x2.z()*detxz + y2.z()*detyz + z2.z()*detzz;
129
130 // S E T T R A N S F O R M A T I O N
131
132 double dx1 = fr0.x(), dy1 = fr0.y(), dz1 = fr0.z();
133 double dx2 = to0.x(), dy2 = to0.y(), dz2 = to0.z();
134
135 setTransform(txx, txy, txz, dx2-txx*dx1-txy*dy1-txz*dz1,
136 tyx, tyy, tyz, dy2-tyx*dx1-tyy*dy1-tyz*dz1,
137 tzx, tzy, tzz, dz2-tzx*dx1-tzy*dy1-tzz*dz1);
138 }
139 }
140
141 // -------------------------------------------------------------------------
142 Transform3D Transform3D::inverse() const
143 /***********************************************************************
144 * *
145 * Name: Transform3D::inverse Date: 24.09.96 *
146 * Author: E.Chernyaev (IHEP/Protvino) Revised: *
147 * *
148 * Function: Find inverse affine transformation *
149 * *
150 ***********************************************************************/
151 {
152 double detxx = yy_*zz_-yz_*zy_;
153 double detxy = yx_*zz_-yz_*zx_;
154 double detxz = yx_*zy_-yy_*zx_;
155 double det = xx_*detxx - xy_*detxy + xz_*detxz;
156 if (det == 0) {
157 std::cerr << "Transform3D::inverse error: zero determinant" << std::endl;
158 return Transform3D();
159 }
160 det = 1./det; detxx *= det; detxy *= det; detxz *= det;
161 double detyx = (xy_*zz_ - xz_*zy_)*det;
162 double detyy = (xx_*zz_ - xz_*zx_)*det;
163 double detyz = (xx_*zy_ - xy_*zx_)*det;
164 double detzx = (xy_*yz_ - xz_*yy_)*det;
165 double detzy = (xx_*yz_ - xz_*yx_)*det;
166 double detzz = (xx_*yy_ - xy_*yx_)*det;
167 return Transform3D
168 (detxx, -detyx, detzx, -detxx*dx_+detyx*dy_-detzx*dz_,
169 -detxy, detyy, -detzy, detxy*dx_-detyy*dy_+detzy*dz_,
170 detxz, -detyz, detzz, -detxz*dx_+detyz*dy_-detzz*dz_);
171 }
172
173 // -------------------------------------------------------------------------
174 void Transform3D::getDecomposition(Scale3D & scale,
175 Rotate3D & rotation,
176 Translate3D & translation) const
177 /***********************************************************************
178 * CLHEP-1.7 *
179 * Name: Transform3D::getDecomposition Date: 09.06.01 *
180 * Author: E.Chernyaev (IHEP/Protvino) Revised: *
181 * *
182 * Function: Gets decomposition of general transformation on *
183 * three consequentive specific transformations: *
184 * Scale, then Rotation, then Translation. *
185 * If there was a reflection, then ScaleZ will be negative. *
186 * *
187 ***********************************************************************/
188 {
189 double sx = std::sqrt(xx_*xx_ + yx_*yx_ + zx_*zx_);
190 double sy = std::sqrt(xy_*xy_ + yy_*yy_ + zy_*zy_);
191 double sz = std::sqrt(xz_*xz_ + yz_*yz_ + zz_*zz_);
192
193 if (xx_*(yy_*zz_-yz_*zy_) -
194 xy_*(yx_*zz_-yz_*zx_) +
195 xz_*(yx_*zy_-yy_*zx_) < 0) sz = -sz;
196 scale.setTransform(sx,0,0,0, 0,sy,0,0, 0,0,sz,0);
197 rotation.setTransform(xx_/sx,xy_/sy,xz_/sz,0,
198 yx_/sx,yy_/sy,yz_/sz,0,
199 zx_/sx,zy_/sy,zz_/sz,0);
200 translation.setTransform(1,0,0,dx_, 0,1,0,dy_, 0,0,1,dz_);
201 }
202
203 // -------------------------------------------------------------------------
204 bool Transform3D::isNear(const Transform3D & t, double tolerance) const
205 {
206 return ( (std::abs(xx_ - t.xx_) <= tolerance) &&
207 (std::abs(xy_ - t.xy_) <= tolerance) &&
208 (std::abs(xz_ - t.xz_) <= tolerance) &&
209 (std::abs(dx_ - t.dx_) <= tolerance) &&
210 (std::abs(yx_ - t.yx_) <= tolerance) &&
211 (std::abs(yy_ - t.yy_) <= tolerance) &&
212 (std::abs(yz_ - t.yz_) <= tolerance) &&
213 (std::abs(dy_ - t.dy_) <= tolerance) &&
214 (std::abs(zx_ - t.zx_) <= tolerance) &&
215 (std::abs(zy_ - t.zy_) <= tolerance) &&
216 (std::abs(zz_ - t.zz_) <= tolerance) &&
217 (std::abs(dz_ - t.dz_) <= tolerance) );
218 }
219
220 // -------------------------------------------------------------------------
221 bool Transform3D::operator==(const Transform3D & t) const
222 {
223 return (this == &t) ? true :
224 (xx_==t.xx_ && xy_==t.xy_ && xz_==t.xz_ && dx_==t.dx_ &&
225 yx_==t.yx_ && yy_==t.yy_ && yz_==t.yz_ && dy_==t.dy_ &&
226 zx_==t.zx_ && zy_==t.zy_ && zz_==t.zz_ && dz_==t.dz_ );
227 }
228
229 // 3 D R O T A T I O N -------------------------------------------------
230
231 Rotate3D::Rotate3D(double a,
232 const Point3D<double> & p1,
233 const Point3D<double> & p2) : Transform3D()
234 /***********************************************************************
235 * *
236 * Name: Rotate3D::Rotate3D Date: 24.09.96 *
237 * Author: E.Chernyaev (IHEP/Protvino) Revised: *
238 * *
239 * Function: Create 3D Rotation through angle "a" (counterclockwise) *
240 * around the axis p1->p2 *
241 * *
242 ***********************************************************************/
243 {
244 if (a == 0) return;
245
246 double cx = p2.x()-p1.x(), cy = p2.y()-p1.y(), cz = p2.z()-p1.z();
247 double ll = std::sqrt(cx*cx + cy*cy + cz*cz);
248 if (ll == 0) {
249 std::cerr << "Rotate3D: zero axis" << std::endl;
250 }else{
251 double cosa = std::cos(a), sina = std::sin(a);
252 cx /= ll; cy /= ll; cz /= ll;
253
254 double txx = cosa + (1-cosa)*cx*cx;
255 double txy = (1-cosa)*cx*cy - sina*cz;
256 double txz = (1-cosa)*cx*cz + sina*cy;
257
258 double tyx = (1-cosa)*cy*cx + sina*cz;
259 double tyy = cosa + (1-cosa)*cy*cy;
260 double tyz = (1-cosa)*cy*cz - sina*cx;
261
262 double tzx = (1-cosa)*cz*cx - sina*cy;
263 double tzy = (1-cosa)*cz*cy + sina*cx;
264 double tzz = cosa + (1-cosa)*cz*cz;
265
266 double tdx = p1.x(), tdy = p1.y(), tdz = p1.z();
267
268 setTransform(txx, txy, txz, tdx-txx*tdx-txy*tdy-txz*tdz,
269 tyx, tyy, tyz, tdy-tyx*tdx-tyy*tdy-tyz*tdz,
270 tzx, tzy, tzz, tdz-tzx*tdx-tzy*tdy-tzz*tdz);
271 }
272 }
273
274 // 3 D R E F L E C T I O N ---------------------------------------------
275
276 Reflect3D::Reflect3D(double a, double b, double c, double d)
277 /***********************************************************************
278 * *
279 * Name: Reflect3D::Reflect3D Date: 24.09.96 *
280 * Author: E.Chernyaev (IHEP/Protvino) Revised: *
281 * *
282 * Function: Create 3D Reflection in a plane a*x + b*y + c*z + d = 0 *
283 * *
284 ***********************************************************************/
285 {
286 double ll = a*a+b*b+c*c;
287 if (ll == 0) {
288 std::cerr << "Reflect3D: zero normal" << std::endl;
289 setIdentity();
290 }else{
291 ll = 1/ll;
292 double aa = a*a*ll, ab = a*b*ll, ac = a*c*ll, ad = a*d*ll,
293 bb = b*b*ll, bc = b*c*ll, bd = b*d*ll,
294 cc = c*c*ll, cd = c*d*ll;
295 setTransform(-aa+bb+cc, -ab-ab, -ac-ac, -ad-ad,
296 -ab-ab, aa-bb+cc, -bc-bc, -bd-bd,
297 -ac-ac, -bc-bc, aa+bb-cc, -cd-cd);
298 }
299 }
300} /* namespace HepGeom */
HepGeom::BasicVector3D::cross
BasicVector3D< T > cross(const BasicVector3D< T > &v) const
Definition: BasicVector3D.h:275
HepGeom::Transform3D::operator()
double operator()(int, int) const
Definition: Transform3D.cc:24
HepGeom::Transform3D::Identity
static const Transform3D Identity
Definition: Transform3D.h:197
HepGeom::Transform3D::isNear
bool isNear(const Transform3D &t, double tolerance=2.2E-14) const
Definition: Transform3D.cc:204
HepGeom::Transform3D::operator*
Transform3D operator*(const Transform3D &b) const
Definition: Transform3D.cc:52
HepGeom::Transform3D::operator==
bool operator==(const Transform3D &transform) const
Definition: Transform3D.cc:221
HepGeom::Transform3D::getDecomposition
void getDecomposition(Scale3D &scale, Rotate3D &rotation, Translate3D &translation) const
Definition: Transform3D.cc:174
HepGeom::Transform3D::setTransform
void setTransform(double XX, double XY, double XZ, double DX, double YX, double YY, double YZ, double DY, double ZX, double ZY, double ZZ, double DZ)
Definition: Transform3D.h:186
HepGeom::Transform3D::inverse
Transform3D inverse() const
Definition: Transform3D.cc:142
const
#define const
Definition: zconf.h:118