public static bool FindClosestRayIntersection(ISampledCurve3d c, double segRadius, Ray3d ray, out double minRayT)
{
minRayT = double.MaxValue;
int nNearSegment = -1;
int nSegs = c.SegmentCount;
for (int i = 0; i < nSegs; ++i)
{
Segment3d seg = c.GetSegment(i);
// raycast to line bounding-sphere first (is this going ot be faster??)
double fSphereHitT;
bool bHitBoundSphere = RayIntersection.SphereSigned(ref ray.Origin, ref ray.Direction,
ref seg.Center, seg.Extent + segRadius, out fSphereHitT);
if (bHitBoundSphere == false)
{
continue;
}
double rayt, segt;
double dSqr = DistRay3Segment3.SquaredDistance(ref ray, ref seg, out rayt, out segt);
if (dSqr < segRadius * segRadius)
{
if (rayt < minRayT)
{
minRayT = rayt;
nNearSegment = i;
}
}
}
return(nNearSegment >= 0);
}
void find_min_distance(ref Ray3d ray, ref double min_dist, ref int min_dist_seg, ref double min_dist_segt, ref double min_dist_rayt, int bi, int iLayerStart, int iLayer)
{
// hit polygon layer, check segments
if (iLayer == 0)
{
int seg_i = 2 * bi;
Segment3d seg_a = Curve.GetSegment(seg_i);
double segt, rayt;
double segdist_sqr = DistRay3Segment3.SquaredDistance(ref ray, ref seg_a, out rayt, out segt);
double segdist = Math.Sqrt(segdist_sqr);
if (segdist <= min_dist)
{
min_dist = segdist;
min_dist_seg = seg_i;
min_dist_segt = segt;
min_dist_rayt = rayt;
}
if ((seg_i + 1) < Curve.SegmentCount)
{
Segment3d seg_b = Curve.GetSegment(seg_i + 1);
segdist_sqr = DistRay3Segment3.SquaredDistance(ref ray, ref seg_b, out rayt, out segt);
segdist = Math.Sqrt(segdist_sqr);
if (segdist <= min_dist)
{
min_dist = segdist;
min_dist_seg = seg_i + 1;
min_dist_segt = segt;
min_dist_rayt = rayt;
}
}
return;
}
// test both boxes and recurse
// TODO: verify that this intersection strategy makes sense?
int prev_layer = iLayer - 1;
int prev_count = layer_counts[prev_layer];
int prev_start = iLayerStart - prev_count;
int prev_a = prev_start + 2 * bi;
bool intersects = IntrRay3Box3.Intersects(ref ray, ref boxes[prev_a], min_dist);
if (intersects)
{
find_min_distance(ref ray, ref min_dist, ref min_dist_seg, ref min_dist_segt, ref min_dist_rayt, 2 * bi, prev_start, prev_layer);
}
if ((2 * bi + 1) >= prev_count)
{
return;
}
int prev_b = prev_a + 1;
bool intersects2 = IntrRay3Box3.Intersects(ref ray, ref boxes[prev_b], min_dist);
if (intersects2)
{
find_min_distance(ref ray, ref min_dist, ref min_dist_seg, ref min_dist_segt, ref min_dist_rayt, 2 * bi + 1, prev_start, prev_layer);
}
}
void find_closest_ray_intersction(ref Ray3d ray, double radius, ref int nearestSegment, ref double nearest_ray_t, int bi, int iLayerStart, int iLayer)
{
// hit polygon layer, check segments
if (iLayer == 0)
{
int seg_i = 2 * bi;
Segment3d seg_a = Curve.GetSegment(seg_i);
double segt, rayt;
double segdist_sqr = DistRay3Segment3.SquaredDistance(ref ray, ref seg_a, out rayt, out segt);
if (segdist_sqr <= radius * radius && rayt < nearest_ray_t)
{
nearestSegment = seg_i;
nearest_ray_t = rayt;
}
if ((seg_i + 1) < Curve.SegmentCount)
{
Segment3d seg_b = Curve.GetSegment(seg_i + 1);
segdist_sqr = DistRay3Segment3.SquaredDistance(ref ray, ref seg_b, out rayt, out segt);
if (segdist_sqr <= radius * radius && rayt < nearest_ray_t)
{
nearestSegment = seg_i + 1;
nearest_ray_t = rayt;
}
}
return;
}
// test both boxes and recurse
// TODO: verify that this intersection strategy makes sense?
int prev_layer = iLayer - 1;
int prev_count = layer_counts[prev_layer];
int prev_start = iLayerStart - prev_count;
int prev_a = prev_start + 2 * bi;
bool intersects = IntrRay3Box3.Intersects(ref ray, ref boxes[prev_a], radius);
if (intersects)
{
find_closest_ray_intersction(ref ray, radius, ref nearestSegment, ref nearest_ray_t, 2 * bi, prev_start, prev_layer);
}
if ((2 * bi + 1) >= prev_count)
{
return;
}
int prev_b = prev_a + 1;
bool intersects2 = IntrRay3Box3.Intersects(ref ray, ref boxes[prev_b], radius);
if (intersects2)
{
find_closest_ray_intersction(ref ray, radius, ref nearestSegment, ref nearest_ray_t, 2 * bi + 1, prev_start, prev_layer);
}
}