| Geant4 Cross Reference |
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 // class G4GeomTools implementation
27 //
28 // 10.10.2016, E.Tcherniaev: initial version.
29 // --------------------------------------------------------------------
30
31 #include "G4GeomTools.hh"
32
33 #include "geomdefs.hh"
34 #include "G4SystemOfUnits.hh"
35 #include "G4GeometryTolerance.hh"
36
37 ///////////////////////////////////////////////////////////////////////
38 //
39 // Calculate area of a triangle in 2D
40
41 G4double G4GeomTools::TriangleArea(G4double Ax, G4double Ay,
42 G4double Bx, G4double By,
43 G4double Cx, G4double Cy)
44 {
45 return ((Bx-Ax)*(Cy-Ay) - (By-Ay)*(Cx-Ax))*0.5;
46 }
47
48 ///////////////////////////////////////////////////////////////////////
49 //
50 // Calculate area of a triangle in 2D
51
52 G4double G4GeomTools::TriangleArea(const G4TwoVector& A,
53 const G4TwoVector& B,
54 const G4TwoVector& C)
55 {
56 G4double Ax = A.x(), Ay = A.y();
57 return ((B.x()-Ax)*(C.y()-Ay) - (B.y()-Ay)*(C.x()-Ax))*0.5;
58 }
59
60 ///////////////////////////////////////////////////////////////////////
61 //
62 // Calculate area of a quadrilateral in 2D
63
64 G4double G4GeomTools::QuadArea(const G4TwoVector& A,
65 const G4TwoVector& B,
66 const G4TwoVector& C,
67 const G4TwoVector& D)
68 {
69 return ((C.x()-A.x())*(D.y()-B.y()) - (C.y()-A.y())*(D.x()-B.x()))*0.5;
70 }
71
72 ///////////////////////////////////////////////////////////////////////
73 //
74 // Calculate area of a polygon in 2D
75
76 G4double G4GeomTools::PolygonArea(const G4TwoVectorList& p)
77 {
78 auto n = (G4int)p.size();
79 if (n < 3) return 0.0; // degenerate polygon
80 G4double area = p[n-1].x()*p[0].y() - p[0].x()*p[n-1].y();
81 for(G4int i=1; i<n; ++i)
82 {
83 area += p[i-1].x()*p[i].y() - p[i].x()*p[i-1].y();
84 }
85 return area*0.5;
86 }
87
88 ///////////////////////////////////////////////////////////////////////
89 //
90 // Point inside 2D triangle
91
92 G4bool G4GeomTools::PointInTriangle(G4double Ax, G4double Ay,
93 G4double Bx, G4double By,
94 G4double Cx, G4double Cy,
95 G4double Px, G4double Py)
96
97 {
98 if ((Bx-Ax)*(Cy-Ay) - (By-Ay)*(Cx-Ax) > 0.)
99 {
100 if ((Ax-Cx)*(Py-Cy) - (Ay-Cy)*(Px-Cx) < 0.) return false;
101 if ((Bx-Ax)*(Py-Ay) - (By-Ay)*(Px-Ax) < 0.) return false;
102 if ((Cx-Bx)*(Py-By) - (Cy-By)*(Px-Bx) < 0.) return false;
103 }
104 else
105 {
106 if ((Ax-Cx)*(Py-Cy) - (Ay-Cy)*(Px-Cx) > 0.) return false;
107 if ((Bx-Ax)*(Py-Ay) - (By-Ay)*(Px-Ax) > 0.) return false;
108 if ((Cx-Bx)*(Py-By) - (Cy-By)*(Px-Bx) > 0.) return false;
109 }
110 return true;
111 }
112
113 ///////////////////////////////////////////////////////////////////////
114 //
115 // Point inside 2D triangle
116
117 G4bool G4GeomTools::PointInTriangle(const G4TwoVector& A,
118 const G4TwoVector& B,
119 const G4TwoVector& C,
120 const G4TwoVector& P)
121 {
122 G4double Ax = A.x(), Ay = A.y();
123 G4double Bx = B.x(), By = B.y();
124 G4double Cx = C.x(), Cy = C.y();
125 G4double Px = P.x(), Py = P.y();
126 if ((Bx-Ax)*(Cy-Ay) - (By-Ay)*(Cx-Ax) > 0.)
127 {
128 if ((Ax-Cx)*(Py-Cy) - (Ay-Cy)*(Px-Cx) < 0.) return false;
129 if ((Bx-Ax)*(Py-Ay) - (By-Ay)*(Px-Ax) < 0.) return false;
130 if ((Cx-Bx)*(Py-By) - (Cy-By)*(Px-Bx) < 0.) return false;
131 }
132 else
133 {
134 if ((Ax-Cx)*(Py-Cy) - (Ay-Cy)*(Px-Cx) > 0.) return false;
135 if ((Bx-Ax)*(Py-Ay) - (By-Ay)*(Px-Ax) > 0.) return false;
136 if ((Cx-Bx)*(Py-By) - (Cy-By)*(Px-Bx) > 0.) return false;
137 }
138 return true;
139 }
140
141 ///////////////////////////////////////////////////////////////////////
142 //
143 // Point inside 2D polygon
144
145 G4bool G4GeomTools::PointInPolygon(const G4TwoVector& p,
146 const G4TwoVectorList& v)
147 {
148 auto Nv = (G4int)v.size();
149 G4bool in = false;
150 for (G4int i = 0, k = Nv - 1; i < Nv; k = i++)
151 {
152 if ((v[i].y() > p.y()) != (v[k].y() > p.y()))
153 {
154 G4double ctg = (v[k].x()-v[i].x())/(v[k].y()-v[i].y());
155 in ^= static_cast<int>(p.x() < (p.y()-v[i].y())*ctg + v[i].x());
156 }
157 }
158 return in;
159 }
160
161 ///////////////////////////////////////////////////////////////////////
162 //
163 // Detemine whether 2D polygon is convex or not
164
165 G4bool G4GeomTools::IsConvex(const G4TwoVectorList& polygon)
166 {
167 static const G4double kCarTolerance =
168 G4GeometryTolerance::GetInstance()->GetSurfaceTolerance();
169
170 G4bool gotNegative = false;
171 G4bool gotPositive = false;
172 auto n = (G4int)polygon.size();
173 if (n <= 0) return false;
174 for (G4int icur=0; icur<n; ++icur)
175 {
176 G4int iprev = (icur == 0) ? n-1 : icur-1;
177 G4int inext = (icur == n-1) ? 0 : icur+1;
178 G4TwoVector e1 = polygon[icur] - polygon[iprev];
179 G4TwoVector e2 = polygon[inext] - polygon[icur];
180 G4double cross = e1.x()*e2.y() - e1.y()*e2.x();
181 if (std::abs(cross) < kCarTolerance) return false;
182 if (cross < 0) gotNegative = true;
183 if (cross > 0) gotPositive = true;
184 if (gotNegative && gotPositive) return false;
185 }
186 return true;
187 }
188
189 ///////////////////////////////////////////////////////////////////////
190 //
191 // Triangulate simple polygon
192
193 G4bool G4GeomTools::TriangulatePolygon(const G4TwoVectorList& polygon,
194 G4TwoVectorList& result)
195 {
196 result.resize(0);
197 std::vector<G4int> triangles;
198 G4bool reply = TriangulatePolygon(polygon,triangles);
199
200 auto n = (G4int)triangles.size();
201 for (G4int i=0; i<n; ++i) result.push_back(polygon[triangles[i]]);
202 return reply;
203 }
204
205 ///////////////////////////////////////////////////////////////////////
206 //
207 // Triangulation of a simple polygon by "ear clipping"
208
209 G4bool G4GeomTools::TriangulatePolygon(const G4TwoVectorList& polygon,
210 std::vector<G4int>& result)
211 {
212 result.resize(0);
213
214 // allocate and initialize list of Vertices in polygon
215 //
216 auto n = (G4int)polygon.size();
217 if (n < 3) return false;
218
219 // we want a counter-clockwise polygon in V
220 //
221 G4double area = G4GeomTools::PolygonArea(polygon);
222 auto V = new G4int[n];
223 if (area > 0.)
224 for (G4int i=0; i<n; ++i) V[i] = i;
225 else
226 for (G4int i=0; i<n; ++i) V[i] = (n-1)-i;
227
228 // Triangulation: remove nv-2 Vertices, creating 1 triangle every time
229 //
230 G4int nv = n;
231 G4int count = 2*nv; // error detection counter
232 for(G4int b=nv-1; nv>2; )
233 {
234 // ERROR: if we loop, it is probably a non-simple polygon
235 if ((count--) <= 0)
236 {
237 delete [] V;
238 if (area < 0.) std::reverse(result.begin(),result.end());
239 return false;
240 }
241
242 // three consecutive vertices in current polygon, <a,b,c>
243 G4int a = (b < nv) ? b : 0; // previous
244 b = (a+1 < nv) ? a+1 : 0; // current
245 G4int c = (b+1 < nv) ? b+1 : 0; // next
246
247 if (CheckSnip(polygon, a,b,c, nv,V))
248 {
249 // output Triangle
250 result.push_back(V[a]);
251 result.push_back(V[b]);
252 result.push_back(V[c]);
253
254 // remove vertex b from remaining polygon
255 nv--;
256 for(G4int i=b; i<nv; ++i) V[i] = V[i+1];
257
258 count = 2*nv; // resest error detection counter
259 }
260 }
261 delete [] V;
262 if (area < 0.) std::reverse(result.begin(),result.end());
263 return true;
264 }
265
266 ///////////////////////////////////////////////////////////////////////
267 //
268 // Helper function for "ear clipping" polygon triangulation.
269 // Check for a valid snip
270
271 G4bool G4GeomTools::CheckSnip(const G4TwoVectorList& contour,
272 G4int a, G4int b, G4int c,
273 G4int n, const G4int* V)
274 {
275 static const G4double kCarTolerance =
276 G4GeometryTolerance::GetInstance()->GetSurfaceTolerance();
277
278 // check orientation of Triangle
279 G4double Ax = contour[V[a]].x(), Ay = contour[V[a]].y();
280 G4double Bx = contour[V[b]].x(), By = contour[V[b]].y();
281 G4double Cx = contour[V[c]].x(), Cy = contour[V[c]].y();
282 if ((Bx-Ax)*(Cy-Ay) - (By-Ay)*(Cx-Ax) < kCarTolerance) return false;
283
284 // check that there is no point inside Triangle
285 G4double xmin = std::min(std::min(Ax,Bx),Cx);
286 G4double xmax = std::max(std::max(Ax,Bx),Cx);
287 G4double ymin = std::min(std::min(Ay,By),Cy);
288 G4double ymax = std::max(std::max(Ay,By),Cy);
289 for (G4int i=0; i<n; ++i)
290 {
291 if((i == a) || (i == b) || (i == c)) continue;
292 G4double Px = contour[V[i]].x();
293 if (Px < xmin || Px > xmax) continue;
294 G4double Py = contour[V[i]].y();
295 if (Py < ymin || Py > ymax) continue;
296 if (PointInTriangle(Ax,Ay,Bx,By,Cx,Cy,Px,Py)) return false;
297 }
298 return true;
299 }
300
301 ///////////////////////////////////////////////////////////////////////
302 //
303 // Remove collinear and coincident points from 2D polygon
304
305 void G4GeomTools::RemoveRedundantVertices(G4TwoVectorList& polygon,
306 std::vector<G4int>& iout,
307 G4double tolerance)
308 {
309 iout.resize(0);
310 // set tolerance squared
311 G4double delta = sqr(tolerance);
312 // set special value to mark vertices for removal
313 G4double removeIt = kInfinity;
314
315 auto nv = (G4int)polygon.size();
316
317 // Main loop: check every three consecutive points, if the points
318 // are collinear then mark middle point for removal
319 //
320 G4int icur = 0, iprev = 0, inext = 0, nout = 0;
321 for (G4int i=0; i<nv; ++i)
322 {
323 icur = i; // index of current point
324
325 for (G4int k=1; k<nv+1; ++k) // set index of previous point
326 {
327 iprev = icur - k;
328 if (iprev < 0) iprev += nv;
329 if (polygon[iprev].x() != removeIt) break;
330 }
331
332 for (G4int k=1; k<nv+1; ++k) // set index of next point
333 {
334 inext = icur + k;
335 if (inext >= nv) inext -= nv;
336 if (polygon[inext].x() != removeIt) break;
337 }
338
339 if (iprev == inext) break; // degenerate polygon, stop
340
341 // Calculate parameters of triangle (iprev->icur->inext),
342 // if triangle is too small or too narrow then mark current
343 // point for removal
344 G4TwoVector e1 = polygon[iprev] - polygon[icur];
345 G4TwoVector e2 = polygon[inext] - polygon[icur];
346
347 // Check length of edges, then check height of the triangle
348 G4double leng1 = e1.mag2();
349 G4double leng2 = e2.mag2();
350 G4double leng3 = (e2-e1).mag2();
351 if (leng1 <= delta || leng2 <= delta || leng3 <= delta)
352 {
353 polygon[icur].setX(removeIt); ++nout;
354 }
355 else
356 {
357 G4double lmax = std::max(std::max(leng1,leng2),leng3);
358 G4double area = std::abs(e1.x()*e2.y()-e1.y()*e2.x())*0.5;
359 if (area/std::sqrt(lmax) <= std::abs(tolerance))
360 {
361 polygon[icur].setX(removeIt); ++nout;
362 }
363 }
364 }
365
366 // Remove marked points
367 //
368 icur = 0;
369 if (nv - nout < 3) // degenerate polygon, remove all points
370 {
371 for (G4int i=0; i<nv; ++i) iout.push_back(i);
372 polygon.resize(0);
373 nv = 0;
374 }
375 for (G4int i=0; i<nv; ++i) // move points, if required
376 {
377 if (polygon[i].x() != removeIt)
378 polygon[icur++] = polygon[i];
379 else
380 iout.push_back(i);
381 }
382 if (icur < nv) polygon.resize(icur);
383 return;
384 }
385
386 ///////////////////////////////////////////////////////////////////////
387 //
388 // Find bounding rectangle of a disk sector
389
390 G4bool G4GeomTools::DiskExtent(G4double rmin, G4double rmax,
391 G4double startPhi, G4double delPhi,
392 G4TwoVector& pmin, G4TwoVector& pmax)
393 {
394 static const G4double kCarTolerance =
395 G4GeometryTolerance::GetInstance()->GetSurfaceTolerance();
396
397 // check parameters
398 //
399 pmin.set(0,0);
400 pmax.set(0,0);
401 if (rmin < 0) return false;
402 if (rmax <= rmin + kCarTolerance) return false;
403 if (delPhi <= 0 + kCarTolerance) return false;
404
405 // calculate extent
406 //
407 pmin.set(-rmax,-rmax);
408 pmax.set( rmax, rmax);
409 if (delPhi >= CLHEP::twopi) return true;
410
411 DiskExtent(rmin,rmax,
412 std::sin(startPhi),std::cos(startPhi),
413 std::sin(startPhi+delPhi),std::cos(startPhi+delPhi),
414 pmin,pmax);
415 return true;
416 }
417
418 ///////////////////////////////////////////////////////////////////////
419 //
420 // Find bounding rectangle of a disk sector, fast version.
421 // No check of parameters !!!
422
423 void G4GeomTools::DiskExtent(G4double rmin, G4double rmax,
424 G4double sinStart, G4double cosStart,
425 G4double sinEnd, G4double cosEnd,
426 G4TwoVector& pmin, G4TwoVector& pmax)
427 {
428 static const G4double kCarTolerance =
429 G4GeometryTolerance::GetInstance()->GetSurfaceTolerance();
430
431 // check if 360 degrees
432 //
433 pmin.set(-rmax,-rmax);
434 pmax.set( rmax, rmax);
435
436 if (std::abs(sinEnd-sinStart) < kCarTolerance &&
437 std::abs(cosEnd-cosStart) < kCarTolerance) return;
438
439 // get start and end quadrants
440 //
441 // 1 | 0
442 // ---+---
443 // 3 | 2
444 //
445 G4int icase = (cosEnd < 0) ? 1 : 0;
446 if (sinEnd < 0) icase += 2;
447 if (cosStart < 0) icase += 4;
448 if (sinStart < 0) icase += 8;
449
450 switch (icase)
451 {
452 // start quadrant 0
453 case 0: // start->end : 0->0
454 if (sinEnd < sinStart) break;
455 pmin.set(rmin*cosEnd,rmin*sinStart);
456 pmax.set(rmax*cosStart,rmax*sinEnd );
457 break;
458 case 1: // start->end : 0->1
459 pmin.set(rmax*cosEnd,std::min(rmin*sinStart,rmin*sinEnd));
460 pmax.set(rmax*cosStart,rmax );
461 break;
462 case 2: // start->end : 0->2
463 pmin.set(-rmax,-rmax);
464 pmax.set(std::max(rmax*cosStart,rmax*cosEnd),rmax);
465 break;
466 case 3: // start->end : 0->3
467 pmin.set(-rmax,rmax*sinEnd);
468 pmax.set(rmax*cosStart,rmax);
469 break;
470 // start quadrant 1
471 case 4: // start->end : 1->0
472 pmin.set(-rmax,-rmax);
473 pmax.set(rmax,std::max(rmax*sinStart,rmax*sinEnd));
474 break;
475 case 5: // start->end : 1->1
476 if (sinEnd > sinStart) break;
477 pmin.set(rmax*cosEnd,rmin*sinEnd );
478 pmax.set(rmin*cosStart,rmax*sinStart);
479 break;
480 case 6: // start->end : 1->2
481 pmin.set(-rmax,-rmax);
482 pmax.set(rmax*cosEnd,rmax*sinStart);
483 break;
484 case 7: // start->end : 1->3
485 pmin.set(-rmax,rmax*sinEnd);
486 pmax.set(std::max(rmin*cosStart,rmin*cosEnd),rmax*sinStart);
487 break;
488 // start quadrant 2
489 case 8: // start->end : 2->0
490 pmin.set(std::min(rmin*cosStart,rmin*cosEnd),rmax*sinStart);
491 pmax.set(rmax,rmax*sinEnd);
492 break;
493 case 9: // start->end : 2->1
494 pmin.set(rmax*cosEnd,rmax*sinStart);
495 pmax.set(rmax,rmax);
496 break;
497 case 10: // start->end : 2->2
498 if (sinEnd < sinStart) break;
499 pmin.set(rmin*cosStart,rmax*sinStart);
500 pmax.set(rmax*cosEnd,rmin*sinEnd );
501 break;
502 case 11: // start->end : 2->3
503 pmin.set(-rmax,std::min(rmax*sinStart,rmax*sinEnd));
504 pmax.set(rmax,rmax);
505 break;
506 // start quadrant 3
507 case 12: // start->end : 3->0
508 pmin.set(rmax*cosStart,-rmax);
509 pmax.set(rmax,rmax*sinEnd);
510 break;
511 case 13: // start->end : 3->1
512 pmin.set(std::min(rmax*cosStart,rmax*cosEnd),-rmax);
513 pmax.set(rmax,rmax);
514 break;
515 case 14: // start->end : 3->2
516 pmin.set(rmax*cosStart,-rmax);
517 pmax.set(rmax*cosEnd,std::max(rmin*sinStart,rmin*sinEnd));
518 break;
519 case 15: // start->end : 3->3
520 if (sinEnd > sinStart) break;
521 pmin.set(rmax*cosStart,rmax*sinEnd);
522 pmax.set(rmin*cosEnd,rmin*sinStart);
523 break;
524 }
525 return;
526 }
527
528 ///////////////////////////////////////////////////////////////////////
529 //
530 // Compute the circumference (perimeter) of an ellipse
531
532 G4double G4GeomTools::EllipsePerimeter(G4double pA, G4double pB)
533 {
534 G4double x = std::abs(pA);
535 G4double y = std::abs(pB);
536 G4double a = std::max(x,y);
537 G4double b = std::min(x,y);
538 G4double e = std::sqrt((1. - b/a)*(1. + b/a));
539 return 4. * a * comp_ellint_2(e);
540 }
541
542 ///////////////////////////////////////////////////////////////////////
543 //
544 // Compute the lateral surface area of an elliptic cone
545
546 G4double G4GeomTools::EllipticConeLateralArea(G4double pA,
547 G4double pB,
548 G4double pH)
549 {
550 G4double x = std::abs(pA);
551 G4double y = std::abs(pB);
552 G4double h = std::abs(pH);
553 G4double a = std::max(x,y);
554 G4double b = std::min(x,y);
555 G4double e = std::sqrt((1. - b/a)*(1. + b/a)) / std::hypot(1.,b/h);
556 return 2. * a * std::hypot(b,h) * comp_ellint_2(e);
557 }
558
559 ///////////////////////////////////////////////////////////////////////
560 //
561 // Compute Elliptical Integral of the Second Kind
562 //
563 // The algorithm is based upon Carlson B.C., "Computation of real
564 // or complex elliptic integrals", Numerical Algorithms,
565 // Volume 10, Issue 1, 1995 (see equations 2.36 - 2.39)
566 //
567 // The code was adopted from C code at:
568 // http://paulbourke.net/geometry/ellipsecirc/
569
570 G4double G4GeomTools::comp_ellint_2(G4double e)
571 {
572 const G4double eps = 1. / 134217728.; // 1 / 2^27
573
574 G4double a = 1.;
575 G4double b = std::sqrt((1. - e)*(1. + e));
576 if (b == 1.) return CLHEP::halfpi;
577 if (b == 0.) return 1.;
578
579 G4double x = 1.;
580 G4double y = b;
581 G4double S = 0.;
582 G4double M = 1.;
583 while (x - y > eps*y)
584 {
585 G4double tmp = (x + y) * 0.5;
586 y = std::sqrt(x*y);
587 x = tmp;
588 M += M;
589 S += M * (x - y)*(x - y);
590 }
591 return 0.5 * CLHEP::halfpi * ((a + b)*(a + b) - S) / (x + y);
592 }
593
594 ///////////////////////////////////////////////////////////////////////
595 //
596 // Calcuate area of a triangle in 3D
597
598 G4ThreeVector G4GeomTools::TriangleAreaNormal(const G4ThreeVector& A,
599 const G4ThreeVector& B,
600 const G4ThreeVector& C)
601 {
602 return ((B-A).cross(C-A))*0.5;
603 }
604
605 ///////////////////////////////////////////////////////////////////////
606 //
607 // Calcuate area of a quadrilateral in 3D
608
609 G4ThreeVector G4GeomTools::QuadAreaNormal(const G4ThreeVector& A,
610 const G4ThreeVector& B,
611 const G4ThreeVector& C,
612 const G4ThreeVector& D)
613 {
614 return ((C-A).cross(D-B))*0.5;
615 }
616
617 ///////////////////////////////////////////////////////////////////////
618 //
619 // Calculate area of a polygon in 3D
620
621 G4ThreeVector G4GeomTools::PolygonAreaNormal(const G4ThreeVectorList& p)
622 {
623 auto n = (G4int)p.size();
624 if (n < 3) return {0,0,0}; // degerate polygon
625 G4ThreeVector normal = p[n-1].cross(p[0]);
626 for(G4int i=1; i<n; ++i)
627 {
628 normal += p[i-1].cross(p[i]);
629 }
630 return normal*0.5;
631 }
632
633 ///////////////////////////////////////////////////////////////////////
634 //
635 // Calculate distance between point P and line segment AB in 3D
636
637 G4double G4GeomTools::DistancePointSegment(const G4ThreeVector& P,
638 const G4ThreeVector& A,
639 const G4ThreeVector& B)
640 {
641 G4ThreeVector AP = P - A;
642 G4ThreeVector AB = B - A;
643
644 G4double u = AP.dot(AB);
645 if (u <= 0) return AP.mag(); // closest point is A
646
647 G4double len2 = AB.mag2();
648 if (u >= len2) return (B-P).mag(); // closest point is B
649
650 return ((u/len2)*AB - AP).mag(); // distance to line
651 }
652
653 ///////////////////////////////////////////////////////////////////////
654 //
655 // Find closest point on line segment in 3D
656
657 G4ThreeVector
658 G4GeomTools::ClosestPointOnSegment(const G4ThreeVector& P,
659 const G4ThreeVector& A,
660 const G4ThreeVector& B)
661 {
662 G4ThreeVector AP = P - A;
663 G4ThreeVector AB = B - A;
664
665 G4double u = AP.dot(AB);
666 if (u <= 0) return A; // closest point is A
667
668 G4double len2 = AB.mag2();
669 if (u >= len2) return B; // closest point is B
670
671 G4double t = u/len2;
672 return A + t*AB; // closest point on segment
673 }
674
675 ///////////////////////////////////////////////////////////////////////
676 //
677 // Find closest point on triangle in 3D.
678 //
679 // The implementation is based on the algorithm published in
680 // "Geometric Tools for Computer Graphics", Philip J Scheider and
681 // David H Eberly, Elsevier Science (USA), 2003.
682 //
683 // The algorithm is also available at:
684 // http://www.geometrictools.com/Documentation/DistancePoint3Triangle3.pdf
685
686 G4ThreeVector
687 G4GeomTools::ClosestPointOnTriangle(const G4ThreeVector& P,
688 const G4ThreeVector& A,
689 const G4ThreeVector& B,
690 const G4ThreeVector& C)
691 {
692 G4ThreeVector diff = A - P;
693 G4ThreeVector edge0 = B - A;
694 G4ThreeVector edge1 = C - A;
695
696 G4double a = edge0.mag2();
697 G4double b = edge0.dot(edge1);
698 G4double c = edge1.mag2();
699 G4double d = diff.dot(edge0);
700 G4double e = diff.dot(edge1);
701
702 G4double det = a*c - b*b;
703 G4double t0 = b*e - c*d;
704 G4double t1 = b*d - a*e;
705
706 /*
707 ^ t1
708 \ 2 |
709 \ |
710 \ | regions
711 \|
712 C
713 |\
714 3 | \ 1
715 | \
716 | 0 \
717 | \
718 ---- A --- B ----> t0
719 | \
720 4 | 5 \ 6
721 | \
722 */
723
724 G4int region = -1;
725 if (t0+t1 <= det)
726 region = (t0 < 0) ? ((t1 < 0) ? 4 : 3) : ((t1 < 0) ? 5 : 0);
727 else
728 region = (t0 < 0) ? 2 : ((t1 < 0) ? 6 : 1);
729
730 switch (region)
731 {
732 case 0: // interior of triangle
733 {
734 G4double invDet = 1./det;
735 return A + (t0*invDet)*edge0 + (t1*invDet)*edge1;
736 }
737 case 1: // edge BC
738 {
739 G4double numer = c + e - b - d;
740 if (numer <= 0) return C;
741 G4double denom = a - 2*b + c;
742 return (numer >= denom) ? B : C + (numer/denom)*(edge0-edge1);
743 }
744 case 2: // edge AC or BC
745 {
746 G4double tmp0 = b + d;
747 G4double tmp1 = c + e;
748 if (tmp1 > tmp0)
749 {
750 G4double numer = tmp1 - tmp0;
751 G4double denom = a - 2*b + c;
752 return (numer >= denom) ? B : C + (numer/denom)*(edge0-edge1);
753 }
754 // same: (e >= 0) ? A : ((-e >= c) ? C : A + (-e/c)*edge1)
755 return (tmp1 <= 0) ? C : (( e >= 0) ? A : A + (-e/c)*edge1);
756 }
757 case 3: // edge AC
758 return (e >= 0) ? A : ((-e >= c) ? C : A + (-e/c)*edge1);
759
760 case 4: // edge AB or AC
761 if (d < 0) return (-d >= a) ? B : A + (-d/a)*edge0;
762 return (e >= 0) ? A : ((-e >= c) ? C : A + (-e/c)*edge1);
763
764 case 5: // edge AB
765 return (d >= 0) ? A : ((-d >= a) ? B : A + (-d/a)*edge0);
766
767 case 6: // edge AB or BC
768 {
769 G4double tmp0 = b + e;
770 G4double tmp1 = a + d;
771 if (tmp1 > tmp0)
772 {
773 G4double numer = tmp1 - tmp0;
774 G4double denom = a - 2*b + c;
775 return (numer >= denom) ? C : B + (numer/denom)*(edge1-edge0);
776 }
777 // same: (d >= 0) ? A : ((-d >= a) ? B : A + (-d/a)*edge0)
778 return (tmp1 <= 0) ? B : (( d >= 0) ? A : A + (-d/a)*edge0);
779 }
780 default: // impossible case
781 return {kInfinity,kInfinity,kInfinity};
782 }
783 }
784
785 ///////////////////////////////////////////////////////////////////////
786 //
787 // Calculate bounding box of a spherical sector
788
789 G4bool
790 G4GeomTools::SphereExtent(G4double rmin, G4double rmax,
791 G4double startTheta, G4double delTheta,
792 G4double startPhi, G4double delPhi,
793 G4ThreeVector& pmin, G4ThreeVector& pmax)
794 {
795 static const G4double kCarTolerance =
796 G4GeometryTolerance::GetInstance()->GetSurfaceTolerance();
797
798 // check parameters
799 //
800 pmin.set(0,0,0);
801 pmax.set(0,0,0);
802 if (rmin < 0) return false;
803 if (rmax <= rmin + kCarTolerance) return false;
804 if (delTheta <= 0 + kCarTolerance) return false;
805 if (delPhi <= 0 + kCarTolerance) return false;
806
807 G4double stheta = startTheta;
808 G4double dtheta = delTheta;
809 if (stheta < 0 && stheta > CLHEP::pi) return false;
810 if (stheta + dtheta > CLHEP::pi) dtheta = CLHEP::pi - stheta;
811 if (dtheta <= 0 + kCarTolerance) return false;
812
813 // calculate extent
814 //
815 pmin.set(-rmax,-rmax,-rmax);
816 pmax.set( rmax, rmax, rmax);
817 if (dtheta >= CLHEP::pi && delPhi >= CLHEP::twopi) return true;
818
819 G4double etheta = stheta + dtheta;
820 G4double sinStart = std::sin(stheta);
821 G4double cosStart = std::cos(stheta);
822 G4double sinEnd = std::sin(etheta);
823 G4double cosEnd = std::cos(etheta);
824
825 G4double rhomin = rmin*std::min(sinStart,sinEnd);
826 G4double rhomax = rmax;
827 if (stheta > CLHEP::halfpi) rhomax = rmax*sinStart;
828 if (etheta < CLHEP::halfpi) rhomax = rmax*sinEnd;
829
830 G4TwoVector xymin,xymax;
831 DiskExtent(rhomin,rhomax,
832 std::sin(startPhi),std::cos(startPhi),
833 std::sin(startPhi+delPhi),std::cos(startPhi+delPhi),
834 xymin,xymax);
835
836 G4double zmin = std::min(rmin*cosEnd,rmax*cosEnd);
837 G4double zmax = std::max(rmin*cosStart,rmax*cosStart);
838 pmin.set(xymin.x(),xymin.y(),zmin);
839 pmax.set(xymax.x(),xymax.y(),zmax);
840 return true;
841 }
842