CLHEP VERSION Reference Documentation
   
CLHEP Home Page     CLHEP Documentation     CLHEP Bug Reports

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