67 centres[0] = 0.5*(point1_ + point2_);
70 if (radius1_ > radius2_)
87 auto& pts = tpts.ref();
96 void Foam::searchableCone::findNearestAndNormal
99 const scalar nearestDistSqr,
104 vector v(sample - point1_);
107 const scalar parallel = (v & unitDir_);
110 v -= parallel*unitDir_;
112 const scalar magV =
mag(v);
116 point disk1Point(point1_ +
min(
max(magV, innerRadius1_), radius1_)*v);
117 vector disk1Normal(-unitDir_);
120 point disk2Point(point2_ +
min(
max(magV, innerRadius2_), radius2_)*v);
121 vector disk2Normal(unitDir_);
125 point nearCone(GREAT, GREAT, GREAT);
126 vector normalCone(1, 0, 0);
129 point iCnearCone(GREAT, GREAT, GREAT);
130 vector iCnormalCone(1, 0, 0);
132 point projPt1 = point1_+ radius1_*v;
133 point projPt2 = point2_+ radius2_*v;
135 vector p1 = (projPt1 - point1_);
136 if (
mag(p1) > ROOTVSMALL)
144 vector a = (sample - projPt1);
146 if (
mag(a) <= ROOTVSMALL)
149 a = (sample - projPt2);
152 nearCone = (a &
b)*
b + projPt2;
160 nearCone = (a &
b)*
b + projPt1;
167 if (innerRadius1_ > 0 || innerRadius2_ > 0)
170 point iCprojPt1 = point1_+ innerRadius1_*v;
171 point iCprojPt2 = point2_+ innerRadius2_*v;
180 vector iCa(sample - iCprojPt1);
182 if (
mag(iCa) <= ROOTVSMALL)
184 iCa = (sample - iCprojPt2);
187 iCnearCone = (iCa & iCb)*iCb+iCprojPt2;
189 vector b1 = (iCp1 & iCb)*iCb;
195 iCnearCone = (iCa & iCb)*iCb+iCprojPt1;
198 vector b1 = (iCp1 & iCb)*iCb;
208 FixedList<scalar, 4> dist;
209 dist[0] =
magSqr(nearCone-sample);
210 dist[1] =
magSqr(disk1Point-sample);
211 dist[2] =
magSqr(disk2Point-sample);
212 dist[3] =
magSqr(iCnearCone-sample);
214 const label minI =
findMin(dist);
224 vector v1(nearCone-point1_);
225 scalar para = (v1 & unitDir_);
228 const scalar magV1 =
mag(v1);
231 if (para < 0.0 && magV1 >= radius1_)
235 nearCone = disk1Point;
237 else if (para < 0.0 && magV1 < radius1_)
240 nearCone = disk1Point;
241 normalCone = disk1Normal;
243 else if (para > magDir_ && magV1 >= radius2_)
247 nearCone = disk2Point;
249 else if (para > magDir_ && magV1 < radius2_)
252 nearCone = disk2Point;
253 normalCone = disk2Normal;
257 nearNormal = normalCone;
262 nearNormal = disk1Normal;
267 nearNormal = disk2Normal;
272 vector v1(iCnearCone-point1_);
273 scalar para = (v1 & unitDir_);
277 const scalar magV1 =
mag(v1);
280 if (para < 0.0 && magV1 >= innerRadius1_)
282 iCnearCone = disk1Point;
284 else if (para < 0.0 && magV1 < innerRadius1_)
286 iCnearCone = disk1Point;
287 iCnormalCone = disk1Normal;
289 else if (para > magDir_ && magV1 >= innerRadius2_)
291 iCnearCone = disk2Point;
293 else if (para > magDir_ && magV1 < innerRadius2_)
295 iCnearCone = disk2Point;
296 iCnormalCone = disk2Normal;
300 nearNormal = iCnormalCone;
312 Foam::scalar Foam::searchableCone::radius2
314 const searchableCone& cone,
318 const vector x = (pt-cone.point1_) ^ cone.unitDir_;
325 void Foam::searchableCone::findLineAll
327 const searchableCone& cone,
328 const scalar innerRadius1,
329 const scalar innerRadius2,
339 vector point1Start(start-cone.point1_);
340 vector point2Start(start-cone.point2_);
344 scalar s1 = point1Start & (cone.unitDir_);
345 scalar s2 = point1End & (cone.unitDir_);
347 if ((s1 < 0.0 && s2 < 0.0) || (s1 > cone.magDir_ && s2 > cone.magDir_))
354 scalar magV =
mag(V);
355 if (magV < ROOTVSMALL)
371 scalar tNear = VGREAT;
372 scalar tFar = VGREAT;
374 scalar radius_sec = cone.radius1_;
378 scalar
s = (V & unitDir_);
382 tPoint2 = -(point2Start&(cone.unitDir_))/
s;
384 if (tPoint2 < tPoint1)
386 Swap(tPoint1, tPoint2);
388 if (tPoint1 > magV || tPoint2 < 0)
393 if (tPoint1 >= 0 && tPoint1 <= magV)
395 scalar r2 = radius2(cone, start+tPoint1*V);
396 vector p1 = (start+tPoint1*V-point1_);
397 vector p2 = (start+tPoint1*V-point2_);
398 radius_sec = cone.radius1_;
399 scalar inC_radius_sec = innerRadius1_;
401 if (
mag(p2&(cone.unitDir_)) <
mag(p1&(cone.unitDir_)))
403 radius_sec = cone.radius2_;
404 inC_radius_sec = innerRadius2_;
407 if (r2 <=
sqr(radius_sec) && r2 >=
sqr(inC_radius_sec))
412 if (tPoint2 >= 0 && tPoint2 <= magV)
415 vector p1 = (start+tPoint2*V-cone.point1_);
416 vector p2 = (start+tPoint2*V-cone.point2_);
417 radius_sec = cone.radius1_;
418 scalar inC_radius_sec = innerRadius1_;
419 if (
mag(p2&(cone.unitDir_)) <
mag(p1&(cone.unitDir_)))
421 radius_sec = cone.radius2_;
422 inC_radius_sec = innerRadius2_;
424 scalar r2 = radius2(cone, start+tPoint2*V);
425 if (r2 <=
sqr(radius_sec) && r2 >=
sqr(inC_radius_sec))
452 scalar deltaRadius = cone.radius2_-cone.radius1_;
453 if (
mag(deltaRadius) <= ROOTVSMALL)
455 vector point1Start(start-cone.point1_);
456 const vector x = point1Start ^ cone.unitDir_;
457 const vector y = V ^ cone.unitDir_;
458 const scalar d =
sqr(0.5*(cone.radius1_ + cone.radius2_));
466 vector va = cone.unitDir_;
473 cone.unitDir_*cone.radius1_*
mag(cone.point2_-cone.point1_)
477 scalar l2 =
sqr(deltaRadius)+
sqr(cone.magDir_);
478 scalar sqrCosAlpha =
sqr(cone.magDir_)/l2;
479 scalar sqrSinAlpha =
sqr(deltaRadius)/l2;
483 vector p1 = (delP-(delP&va)*va);
485 a = sqrCosAlpha*((v1-
p*va)&(v1-
p*va))-sqrSinAlpha*
p*
p;
487 2.0*sqrCosAlpha*(a1&p1)
488 -2.0*sqrSinAlpha*(v1&va)*(delP&va);
491 *((delP-(delP&va)*va)&(delP-(delP&va)*va))
492 -sqrSinAlpha*
sqr(delP&va);
495 const scalar disc =
b*
b-4.0*a*
c;
505 else if (disc < ROOTVSMALL)
508 if (
mag(a) > ROOTVSMALL)
512 if (t1 >= 0.0 && t1 <= magV && t1 >= tPoint1 && t1 <= tPoint2)
541 if (
mag(a) > ROOTVSMALL)
543 scalar sqrtDisc =
sqrt(disc);
545 t1 = (-
b - sqrtDisc)/(2.0*a);
546 t2 = (-
b + sqrtDisc)/(2.0*a);
552 if (t1 >= 0.0 && t1 <= magV && t1 >= tPoint1 && t1 <= tPoint2)
565 if (t2>=0 && t2 <= magV && t2 >= tPoint1 && t2 <= tPoint2)
590 if (tNear>=0 && tNear <= magV)
592 near.setPoint(start+tNear*V);
597 far.setPoint(start+tFar*V);
602 else if (tFar>=0 && tFar <= magV)
604 near.setPoint(start+tFar*V);
611 void Foam::searchableCone::insertHit
615 List<pointIndexHit>& info,
619 scalar smallDistSqr = SMALL*
magSqr(
end-start);
621 scalar hitMagSqr =
magSqr(hit.hitPoint()-start);
625 scalar d2 =
magSqr(info[i].hitPoint()-start);
627 if (d2 > hitMagSqr+smallDistSqr)
630 label sz = info.size();
632 for (label j = sz; j > i; --j)
639 else if (d2 > hitMagSqr-smallDistSqr)
646 label sz = info.size();
687 if (radius2_ >= radius1_)
702 return boundBox(
min,
max);
708 Foam::searchableCone::searchableCone
712 const scalar radius1,
713 const scalar innerRadius1,
715 const scalar radius2,
716 const scalar innerRadius2
722 innerRadius1_(innerRadius1),
725 innerRadius2_(innerRadius2),
726 magDir_(
mag(point2_-point1_)),
727 unitDir_((point2_-point1_)/magDir_)
729 bounds() = calcBounds();
733 Foam::searchableCone::searchableCone
741 radius1_(
dict.
get<scalar>(
"radius1")),
744 radius2_(
dict.
get<scalar>(
"radius2")),
746 magDir_(
mag(point2_-point1_)),
747 unitDir_((point2_-point1_)/magDir_)
749 bounds() = calcBounds();
757 if (regions_.empty())
760 regions_[0] =
"region0";
778 findNearestAndNormal(
samples[i], nearestDistSqr[i], info[i], normal);
806 if (!info[i].hit() &&
b.hit())
813 if (innerRadius1_ > 0.0 || innerRadius2_ > 0.0)
833 newEnd = info[i].hitPoint();
888 if (!info[i].hit() &&
b.hit())
893 if (innerRadius1_ > 0.0 || innerRadius2_ > 0.0)
923 if (!info[i].hit() &&
b.hit())
933 void Foam::searchableCone::findLineAll
984 if (innerRadius1_ > 0.0 || innerRadius2_ > 0.0)
1016 insertHit(start[i],
end[i], info[i], near);
1020 insertHit(start[i],
end[i], info[i], far);
1044 normal.setSize(info.size());
1052 findNearestAndNormal
1081 const scalar parallel = (v & unitDir_);
1084 if (parallel < 0 || parallel > magDir_)
1089 const scalar radius_sec =
1090 radius1_ + parallel * (radius2_-radius1_)/magDir_;
1092 const scalar radius_sec_inner =
1093 innerRadius1_ + parallel * (innerRadius2_-innerRadius1_)/magDir_;
1096 v -= parallel*unitDir_;
1098 if (
mag(v) >= radius_sec_inner &&
mag(v) <= radius_sec)