Geant4: Transform3D.cc Source File

00001 // -*- C++ -*-
00002 // $Id:$
00003 // ---------------------------------------------------------------------------
00004 //
00005 // This file is a part of the CLHEP - a Class Library for High Energy Physics.
00006 //
00007 // Hep geometrical 3D Transformation library
00008 //
00009 // Author: Evgeni Chernyaev <Evgueni.Tcherniaev@cern.ch>
00010 //
00011 // History:
00012 // 24.09.96 E.Chernyaev - initial version
00013 
00014 #include <iostream>
00015 #include <cmath>        // double std::abs()
00016 #include "CLHEP/Geometry/Transform3D.h"
00017 
00018 namespace HepGeom {
00019 
00020   const Transform3D Transform3D::Identity = Transform3D ();
00021 
00022   //   T R A N S F O R M A T I O N -------------------------------------------
00023 
00024   double Transform3D::operator () (int i, int j) const {
00025     if (i == 0) {
00026       if (j == 0) { return xx_; }
00027       if (j == 1) { return xy_; }
00028       if (j == 2) { return xz_; } 
00029       if (j == 3) { return dx_; } 
00030     } else if (i == 1) {
00031       if (j == 0) { return yx_; }
00032       if (j == 1) { return yy_; }
00033       if (j == 2) { return yz_; } 
00034       if (j == 3) { return dy_; } 
00035     } else if (i == 2) {
00036       if (j == 0) { return zx_; }
00037       if (j == 1) { return zy_; }
00038       if (j == 2) { return zz_; } 
00039       if (j == 3) { return dz_; } 
00040     } else if (i == 3) {
00041       if (j == 0) { return 0.0; }
00042       if (j == 1) { return 0.0; }
00043       if (j == 2) { return 0.0; } 
00044       if (j == 3) { return 1.0; } 
00045     } 
00046     std::cerr << "Transform3D subscripting: bad indeces "
00047               << "(" << i << "," << j << ")" << std::endl;
00048     return 0.0;
00049   }
00050   
00051   //--------------------------------------------------------------------------
00052   Transform3D Transform3D::operator*(const Transform3D & b) const {
00053     return Transform3D
00054       (xx_*b.xx_+xy_*b.yx_+xz_*b.zx_, xx_*b.xy_+xy_*b.yy_+xz_*b.zy_,
00055        xx_*b.xz_+xy_*b.yz_+xz_*b.zz_, xx_*b.dx_+xy_*b.dy_+xz_*b.dz_+dx_,
00056        yx_*b.xx_+yy_*b.yx_+yz_*b.zx_, yx_*b.xy_+yy_*b.yy_+yz_*b.zy_,
00057        yx_*b.xz_+yy_*b.yz_+yz_*b.zz_, yx_*b.dx_+yy_*b.dy_+yz_*b.dz_+dy_,
00058        zx_*b.xx_+zy_*b.yx_+zz_*b.zx_, zx_*b.xy_+zy_*b.yy_+zz_*b.zy_,
00059        zx_*b.xz_+zy_*b.yz_+zz_*b.zz_, zx_*b.dx_+zy_*b.dy_+zz_*b.dz_+dz_);
00060   }
00061 
00062   // -------------------------------------------------------------------------
00063   Transform3D::Transform3D(const Point3D<double> & fr0,
00064                            const Point3D<double> & fr1,
00065                            const Point3D<double> & fr2,
00066                            const Point3D<double> & to0,
00067                            const Point3D<double> & to1,
00068                            const Point3D<double> & to2)
00069   /***********************************************************************
00070    *                                                                     *
00071    * Name: Transform3D::Transform3D              Date:    24.09.96 *
00072    * Author: E.Chernyaev (IHEP/Protvino)               Revised:          *
00073    *                                                                     *
00074    * Function: Create 3D Transformation from one coordinate system       *
00075    *           defined by its origin "fr0" and two axes "fr0->fr1",      *
00076    *           "fr0->fr2" to another coordinate system "to0", "to0->to1" *
00077    *           and "to0->to2"                                            *
00078    *                                                                     *
00079    ***********************************************************************/
00080   {
00081     Vector3D<double> x1,y1,z1, x2,y2,z2;
00082     x1 = (fr1 - fr0).unit();
00083     y1 = (fr2 - fr0).unit();
00084     x2 = (to1 - to0).unit();
00085     y2 = (to2 - to0).unit();
00086     
00087     //   C H E C K   A N G L E S
00088     
00089     double cos1, cos2;
00090     cos1 = x1*y1;
00091     cos2 = x2*y2;
00092     
00093     if (std::abs(1.0-cos1) <= 0.000001 || std::abs(1.0-cos2) <= 0.000001) {
00094       std::cerr << "Transform3D: zero angle between axes" << std::endl;
00095       setIdentity();
00096     }else{
00097       if (std::abs(cos1-cos2) > 0.000001) {
00098         std::cerr << "Transform3D: angles between axes are not equal"
00099                   << std::endl;
00100       }
00101       
00102       //   F I N D   R O T A T I O N   M A T R I X
00103       
00104       z1 = (x1.cross(y1)).unit();
00105       y1  = z1.cross(x1);
00106     
00107       z2 = (x2.cross(y2)).unit();
00108       y2  = z2.cross(x2);
00109       
00110       double detxx =  (y1.y()*z1.z() - z1.y()*y1.z());
00111       double detxy = -(y1.x()*z1.z() - z1.x()*y1.z());
00112       double detxz =  (y1.x()*z1.y() - z1.x()*y1.y());
00113       double detyx = -(x1.y()*z1.z() - z1.y()*x1.z());
00114       double detyy =  (x1.x()*z1.z() - z1.x()*x1.z());
00115       double detyz = -(x1.x()*z1.y() - z1.x()*x1.y());
00116       double detzx =  (x1.y()*y1.z() - y1.y()*x1.z());
00117       double detzy = -(x1.x()*y1.z() - y1.x()*x1.z());
00118       double detzz =  (x1.x()*y1.y() - y1.x()*x1.y());
00119  
00120       double txx = x2.x()*detxx + y2.x()*detyx + z2.x()*detzx; 
00121       double txy = x2.x()*detxy + y2.x()*detyy + z2.x()*detzy; 
00122       double txz = x2.x()*detxz + y2.x()*detyz + z2.x()*detzz; 
00123       double tyx = x2.y()*detxx + y2.y()*detyx + z2.y()*detzx; 
00124       double tyy = x2.y()*detxy + y2.y()*detyy + z2.y()*detzy; 
00125       double tyz = x2.y()*detxz + y2.y()*detyz + z2.y()*detzz; 
00126       double tzx = x2.z()*detxx + y2.z()*detyx + z2.z()*detzx; 
00127       double tzy = x2.z()*detxy + y2.z()*detyy + z2.z()*detzy; 
00128       double tzz = x2.z()*detxz + y2.z()*detyz + z2.z()*detzz; 
00129 
00130       //   S E T    T R A N S F O R M A T I O N 
00131 
00132       double dx1 = fr0.x(), dy1 = fr0.y(), dz1 = fr0.z();
00133       double dx2 = to0.x(), dy2 = to0.y(), dz2 = to0.z();
00134 
00135       setTransform(txx, txy, txz, dx2-txx*dx1-txy*dy1-txz*dz1,
00136                    tyx, tyy, tyz, dy2-tyx*dx1-tyy*dy1-tyz*dz1,
00137                    tzx, tzy, tzz, dz2-tzx*dx1-tzy*dy1-tzz*dz1);
00138     }
00139   }
00140 
00141   // -------------------------------------------------------------------------
00142   Transform3D Transform3D::inverse() const
00143   /***********************************************************************
00144    *                                                                     *
00145    * Name: Transform3D::inverse                     Date:    24.09.96 *
00146    * Author: E.Chernyaev (IHEP/Protvino)               Revised:          *
00147    *                                                                     *
00148    * Function: Find inverse affine transformation                        *
00149    *                                                                     *
00150    ***********************************************************************/
00151   {
00152     double detxx = yy_*zz_-yz_*zy_;
00153     double detxy = yx_*zz_-yz_*zx_;
00154     double detxz = yx_*zy_-yy_*zx_;
00155     double det   = xx_*detxx - xy_*detxy + xz_*detxz;
00156     if (det == 0) {
00157       std::cerr << "Transform3D::inverse error: zero determinant" << std::endl;
00158       return Transform3D();
00159     }
00160     det = 1./det; detxx *= det; detxy *= det; detxz *= det;
00161     double detyx = (xy_*zz_ - xz_*zy_)*det;
00162     double detyy = (xx_*zz_ - xz_*zx_)*det;
00163     double detyz = (xx_*zy_ - xy_*zx_)*det;
00164     double detzx = (xy_*yz_ - xz_*yy_)*det;
00165     double detzy = (xx_*yz_ - xz_*yx_)*det;
00166     double detzz = (xx_*yy_ - xy_*yx_)*det;
00167     return Transform3D
00168       (detxx, -detyx,  detzx, -detxx*dx_+detyx*dy_-detzx*dz_,
00169       -detxy,  detyy, -detzy,  detxy*dx_-detyy*dy_+detzy*dz_,
00170        detxz, -detyz,  detzz, -detxz*dx_+detyz*dy_-detzz*dz_);
00171   }
00172 
00173   // -------------------------------------------------------------------------
00174   void Transform3D::getDecomposition(Scale3D & scale,
00175                                      Rotate3D & rotation,
00176                                      Translate3D & translation) const
00177   /***********************************************************************
00178    *                                                           CLHEP-1.7 *
00179    * Name: Transform3D::getDecomposition            Date:       09.06.01 *
00180    * Author: E.Chernyaev (IHEP/Protvino)            Revised:             *
00181    *                                                                     *
00182    * Function: Gets decomposition of general transformation on           *
00183    *           three consequentive specific transformations:             *
00184    *           Scale, then Rotation, then Translation.                   *
00185    *           If there was a reflection, then ScaleZ will be negative.  *
00186    *                                                                     *
00187    ***********************************************************************/
00188   {
00189     double sx = std::sqrt(xx_*xx_ + yx_*yx_ + zx_*zx_);
00190     double sy = std::sqrt(xy_*xy_ + yy_*yy_ + zy_*zy_);
00191     double sz = std::sqrt(xz_*xz_ + yz_*yz_ + zz_*zz_);
00192 
00193     if (xx_*(yy_*zz_-yz_*zy_) -
00194         xy_*(yx_*zz_-yz_*zx_) +
00195         xz_*(yx_*zy_-yy_*zx_) < 0) sz = -sz;
00196     scale.setTransform(sx,0,0,0,  0,sy,0,0, 0,0,sz,0);
00197     rotation.setTransform(xx_/sx,xy_/sy,xz_/sz,0,
00198                           yx_/sx,yy_/sy,yz_/sz,0,
00199                           zx_/sx,zy_/sy,zz_/sz,0); 
00200     translation.setTransform(1,0,0,dx_, 0,1,0,dy_, 0,0,1,dz_);
00201   }
00202 
00203   // -------------------------------------------------------------------------
00204   bool Transform3D::isNear(const Transform3D & t, double tolerance) const
00205   { 
00206     return ( (std::abs(xx_ - t.xx_) <= tolerance) && 
00207              (std::abs(xy_ - t.xy_) <= tolerance) &&
00208              (std::abs(xz_ - t.xz_) <= tolerance) &&
00209              (std::abs(dx_ - t.dx_) <= tolerance) &&
00210              (std::abs(yx_ - t.yx_) <= tolerance) &&
00211              (std::abs(yy_ - t.yy_) <= tolerance) &&
00212              (std::abs(yz_ - t.yz_) <= tolerance) &&
00213              (std::abs(dy_ - t.dy_) <= tolerance) &&
00214              (std::abs(zx_ - t.zx_) <= tolerance) &&
00215              (std::abs(zy_ - t.zy_) <= tolerance) &&
00216              (std::abs(zz_ - t.zz_) <= tolerance) &&
00217              (std::abs(dz_ - t.dz_) <= tolerance) );
00218   }
00219 
00220   // -------------------------------------------------------------------------
00221   bool Transform3D::operator==(const Transform3D & t) const
00222   {
00223     return (this == &t) ? true :
00224       (xx_==t.xx_ && xy_==t.xy_ && xz_==t.xz_ && dx_==t.dx_ && 
00225        yx_==t.yx_ && yy_==t.yy_ && yz_==t.yz_ && dy_==t.dy_ &&
00226        zx_==t.zx_ && zy_==t.zy_ && zz_==t.zz_ && dz_==t.dz_ );
00227   }
00228 
00229   //   3 D   R O T A T I O N -------------------------------------------------
00230 
00231   Rotate3D::Rotate3D(double a,
00232                      const Point3D<double> & p1,
00233                      const Point3D<double> & p2) : Transform3D()
00234   /***********************************************************************
00235    *                                                                     *
00236    * Name: Rotate3D::Rotate3D                       Date:    24.09.96 *
00237    * Author: E.Chernyaev (IHEP/Protvino)               Revised:          *
00238    *                                                                     *
00239    * Function: Create 3D Rotation through angle "a" (counterclockwise)   *
00240    *           around the axis p1->p2                                    *
00241    *                                                                     *
00242    ***********************************************************************/
00243   {
00244     if (a == 0) return;
00245 
00246     double cx = p2.x()-p1.x(), cy = p2.y()-p1.y(), cz = p2.z()-p1.z();
00247     double ll = std::sqrt(cx*cx + cy*cy + cz*cz); 
00248     if (ll == 0) {
00249       std::cerr << "Rotate3D: zero axis" << std::endl;
00250     }else{
00251       double cosa = std::cos(a), sina = std::sin(a);
00252       cx /= ll; cy /= ll; cz /= ll;   
00253     
00254       double txx = cosa + (1-cosa)*cx*cx;
00255       double txy =        (1-cosa)*cx*cy - sina*cz;
00256       double txz =        (1-cosa)*cx*cz + sina*cy;
00257     
00258       double tyx =        (1-cosa)*cy*cx + sina*cz;
00259       double tyy = cosa + (1-cosa)*cy*cy; 
00260       double tyz =        (1-cosa)*cy*cz - sina*cx;
00261     
00262       double tzx =        (1-cosa)*cz*cx - sina*cy;
00263       double tzy =        (1-cosa)*cz*cy + sina*cx;
00264       double tzz = cosa + (1-cosa)*cz*cz;
00265     
00266       double tdx = p1.x(), tdy = p1.y(), tdz = p1.z(); 
00267     
00268       setTransform(txx, txy, txz, tdx-txx*tdx-txy*tdy-txz*tdz,
00269                    tyx, tyy, tyz, tdy-tyx*tdx-tyy*tdy-tyz*tdz,
00270                    tzx, tzy, tzz, tdz-tzx*tdx-tzy*tdy-tzz*tdz);
00271     }
00272   }
00273 
00274   //   3 D   R E F L E C T I O N ---------------------------------------------
00275 
00276   Reflect3D::Reflect3D(double a, double b, double c, double d)
00277   /***********************************************************************
00278    *                                                                     *
00279    * Name: Reflect3D::Reflect3D                        Date:    24.09.96 *
00280    * Author: E.Chernyaev (IHEP/Protvino)               Revised:          *
00281    *                                                                     *
00282    * Function: Create 3D Reflection in a plane a*x + b*y + c*z + d = 0   *
00283    *                                                                     *
00284    ***********************************************************************/
00285   {
00286     double ll = a*a+b*b+c*c;
00287     if (ll == 0) {
00288       std::cerr << "Reflect3D: zero normal" << std::endl;
00289       setIdentity();
00290     }else{
00291       ll = 1/ll;
00292       double aa = a*a*ll, ab = a*b*ll, ac = a*c*ll, ad = a*d*ll,
00293              bb = b*b*ll, bc = b*c*ll, bd = b*d*ll,
00294              cc = c*c*ll, cd = c*d*ll;
00295       setTransform(-aa+bb+cc, -ab-ab,    -ac-ac,    -ad-ad,
00296                    -ab-ab,     aa-bb+cc, -bc-bc,    -bd-bd,
00297                    -ac-ac,    -bc-bc,     aa+bb-cc, -cd-cd);
00298     }
00299   }
00300 } /* namespace HepGeom */