CaloGeometryTools/src/Transform3DPJ.cc

0001 // @(#)root/mathcore:$Name: V02-02-29 $:$Id: Transform3DPJ.cc,v 1.2 2008/01/22 20:41:27 muzaffar Exp $
0002 // Authors: W. Brown, M. Fischler, L. Moneta    2005
0003
0004 /**********************************************************************
0005  *                                                                    *
0006  * Copyright (c) 2005 , LCG ROOT MathLib Team                         *
0007  *                                                                    *
0008  *                                                                    *
0009  **********************************************************************/
0010
0011 // implementation file for class Transform3D
0012 //
0013 // Created by: Lorenzo Moneta  October 27 2005
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     // ========== Constructors and Assignment =====================
0030
0031     // construct from two ref frames
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       // takes impl. from CLHEP ( E.Chernyaev). To be checked
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       //   C H E C K   A N G L E S
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         //   F I N D   R O T A T I O N   M A T R I X
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         //   S E T    T R A N S F O R M A T I O N
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     // inversion (from CLHEP)
0117     void Transform3DPJ::Invert() {
0118       //
0119       // Name: Transform3DPJ::inverse                     Date:    24.09.96
0120       // Author: E.Chernyaev (IHEP/Protvino)            Revised:
0121       //
0122       // Function: Find inverse affine transformation.
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     // get rotations and translations
0157     void Transform3DPJ::GetDecomposition(Rotation3D &r, XYZVector &v) const {
0158       // decompose a trasfomation in a 3D rotation and in a 3D vector (cartesian coordinates)
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     // transformation on Position Vector (rotation + translations)
0165     XYZPoint Transform3DPJ::operator()(const XYZPoint &p) const {
0166       // pass through rotation class (could be implemented directly to be faster)
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     // transformation on Displacement Vector (only rotation)
0177     XYZVector Transform3DPJ::operator()(const XYZVector &v) const {
0178       // pass through rotation class ( could be implemented directly to be faster)
0179
0180       Rotation3D r;
0181       XYZVector t;
0182       GetDecomposition(r, t);
0183       // only rotation
0184       return r(v);
0185     }
0186
0187     Transform3DPJ &Transform3DPJ::operator*=(const Transform3DPJ &t) {
0188       // combination of transformations
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       //set identity ( identity rotation and zero translation)
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       // assignment  from rotation + translation
0226
0227       double rotData[9];
0228       r.GetComponents(rotData, rotData + 9);
0229       // first raw
0230       for (int i = 0; i < 3; ++i)
0231         fM[i] = rotData[i];
0232       // second raw
0233       for (int i = 0; i < 3; ++i)
0234         fM[kYX + i] = rotData[3 + i];
0235       // third raw
0236       for (int i = 0; i < 3; ++i)
0237         fM[kZX + i] = rotData[6 + i];
0238
0239       // translation data
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       // assign from only a rotation  (null translation)
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         // empty vector data
0255         fM[4 * i + 3] = 0;
0256       }
0257     }
0258
0259     void Transform3DPJ::AssignFrom(const XYZVector &v) {
0260       // assign from a translation only (identity rotations)
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       // transformations on a 3D plane
0277       XYZVector n = plane.Normal();
0278       // take a point on the plane. Use origin projection on the plane
0279       // ( -ad, -bd, -cd) if (a**2 + b**2 + c**2 ) = 1
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       // TODO - this will need changing for machine-readable issues
0287       //        and even the human readable form needs formatiing improvements
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   }  // end namespace Math
0298 }  // end namespace ROOT