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