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