[831] | 1 | // |
---|
| 2 | // ******************************************************************** |
---|
| 3 | // * License and Disclaimer * |
---|
| 4 | // * * |
---|
| 5 | // * The Geant4 software is copyright of the Copyright Holders of * |
---|
| 6 | // * the Geant4 Collaboration. It is provided under the terms and * |
---|
| 7 | // * conditions of the Geant4 Software License, included in the file * |
---|
| 8 | // * LICENSE and available at http://cern.ch/geant4/license . These * |
---|
| 9 | // * include a list of copyright holders. * |
---|
| 10 | // * * |
---|
| 11 | // * Neither the authors of this software system, nor their employing * |
---|
| 12 | // * institutes,nor the agencies providing financial support for this * |
---|
| 13 | // * work make any representation or warranty, express or implied, * |
---|
| 14 | // * regarding this software system or assume any liability for its * |
---|
| 15 | // * use. Please see the license in the file LICENSE and URL above * |
---|
| 16 | // * for the full disclaimer and the limitation of liability. * |
---|
| 17 | // * * |
---|
| 18 | // * This code implementation is the result of the scientific and * |
---|
| 19 | // * technical work of the GEANT4 collaboration. * |
---|
| 20 | // * By using, copying, modifying or distributing the software (or * |
---|
| 21 | // * any work based on the software) you agree to acknowledge its * |
---|
| 22 | // * use in resulting scientific publications, and indicate your * |
---|
| 23 | // * acceptance of all terms of the Geant4 Software license. * |
---|
| 24 | // ******************************************************************** |
---|
| 25 | // |
---|
| 26 | // |
---|
| 27 | // $Id: G4ToroidalSurface.cc,v 1.10 2006/06/29 18:42:59 gunter Exp $ |
---|
[850] | 28 | // GEANT4 tag $Name: HEAD $ |
---|
[831] | 29 | // |
---|
| 30 | // ---------------------------------------------------------------------- |
---|
| 31 | // GEANT 4 class source file |
---|
| 32 | // |
---|
| 33 | // G4ToroidalSurface.cc |
---|
| 34 | // |
---|
| 35 | // ---------------------------------------------------------------------- |
---|
| 36 | |
---|
| 37 | #include "G4ToroidalSurface.hh" |
---|
| 38 | |
---|
| 39 | |
---|
| 40 | G4ToroidalSurface::G4ToroidalSurface() |
---|
| 41 | : EQN_EPS(1e-9) |
---|
| 42 | { |
---|
| 43 | } |
---|
| 44 | |
---|
| 45 | G4ToroidalSurface::G4ToroidalSurface(const G4Vector3D& Location, |
---|
| 46 | const G4Vector3D& Ax, |
---|
| 47 | const G4Vector3D& Dir, |
---|
| 48 | G4double MinRad, |
---|
| 49 | G4double MaxRad) |
---|
| 50 | : EQN_EPS(1e-9) |
---|
| 51 | { |
---|
| 52 | Placement.Init(Dir, Ax, Location); |
---|
| 53 | |
---|
| 54 | MinRadius = MinRad; |
---|
| 55 | MaxRadius = MaxRad; |
---|
| 56 | TransMatrix= new G4PointMatrix(4,4); |
---|
| 57 | } |
---|
| 58 | |
---|
| 59 | |
---|
| 60 | G4ToroidalSurface::~G4ToroidalSurface() |
---|
| 61 | { |
---|
| 62 | } |
---|
| 63 | |
---|
| 64 | |
---|
| 65 | void G4ToroidalSurface::CalcBBox() |
---|
| 66 | { |
---|
| 67 | // L. Broglia |
---|
| 68 | // G4Point3D Origin = Placement.GetSrfPoint(); |
---|
| 69 | G4Point3D Origin = Placement.GetLocation(); |
---|
| 70 | |
---|
| 71 | G4Point3D Min(Origin.x()-MaxRadius, |
---|
| 72 | Origin.y()-MaxRadius, |
---|
| 73 | Origin.z()-MaxRadius); |
---|
| 74 | G4Point3D Max(Origin.x()+MaxRadius, |
---|
| 75 | Origin.y()+MaxRadius, |
---|
| 76 | Origin.z()+MaxRadius); |
---|
| 77 | |
---|
| 78 | bbox = new G4BoundingBox3D(Min,Max); |
---|
| 79 | } |
---|
| 80 | |
---|
| 81 | |
---|
| 82 | G4Vector3D G4ToroidalSurface::SurfaceNormal(const G4Point3D&) const |
---|
| 83 | { |
---|
| 84 | return G4Vector3D(0,0,0); |
---|
| 85 | } |
---|
| 86 | |
---|
| 87 | |
---|
| 88 | G4double G4ToroidalSurface::ClosestDistanceToPoint(const G4Point3D &Pt) |
---|
| 89 | { |
---|
| 90 | // L. Broglia |
---|
| 91 | // G4Point3D Origin = Placement.GetSrfPoint(); |
---|
| 92 | G4Point3D Origin = Placement.GetLocation(); |
---|
| 93 | |
---|
| 94 | G4double Dist = Pt.distance(Origin); |
---|
| 95 | |
---|
| 96 | return ((Dist - MaxRadius)*(Dist - MaxRadius)); |
---|
| 97 | } |
---|
| 98 | |
---|
| 99 | |
---|
| 100 | G4int G4ToroidalSurface::Intersect(const G4Ray& Ray) |
---|
| 101 | { |
---|
| 102 | // ---- inttor - Intersect a ray with a torus. ------------------------ |
---|
| 103 | // from GraphicsGems II by |
---|
| 104 | |
---|
| 105 | // Description: |
---|
| 106 | // Inttor determines the intersection of a ray with a torus. |
---|
| 107 | // |
---|
| 108 | // On entry: |
---|
| 109 | // raybase = The coordinate defining the base of the |
---|
| 110 | // intersecting ray. |
---|
| 111 | // raycos = The G4Vector3D cosines of the above ray. |
---|
| 112 | // center = The center location of the torus. |
---|
| 113 | // radius = The major radius of the torus. |
---|
| 114 | // rplane = The minor radius in the G4Plane of the torus. |
---|
| 115 | // rnorm = The minor radius Normal to the G4Plane of the torus. |
---|
| 116 | // tran = A 4x4 transformation matrix that will position |
---|
| 117 | // the torus at the origin and orient it such that |
---|
| 118 | // the G4Plane of the torus lyes in the x-z G4Plane. |
---|
| 119 | // |
---|
| 120 | // On return: |
---|
| 121 | // nhits = The number of intersections the ray makes with |
---|
| 122 | // the torus. |
---|
| 123 | // rhits = The entering/leaving distances of the |
---|
| 124 | // intersections. |
---|
| 125 | // |
---|
| 126 | // Returns: True if the ray intersects the torus. |
---|
| 127 | // |
---|
| 128 | // -------------------------------------------------------------------- |
---|
| 129 | |
---|
| 130 | // Variables. Should be optimized later... |
---|
| 131 | G4Point3D Base = Ray.GetStart(); // Base of the intersection ray |
---|
| 132 | G4Vector3D DCos = Ray.GetDir(); // Direction cosines of the ray |
---|
| 133 | G4int nhits=0; // Number of intersections |
---|
| 134 | G4double rhits[4]; // Intersection distances |
---|
| 135 | G4double hits[4]; // Ordered intersection distances |
---|
| 136 | G4double rho, a0, b0; // Related constants |
---|
| 137 | G4double f, l, t, g, q, m, u; // Ray dependent terms |
---|
| 138 | G4double C[5]; // Quartic coefficients |
---|
| 139 | |
---|
| 140 | // Transform the intersection ray |
---|
| 141 | |
---|
| 142 | |
---|
| 143 | // MultiplyPointByMatrix (Base); // Matriisi puuttuu viela! |
---|
| 144 | // MultiplyVectorByMatrix (DCos); |
---|
| 145 | |
---|
| 146 | // Compute constants related to the torus. |
---|
| 147 | G4double rnorm = MaxRadius - MinRadius; // ei tietoa onko oikein... |
---|
| 148 | rho = MinRadius*MinRadius / (rnorm*rnorm); |
---|
| 149 | a0 = 4. * MaxRadius*MaxRadius; |
---|
| 150 | b0 = MaxRadius*MaxRadius - MinRadius*MinRadius; |
---|
| 151 | |
---|
| 152 | // Compute ray dependent terms. |
---|
| 153 | f = 1. - DCos.y()*DCos.y(); |
---|
| 154 | l = 2. * (Base.x()*DCos.x() + Base.z()*DCos.z()); |
---|
| 155 | t = Base.x()*Base.x() + Base.z()*Base.z(); |
---|
| 156 | g = f + rho * DCos.y()*DCos.y(); |
---|
| 157 | q = a0 / (g*g); |
---|
| 158 | m = (l + 2.*rho*DCos.y()*Base.y()) / g; |
---|
| 159 | u = (t + rho*Base.y()*Base.y() + b0) / g; |
---|
| 160 | |
---|
| 161 | // Compute the coefficients of the quartic. |
---|
| 162 | |
---|
| 163 | C[4] = 1.0; |
---|
| 164 | C[3] = 2. * m; |
---|
| 165 | C[2] = m*m + 2.*u - q*f; |
---|
| 166 | C[1] = 2.*m*u - q*l; |
---|
| 167 | C[0] = u*u - q*t; |
---|
| 168 | |
---|
| 169 | // Use quartic root solver found in "Graphics Gems" by Jochen Schwarze. |
---|
| 170 | nhits = SolveQuartic (C,rhits); |
---|
| 171 | |
---|
| 172 | // SolveQuartic returns root pairs in reversed order. |
---|
| 173 | m = rhits[0]; u = rhits[1]; rhits[0] = u; rhits[1] = m; |
---|
| 174 | m = rhits[2]; u = rhits[3]; rhits[2] = u; rhits[3] = m; |
---|
| 175 | |
---|
| 176 | // return (*nhits != 0); |
---|
| 177 | |
---|
| 178 | if(nhits != 0) |
---|
| 179 | { |
---|
| 180 | // Convert Hit distances to intersection points |
---|
| 181 | /* |
---|
| 182 | G4Point3D** IntersectionPoints = new G4Point3D*[nhits]; |
---|
| 183 | for(G4int a=0;a<nhits;a++) |
---|
| 184 | { |
---|
| 185 | G4double Dist = rhits[a]; |
---|
| 186 | IntersectionPoints[a] = new G4Point3D((Base - Dist * DCos)); |
---|
| 187 | } |
---|
| 188 | // Check wether any of the hits are on the actual surface |
---|
| 189 | // Start with checking for the intersections that are Inside the bbox |
---|
| 190 | |
---|
| 191 | G4Point3D* Hit; |
---|
| 192 | G4int InsideBox[2]; // Max 2 intersections on the surface |
---|
| 193 | G4int Counter=0; |
---|
| 194 | */ |
---|
| 195 | |
---|
| 196 | G4Point3D BoxMin = bbox->GetBoxMin(); |
---|
| 197 | G4Point3D BoxMax = bbox->GetBoxMax(); |
---|
| 198 | G4Point3D Hit; |
---|
| 199 | G4int c1 = 0; |
---|
| 200 | G4int c2; |
---|
| 201 | G4double tempVec[4]; |
---|
| 202 | |
---|
| 203 | for(G4int a=0;a<nhits;a++) |
---|
| 204 | { |
---|
| 205 | while ( (c1 < 4) && (hits[c1] <= rhits[a]) ) |
---|
| 206 | { |
---|
| 207 | tempVec[c1]=hits[c1]; |
---|
| 208 | c1++; |
---|
| 209 | } |
---|
| 210 | |
---|
| 211 | for(c2=c1+1;c2<4;c2++) |
---|
| 212 | tempVec[c2]=hits[c2-1]; |
---|
| 213 | |
---|
| 214 | if(c1<4) |
---|
| 215 | { |
---|
| 216 | tempVec[c1]=rhits[a]; |
---|
| 217 | |
---|
| 218 | for(c2=0;c2<4;c2++) |
---|
| 219 | hits[c2]=tempVec[c2]; |
---|
| 220 | } |
---|
| 221 | } |
---|
| 222 | |
---|
| 223 | for(G4int b=0;b<nhits;b++) |
---|
| 224 | { |
---|
| 225 | // Hit = IntersectionPoints[b]; |
---|
| 226 | if(hits[b] >=kCarTolerance*0.5) |
---|
| 227 | { |
---|
| 228 | Hit = Base + (hits[b]*DCos); |
---|
| 229 | // InsideBox[Counter]=b; |
---|
| 230 | if( (Hit.x() > BoxMin.x()) && |
---|
| 231 | (Hit.x() < BoxMax.x()) && |
---|
| 232 | (Hit.y() > BoxMin.y()) && |
---|
| 233 | (Hit.y() < BoxMax.y()) && |
---|
| 234 | (Hit.z() > BoxMin.z()) && |
---|
| 235 | (Hit.z() < BoxMax.z()) ) |
---|
| 236 | { |
---|
| 237 | closest_hit = Hit; |
---|
| 238 | distance = hits[b]*hits[b]; |
---|
| 239 | return 1; |
---|
| 240 | } |
---|
| 241 | |
---|
| 242 | // Counter++; |
---|
| 243 | } |
---|
| 244 | } |
---|
| 245 | |
---|
| 246 | // If two Inside bbox, find closest |
---|
| 247 | // G4int Closest=0; |
---|
| 248 | |
---|
| 249 | // if(Counter>1) |
---|
| 250 | // if(rhits[InsideBox[0]] > rhits[InsideBox[1]]) |
---|
| 251 | // Closest=1; |
---|
| 252 | |
---|
| 253 | // Project polygon and do point in polygon |
---|
| 254 | // Projection also for curves etc. |
---|
| 255 | // Should probably be implemented in the curve class. |
---|
| 256 | // G4Plane Plane1 = Ray.GetPlane(1); |
---|
| 257 | // G4Plane Plane2 = Ray.GetPlane(2); |
---|
| 258 | |
---|
| 259 | // Point in polygon |
---|
| 260 | return 1; |
---|
| 261 | } |
---|
| 262 | return 0; |
---|
| 263 | } |
---|
| 264 | |
---|
| 265 | |
---|
| 266 | G4int G4ToroidalSurface::SolveQuartic(G4double c[], G4double s[] ) |
---|
| 267 | { |
---|
| 268 | // From Graphics Gems I by Jochen Schwartz |
---|
| 269 | |
---|
| 270 | G4double coeffs[ 4 ]; |
---|
| 271 | G4double z, u, v, sub; |
---|
| 272 | G4double A, B, C, D; |
---|
| 273 | G4double sq_A, p, q, r; |
---|
| 274 | G4int i, num; |
---|
| 275 | |
---|
| 276 | // Normal form: x^4 + Ax^3 + Bx^2 + Cx + D = 0 |
---|
| 277 | |
---|
| 278 | A = c[ 3 ] / c[ 4 ]; |
---|
| 279 | B = c[ 2 ] / c[ 4 ]; |
---|
| 280 | C = c[ 1 ] / c[ 4 ]; |
---|
| 281 | D = c[ 0 ] / c[ 4 ]; |
---|
| 282 | |
---|
| 283 | // substitute x = y - A/4 to eliminate cubic term: |
---|
| 284 | // x^4 + px^2 + qx + r = 0 |
---|
| 285 | |
---|
| 286 | sq_A = A * A; |
---|
| 287 | p = - 3.0/8 * sq_A + B; |
---|
| 288 | q = 1.0/8 * sq_A * A - 1.0/2 * A * B + C; |
---|
| 289 | r = - 3.0/256*sq_A*sq_A + 1.0/16*sq_A*B - 1.0/4*A*C + D; |
---|
| 290 | |
---|
| 291 | if (IsZero(r)) |
---|
| 292 | { |
---|
| 293 | // no absolute term: y(y^3 + py + q) = 0 |
---|
| 294 | |
---|
| 295 | coeffs[ 0 ] = q; |
---|
| 296 | coeffs[ 1 ] = p; |
---|
| 297 | coeffs[ 2 ] = 0; |
---|
| 298 | coeffs[ 3 ] = 1; |
---|
| 299 | |
---|
| 300 | num = SolveCubic(coeffs, s); |
---|
| 301 | |
---|
| 302 | s[ num++ ] = 0; |
---|
| 303 | } |
---|
| 304 | else |
---|
| 305 | { |
---|
| 306 | // solve the resolvent cubic ... |
---|
| 307 | coeffs[ 0 ] = 1.0/2 * r * p - 1.0/8 * q * q; |
---|
| 308 | coeffs[ 1 ] = - r; |
---|
| 309 | coeffs[ 2 ] = - 1.0/2 * p; |
---|
| 310 | coeffs[ 3 ] = 1; |
---|
| 311 | |
---|
| 312 | (void) SolveCubic(coeffs, s); |
---|
| 313 | |
---|
| 314 | // ... and take the one real solution ... |
---|
| 315 | z = s[ 0 ]; |
---|
| 316 | |
---|
| 317 | // ... to Build two quadric equations |
---|
| 318 | u = z * z - r; |
---|
| 319 | v = 2 * z - p; |
---|
| 320 | |
---|
| 321 | if (IsZero(u)) |
---|
| 322 | u = 0; |
---|
| 323 | else if (u > 0) |
---|
| 324 | u = std::sqrt(u); |
---|
| 325 | else |
---|
| 326 | return 0; |
---|
| 327 | |
---|
| 328 | if (IsZero(v)) |
---|
| 329 | v = 0; |
---|
| 330 | else if (v > 0) |
---|
| 331 | v = std::sqrt(v); |
---|
| 332 | else |
---|
| 333 | return 0; |
---|
| 334 | |
---|
| 335 | coeffs[ 0 ] = z - u; |
---|
| 336 | coeffs[ 1 ] = q < 0 ? -v : v; |
---|
| 337 | coeffs[ 2 ] = 1; |
---|
| 338 | |
---|
| 339 | num = SolveQuadric(coeffs, s); |
---|
| 340 | |
---|
| 341 | coeffs[ 0 ]= z + u; |
---|
| 342 | coeffs[ 1 ] = q < 0 ? v : -v; |
---|
| 343 | coeffs[ 2 ] = 1; |
---|
| 344 | |
---|
| 345 | num += SolveQuadric(coeffs, s + num); |
---|
| 346 | } |
---|
| 347 | |
---|
| 348 | // resubstitute |
---|
| 349 | |
---|
| 350 | sub = 1.0/4 * A; |
---|
| 351 | |
---|
| 352 | for (i = 0; i < num; ++i) |
---|
| 353 | s[ i ] -= sub; |
---|
| 354 | |
---|
| 355 | return num; |
---|
| 356 | } |
---|
| 357 | |
---|
| 358 | |
---|
| 359 | G4int G4ToroidalSurface::SolveCubic(G4double c[], G4double s[] ) |
---|
| 360 | { |
---|
| 361 | // From Graphics Gems I bu Jochen Schwartz |
---|
| 362 | G4int i, num; |
---|
| 363 | G4double sub; |
---|
| 364 | G4double A, B, C; |
---|
| 365 | G4double sq_A, p, q; |
---|
| 366 | G4double cb_p, D; |
---|
| 367 | |
---|
| 368 | // Normal form: x^3 + Ax^2 + Bx + C = 0 |
---|
| 369 | A = c[ 2 ] / c[ 3 ]; |
---|
| 370 | B = c[ 1 ] / c[ 3 ]; |
---|
| 371 | C = c[ 0 ] / c[ 3 ]; |
---|
| 372 | |
---|
| 373 | // substitute x = y - A/3 to eliminate quadric term: |
---|
| 374 | // x^3 +px + q = 0 |
---|
| 375 | sq_A = A * A; |
---|
| 376 | p = 1.0/3 * (- 1.0/3 * sq_A + B); |
---|
| 377 | q = 1.0/2 * (2.0/27 * A * sq_A - 1.0/3 * A * B + C); |
---|
| 378 | |
---|
| 379 | // use Cardano's formula |
---|
| 380 | cb_p = p * p * p; |
---|
| 381 | D = q * q + cb_p; |
---|
| 382 | |
---|
| 383 | if (IsZero(D)) |
---|
| 384 | { |
---|
| 385 | if (IsZero(q)) // one triple solution |
---|
| 386 | { |
---|
| 387 | s[ 0 ] = 0; |
---|
| 388 | num = 1; |
---|
| 389 | } |
---|
| 390 | else // one single and one G4double solution |
---|
| 391 | { |
---|
| 392 | G4double u = std::pow(-q,1./3.); |
---|
| 393 | s[ 0 ] = 2 * u; |
---|
| 394 | s[ 1 ] = - u; |
---|
| 395 | num = 2; |
---|
| 396 | } |
---|
| 397 | } |
---|
| 398 | else if (D < 0) // Casus irreducibilis: three real solutions |
---|
| 399 | { |
---|
| 400 | G4double phi = 1.0/3 * std::acos(-q / std::sqrt(-cb_p)); |
---|
| 401 | G4double t = 2 * std::sqrt(-p); |
---|
| 402 | |
---|
| 403 | s[ 0 ] = t * std::cos(phi); |
---|
| 404 | s[ 1 ] = - t * std::cos(phi + pi / 3); |
---|
| 405 | s[ 2 ] = - t * std::cos(phi - pi / 3); |
---|
| 406 | num = 3; |
---|
| 407 | } |
---|
| 408 | else // one real solution |
---|
| 409 | { |
---|
| 410 | G4double sqrt_D = std::sqrt(D); |
---|
| 411 | G4double u = std::pow(sqrt_D - q,1./3.); |
---|
| 412 | G4double v = - std::pow(sqrt_D + q,1./3.); |
---|
| 413 | |
---|
| 414 | s[ 0 ] = u + v; |
---|
| 415 | num = 1; |
---|
| 416 | } |
---|
| 417 | |
---|
| 418 | // resubstitute |
---|
| 419 | sub = 1.0/3 * A; |
---|
| 420 | |
---|
| 421 | for (i = 0; i < num; ++i) |
---|
| 422 | s[ i ] -= sub; |
---|
| 423 | |
---|
| 424 | return num; |
---|
| 425 | } |
---|
| 426 | |
---|
| 427 | |
---|
| 428 | G4int G4ToroidalSurface::SolveQuadric(G4double c[], G4double s[] ) |
---|
| 429 | { |
---|
| 430 | // From Graphics Gems I by Jochen Schwartz |
---|
| 431 | G4double p, q, D; |
---|
| 432 | |
---|
| 433 | // Normal form: x^2 + px + q = 0 |
---|
| 434 | p = c[ 1 ] / (2 * c[ 2 ]); |
---|
| 435 | q = c[ 0 ] / c[ 2 ]; |
---|
| 436 | |
---|
| 437 | D = p * p - q; |
---|
| 438 | |
---|
| 439 | if (IsZero(D)) |
---|
| 440 | { |
---|
| 441 | s[ 0 ] = - p; |
---|
| 442 | return 1; |
---|
| 443 | } |
---|
| 444 | else if (D < 0) |
---|
| 445 | { |
---|
| 446 | return 0; |
---|
| 447 | } |
---|
| 448 | else if (D > 0) |
---|
| 449 | { |
---|
| 450 | G4double sqrt_D = std::sqrt(D); |
---|
| 451 | |
---|
| 452 | s[ 0 ] = sqrt_D - p; |
---|
| 453 | s[ 1 ] = - sqrt_D - p; |
---|
| 454 | return 2; |
---|
| 455 | } |
---|
| 456 | |
---|
| 457 | return 0; |
---|
| 458 | } |
---|