Reference Manual

Go to the documentation of this file.
00001 // @(#)root/mathcore:$Name: CMSSW_6_2_0 $:$Id: Transform3DPJ.cc,v 1.3 2012/11/12 21:20:42 dlange Exp $
00002 // Authors: W. Brown, M. Fischler, L. Moneta    2005
00003 
00004 /**********************************************************************
00005  *                                                                    *
00006  * Copyright (c) 2005 , LCG ROOT MathLib Team                         *
00007  *                                                                    *
00008  *                                                                    *
00009  **********************************************************************/
00010 
00011 // implementation file for class Transform3D
00012 //
00013 // Created by: Lorenzo Moneta  October 27 2005
00014 //
00015 //
00016 
00017 #include "FastSimulation/CaloGeometryTools/interface/Transform3DPJ.h"
00018 #include "Math/GenVector/Plane3D.h"
00019 
00020 #include <cmath>
00021 #include <algorithm>
00022 
00023 
00024 
00025 
00026 namespace ROOT {
00027 
00028 namespace Math {
00029 
00030 
00031 typedef Transform3DPJ::Point  XYZPoint; 
00032 typedef Transform3DPJ::Vector XYZVector; 
00033 
00034 
00035 // ========== Constructors and Assignment =====================
00036 
00037 
00038 
00039 // construct from two ref frames
00040 Transform3DPJ::Transform3DPJ(const XYZPoint & fr0, const XYZPoint & fr1, const XYZPoint & fr2,
00041                          const XYZPoint & to0, const XYZPoint & to1, const XYZPoint & to2 )
00042 {
00043    // takes impl. from CLHEP ( E.Chernyaev). To be checked
00044    
00045    XYZVector x1,y1,z1, x2,y2,z2;
00046    x1 = (fr1 - fr0).Unit();
00047    y1 = (fr2 - fr0).Unit();
00048    x2 = (to1 - to0).Unit();
00049    y2 = (to2 - to0).Unit();
00050    
00051    //   C H E C K   A N G L E S
00052    
00053    double cos1, cos2;
00054    cos1 = x1.Dot(y1);
00055    cos2 = x2.Dot(y2);
00056    
00057    if (std::fabs(1.0-cos1) <= 0.000001 || std::fabs(1.0-cos2) <= 0.000001) {
00058       std::cerr << "Transform3DPJ: Error : zero angle between axes" << std::endl;
00059       SetIdentity();
00060    } else {
00061       if (std::fabs(cos1-cos2) > 0.000001) {
00062          std::cerr << "Transform3DPJ: Warning: angles between axes are not equal"
00063          << std::endl;
00064       }
00065       
00066       //   F I N D   R O T A T I O N   M A T R I X
00067       
00068       z1 = (x1.Cross(y1)).Unit();
00069       y1  = z1.Cross(x1);
00070       
00071       z2 = (x2.Cross(y2)).Unit();
00072       y2  = z2.Cross(x2);
00073 
00074       double x1x = x1.X(), x1y = x1.Y(), x1z = x1.Z();
00075       double y1x = y1.X(), y1y = y1.Y(), y1z = y1.Z();
00076       double z1x = z1.X(), z1y = z1.Y(), z1z = z1.Z();
00077       
00078       double detxx =  (y1y*z1z - z1y*y1z);
00079       double detxy = -(y1x*z1z - z1x*y1z);
00080       double detxz =  (y1x*z1y - z1x*y1y);
00081       double detyx = -(x1y*z1z - z1y*x1z);
00082       double detyy =  (x1x*z1z - z1x*x1z);
00083       double detyz = -(x1x*z1y - z1x*x1y);
00084       double detzx =  (x1y*y1z - y1y*x1z);
00085       double detzy = -(x1x*y1z - y1x*x1z);
00086       double detzz =  (x1x*y1y - y1x*x1y);
00087 
00088       double x2x = x2.X(), x2y = x2.Y(), x2z = x2.Z();
00089       double y2x = y2.X(), y2y = y2.Y(), y2z = y2.Z();
00090       double z2x = z2.X(), z2y = z2.Y(), z2z = z2.Z();
00091 
00092       double txx = x2x*detxx + y2x*detyx + z2x*detzx;
00093       double txy = x2x*detxy + y2x*detyy + z2x*detzy;
00094       double txz = x2x*detxz + y2x*detyz + z2x*detzz;
00095       double tyx = x2y*detxx + y2y*detyx + z2y*detzx;
00096       double tyy = x2y*detxy + y2y*detyy + z2y*detzy;
00097       double tyz = x2y*detxz + y2y*detyz + z2y*detzz;
00098       double tzx = x2z*detxx + y2z*detyx + z2z*detzx;
00099       double tzy = x2z*detxy + y2z*detyy + z2z*detzy;
00100       double tzz = x2z*detxz + y2z*detyz + z2z*detzz;
00101       
00102       //   S E T    T R A N S F O R M A T I O N
00103       
00104       double dx1 = fr0.X(), dy1 = fr0.Y(), dz1 = fr0.Z();
00105       double dx2 = to0.X(), dy2 = to0.Y(), dz2 = to0.Z();
00106       
00107       SetComponents(txx, txy, txz, dx2-txx*dx1-txy*dy1-txz*dz1,
00108                     tyx, tyy, tyz, dy2-tyx*dx1-tyy*dy1-tyz*dz1,
00109                     tzx, tzy, tzz, dz2-tzx*dx1-tzy*dy1-tzz*dz1);
00110    }
00111 }
00112 
00113 
00114 // inversion (from CLHEP)
00115 void Transform3DPJ::Invert()
00116 {
00117    //
00118    // Name: Transform3DPJ::inverse                     Date:    24.09.96
00119    // Author: E.Chernyaev (IHEP/Protvino)            Revised:
00120    //
00121    // Function: Find inverse affine transformation.
00122    
00123    double detxx = fM[kYY]*fM[kZZ] - fM[kYZ]*fM[kZY];
00124    double detxy = fM[kYX]*fM[kZZ] - fM[kYZ]*fM[kZX];
00125    double detxz = fM[kYX]*fM[kZY] - fM[kYY]*fM[kZX];
00126    double det   = fM[kXX]*detxx - fM[kXY]*detxy + fM[kXZ]*detxz;
00127    if (det == 0) {
00128       std::cerr << "Transform3DPJ::inverse error: zero determinant" << std::endl;
00129       return;
00130    }
00131    det = 1./det; detxx *= det; detxy *= det; detxz *= det;
00132    double detyx = (fM[kXY]*fM[kZZ] - fM[kXZ]*fM[kZY] )*det;
00133    double detyy = (fM[kXX]*fM[kZZ] - fM[kXZ]*fM[kZX] )*det;
00134    double detyz = (fM[kXX]*fM[kZY] - fM[kXY]*fM[kZX] )*det;
00135    double detzx = (fM[kXY]*fM[kYZ] - fM[kXZ]*fM[kYY] )*det;
00136    double detzy = (fM[kXX]*fM[kYZ] - fM[kXZ]*fM[kYX] )*det;
00137    double detzz = (fM[kXX]*fM[kYY] - fM[kXY]*fM[kYX] )*det;
00138    SetComponents
00139       (detxx, -detyx,  detzx, -detxx*fM[kDX]+detyx*fM[kDY]-detzx*fM[kDZ],
00140        -detxy,  detyy, -detzy,  detxy*fM[kDX]-detyy*fM[kDY]+detzy*fM[kDZ],
00141        detxz, -detyz,  detzz, -detxz*fM[kDX]+detyz*fM[kDY]-detzz*fM[kDZ]);
00142 }
00143 
00144 
00145 // get rotations and translations
00146 void Transform3DPJ::GetDecomposition ( Rotation3D &r, XYZVector &v) const
00147 {
00148    // decompose a trasfomation in a 3D rotation and in a 3D vector (cartesian coordinates) 
00149    r.SetComponents( fM[kXX], fM[kXY], fM[kXZ],
00150                     fM[kYX], fM[kYY], fM[kYZ],
00151                     fM[kZX], fM[kZY], fM[kZZ] );
00152    
00153    v.SetCoordinates( fM[kDX], fM[kDY], fM[kDZ] );
00154 }
00155 
00156 // transformation on Position Vector (rotation + translations)
00157 XYZPoint Transform3DPJ::operator() (const XYZPoint & p) const
00158 {
00159    // pass through rotation class (could be implemented directly to be faster)
00160    
00161    Rotation3D r;
00162    XYZVector  t;
00163    GetDecomposition(r, t);
00164    XYZPoint pnew = r(p);
00165    pnew += t;
00166    return pnew;
00167 }
00168 
00169 // transformation on Displacement Vector (only rotation)
00170 XYZVector Transform3DPJ::operator() (const XYZVector & v) const
00171 {
00172    // pass through rotation class ( could be implemented directly to be faster)
00173    
00174    Rotation3D r;
00175    XYZVector  t;
00176    GetDecomposition(r, t);
00177    // only rotation
00178    return r(v);
00179 }
00180 
00181 Transform3DPJ & Transform3DPJ::operator *= (const Transform3DPJ  & t)
00182 {
00183    // combination of transformations
00184    
00185    SetComponents(fM[kXX]*t.fM[kXX]+fM[kXY]*t.fM[kYX]+fM[kXZ]*t.fM[kZX],
00186                  fM[kXX]*t.fM[kXY]+fM[kXY]*t.fM[kYY]+fM[kXZ]*t.fM[kZY],
00187                  fM[kXX]*t.fM[kXZ]+fM[kXY]*t.fM[kYZ]+fM[kXZ]*t.fM[kZZ],
00188                  fM[kXX]*t.fM[kDX]+fM[kXY]*t.fM[kDY]+fM[kXZ]*t.fM[kDZ]+fM[kDX],
00189                  
00190                  fM[kYX]*t.fM[kXX]+fM[kYY]*t.fM[kYX]+fM[kYZ]*t.fM[kZX],
00191                  fM[kYX]*t.fM[kXY]+fM[kYY]*t.fM[kYY]+fM[kYZ]*t.fM[kZY],
00192                  fM[kYX]*t.fM[kXZ]+fM[kYY]*t.fM[kYZ]+fM[kYZ]*t.fM[kZZ],
00193                  fM[kYX]*t.fM[kDX]+fM[kYY]*t.fM[kDY]+fM[kYZ]*t.fM[kDZ]+fM[kDY],
00194                  
00195                  fM[kZX]*t.fM[kXX]+fM[kZY]*t.fM[kYX]+fM[kZZ]*t.fM[kZX],
00196                  fM[kZX]*t.fM[kXY]+fM[kZY]*t.fM[kYY]+fM[kZZ]*t.fM[kZY],
00197                  fM[kZX]*t.fM[kXZ]+fM[kZY]*t.fM[kYZ]+fM[kZZ]*t.fM[kZZ],
00198                  fM[kZX]*t.fM[kDX]+fM[kZY]*t.fM[kDY]+fM[kZZ]*t.fM[kDZ]+fM[kDZ]);
00199    
00200    return *this;
00201 }
00202 
00203 void Transform3DPJ::SetIdentity()
00204 {
00205    //set identity ( identity rotation and zero translation)
00206    fM[kXX] = 1.0;  fM[kXY] = 0.0; fM[kXZ] = 0.0; fM[kDX] = 0.0;
00207    fM[kYX] = 0.0;  fM[kYY] = 1.0; fM[kYZ] = 0.0; fM[kDY] = 0.0;
00208    fM[kZX] = 0.0;  fM[kZY] = 0.0; fM[kZZ] = 1.0; fM[kDZ] = 0.0;
00209 }
00210 
00211 
00212 void Transform3DPJ::AssignFrom (const Rotation3D  & r,  const XYZVector & v)
00213 {
00214    // assignment  from rotation + translation
00215    
00216    double rotData[9];
00217    r.GetComponents(rotData, rotData +9);
00218    // first raw
00219    for (int i = 0; i < 3; ++i)
00220       fM[i] = rotData[i];
00221    // second raw
00222    for (int i = 0; i < 3; ++i)
00223       fM[kYX+i] = rotData[3+i];
00224    // third raw
00225    for (int i = 0; i < 3; ++i)
00226       fM[kZX+i] = rotData[6+i];
00227    
00228    // translation data
00229    double vecData[3];
00230    v.GetCoordinates(vecData, vecData+3);
00231    fM[kDX] = vecData[0];
00232    fM[kDY] = vecData[1];
00233    fM[kDZ] = vecData[2];
00234 }
00235 
00236 
00237 void Transform3DPJ::AssignFrom(const Rotation3D & r)
00238 {
00239    // assign from only a rotation  (null translation)
00240    double rotData[9];
00241    r.GetComponents(rotData, rotData +9);
00242    for (int i = 0; i < 3; ++i) {
00243       for (int j = 0; j < 3; ++j)
00244          fM[4*i + j] = rotData[3*i+j];
00245       // empty vector data
00246       fM[4*i + 3] = 0;
00247    }
00248 }
00249 
00250 void Transform3DPJ::AssignFrom(const XYZVector & v)
00251 {
00252    // assign from a translation only (identity rotations)
00253    fM[kXX] = 1.0;  fM[kXY] = 0.0; fM[kXZ] = 0.0; fM[kDX] = v.X();
00254    fM[kYX] = 0.0;  fM[kYY] = 1.0; fM[kYZ] = 0.0; fM[kDY] = v.Y();
00255    fM[kZX] = 0.0;  fM[kZY] = 0.0; fM[kZZ] = 1.0; fM[kDZ] = v.Z();
00256 }
00257 
00258 Plane3D Transform3DPJ::operator() (const Plane3D & plane) const
00259 {
00260    // transformations on a 3D plane
00261    XYZVector n = plane.Normal();
00262    // take a point on the plane. Use origin projection on the plane
00263    // ( -ad, -bd, -cd) if (a**2 + b**2 + c**2 ) = 1
00264    double d = plane.HesseDistance();
00265    XYZPoint p( - d * n.X() , - d *n.Y(), -d *n.Z() );
00266    return Plane3D ( operator() (n), operator() (p) );
00267 }
00268 
00269 std::ostream & operator<< (std::ostream & os, const Transform3DPJ & t)
00270 {
00271    // TODO - this will need changing for machine-readable issues
00272    //        and even the human readable form needs formatiing improvements
00273    
00274    double m[12];
00275    t.GetComponents(m, m+12);
00276    os << "\n" << m[0] << "  " << m[1] << "  " << m[2] << "  " << m[3] ;
00277    os << "\n" << m[4] << "  " << m[5] << "  " << m[6] << "  " << m[7] ;
00278    os << "\n" << m[8] << "  " << m[9] << "  " << m[10]<< "  " << m[11] << "\n";
00279    return os;
00280 }
00281 
00282 }  // end namespace Math
00283 }  // end namespace ROOT
/afs/cern.ch/work/a/aaltunda/public/www/CMSSW_6_2_7/src/FastSimulation/CaloGeometryTools/src/Transform3DPJ.cc
