|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
|
// @(#)root/mathcore:$Name: V02-02-29 $:$Id: Transform3DPJ.cc,v 1.2 2008/01/22 20:41:27 muzaffar Exp $
// Authors: W. Brown, M. Fischler, L. Moneta 2005
/**********************************************************************
* *
* Copyright (c) 2005 , LCG ROOT MathLib Team *
* *
* *
**********************************************************************/
// implementation file for class Transform3D
//
// Created by: Lorenzo Moneta October 27 2005
//
//
#include "FastSimulation/CaloGeometryTools/interface/Transform3DPJ.h"
#include <cmath>
#include <algorithm>
namespace ROOT {
namespace Math {
typedef Transform3DPJ::Point XYZPoint;
typedef Transform3DPJ::Vector XYZVector;
// ========== Constructors and Assignment =====================
// construct from two ref frames
Transform3DPJ::Transform3DPJ(const XYZPoint &fr0,
const XYZPoint &fr1,
const XYZPoint &fr2,
const XYZPoint &to0,
const XYZPoint &to1,
const XYZPoint &to2) {
// takes impl. from CLHEP ( E.Chernyaev). To be checked
XYZVector x1, y1, z1, x2, y2, z2;
x1 = (fr1 - fr0).Unit();
y1 = (fr2 - fr0).Unit();
x2 = (to1 - to0).Unit();
y2 = (to2 - to0).Unit();
// C H E C K A N G L E S
double cos1, cos2;
cos1 = x1.Dot(y1);
cos2 = x2.Dot(y2);
if (std::fabs(1.0 - cos1) <= 0.000001 || std::fabs(1.0 - cos2) <= 0.000001) {
std::cerr << "Transform3DPJ: Error : zero angle between axes" << std::endl;
SetIdentity();
} else {
if (std::fabs(cos1 - cos2) > 0.000001) {
std::cerr << "Transform3DPJ: Warning: angles between axes are not equal" << std::endl;
}
// F I N D R O T A T I O N M A T R I X
z1 = (x1.Cross(y1)).Unit();
y1 = z1.Cross(x1);
z2 = (x2.Cross(y2)).Unit();
y2 = z2.Cross(x2);
double x1x = x1.X(), x1y = x1.Y(), x1z = x1.Z();
double y1x = y1.X(), y1y = y1.Y(), y1z = y1.Z();
double z1x = z1.X(), z1y = z1.Y(), z1z = z1.Z();
double detxx = (y1y * z1z - z1y * y1z);
double detxy = -(y1x * z1z - z1x * y1z);
double detxz = (y1x * z1y - z1x * y1y);
double detyx = -(x1y * z1z - z1y * x1z);
double detyy = (x1x * z1z - z1x * x1z);
double detyz = -(x1x * z1y - z1x * x1y);
double detzx = (x1y * y1z - y1y * x1z);
double detzy = -(x1x * y1z - y1x * x1z);
double detzz = (x1x * y1y - y1x * x1y);
double x2x = x2.X(), x2y = x2.Y(), x2z = x2.Z();
double y2x = y2.X(), y2y = y2.Y(), y2z = y2.Z();
double z2x = z2.X(), z2y = z2.Y(), z2z = z2.Z();
double txx = x2x * detxx + y2x * detyx + z2x * detzx;
double txy = x2x * detxy + y2x * detyy + z2x * detzy;
double txz = x2x * detxz + y2x * detyz + z2x * detzz;
double tyx = x2y * detxx + y2y * detyx + z2y * detzx;
double tyy = x2y * detxy + y2y * detyy + z2y * detzy;
double tyz = x2y * detxz + y2y * detyz + z2y * detzz;
double tzx = x2z * detxx + y2z * detyx + z2z * detzx;
double tzy = x2z * detxy + y2z * detyy + z2z * detzy;
double tzz = x2z * detxz + y2z * detyz + z2z * detzz;
// S E T T R A N S F O R M A T I O N
double dx1 = fr0.X(), dy1 = fr0.Y(), dz1 = fr0.Z();
double dx2 = to0.X(), dy2 = to0.Y(), dz2 = to0.Z();
SetComponents(txx,
txy,
txz,
dx2 - txx * dx1 - txy * dy1 - txz * dz1,
tyx,
tyy,
tyz,
dy2 - tyx * dx1 - tyy * dy1 - tyz * dz1,
tzx,
tzy,
tzz,
dz2 - tzx * dx1 - tzy * dy1 - tzz * dz1);
}
}
// inversion (from CLHEP)
void Transform3DPJ::Invert() {
//
// Name: Transform3DPJ::inverse Date: 24.09.96
// Author: E.Chernyaev (IHEP/Protvino) Revised:
//
// Function: Find inverse affine transformation.
double detxx = fM[kYY] * fM[kZZ] - fM[kYZ] * fM[kZY];
double detxy = fM[kYX] * fM[kZZ] - fM[kYZ] * fM[kZX];
double detxz = fM[kYX] * fM[kZY] - fM[kYY] * fM[kZX];
double det = fM[kXX] * detxx - fM[kXY] * detxy + fM[kXZ] * detxz;
if (det == 0) {
std::cerr << "Transform3DPJ::inverse error: zero determinant" << std::endl;
return;
}
det = 1. / det;
detxx *= det;
detxy *= det;
detxz *= det;
double detyx = (fM[kXY] * fM[kZZ] - fM[kXZ] * fM[kZY]) * det;
double detyy = (fM[kXX] * fM[kZZ] - fM[kXZ] * fM[kZX]) * det;
double detyz = (fM[kXX] * fM[kZY] - fM[kXY] * fM[kZX]) * det;
double detzx = (fM[kXY] * fM[kYZ] - fM[kXZ] * fM[kYY]) * det;
double detzy = (fM[kXX] * fM[kYZ] - fM[kXZ] * fM[kYX]) * det;
double detzz = (fM[kXX] * fM[kYY] - fM[kXY] * fM[kYX]) * det;
SetComponents(detxx,
-detyx,
detzx,
-detxx * fM[kDX] + detyx * fM[kDY] - detzx * fM[kDZ],
-detxy,
detyy,
-detzy,
detxy * fM[kDX] - detyy * fM[kDY] + detzy * fM[kDZ],
detxz,
-detyz,
detzz,
-detxz * fM[kDX] + detyz * fM[kDY] - detzz * fM[kDZ]);
}
// get rotations and translations
void Transform3DPJ::GetDecomposition(Rotation3D &r, XYZVector &v) const {
// decompose a trasfomation in a 3D rotation and in a 3D vector (cartesian coordinates)
r.SetComponents(fM[kXX], fM[kXY], fM[kXZ], fM[kYX], fM[kYY], fM[kYZ], fM[kZX], fM[kZY], fM[kZZ]);
v.SetCoordinates(fM[kDX], fM[kDY], fM[kDZ]);
}
// transformation on Position Vector (rotation + translations)
XYZPoint Transform3DPJ::operator()(const XYZPoint &p) const {
// pass through rotation class (could be implemented directly to be faster)
Rotation3D r;
XYZVector t;
GetDecomposition(r, t);
XYZPoint pnew = r(p);
pnew += t;
return pnew;
}
// transformation on Displacement Vector (only rotation)
XYZVector Transform3DPJ::operator()(const XYZVector &v) const {
// pass through rotation class ( could be implemented directly to be faster)
Rotation3D r;
XYZVector t;
GetDecomposition(r, t);
// only rotation
return r(v);
}
Transform3DPJ &Transform3DPJ::operator*=(const Transform3DPJ &t) {
// combination of transformations
SetComponents(fM[kXX] * t.fM[kXX] + fM[kXY] * t.fM[kYX] + fM[kXZ] * t.fM[kZX],
fM[kXX] * t.fM[kXY] + fM[kXY] * t.fM[kYY] + fM[kXZ] * t.fM[kZY],
fM[kXX] * t.fM[kXZ] + fM[kXY] * t.fM[kYZ] + fM[kXZ] * t.fM[kZZ],
fM[kXX] * t.fM[kDX] + fM[kXY] * t.fM[kDY] + fM[kXZ] * t.fM[kDZ] + fM[kDX],
fM[kYX] * t.fM[kXX] + fM[kYY] * t.fM[kYX] + fM[kYZ] * t.fM[kZX],
fM[kYX] * t.fM[kXY] + fM[kYY] * t.fM[kYY] + fM[kYZ] * t.fM[kZY],
fM[kYX] * t.fM[kXZ] + fM[kYY] * t.fM[kYZ] + fM[kYZ] * t.fM[kZZ],
fM[kYX] * t.fM[kDX] + fM[kYY] * t.fM[kDY] + fM[kYZ] * t.fM[kDZ] + fM[kDY],
fM[kZX] * t.fM[kXX] + fM[kZY] * t.fM[kYX] + fM[kZZ] * t.fM[kZX],
fM[kZX] * t.fM[kXY] + fM[kZY] * t.fM[kYY] + fM[kZZ] * t.fM[kZY],
fM[kZX] * t.fM[kXZ] + fM[kZY] * t.fM[kYZ] + fM[kZZ] * t.fM[kZZ],
fM[kZX] * t.fM[kDX] + fM[kZY] * t.fM[kDY] + fM[kZZ] * t.fM[kDZ] + fM[kDZ]);
return *this;
}
void Transform3DPJ::SetIdentity() {
//set identity ( identity rotation and zero translation)
fM[kXX] = 1.0;
fM[kXY] = 0.0;
fM[kXZ] = 0.0;
fM[kDX] = 0.0;
fM[kYX] = 0.0;
fM[kYY] = 1.0;
fM[kYZ] = 0.0;
fM[kDY] = 0.0;
fM[kZX] = 0.0;
fM[kZY] = 0.0;
fM[kZZ] = 1.0;
fM[kDZ] = 0.0;
}
void Transform3DPJ::AssignFrom(const Rotation3D &r, const XYZVector &v) {
// assignment from rotation + translation
double rotData[9];
r.GetComponents(rotData, rotData + 9);
// first raw
for (int i = 0; i < 3; ++i)
fM[i] = rotData[i];
// second raw
for (int i = 0; i < 3; ++i)
fM[kYX + i] = rotData[3 + i];
// third raw
for (int i = 0; i < 3; ++i)
fM[kZX + i] = rotData[6 + i];
// translation data
double vecData[3];
v.GetCoordinates(vecData, vecData + 3);
fM[kDX] = vecData[0];
fM[kDY] = vecData[1];
fM[kDZ] = vecData[2];
}
void Transform3DPJ::AssignFrom(const Rotation3D &r) {
// assign from only a rotation (null translation)
double rotData[9];
r.GetComponents(rotData, rotData + 9);
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j)
fM[4 * i + j] = rotData[3 * i + j];
// empty vector data
fM[4 * i + 3] = 0;
}
}
void Transform3DPJ::AssignFrom(const XYZVector &v) {
// assign from a translation only (identity rotations)
fM[kXX] = 1.0;
fM[kXY] = 0.0;
fM[kXZ] = 0.0;
fM[kDX] = v.X();
fM[kYX] = 0.0;
fM[kYY] = 1.0;
fM[kYZ] = 0.0;
fM[kDY] = v.Y();
fM[kZX] = 0.0;
fM[kZY] = 0.0;
fM[kZZ] = 1.0;
fM[kDZ] = v.Z();
}
Plane3D Transform3DPJ::operator()(const Plane3D &plane) const {
// transformations on a 3D plane
XYZVector n = plane.Normal();
// take a point on the plane. Use origin projection on the plane
// ( -ad, -bd, -cd) if (a**2 + b**2 + c**2 ) = 1
double d = plane.HesseDistance();
XYZPoint p(-d * n.X(), -d * n.Y(), -d * n.Z());
return Plane3D(operator()(n), operator()(p));
}
std::ostream &operator<<(std::ostream &os, const Transform3DPJ &t) {
// TODO - this will need changing for machine-readable issues
// and even the human readable form needs formatiing improvements
double m[12];
t.GetComponents(m, m + 12);
os << "\n" << m[0] << " " << m[1] << " " << m[2] << " " << m[3];
os << "\n" << m[4] << " " << m[5] << " " << m[6] << " " << m[7];
os << "\n" << m[8] << " " << m[9] << " " << m[10] << " " << m[11] << "\n";
return os;
}
} // end namespace Math
} // end namespace ROOT
|