Geant4 Cross Reference |
1 // -*- C++ -*- 2 // --------------------------------------------------------------------------- 3 // 4 // This file is a part of the CLHEP - a Class Library for High Energy Physics. 5 // 6 // Hep geometrical 3D Transformation library 7 // 8 // Author: Evgeni Chernyaev <Evgueni.Tcherniaev@cern.ch> 9 // 10 // History: 11 // 24.09.96 E.Chernyaev - initial version 12 13 #include <iostream> 14 #include <cmath> // double std::abs() 15 #include "CLHEP/Geometry/Transform3D.h" 16 17 namespace HepGeom { 18 19 const Transform3D Transform3D::Identity = Transform3D (); 20 21 // T R A N S F O R M A T I O N ------------------------------------------- 22 23 double Transform3D::operator () (int i, int j) const { 24 if (i == 0) { 25 if (j == 0) { return xx_; } 26 if (j == 1) { return xy_; } 27 if (j == 2) { return xz_; } 28 if (j == 3) { return dx_; } 29 } else if (i == 1) { 30 if (j == 0) { return yx_; } 31 if (j == 1) { return yy_; } 32 if (j == 2) { return yz_; } 33 if (j == 3) { return dy_; } 34 } else if (i == 2) { 35 if (j == 0) { return zx_; } 36 if (j == 1) { return zy_; } 37 if (j == 2) { return zz_; } 38 if (j == 3) { return dz_; } 39 } else if (i == 3) { 40 if (j == 0) { return 0.0; } 41 if (j == 1) { return 0.0; } 42 if (j == 2) { return 0.0; } 43 if (j == 3) { return 1.0; } 44 } 45 std::cerr << "Transform3D subscripting: bad indices " 46 << "(" << i << "," << j << ")" << std::endl; 47 return 0.0; 48 } 49 50 //-------------------------------------------------------------------------- 51 Transform3D Transform3D::operator*(const Transform3D & b) const { 52 return Transform3D 53 (xx_*b.xx_+xy_*b.yx_+xz_*b.zx_, xx_*b.xy_+xy_*b.yy_+xz_*b.zy_, 54 xx_*b.xz_+xy_*b.yz_+xz_*b.zz_, xx_*b.dx_+xy_*b.dy_+xz_*b.dz_+dx_, 55 yx_*b.xx_+yy_*b.yx_+yz_*b.zx_, yx_*b.xy_+yy_*b.yy_+yz_*b.zy_, 56 yx_*b.xz_+yy_*b.yz_+yz_*b.zz_, yx_*b.dx_+yy_*b.dy_+yz_*b.dz_+dy_, 57 zx_*b.xx_+zy_*b.yx_+zz_*b.zx_, zx_*b.xy_+zy_*b.yy_+zz_*b.zy_, 58 zx_*b.xz_+zy_*b.yz_+zz_*b.zz_, zx_*b.dx_+zy_*b.dy_+zz_*b.dz_+dz_); 59 } 60 61 // ------------------------------------------------------------------------- 62 Transform3D::Transform3D(const Point3D<double> & fr0, 63 const Point3D<double> & fr1, 64 const Point3D<double> & fr2, 65 const Point3D<double> & to0, 66 const Point3D<double> & to1, 67 const Point3D<double> & to2) 68 /*********************************************************************** 69 * * 70 * Name: Transform3D::Transform3D Date: 24.09.96 * 71 * Author: E.Chernyaev (IHEP/Protvino) Revised: * 72 * * 73 * Function: Create 3D Transformation from one coordinate system * 74 * defined by its origin "fr0" and two axes "fr0->fr1", * 75 * "fr0->fr2" to another coordinate system "to0", "to0->to1" * 76 * and "to0->to2" * 77 * * 78 ***********************************************************************/ 79 { 80 Vector3D<double> x1,y1,z1, x2,y2,z2; 81 x1 = (fr1 - fr0).unit(); 82 y1 = (fr2 - fr0).unit(); 83 x2 = (to1 - to0).unit(); 84 y2 = (to2 - to0).unit(); 85 86 // C H E C K A N G L E S 87 88 double cos1, cos2; 89 cos1 = x1*y1; 90 cos2 = x2*y2; 91 92 if (std::abs(1.0-cos1) <= 0.000001 || std::abs(1.0-cos2) <= 0.000001) { 93 std::cerr << "Transform3D: zero angle between axes" << std::endl; 94 setIdentity(); 95 }else{ 96 if (std::abs(cos1-cos2) > 0.000001) { 97 std::cerr << "Transform3D: angles between axes are not equal" 98 << std::endl; 99 } 100 101 // F I N D R O T A T I O N M A T R I X 102 103 z1 = (x1.cross(y1)).unit(); 104 y1 = z1.cross(x1); 105 106 z2 = (x2.cross(y2)).unit(); 107 y2 = z2.cross(x2); 108 109 double detxx = (y1.y()*z1.z() - z1.y()*y1.z()); 110 double detxy = -(y1.x()*z1.z() - z1.x()*y1.z()); 111 double detxz = (y1.x()*z1.y() - z1.x()*y1.y()); 112 double detyx = -(x1.y()*z1.z() - z1.y()*x1.z()); 113 double detyy = (x1.x()*z1.z() - z1.x()*x1.z()); 114 double detyz = -(x1.x()*z1.y() - z1.x()*x1.y()); 115 double detzx = (x1.y()*y1.z() - y1.y()*x1.z()); 116 double detzy = -(x1.x()*y1.z() - y1.x()*x1.z()); 117 double detzz = (x1.x()*y1.y() - y1.x()*x1.y()); 118 119 double txx = x2.x()*detxx + y2.x()*detyx + z2.x()*detzx; 120 double txy = x2.x()*detxy + y2.x()*detyy + z2.x()*detzy; 121 double txz = x2.x()*detxz + y2.x()*detyz + z2.x()*detzz; 122 double tyx = x2.y()*detxx + y2.y()*detyx + z2.y()*detzx; 123 double tyy = x2.y()*detxy + y2.y()*detyy + z2.y()*detzy; 124 double tyz = x2.y()*detxz + y2.y()*detyz + z2.y()*detzz; 125 double tzx = x2.z()*detxx + y2.z()*detyx + z2.z()*detzx; 126 double tzy = x2.z()*detxy + y2.z()*detyy + z2.z()*detzy; 127 double tzz = x2.z()*detxz + y2.z()*detyz + z2.z()*detzz; 128 129 // S E T T R A N S F O R M A T I O N 130 131 double dx1 = fr0.x(), dy1 = fr0.y(), dz1 = fr0.z(); 132 double dx2 = to0.x(), dy2 = to0.y(), dz2 = to0.z(); 133 134 setTransform(txx, txy, txz, dx2-txx*dx1-txy*dy1-txz*dz1, 135 tyx, tyy, tyz, dy2-tyx*dx1-tyy*dy1-tyz*dz1, 136 tzx, tzy, tzz, dz2-tzx*dx1-tzy*dy1-tzz*dz1); 137 } 138 } 139 140 // ------------------------------------------------------------------------- 141 Transform3D Transform3D::inverse() const 142 /*********************************************************************** 143 * * 144 * Name: Transform3D::inverse Date: 24.09.96 * 145 * Author: E.Chernyaev (IHEP/Protvino) Revised: * 146 * * 147 * Function: Find inverse affine transformation * 148 * * 149 ***********************************************************************/ 150 { 151 double detxx = yy_*zz_-yz_*zy_; 152 double detxy = yx_*zz_-yz_*zx_; 153 double detxz = yx_*zy_-yy_*zx_; 154 double det = xx_*detxx - xy_*detxy + xz_*detxz; 155 if (det == 0) { 156 std::cerr << "Transform3D::inverse error: zero determinant" << std::endl; 157 return Transform3D(); 158 } 159 det = 1./det; detxx *= det; detxy *= det; detxz *= det; 160 double detyx = (xy_*zz_ - xz_*zy_)*det; 161 double detyy = (xx_*zz_ - xz_*zx_)*det; 162 double detyz = (xx_*zy_ - xy_*zx_)*det; 163 double detzx = (xy_*yz_ - xz_*yy_)*det; 164 double detzy = (xx_*yz_ - xz_*yx_)*det; 165 double detzz = (xx_*yy_ - xy_*yx_)*det; 166 return Transform3D 167 (detxx, -detyx, detzx, -detxx*dx_+detyx*dy_-detzx*dz_, 168 -detxy, detyy, -detzy, detxy*dx_-detyy*dy_+detzy*dz_, 169 detxz, -detyz, detzz, -detxz*dx_+detyz*dy_-detzz*dz_); 170 } 171 172 // ------------------------------------------------------------------------- 173 void Transform3D::getDecomposition(Scale3D & scale, 174 Rotate3D & rotation, 175 Translate3D & translation) const 176 /*********************************************************************** 177 * CLHEP-1.7 * 178 * Name: Transform3D::getDecomposition Date: 09.06.01 * 179 * Author: E.Chernyaev (IHEP/Protvino) Revised: * 180 * * 181 * Function: Gets decomposition of general transformation on * 182 * three consequentive specific transformations: * 183 * Scale, then Rotation, then Translation. * 184 * If there was a reflection, then ScaleZ will be negative. * 185 * * 186 ***********************************************************************/ 187 { 188 double sx = std::sqrt(xx_*xx_ + yx_*yx_ + zx_*zx_); 189 double sy = std::sqrt(xy_*xy_ + yy_*yy_ + zy_*zy_); 190 double sz = std::sqrt(xz_*xz_ + yz_*yz_ + zz_*zz_); 191 192 if (xx_*(yy_*zz_-yz_*zy_) - 193 xy_*(yx_*zz_-yz_*zx_) + 194 xz_*(yx_*zy_-yy_*zx_) < 0) sz = -sz; 195 scale.setTransform(sx,0,0,0, 0,sy,0,0, 0,0,sz,0); 196 rotation.setTransform(xx_/sx,xy_/sy,xz_/sz,0, 197 yx_/sx,yy_/sy,yz_/sz,0, 198 zx_/sx,zy_/sy,zz_/sz,0); 199 translation.setTransform(1,0,0,dx_, 0,1,0,dy_, 0,0,1,dz_); 200 } 201 202 // ------------------------------------------------------------------------- 203 bool Transform3D::isNear(const Transform3D & t, double tolerance) const 204 { 205 return ( (std::abs(xx_ - t.xx_) <= tolerance) && 206 (std::abs(xy_ - t.xy_) <= tolerance) && 207 (std::abs(xz_ - t.xz_) <= tolerance) && 208 (std::abs(dx_ - t.dx_) <= tolerance) && 209 (std::abs(yx_ - t.yx_) <= tolerance) && 210 (std::abs(yy_ - t.yy_) <= tolerance) && 211 (std::abs(yz_ - t.yz_) <= tolerance) && 212 (std::abs(dy_ - t.dy_) <= tolerance) && 213 (std::abs(zx_ - t.zx_) <= tolerance) && 214 (std::abs(zy_ - t.zy_) <= tolerance) && 215 (std::abs(zz_ - t.zz_) <= tolerance) && 216 (std::abs(dz_ - t.dz_) <= tolerance) ); 217 } 218 219 // ------------------------------------------------------------------------- 220 bool Transform3D::operator==(const Transform3D & t) const 221 { 222 return (this == &t) ? true : 223 (xx_==t.xx_ && xy_==t.xy_ && xz_==t.xz_ && dx_==t.dx_ && 224 yx_==t.yx_ && yy_==t.yy_ && yz_==t.yz_ && dy_==t.dy_ && 225 zx_==t.zx_ && zy_==t.zy_ && zz_==t.zz_ && dz_==t.dz_ ); 226 } 227 228 // 3 D R O T A T I O N ------------------------------------------------- 229 230 Rotate3D::Rotate3D(double a, 231 const Point3D<double> & p1, 232 const Point3D<double> & p2) : Transform3D() 233 /*********************************************************************** 234 * * 235 * Name: Rotate3D::Rotate3D Date: 24.09.96 * 236 * Author: E.Chernyaev (IHEP/Protvino) Revised: * 237 * * 238 * Function: Create 3D Rotation through angle "a" (counterclockwise) * 239 * around the axis p1->p2 * 240 * * 241 ***********************************************************************/ 242 { 243 if (a == 0) return; 244 245 double cx = p2.x()-p1.x(), cy = p2.y()-p1.y(), cz = p2.z()-p1.z(); 246 double ll = std::sqrt(cx*cx + cy*cy + cz*cz); 247 if (ll == 0) { 248 std::cerr << "Rotate3D: zero axis" << std::endl; 249 }else{ 250 double cosa = std::cos(a), sina = std::sin(a); 251 cx /= ll; cy /= ll; cz /= ll; 252 253 double txx = cosa + (1-cosa)*cx*cx; 254 double txy = (1-cosa)*cx*cy - sina*cz; 255 double txz = (1-cosa)*cx*cz + sina*cy; 256 257 double tyx = (1-cosa)*cy*cx + sina*cz; 258 double tyy = cosa + (1-cosa)*cy*cy; 259 double tyz = (1-cosa)*cy*cz - sina*cx; 260 261 double tzx = (1-cosa)*cz*cx - sina*cy; 262 double tzy = (1-cosa)*cz*cy + sina*cx; 263 double tzz = cosa + (1-cosa)*cz*cz; 264 265 double tdx = p1.x(), tdy = p1.y(), tdz = p1.z(); 266 267 setTransform(txx, txy, txz, tdx-txx*tdx-txy*tdy-txz*tdz, 268 tyx, tyy, tyz, tdy-tyx*tdx-tyy*tdy-tyz*tdz, 269 tzx, tzy, tzz, tdz-tzx*tdx-tzy*tdy-tzz*tdz); 270 } 271 } 272 273 // 3 D R E F L E C T I O N --------------------------------------------- 274 275 Reflect3D::Reflect3D(double a, double b, double c, double d) 276 /*********************************************************************** 277 * * 278 * Name: Reflect3D::Reflect3D Date: 24.09.96 * 279 * Author: E.Chernyaev (IHEP/Protvino) Revised: * 280 * * 281 * Function: Create 3D Reflection in a plane a*x + b*y + c*z + d = 0 * 282 * * 283 ***********************************************************************/ 284 { 285 double ll = a*a+b*b+c*c; 286 if (ll == 0) { 287 std::cerr << "Reflect3D: zero normal" << std::endl; 288 setIdentity(); 289 }else{ 290 ll = 1/ll; 291 double aa = a*a*ll, ab = a*b*ll, ac = a*c*ll, ad = a*d*ll, 292 bb = b*b*ll, bc = b*c*ll, bd = b*d*ll, 293 cc = c*c*ll, cd = c*d*ll; 294 setTransform(-aa+bb+cc, -ab-ab, -ac-ac, -ad-ad, 295 -ab-ab, aa-bb+cc, -bc-bc, -bd-bd, 296 -ac-ac, -bc-bc, aa+bb-cc, -cd-cd); 297 } 298 } 299 } /* namespace HepGeom */ 300