File indexing completed on 2023-03-17 11:00:39
0001
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014
0015
0016
0017 #include "FastSimulation/CaloGeometryTools/interface/Transform3DPJ.h"
0018
0019 #include <cmath>
0020 #include <algorithm>
0021
0022 namespace ROOT {
0023
0024 namespace Math {
0025
0026 typedef Transform3DPJ::Point XYZPoint;
0027 typedef Transform3DPJ::Vector XYZVector;
0028
0029
0030
0031
0032 Transform3DPJ::Transform3DPJ(const XYZPoint &fr0,
0033 const XYZPoint &fr1,
0034 const XYZPoint &fr2,
0035 const XYZPoint &to0,
0036 const XYZPoint &to1,
0037 const XYZPoint &to2) {
0038
0039
0040 XYZVector x1, y1, z1, x2, y2, z2;
0041 x1 = (fr1 - fr0).Unit();
0042 y1 = (fr2 - fr0).Unit();
0043 x2 = (to1 - to0).Unit();
0044 y2 = (to2 - to0).Unit();
0045
0046
0047
0048 double cos1, cos2;
0049 cos1 = x1.Dot(y1);
0050 cos2 = x2.Dot(y2);
0051
0052 if (std::fabs(1.0 - cos1) <= 0.000001 || std::fabs(1.0 - cos2) <= 0.000001) {
0053 std::cerr << "Transform3DPJ: Error : zero angle between axes" << std::endl;
0054 SetIdentity();
0055 } else {
0056 if (std::fabs(cos1 - cos2) > 0.000001) {
0057 std::cerr << "Transform3DPJ: Warning: angles between axes are not equal" << std::endl;
0058 }
0059
0060
0061
0062 z1 = (x1.Cross(y1)).Unit();
0063 y1 = z1.Cross(x1);
0064
0065 z2 = (x2.Cross(y2)).Unit();
0066 y2 = z2.Cross(x2);
0067
0068 double x1x = x1.X(), x1y = x1.Y(), x1z = x1.Z();
0069 double y1x = y1.X(), y1y = y1.Y(), y1z = y1.Z();
0070 double z1x = z1.X(), z1y = z1.Y(), z1z = z1.Z();
0071
0072 double detxx = (y1y * z1z - z1y * y1z);
0073 double detxy = -(y1x * z1z - z1x * y1z);
0074 double detxz = (y1x * z1y - z1x * y1y);
0075 double detyx = -(x1y * z1z - z1y * x1z);
0076 double detyy = (x1x * z1z - z1x * x1z);
0077 double detyz = -(x1x * z1y - z1x * x1y);
0078 double detzx = (x1y * y1z - y1y * x1z);
0079 double detzy = -(x1x * y1z - y1x * x1z);
0080 double detzz = (x1x * y1y - y1x * x1y);
0081
0082 double x2x = x2.X(), x2y = x2.Y(), x2z = x2.Z();
0083 double y2x = y2.X(), y2y = y2.Y(), y2z = y2.Z();
0084 double z2x = z2.X(), z2y = z2.Y(), z2z = z2.Z();
0085
0086 double txx = x2x * detxx + y2x * detyx + z2x * detzx;
0087 double txy = x2x * detxy + y2x * detyy + z2x * detzy;
0088 double txz = x2x * detxz + y2x * detyz + z2x * detzz;
0089 double tyx = x2y * detxx + y2y * detyx + z2y * detzx;
0090 double tyy = x2y * detxy + y2y * detyy + z2y * detzy;
0091 double tyz = x2y * detxz + y2y * detyz + z2y * detzz;
0092 double tzx = x2z * detxx + y2z * detyx + z2z * detzx;
0093 double tzy = x2z * detxy + y2z * detyy + z2z * detzy;
0094 double tzz = x2z * detxz + y2z * detyz + z2z * detzz;
0095
0096
0097
0098 double dx1 = fr0.X(), dy1 = fr0.Y(), dz1 = fr0.Z();
0099 double dx2 = to0.X(), dy2 = to0.Y(), dz2 = to0.Z();
0100
0101 SetComponents(txx,
0102 txy,
0103 txz,
0104 dx2 - txx * dx1 - txy * dy1 - txz * dz1,
0105 tyx,
0106 tyy,
0107 tyz,
0108 dy2 - tyx * dx1 - tyy * dy1 - tyz * dz1,
0109 tzx,
0110 tzy,
0111 tzz,
0112 dz2 - tzx * dx1 - tzy * dy1 - tzz * dz1);
0113 }
0114 }
0115
0116
0117 void Transform3DPJ::Invert() {
0118
0119
0120
0121
0122
0123
0124 double detxx = fM[kYY] * fM[kZZ] - fM[kYZ] * fM[kZY];
0125 double detxy = fM[kYX] * fM[kZZ] - fM[kYZ] * fM[kZX];
0126 double detxz = fM[kYX] * fM[kZY] - fM[kYY] * fM[kZX];
0127 double det = fM[kXX] * detxx - fM[kXY] * detxy + fM[kXZ] * detxz;
0128 if (det == 0) {
0129 std::cerr << "Transform3DPJ::inverse error: zero determinant" << std::endl;
0130 return;
0131 }
0132 det = 1. / det;
0133 detxx *= det;
0134 detxy *= det;
0135 detxz *= det;
0136 double detyx = (fM[kXY] * fM[kZZ] - fM[kXZ] * fM[kZY]) * det;
0137 double detyy = (fM[kXX] * fM[kZZ] - fM[kXZ] * fM[kZX]) * det;
0138 double detyz = (fM[kXX] * fM[kZY] - fM[kXY] * fM[kZX]) * det;
0139 double detzx = (fM[kXY] * fM[kYZ] - fM[kXZ] * fM[kYY]) * det;
0140 double detzy = (fM[kXX] * fM[kYZ] - fM[kXZ] * fM[kYX]) * det;
0141 double detzz = (fM[kXX] * fM[kYY] - fM[kXY] * fM[kYX]) * det;
0142 SetComponents(detxx,
0143 -detyx,
0144 detzx,
0145 -detxx * fM[kDX] + detyx * fM[kDY] - detzx * fM[kDZ],
0146 -detxy,
0147 detyy,
0148 -detzy,
0149 detxy * fM[kDX] - detyy * fM[kDY] + detzy * fM[kDZ],
0150 detxz,
0151 -detyz,
0152 detzz,
0153 -detxz * fM[kDX] + detyz * fM[kDY] - detzz * fM[kDZ]);
0154 }
0155
0156
0157 void Transform3DPJ::GetDecomposition(Rotation3D &r, XYZVector &v) const {
0158
0159 r.SetComponents(fM[kXX], fM[kXY], fM[kXZ], fM[kYX], fM[kYY], fM[kYZ], fM[kZX], fM[kZY], fM[kZZ]);
0160
0161 v.SetCoordinates(fM[kDX], fM[kDY], fM[kDZ]);
0162 }
0163
0164
0165 XYZPoint Transform3DPJ::operator()(const XYZPoint &p) const {
0166
0167
0168 Rotation3D r;
0169 XYZVector t;
0170 GetDecomposition(r, t);
0171 XYZPoint pnew = r(p);
0172 pnew += t;
0173 return pnew;
0174 }
0175
0176
0177 XYZVector Transform3DPJ::operator()(const XYZVector &v) const {
0178
0179
0180 Rotation3D r;
0181 XYZVector t;
0182 GetDecomposition(r, t);
0183
0184 return r(v);
0185 }
0186
0187 Transform3DPJ &Transform3DPJ::operator*=(const Transform3DPJ &t) {
0188
0189
0190 SetComponents(fM[kXX] * t.fM[kXX] + fM[kXY] * t.fM[kYX] + fM[kXZ] * t.fM[kZX],
0191 fM[kXX] * t.fM[kXY] + fM[kXY] * t.fM[kYY] + fM[kXZ] * t.fM[kZY],
0192 fM[kXX] * t.fM[kXZ] + fM[kXY] * t.fM[kYZ] + fM[kXZ] * t.fM[kZZ],
0193 fM[kXX] * t.fM[kDX] + fM[kXY] * t.fM[kDY] + fM[kXZ] * t.fM[kDZ] + fM[kDX],
0194
0195 fM[kYX] * t.fM[kXX] + fM[kYY] * t.fM[kYX] + fM[kYZ] * t.fM[kZX],
0196 fM[kYX] * t.fM[kXY] + fM[kYY] * t.fM[kYY] + fM[kYZ] * t.fM[kZY],
0197 fM[kYX] * t.fM[kXZ] + fM[kYY] * t.fM[kYZ] + fM[kYZ] * t.fM[kZZ],
0198 fM[kYX] * t.fM[kDX] + fM[kYY] * t.fM[kDY] + fM[kYZ] * t.fM[kDZ] + fM[kDY],
0199
0200 fM[kZX] * t.fM[kXX] + fM[kZY] * t.fM[kYX] + fM[kZZ] * t.fM[kZX],
0201 fM[kZX] * t.fM[kXY] + fM[kZY] * t.fM[kYY] + fM[kZZ] * t.fM[kZY],
0202 fM[kZX] * t.fM[kXZ] + fM[kZY] * t.fM[kYZ] + fM[kZZ] * t.fM[kZZ],
0203 fM[kZX] * t.fM[kDX] + fM[kZY] * t.fM[kDY] + fM[kZZ] * t.fM[kDZ] + fM[kDZ]);
0204
0205 return *this;
0206 }
0207
0208 void Transform3DPJ::SetIdentity() {
0209
0210 fM[kXX] = 1.0;
0211 fM[kXY] = 0.0;
0212 fM[kXZ] = 0.0;
0213 fM[kDX] = 0.0;
0214 fM[kYX] = 0.0;
0215 fM[kYY] = 1.0;
0216 fM[kYZ] = 0.0;
0217 fM[kDY] = 0.0;
0218 fM[kZX] = 0.0;
0219 fM[kZY] = 0.0;
0220 fM[kZZ] = 1.0;
0221 fM[kDZ] = 0.0;
0222 }
0223
0224 void Transform3DPJ::AssignFrom(const Rotation3D &r, const XYZVector &v) {
0225
0226
0227 double rotData[9];
0228 r.GetComponents(rotData, rotData + 9);
0229
0230 for (int i = 0; i < 3; ++i)
0231 fM[i] = rotData[i];
0232
0233 for (int i = 0; i < 3; ++i)
0234 fM[kYX + i] = rotData[3 + i];
0235
0236 for (int i = 0; i < 3; ++i)
0237 fM[kZX + i] = rotData[6 + i];
0238
0239
0240 double vecData[3];
0241 v.GetCoordinates(vecData, vecData + 3);
0242 fM[kDX] = vecData[0];
0243 fM[kDY] = vecData[1];
0244 fM[kDZ] = vecData[2];
0245 }
0246
0247 void Transform3DPJ::AssignFrom(const Rotation3D &r) {
0248
0249 double rotData[9];
0250 r.GetComponents(rotData, rotData + 9);
0251 for (int i = 0; i < 3; ++i) {
0252 for (int j = 0; j < 3; ++j)
0253 fM[4 * i + j] = rotData[3 * i + j];
0254
0255 fM[4 * i + 3] = 0;
0256 }
0257 }
0258
0259 void Transform3DPJ::AssignFrom(const XYZVector &v) {
0260
0261 fM[kXX] = 1.0;
0262 fM[kXY] = 0.0;
0263 fM[kXZ] = 0.0;
0264 fM[kDX] = v.X();
0265 fM[kYX] = 0.0;
0266 fM[kYY] = 1.0;
0267 fM[kYZ] = 0.0;
0268 fM[kDY] = v.Y();
0269 fM[kZX] = 0.0;
0270 fM[kZY] = 0.0;
0271 fM[kZZ] = 1.0;
0272 fM[kDZ] = v.Z();
0273 }
0274
0275 Plane3D Transform3DPJ::operator()(const Plane3D &plane) const {
0276
0277 XYZVector n = plane.Normal();
0278
0279
0280 double d = plane.HesseDistance();
0281 XYZPoint p(-d * n.X(), -d * n.Y(), -d * n.Z());
0282 return Plane3D(operator()(n), operator()(p));
0283 }
0284
0285 std::ostream &operator<<(std::ostream &os, const Transform3DPJ &t) {
0286
0287
0288
0289 double m[12];
0290 t.GetComponents(m, m + 12);
0291 os << "\n" << m[0] << " " << m[1] << " " << m[2] << " " << m[3];
0292 os << "\n" << m[4] << " " << m[5] << " " << m[6] << " " << m[7];
0293 os << "\n" << m[8] << " " << m[9] << " " << m[10] << " " << m[11] << "\n";
0294 return os;
0295 }
0296
0297 }
0298 }