Geant4  9.6.p02
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
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 
18 namespace 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 */
Generated on Sat May 25 2013 14:33:07 for Geant4 by   doxygen 1.8.4