42 double pRmin1,
double pRmax1,
43 double pRmin2,
double pRmax2,
45 double pSPhi,
double pDPhi)
46 :
VUSolid(pName.c_str()), fRmin1(pRmin1), fRmin2(pRmin2),
47 fRmax1(pRmax1), fRmax2(pRmax2), fDz(pDz), fSPhi(0.), fDPhi(0.)
55 std::ostringstream message;
56 message <<
"Invalid Z half-length for Solid: " <<
GetName() << std::endl
64 if (((pRmin1 >= pRmax1) || (pRmin2 >= pRmax2) || (pRmin1 < 0)) && (pRmin2 < 0))
66 std::ostringstream message;
67 message <<
"Invalid values of radii for Solid: " <<
GetName() << std::endl
68 <<
" pRmin1 = " << pRmin1 <<
", pRmin2 = " << pRmin2
69 <<
", pRmax1 = " << pRmax1 <<
", pRmax2 = " << pRmax2;
73 if ((pRmin1 == 0.0) && (pRmin2 > 0.0))
75 fRmin1 = 1e3 * kRadTolerance;
77 if ((pRmin2 == 0.0) && (pRmin1 > 0.0))
79 fRmin2 = 1e3 * kRadTolerance;
84 CheckPhiAngles(pSPhi, pDPhi);
95 :
VUSolid(
""), kRadTolerance(0.), kAngTolerance(0.),
96 fRmin1(0.), fRmin2(0.), fRmax1(0.), fRmax2(0.), fDz(0.),
97 fSPhi(0.), fDPhi(0.), sinCPhi(0.), cosCPhi(0.), cosHDPhiOT(0.),
98 cosHDPhiIT(0.), sinSPhi(0.), cosSPhi(0.), sinEPhi(0.), cosEPhi(0.),
117 :
VUSolid(rhs), kRadTolerance(rhs.kRadTolerance),
118 kAngTolerance(rhs.kAngTolerance), fRmin1(rhs.fRmin1), fRmin2(rhs.fRmin2),
119 fRmax1(rhs.fRmax1), fRmax2(rhs.fRmax2), fDz(rhs.fDz), fSPhi(rhs.fSPhi),
120 fDPhi(rhs.fDPhi), sinCPhi(rhs.sinCPhi), cosCPhi(rhs.cosCPhi),
121 cosHDPhiOT(rhs.cosHDPhiOT), cosHDPhiIT(rhs.cosHDPhiIT),
122 sinSPhi(rhs.sinSPhi), cosSPhi(rhs.cosSPhi), sinEPhi(rhs.sinEPhi),
123 cosEPhi(rhs.cosEPhi), fPhiFullCone(rhs.fPhiFullCone)
143 VUSolid::operator=(rhs);
147 kRadTolerance = rhs.kRadTolerance;
148 kAngTolerance = rhs.kAngTolerance;
156 sinCPhi = rhs.sinCPhi;
157 cosCPhi = rhs.cosCPhi;
158 cosHDPhiOT = rhs.cosHDPhiOT;
159 cosHDPhiIT = rhs.cosHDPhiIT;
160 sinSPhi = rhs.sinSPhi;
161 cosSPhi = rhs.cosSPhi;
162 sinEPhi = rhs.sinEPhi;
163 cosEPhi = rhs.cosEPhi;
164 fPhiFullCone = rhs.fPhiFullCone;
180 double distZ, distRMin, distRMax;
181 double distSPhi = UUtils::kInfinity, distEPhi = UUtils::kInfinity;
182 double pRMin, widRMin;
183 double pRMax, widRMax;
186 static const double dAngle = 0.5 * kAngTolerance;
189 UVector3 nR, nr(0., 0., 0.), nPs, nPe;
191 distZ = std::fabs(std::fabs(p.
z) - fDz);
192 rho = std::sqrt(p.
x * p.
x + p.
y * p.
y);
194 pRMin = rho - p.
z * tanRMin;
195 widRMin = fRmin2 - fDz * tanRMin;
196 distRMin = std::fabs(pRMin - widRMin) / secRMin;
198 pRMax = rho - p.
z * tanRMax;
199 widRMax = fRmax2 - fDz * tanRMax;
200 distRMax = std::fabs(pRMax - widRMax) / secRMax;
206 pPhi = std::atan2(p.
y, p.
x);
208 if (pPhi < fSPhi - delta)
210 pPhi += 2 * UUtils::kPi;
212 else if (pPhi > fSPhi + fDPhi + delta)
214 pPhi -= 2 * UUtils::kPi;
217 distSPhi = std::fabs(pPhi - fSPhi);
218 distEPhi = std::fabs(pPhi - fSPhi - fDPhi);
220 else if (!(fRmin1) || !(fRmin2))
225 nPs =
UVector3(std::sin(fSPhi), -std::cos(fSPhi), 0);
226 nPe =
UVector3(-std::sin(fSPhi + fDPhi), std::cos(fSPhi + fDPhi), 0);
230 nR =
UVector3(p.
x / rho / secRMax, p.
y / rho / secRMax, -tanRMax / secRMax);
231 if (fRmin1 || fRmin2)
233 nr =
UVector3(-p.
x / rho / secRMin, -p.
y / rho / secRMin, tanRMin / secRMin);
237 if (distRMax <= delta)
242 if ((fRmin1 || fRmin2) && (distRMin <= delta))
249 if (distSPhi <= dAngle)
254 if (distEPhi <= dAngle)
277 Warning, 1,
"Point p is not on surface !?");
279 norm = ApproxSurfaceNormal(p);
281 else if (noSurfaces == 1)
287 norm = sumnorm.
Unit();
292 return (
bool) noSurfaces;
305 double distZ, distRMin, distRMax, distSPhi, distEPhi, distMin;
306 double pRMin, widRMin;
307 double pRMax, widRMax;
309 distZ = std::fabs(std::fabs(p.
z) - fDz);
310 rho = std::sqrt(p.
x * p.
x + p.
y * p.
y);
312 pRMin = rho - p.
z * tanRMin;
313 widRMin = fRmin2 - fDz * tanRMin;
314 distRMin = std::fabs(pRMin - widRMin) / secRMin;
316 pRMax = rho - p.
z * tanRMax;
317 widRMax = fRmax2 - fDz * tanRMax;
318 distRMax = std::fabs(pRMax - widRMax) / secRMax;
320 if (distRMin < distRMax)
322 if (distZ < distRMin)
335 if (distZ < distRMax)
346 if (!fPhiFullCone && rho)
348 phi = std::atan2(p.
y, p.
x);
352 phi += 2 * UUtils::kPi;
357 distSPhi = std::fabs(phi - (fSPhi + 2 * UUtils::kPi)) * rho;
361 distSPhi = std::fabs(phi - fSPhi) * rho;
364 distEPhi = std::fabs(phi - fSPhi - fDPhi) * rho;
368 if (distSPhi < distEPhi)
370 if (distSPhi < distMin)
377 if (distEPhi < distMin)
387 norm =
UVector3(-p.
x / rho, -p.
y / rho, tanRMin / secRMin);
391 norm =
UVector3(p.
x / rho, p.
y / rho, -tanRMax / secRMax);
404 norm =
UVector3(std::sin(fSPhi), -std::cos(fSPhi), 0);
407 norm =
UVector3(-std::sin(fSPhi + fDPhi), std::cos(fSPhi + fDPhi), 0);
412 "Undefined side for valid surface normal to solid.");
445 double snxt = UUtils::kInfinity;
446 const double dRmax = 100 *
std::max(fRmax1, fRmax2);
448 static const double halfRadTolerance = kRadTolerance * 0.5;
450 double rMaxAv, rMaxOAv;
451 double rMinAv, rMinOAv;
454 double tolORMin, tolORMin2, tolIRMin, tolIRMin2;
455 double tolORMax2, tolIRMax, tolIRMax2;
456 double tolODz, tolIDz;
458 double Dist, sd, xi, yi, zi, ri = 0., risec, rhoi2, cosPsi;
461 double nt1, nt2, nt3;
467 rMinAv = (fRmin1 + fRmin2) * 0.5;
469 if (rMinAv > halfRadTolerance)
471 rMinOAv = rMinAv - halfRadTolerance;
477 rMaxAv = (fRmax1 + fRmax2) * 0.5;
478 rMaxOAv = rMaxAv + halfRadTolerance;
482 tolIDz = fDz - halfCarTolerance;
483 tolODz = fDz + halfCarTolerance;
485 if (std::fabs(p.
z) >= tolIDz)
489 sd = (std::fabs(p.
z) - fDz) / std::fabs(v.
z);
498 rhoi2 = xi * xi + yi * yi ;
505 tolORMin = fRmin1 - halfRadTolerance * secRMin;
506 tolIRMin = fRmin1 + halfRadTolerance * secRMin;
507 tolIRMax = fRmax1 - halfRadTolerance * secRMin;
508 tolORMax2 = (fRmax1 + halfRadTolerance * secRMax) *
509 (fRmax1 + halfRadTolerance * secRMax);
513 tolORMin = fRmin2 - halfRadTolerance * secRMin;
514 tolIRMin = fRmin2 + halfRadTolerance * secRMin;
515 tolIRMax = fRmax2 - halfRadTolerance * secRMin;
516 tolORMax2 = (fRmax2 + halfRadTolerance * secRMax) *
517 (fRmax2 + halfRadTolerance * secRMax);
521 tolORMin2 = tolORMin * tolORMin;
522 tolIRMin2 = tolIRMin * tolIRMin;
531 tolIRMax2 = tolIRMax * tolIRMax;
538 if ((tolIRMin2 <= rhoi2) && (rhoi2 <= tolIRMax2))
540 if (!fPhiFullCone && rhoi2)
544 cosPsi = (xi * cosCPhi + yi * sinCPhi) / std::sqrt(rhoi2);
546 if (cosPsi >= cosHDPhiIT)
582 t1 = 1.0 - v.
z * v.
z;
583 t2 = p.
x * v.
x + p.
y * v.
y;
584 t3 = p.
x * p.
x + p.
y * p.
y;
585 rin = tanRMin * p.
z + rMinAv;
586 rout = tanRMax * p.
z + rMaxAv;
591 nt1 = t1 - (tanRMax * v.
z) * (tanRMax * v.
z);
592 nt2 = t2 - tanRMax * v.
z * rout;
593 nt3 = t3 - rout * rout;
595 if (std::fabs(nt1) > kRadTolerance)
600 if ((nt3 > rout * rout * kRadTolerance * kRadTolerance * secRMax * secRMax)
609 if ((rout < 0) && (nt3 <= 0))
616 sd = c / (-b - std::sqrt(d));
620 sd = -b + std::sqrt(d);
625 if ((b <= 0) && (c >= 0))
627 sd = c / (-b + std::sqrt(d));
633 sd = -b + std::sqrt(d);
637 return UUtils::kInfinity;
646 double fTerm = sd - std::fmod(sd, dRmax);
651 if (std::fabs(zi) <= tolODz)
663 ri = rMaxAv + zi * tanRMax;
664 cosPsi = (xi * cosCPhi + yi * sinCPhi) / ri;
666 if (cosPsi >= cosHDPhiIT)
680 if ((t3 > (rin + halfRadTolerance * secRMin)*
681 (rin + halfRadTolerance * secRMin))
682 && (nt2 < 0) && (d >= 0) && (std::fabs(p.
z) <= tolIDz))
689 risec = std::sqrt(xi * xi + yi * yi) * secRMax;
690 norm =
UVector3(xi / risec, yi / risec, -tanRMax / secRMax);
693 cosPsi = (p.
x * cosCPhi + p.
y * sinCPhi) / std::sqrt(t3);
694 if (cosPsi >= cosHDPhiIT)
696 if (norm.
Dot(v) <= 0)
704 if (norm.
Dot(v) <= 0)
714 if (std::fabs(nt2) > kRadTolerance)
716 sd = -0.5 * nt3 / nt2;
720 return UUtils::kInfinity;
726 if ((std::fabs(zi) <= tolODz) && (nt2 < 0))
738 ri = rMaxAv + zi * tanRMax;
739 cosPsi = (xi * cosCPhi + yi * sinCPhi) / ri;
741 if (cosPsi >= cosHDPhiIT)
751 sd = UUtils::kInfinity;
766 nt1 = t1 - (tanRMin * v.
z) * (tanRMin * v.
z);
767 nt2 = t2 - tanRMin * v.
z * rin;
768 nt3 = t3 - rin * rin;
772 if (nt3 > rin * kRadTolerance * secRMin)
784 sd = c / (-b - std::sqrt(d));
788 sd = -b + std::sqrt(d);
796 double fTerm = sd - std::fmod(sd, dRmax);
801 if (std::fabs(zi) <= tolODz)
807 ri = rMinAv + zi * tanRMin;
808 cosPsi = (xi * cosCPhi + yi * sinCPhi) / ri;
810 if (cosPsi >= cosHDPhiIT)
812 if (sd > halfRadTolerance)
820 risec = std::sqrt(xi * xi + yi * yi) * secRMin;
821 norm =
UVector3(-xi / risec, -yi / risec, tanRMin / secRMin);
822 if (norm.
Dot(v) <= 0)
831 if (sd > halfRadTolerance)
841 risec = std::sqrt(xi * xi + yi * yi) * secRMin;
842 norm =
UVector3(-xi / risec, -yi / risec, tanRMin / secRMin);
843 if (norm.
Dot(v) <= 0)
853 else if (nt3 < -rin * kRadTolerance * secRMin)
868 sd = c / (-b - std::sqrt(d));
872 sd = -b + std::sqrt(d);
875 ri = rMinAv + zi * tanRMin;
879 if ((sd >= 0) && (std::fabs(zi) <= tolODz))
884 double fTerm = sd - std::fmod(sd, dRmax);
891 cosPsi = (xi * cosCPhi + yi * sinCPhi) / ri;
893 if (cosPsi >= cosHDPhiOT)
895 if (sd > halfRadTolerance)
903 risec = std::sqrt(xi * xi + yi * yi) * secRMin;
904 norm =
UVector3(-xi / risec, -yi / risec, tanRMin / secRMin);
905 if (norm.
Dot(v) <= 0)
914 if (sd > halfRadTolerance)
924 risec = std::sqrt(xi * xi + yi * yi) * secRMin;
925 norm =
UVector3(-xi / risec, -yi / risec, tanRMin / secRMin);
926 if (norm.
Dot(v) <= 0)
938 sd = -b - std::sqrt(d);
942 sd = c / (-b + std::sqrt(d));
945 ri = rMinAv + zi * tanRMin;
947 if ((sd >= 0) && (ri > 0) && (std::fabs(zi) <= tolODz))
952 double fTerm = sd - std::fmod(sd, dRmax);
959 cosPsi = (xi * cosCPhi + yi * sinCPhi) / ri;
961 if (cosPsi >= cosHDPhiIT)
963 if (sd > halfRadTolerance)
971 risec = std::sqrt(xi * xi + yi * yi) * secRMin;
972 norm =
UVector3(-xi / risec, -yi / risec, tanRMin / secRMin);
973 if (norm.
Dot(v) <= 0)
982 if (sd > halfRadTolerance)
992 risec = std::sqrt(xi * xi + yi * yi) * secRMin;
993 norm =
UVector3(-xi / risec, -yi / risec, tanRMin / secRMin);
994 if (norm.
Dot(v) <= 0)
1011 if (std::fabs(p.
z) <= tolODz)
1019 cosPsi = (p.
x * cosCPhi + p.
y * sinCPhi) / std::sqrt(t3);
1021 if (cosPsi >= cosHDPhiIT)
1044 sd = -b - std::sqrt(d);
1048 sd = c / (-b + std::sqrt(d));
1050 zi = p.
z + sd * v.
z;
1051 ri = rMinAv + zi * tanRMin;
1057 sd = c / (-b - std::sqrt(d));
1061 sd = -b + std::sqrt(d);
1064 zi = p.
z + sd * v.
z;
1066 if ((sd >= 0) && (std::fabs(zi) <= tolODz))
1071 double fTerm = sd - std::fmod(sd, dRmax);
1076 xi = p.
x + sd * v.
x;
1077 yi = p.
y + sd * v.
y;
1078 ri = rMinAv + zi * tanRMin;
1079 cosPsi = (xi * cosCPhi + yi * sinCPhi) / ri;
1081 if (cosPsi >= cosHDPhiIT)
1094 return UUtils::kInfinity;
1109 sd = c / (-b - std::sqrt(d));
1113 sd = -b + std::sqrt(d);
1115 zi = p.
z + sd * v.
z;
1117 if ((sd >= 0) && (std::fabs(zi) <= tolODz))
1122 double fTerm = sd - std::fmod(sd, dRmax);
1127 xi = p.
x + sd * v.
x;
1128 yi = p.
y + sd * v.
y;
1129 ri = rMinAv + zi * tanRMin;
1130 cosPsi = (xi * cosCPhi + yi * sinCPhi) / ri;
1132 if (cosPsi >= cosHDPhiIT)
1161 Comp = v.
x * sinSPhi - v.
y * cosSPhi;
1165 Dist = (p.
y * cosSPhi - p.
x * sinSPhi);
1167 if (Dist < halfCarTolerance)
1178 zi = p.
z + sd * v.
z;
1180 if (std::fabs(zi) <= tolODz)
1182 xi = p.
x + sd * v.
x;
1183 yi = p.
y + sd * v.
y;
1184 rhoi2 = xi * xi + yi * yi;
1185 tolORMin2 = (rMinOAv + zi * tanRMin) * (rMinOAv + zi * tanRMin);
1186 tolORMax2 = (rMaxOAv + zi * tanRMax) * (rMaxOAv + zi * tanRMax);
1188 if ((rhoi2 >= tolORMin2) && (rhoi2 <= tolORMax2))
1193 if ((yi * cosCPhi - xi * sinCPhi) <= 0)
1205 Comp = -(v.
x * sinEPhi - v.
y * cosEPhi);
1209 Dist = -(p.
y * cosEPhi - p.
x * sinEPhi);
1210 if (Dist < halfCarTolerance)
1221 zi = p.
z + sd * v.
z;
1223 if (std::fabs(zi) <= tolODz)
1225 xi = p.
x + sd * v.
x;
1226 yi = p.
y + sd * v.
y;
1227 rhoi2 = xi * xi + yi * yi;
1228 tolORMin2 = (rMinOAv + zi * tanRMin) * (rMinOAv + zi * tanRMin);
1229 tolORMax2 = (rMaxOAv + zi * tanRMax) * (rMaxOAv + zi * tanRMax);
1231 if ((rhoi2 >= tolORMin2) && (rhoi2 <= tolORMax2))
1236 if ((yi * cosCPhi - xi * sinCPhi) >= 0.0)
1246 if (snxt < halfCarTolerance)
1263 double safe = 0.0, rho, safeR1, safeR2, safeZ, safePhi, cosPsi;
1264 double pRMin, pRMax;
1266 rho = std::sqrt(p.
x * p.
x + p.
y * p.
y);
1267 safeZ = std::fabs(p.
z) - fDz;
1269 if (fRmin1 || fRmin2)
1271 pRMin = tanRMin * p.
z + (fRmin1 + fRmin2) * 0.5;
1272 safeR1 = (pRMin - rho) / secRMin;
1274 pRMax = tanRMax * p.
z + (fRmax1 + fRmax2) * 0.5;
1275 safeR2 = (rho - pRMax) / secRMax;
1277 if (safeR1 > safeR2)
1288 pRMax = tanRMax * p.
z + (fRmax1 + fRmax2) * 0.5;
1289 safe = (rho - pRMax) / secRMax;
1296 if (!fPhiFullCone && rho)
1300 cosPsi = (p.
x * cosCPhi + p.
y * sinCPhi) / rho;
1302 if (cosPsi < std::cos(fDPhi * 0.5))
1304 if ((p.
y * cosCPhi - p.
x * sinCPhi) <= 0.0)
1306 safePhi = std::fabs(p.
x * std::sin(fSPhi) - p.
y * std::cos(fSPhi));
1310 safePhi = std::fabs(p.
x * sinEPhi - p.
y * cosEPhi);
1342 ESide side = kNull, sider = kNull, sidephi = kNull;
1345 static const double halfRadTolerance = kRadTolerance * 0.5;
1346 static const double halfAngTolerance = kAngTolerance * 0.5;
1348 double snxt, srd, sphi, pdist;
1353 double t1,
t2, t3, rout, rin, nt1, nt2, nt3;
1354 double b,
c,
d, sr2, sr3;
1358 ESide sidetol = kNull;
1359 double slentol = UUtils::kInfinity;
1363 double pDistS, compS, pDistE, compE, sphi2, xi, yi, risec, vphi;
1364 double zi, ri, deltaRoi2;
1372 if (pdist > halfCarTolerance)
1388 if (pdist > halfCarTolerance)
1390 snxt = -pdist / v.
z;
1395 aNormalVector =
UVector3(0, 0, -1);
1402 snxt = UUtils::kInfinity;
1423 rMaxAv = (fRmax1 + fRmax2) * 0.5;
1425 t1 = 1.0 - v.
z * v.
z;
1426 t2 = p.
x * v.
x + p.
y * v.
y;
1427 t3 = p.
x * p.
x + p.
y * p.
y;
1428 rout = tanRMax * p.
z + rMaxAv;
1430 nt1 = t1 - (tanRMax * v.
z) * (tanRMax * v.
z);
1431 nt2 = t2 - tanRMax * v.
z * rout;
1432 nt3 = t3 - rout * rout;
1436 deltaRoi2 = snxt * snxt * t1 + 2 * snxt * t2 + t3
1437 - fRmax2 * (fRmax2 + kRadTolerance * secRMax);
1441 deltaRoi2 = snxt * snxt * t1 + 2 * snxt * t2 + t3
1442 - fRmax1 * (fRmax1 + kRadTolerance * secRMax);
1449 if (nt1 && (deltaRoi2 > 0.0))
1462 if (nt3 > -halfRadTolerance && nt2 >= 0)
1464 risec = std::sqrt(t3) * secRMax;
1466 aNormalVector =
UVector3(p.
x / risec, p.
y / risec, -tanRMax / secRMax);
1474 srd = -b - std::sqrt(d);
1478 srd = c / (-b + std::sqrt(d));
1481 zi = p.
z + srd * v.
z;
1482 ri = tanRMax * zi + rMaxAv;
1484 if ((ri >= 0) && (-halfRadTolerance <= srd) && (srd <= halfRadTolerance))
1492 if ((ri < 0) || (srd < halfRadTolerance))
1499 sr2 = c / (-b - std::sqrt(d));
1503 sr2 = -b + std::sqrt(d);
1505 zi = p.
z + sr2 * v.
z;
1506 ri = tanRMax * zi + rMaxAv;
1508 if ((ri >= 0) && (sr2 > halfRadTolerance))
1514 srd = UUtils::kInfinity;
1516 if ((-halfRadTolerance <= sr2) && (sr2 <= halfRadTolerance))
1533 risec = std::sqrt(t3) * secRMax;
1535 aNormalVector =
UVector3(p.
x / risec, p.
y / risec, -tanRMax / secRMax);
1539 else if (nt2 && (deltaRoi2 > 0.0))
1543 risec = std::sqrt(t3) * secRMax;
1545 aNormalVector =
UVector3(p.
x / risec, p.
y / risec, -tanRMax / secRMax);
1553 srd = UUtils::kInfinity;
1558 if (slentol <= halfCarTolerance)
1569 xi = p.
x + slentol * v.
x;
1570 yi = p.
y + slentol * v.
y;
1571 risec = std::sqrt(xi * xi + yi * yi) * secRMax;
1574 if (norm.
Dot(v) > 0)
1576 aNormalVector = norm.
Unit();
1583 slentol = UUtils::kInfinity;
1589 if (fRmin1 || fRmin2)
1591 nt1 = t1 - (tanRMin * v.
z) * (tanRMin * v.
z);
1595 rMinAv = (fRmin1 + fRmin2) * 0.5;
1596 rin = tanRMin * p.
z + rMinAv;
1597 nt2 = t2 - tanRMin * v.
z * rin;
1598 nt3 = t3 - rin * rin;
1611 if (nt3 < kRadTolerance * (rin + kRadTolerance * 0.25))
1623 sr2 = -b - std::sqrt(d);
1627 sr2 = c / (-b + std::sqrt(d));
1629 zi = p.
z + sr2 * v.
z;
1630 ri = tanRMin * zi + rMinAv;
1632 if ((ri >= 0.0) && (-halfRadTolerance <= sr2) && (sr2 <= halfRadTolerance))
1640 if ((ri < 0) || (sr2 < halfRadTolerance))
1644 sr3 = c / (-b - std::sqrt(d));
1648 sr3 = -b + std::sqrt(d);
1654 if (sr3 > halfRadTolerance)
1658 zi = p.
z + sr3 * v.
z;
1659 ri = tanRMin * zi + rMinAv;
1668 else if (sr3 > -halfRadTolerance)
1676 else if ((sr2 < srd) && (sr2 > halfCarTolerance))
1681 else if (sr2 > -halfCarTolerance)
1688 if (slentol <= halfCarTolerance)
1697 xi = p.
x + slentol * v.
x;
1698 yi = p.
y + slentol * v.
y;
1699 if (sidetol == kRMax)
1701 risec = std::sqrt(xi * xi + yi * yi) * secRMax;
1702 norm =
UVector3(xi / risec, yi / risec, -tanRMax / secRMax);
1706 risec = std::sqrt(xi * xi + yi * yi) * secRMin;
1707 norm =
UVector3(-xi / risec, -yi / risec, tanRMin / secRMin);
1709 if (norm.
Dot(v) > 0)
1713 aNormalVector = norm.
Unit();
1722 slentol = UUtils::kInfinity;
1739 vphi = std::atan2(v.
y, v.
x);
1741 if (vphi < fSPhi - halfAngTolerance)
1743 vphi += 2 * UUtils::kPi;
1745 else if (vphi > fSPhi + fDPhi + halfAngTolerance)
1747 vphi -= 2 * UUtils::kPi;
1754 pDistS = p.
x * sinSPhi - p.
y * cosSPhi;
1755 pDistE = -p.
x * sinEPhi + p.
y * cosEPhi;
1759 compS = -sinSPhi * v.
x + cosSPhi * v.
y;
1760 compE = sinEPhi * v.
x - cosEPhi * v.
y;
1764 if (((fDPhi <= UUtils::kPi) && ((pDistS <= halfCarTolerance)
1765 && (pDistE <= halfCarTolerance)))
1766 || ((fDPhi > UUtils::kPi) && !((pDistS > halfCarTolerance)
1767 && (pDistE > halfCarTolerance))))
1772 sphi = pDistS / compS;
1773 if (sphi >= -halfCarTolerance)
1775 xi = p.
x + sphi * v.
x;
1776 yi = p.
y + sphi * v.
y;
1785 if ((fSPhi - halfAngTolerance <= vphi)
1786 && (fSPhi + fDPhi + halfAngTolerance >= vphi))
1788 sphi = UUtils::kInfinity;
1791 else if ((yi * cosCPhi - xi * sinCPhi) >= 0)
1793 sphi = UUtils::kInfinity;
1798 if (pDistS > -halfCarTolerance)
1806 sphi = UUtils::kInfinity;
1811 sphi = UUtils::kInfinity;
1816 sphi2 = pDistE / compE;
1820 if ((sphi2 > -halfCarTolerance) && (sphi2 < sphi))
1822 xi = p.
x + sphi2 * v.
x;
1823 yi = p.
y + sphi2 * v.
y;
1832 if (!((fSPhi - halfAngTolerance <= vphi)
1833 && (fSPhi + fDPhi + halfAngTolerance >= vphi)))
1836 if (pDistE <= -halfCarTolerance)
1847 if (yi * cosCPhi - xi * sinCPhi >= 0)
1852 if (pDistE <= -halfCarTolerance)
1866 sphi = UUtils::kInfinity;
1874 if ((fSPhi - halfAngTolerance <= vphi)
1875 && (vphi <= fSPhi + fDPhi + halfAngTolerance))
1877 sphi = UUtils::kInfinity;
1901 xi = p.
x + snxt * v.
x;
1902 yi = p.
y + snxt * v.
y;
1903 risec = std::sqrt(xi * xi + yi * yi) * secRMax;
1904 aNormalVector =
UVector3(xi / risec, yi / risec, -tanRMax / secRMax);
1911 if (fDPhi <= UUtils::kPi)
1913 aNormalVector =
UVector3(sinSPhi, -cosSPhi, 0);
1922 if (fDPhi <= UUtils::kPi)
1924 aNormalVector =
UVector3(-sinEPhi, cosEPhi, 0);
1937 aNormalVector =
UVector3(0, 0, -1);
1943 std::ostringstream message;
1944 int oldprc = message.precision(16);
1945 message <<
"Undefined side for valid surface normal to solid."
1947 <<
"Position:" << std::endl << std::endl
1948 <<
"p.x = " << p.
x <<
" mm" << std::endl
1949 <<
"p.y = " << p.
y <<
" mm" << std::endl
1950 <<
"p.z = " << p.
z <<
" mm" << std::endl << std::endl
1951 <<
"pho at z = " << std::sqrt(p.
x * p.
x + p.
y * p.
y)
1952 <<
" mm" << std::endl << std::endl;
1953 if (p.
x != 0. || p.
y != 0.)
1955 message <<
"point phi = " << std::atan2(p.
y, p.
x) / (UUtils::kPi / 180.0)
1956 <<
" degree" << std::endl << std::endl;
1958 message <<
"Direction:" << std::endl << std::endl
1959 <<
"v.x = " << v.
x << std::endl
1960 <<
"v.y = " << v.
y << std::endl
1961 <<
"v.z = " << v.
z << std::endl << std::endl
1962 <<
"Proposed distance :" << std::endl << std::endl
1963 <<
"snxt = " << snxt <<
" mm" << std::endl;
1964 message.precision(oldprc);
1968 if (snxt < halfCarTolerance)
1982 double safe = 0.0, rho, safeR1, safeR2, safeZ, safePhi;
1989 int oldprc = cout.precision(16);
1992 cout <<
"Position:" << std::endl << std::endl;
1993 cout <<
"p.x = " << p.
x <<
" mm" << std::endl;
1994 cout <<
"p.y = " << p.
y <<
" mm" << std::endl;
1995 cout <<
"p.z = " << p.
z <<
" mm" << std::endl << std::endl;
1996 cout <<
"pho at z = " << std::sqrt(p.
x * p.
x + p.
y * p.
y)
1997 <<
" mm" << std::endl << std::endl;
1998 if ((p.
x != 0.) || (p.
x != 0.))
2000 cout <<
"point phi = " << std::atan2(p.
y, p.
x) /
degree
2001 <<
" degree" << std::endl << std::endl;
2003 cout.precision(oldprc);
2009 rho = std::sqrt(p.
x * p.
x + p.
y * p.
y);
2010 safeZ = fDz - std::fabs(p.
z);
2012 if (fRmin1 || fRmin2)
2014 pRMin = tanRMin * p.
z + (fRmin1 + fRmin2) * 0.5;
2015 safeR1 = (rho - pRMin) / secRMin;
2019 safeR1 = UUtils::kInfinity;
2022 pRMax = tanRMax * p.
z + (fRmax1 + fRmax2) * 0.5;
2023 safeR2 = (pRMax - rho) / secRMax;
2025 if (safeR1 < safeR2)
2044 if ((p.
y * cosCPhi - p.
x * sinCPhi) <= 0)
2046 safePhi = -(p.
x * sinSPhi - p.
y * cosSPhi);
2050 safePhi = (p.
x * sinEPhi - p.
y * cosEPhi);
2072 return std::string(
"Cons");
2082 return new UCons(*
this);
2091 int oldprc = os.precision(16);
2092 os <<
"-----------------------------------------------------------\n"
2093 <<
" *** Dump for solid - " <<
GetName() <<
" ***\n"
2094 <<
" ===================================================\n"
2095 <<
" Solid type: UCons\n"
2096 <<
" Parameters: \n"
2097 <<
" inside -fDz radius: " << fRmin1 <<
" mm \n"
2098 <<
" outside -fDz radius: " << fRmax1 <<
" mm \n"
2099 <<
" inside +fDz radius: " << fRmin2 <<
" mm \n"
2100 <<
" outside +fDz radius: " << fRmax2 <<
" mm \n"
2101 <<
" half length in Z : " << fDz <<
" mm \n"
2102 <<
" starting angle of segment: " << fSPhi / (UUtils::kPi / 180.0) <<
" degrees \n"
2103 <<
" delta angle of segment : " << fDPhi / (UUtils::kPi / 180.0) <<
" degrees \n"
2104 <<
"-----------------------------------------------------------\n";
2105 os.precision(oldprc);
2120 double Aone, Atwo, Athree, Afour, Afive, slin, slout, phi;
2121 double zRand, cosu, sinu, rRand1, rRand2, chose, rone, rtwo, qone, qtwo;
2122 rone = (fRmax1 - fRmax2) / (2.*fDz);
2123 rtwo = (fRmin1 - fRmin2) / (2.*fDz);
2126 if (fRmax1 != fRmax2)
2128 qone = fDz * (fRmax1 + fRmax2) / (fRmax1 - fRmax2);
2130 if (fRmin1 != fRmin2)
2132 qtwo = fDz * (fRmin1 + fRmin2) / (fRmin1 - fRmin2);
2136 Aone = 0.5 * fDPhi * (fRmax2 + fRmax1) * slout;
2137 Atwo = 0.5 * fDPhi * (fRmin2 + fRmin1) * slin;
2138 Athree = 0.5 * fDPhi * (fRmax1 * fRmax1 - fRmin1 * fRmin1);
2139 Afour = 0.5 * fDPhi * (fRmax2 * fRmax2 - fRmin2 * fRmin2);
2140 Afive = fDz * (fRmax1 - fRmin1 + fRmax2 - fRmin2);
2143 cosu = std::cos(phi);
2144 sinu = std::sin(phi);
2148 if ((fSPhi == 0.) && fPhiFullCone)
2152 chose =
UUtils::Random(0., Aone + Atwo + Athree + Afour + 2.*Afive);
2154 if ((chose >= 0.) && (chose < Aone))
2156 if (fRmin1 != fRmin2)
2159 return UVector3(rtwo * cosu * (qtwo - zRand),
2160 rtwo * sinu * (qtwo - zRand), zRand);
2164 return UVector3(fRmin1 * cosu, fRmin2 * sinu,
2168 else if ((chose >= Aone) && (chose <= Aone + Atwo))
2170 if (fRmax1 != fRmax2)
2173 return UVector3(rone * cosu * (qone - zRand),
2174 rone * sinu * (qone - zRand), zRand);
2178 return UVector3(fRmax1 * cosu, fRmax2 * sinu,
2182 else if ((chose >= Aone + Atwo) && (chose < Aone + Atwo + Athree))
2184 return UVector3(rRand1 * cosu, rRand1 * sinu, -1 * fDz);
2186 else if ((chose >= Aone + Atwo + Athree)
2187 && (chose < Aone + Atwo + Athree + Afour))
2189 return UVector3(rRand2 * cosu, rRand2 * sinu, fDz);
2191 else if ((chose >= Aone + Atwo + Athree + Afour)
2192 && (chose < Aone + Atwo + Athree + Afour + Afive))
2195 rRand1 =
UUtils::Random(fRmin2 - ((zRand - fDz) / (2.*fDz)) * (fRmin1 - fRmin2),
2196 fRmax2 - ((zRand - fDz) / (2.*fDz)) * (fRmax1 - fRmax2));
2197 return UVector3(rRand1 * std::cos(fSPhi),
2198 rRand1 * std::sin(fSPhi), zRand);
2203 rRand1 =
UUtils::Random(fRmin2 - ((zRand - fDz) / (2.*fDz)) * (fRmin1 - fRmin2),
2204 fRmax2 - ((zRand - fDz) / (2.*fDz)) * (fRmax1 - fRmax2));
2205 return UVector3(rRand1 * std::cos(fSPhi + fDPhi),
2206 rRand1 * std::sin(fSPhi + fDPhi), zRand);
2213 double max = fRmax1 > fRmax2 ? fRmax1 : fRmax2;
double SafetyFromOutside(const UVector3 &p, bool precise) const
static double frTolerance
VUSolid::EnumInside Inside(const UVector3 &p) const
UGeometryType GetEntityType() const
std::ostream & StreamInfo(std::ostream &os) const
double DistanceToIn(const UVector3 &p, const UVector3 &v, double aPstep=UUtils::kInfinity) const
UCons & operator=(const UCons &rhs)
const std::string & GetName() const
double SafetyFromInside(const UVector3 &p, bool precise) const
double GetOuterRadiusPlusZ() const
static double Tolerance()
double GetStartPhiAngle() const
bool Normal(const UVector3 &p, UVector3 &n) const
double GetZHalfLength() const
static double faTolerance
double Dot(const UVector3 &) const
double GetInnerRadiusPlusZ() const
double GetRadiusInRing(double rmin, double rmax)
double GetOuterRadiusMinusZ() const
void Extent(UVector3 &aMin, UVector3 &aMax) const
virtual void GetParametersList(int, double *) const
T max(const T t1, const T t2)
brief Return the largest of the two arguments
double GetDeltaPhiAngle() const
std::string UGeometryType
double GetInnerRadiusMinusZ() const
UVector3 GetPointOnSurface() const
void Exception(const char *originOfException, const char *exceptionCode, ExceptionSeverity severity, int level, const char *description)
double Random(double min=0.0, double max=1.0)
double DistanceToOut(const UVector3 &aPoint, const UVector3 &aDirection, UVector3 &aNormalVector, bool &aConvex, double aPstep=UUtils::kInfinity) const