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