/// <summary>
/// find squared distance from p to nearest segment on polyline
/// </summary>
public double DistanceSquared(Vector3d p, out int iNearSeg, out double fNearSegT)
{
iNearSeg = -1;
fNearSegT = double.MaxValue;
double dist = double.MaxValue;
int N = (Closed) ? vertices.Count : vertices.Count - 1;
for (int vi = 0; vi < N; ++vi)
{
int a = vi;
int b = (vi + 1) % vertices.Count;
Segment3d seg = new Segment3d(vertices[a], vertices[b]);
double t = (p - seg.Center).Dot(seg.Direction);
double d = double.MaxValue;
if (t >= seg.Extent)
{
d = seg.P1.DistanceSquared(p);
}
else if (t <= -seg.Extent)
{
d = seg.P0.DistanceSquared(p);
}
else
{
d = (seg.PointAt(t) - p).LengthSquared;
}
if (d < dist)
{
dist = d;
iNearSeg = vi;
fNearSegT = t;
}
}
return(dist);
}
public void AppendLine(Segment3d seg, float size)
{
Frame3f f = new Frame3f(seg.Center);
f.AlignAxis(2, (Vector3f)seg.Direction);
AppendBox(f, new Vector3f(size, size, seg.Extent));
}
static public double MinDistance(Ray3d r, Segment3d s)
{
double rayt, segt;
double dsqr = SquaredDistance(ref r, ref s, out rayt, out segt);
return(Math.Sqrt(dsqr));
}
public Box3d(Segment3d seg)
{
Center = seg.Center;
AxisZ = seg.Direction;
Vector3d.MakePerpVectors(ref AxisZ, out AxisX, out AxisY);
Extent = new Vector3d(0, 0, seg.Extent);
}
static public double MinDistanceSegmentParam(Ray3d r, Segment3d s)
{
double rayt, segt;
/*double dsqr = */ SquaredDistance(ref r, ref s, out rayt, out segt);
return(segt);
}
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);
}
protected bool is_on_edge(int eid, Vector3d v)
{
Index2i ev = Target.GetEdgeV(eid);
Segment3d seg = new Segment3d(Target.GetVertex(ev.a), Target.GetVertex(ev.b));
return(seg.DistanceSquared(v) < VertexSnapTol * VertexSnapTol);
}
protected int on_edge(ref Triangle3d tri, ref Vector3d v)
{
var s01 = new Segment3d(tri.V0, tri.V1);
if (s01.DistanceSquared(v) < VertexSnapTol * VertexSnapTol)
{
return(0);
}
var s12 = new Segment3d(tri.V1, tri.V2);
if (s12.DistanceSquared(v) < VertexSnapTol * VertexSnapTol)
{
return(1);
}
var s20 = new Segment3d(tri.V2, tri.V0);
if (s20.DistanceSquared(v) < VertexSnapTol * VertexSnapTol)
{
return(2);
}
return(-1);
}
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);
}
}
public static void AppendLine(DMesh3 mesh, Segment3d seg, float size)
{
Frame3f f = new Frame3f(seg.Center);
f.AlignAxis(2, (Vector3f)seg.Direction);
MeshEditor editor = new MeshEditor(mesh);
editor.AppendBox(f, new Vector3f(size, size, seg.Extent));
}
void find_min_distance(ref Vector3d pt, ref double min_dist, ref int min_dist_seg, ref double min_dist_segt, 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;
double segdist = seg_a.DistanceSquared(pt, out segt);
if (segdist <= min_dist)
{
min_dist = segdist;
min_dist_seg = seg_i;
min_dist_segt = segt;
}
if ((seg_i + 1) < Curve.SegmentCount)
{
Segment3d seg_b = Curve.GetSegment(seg_i + 1);
segdist = seg_b.DistanceSquared(pt, out segt);
if (segdist <= min_dist)
{
min_dist = segdist;
min_dist_seg = seg_i + 1;
min_dist_segt = segt;
}
}
return;
}
// test both boxes and recurse
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;
double dist = boxes[prev_a].DistanceSquared(pt);
if (dist <= min_dist)
{
find_min_distance(ref pt, ref min_dist, ref min_dist_seg, ref min_dist_segt, 2 * bi, prev_start, prev_layer);
}
if ((2 * bi + 1) >= prev_count)
{
return;
}
int prev_b = prev_a + 1;
double dist2 = boxes[prev_b].DistanceSquared(pt);
if (dist2 <= min_dist)
{
find_min_distance(ref pt, ref min_dist, ref min_dist_seg, ref min_dist_segt, 2 * bi + 1, prev_start, prev_layer);
}
}
public void AppendSegments(double r)
{
foreach (var seg in Segments)
{
Segment3d s = new Segment3d(seg.v0.v, seg.v1.v);
if (Target.FindEdge(seg.v0.vtx_id, seg.v1.vtx_id) == DMesh3.InvalidID)
{
MeshEditor.AppendLine(Target, s, (float)r);
}
}
}
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);
}
}
public Vector3d Project(Vector3d vPoint, int identifier = -1)
{
Vector3d vNearest = Vector3d.Zero;
double fNearestSqr = double.MaxValue;
int N = Curve.VertexCount;
for (int i = 0; i < N; ++i)
{
Segment3d seg = new Segment3d(Curve[i], Curve[(i + 1) % N]);
Vector3d pt = seg.NearestPoint(vPoint);
double dsqr = pt.DistanceSquared(vPoint);
if (dsqr < fNearestSqr)
{
fNearestSqr = dsqr;
vNearest = pt;
}
}
return((fNearestSqr < double.MaxValue) ? vNearest : vPoint);
}
protected DMesh3 MakeDebugGraphMesh()
{
DMesh3 graphMesh = new DMesh3();
graphMesh.EnableVertexColors(Vector3f.One);
foreach (int vid in Graph.VertexIndices()) {
if (TipVertices.Contains(vid)) {
MeshEditor.AppendBox(graphMesh, Graph.GetVertex(vid), 0.3f, Colorf.Green);
} else if (TipBaseVertices.Contains(vid)) {
MeshEditor.AppendBox(graphMesh, Graph.GetVertex(vid), 0.225f, Colorf.Magenta);
} else if (GroundVertices.Contains(vid)) {
MeshEditor.AppendBox(graphMesh, Graph.GetVertex(vid), 0.35f, Colorf.Blue);
} else {
MeshEditor.AppendBox(graphMesh, Graph.GetVertex(vid), 0.15f, Colorf.White);
}
}
foreach (int eid in Graph.EdgeIndices()) {
Segment3d seg = Graph.GetEdgeSegment(eid);
MeshEditor.AppendLine(graphMesh, seg, 0.1f);
}
return graphMesh;
}
int find_nearest_edge(DMesh3 mesh, Vector3d v, HashSet <int> vertices)
{
int near_eid = DMesh3.InvalidID;
double nearSqr = VertexSnapTol * VertexSnapTol;
foreach (int eid in mesh.BoundaryEdgeIndices())
{
Index2i ev = mesh.GetEdgeV(eid);
if (vertices.Contains(ev.a) == false || vertices.Contains(ev.b) == false)
{
continue;
}
Segment3d seg = new Segment3d(mesh.GetVertex(ev.a), mesh.GetVertex(ev.b));
double dSqr = seg.DistanceSquared(v);
if (dSqr < nearSqr)
{
near_eid = eid;
nearSqr = dSqr;
}
}
return(near_eid);
}
static public double MinDistanceLineParam(Line3d line, Segment3d segment)
{
return(new DistLine3Segment3(line, segment).Compute().LineParameter);
}
public double GetSquared()
{
if (DistanceSquared >= 0)
{
return(DistanceSquared);
}
// Compare edges of triangle0 to the interior of triangle1.
double sqrDist = Double.MaxValue, sqrDistTmp;
var edge = new Segment3d();
double ratio;
int i0, i1;
for (i0 = 2, i1 = 0; i1 < 3; i0 = i1++)
{
edge.SetEndpoints(triangle0[i0], triangle0[i1]);
var queryST = new DistSegment3Triangle3(edge, triangle1);
sqrDistTmp = queryST.GetSquared();
if (sqrDistTmp < sqrDist)
{
Triangle0Closest = queryST.SegmentClosest;
Triangle1Closest = queryST.TriangleClosest;
sqrDist = sqrDistTmp;
ratio = queryST.SegmentParam / edge.Extent;
Triangle0BaryCoords = Vector3d.Zero;
Triangle0BaryCoords[i0] = (0.5) * (1 - ratio);
Triangle0BaryCoords[i1] = 1 - Triangle0BaryCoords[i0];
Triangle0BaryCoords[3 - i0 - i1] = 0;
Triangle1BaryCoords = queryST.TriangleBaryCoords;
if (sqrDist <= MathUtil.ZeroTolerance)
{
DistanceSquared = 0;
return(0);
}
}
}
// Compare edges of triangle1 to the interior of triangle0.
for (i0 = 2, i1 = 0; i1 < 3; i0 = i1++)
{
edge.SetEndpoints(triangle1[i0], triangle1[i1]);
var queryST = new DistSegment3Triangle3(edge, triangle0);
sqrDistTmp = queryST.GetSquared();
if (sqrDistTmp < sqrDist)
{
Triangle0Closest = queryST.SegmentClosest;
Triangle1Closest = queryST.TriangleClosest;
sqrDist = sqrDistTmp;
ratio = queryST.SegmentParam / edge.Extent;
Triangle1BaryCoords = Vector3d.Zero;
Triangle1BaryCoords[i0] = (0.5) * (1 - ratio);
Triangle1BaryCoords[i1] = 1 - Triangle1BaryCoords[i0];
Triangle1BaryCoords[3 - i0 - i1] = 0;
Triangle0BaryCoords = queryST.TriangleBaryCoords;
if (sqrDist <= MathUtil.ZeroTolerance)
{
DistanceSquared = 0;
return(0);
}
}
}
DistanceSquared = sqrDist;
return(DistanceSquared);
}
public double GetSquared()
{
if (DistanceSquared >= 0)
{
return(DistanceSquared);
}
// Test if line intersects triangle. If so, the squared distance is zero.
Vector3d edge0 = triangle.V1 - triangle.V0;
Vector3d edge1 = triangle.V2 - triangle.V0;
Vector3d normal = edge0.UnitCross(edge1);
double NdD = normal.Dot(line.Direction);
if (Math.Abs(NdD) > MathUtil.ZeroTolerance)
{
// The line and triangle are not parallel, so the line intersects
// the plane of the triangle.
Vector3d diff = line.Origin - triangle.V0;
Vector3d U = Vector3d.Zero, V = Vector3d.Zero;
Vector3d.GenerateComplementBasis(ref U, ref V, line.Direction);
double UdE0 = U.Dot(edge0);
double UdE1 = U.Dot(edge1);
double UdDiff = U.Dot(diff);
double VdE0 = V.Dot(edge0);
double VdE1 = V.Dot(edge1);
double VdDiff = V.Dot(diff);
double invDet = (1) / (UdE0 * VdE1 - UdE1 * VdE0);
// Barycentric coordinates for the point of intersection.
double b1 = (VdE1 * UdDiff - UdE1 * VdDiff) * invDet;
double b2 = (UdE0 * VdDiff - VdE0 * UdDiff) * invDet;
double b0 = 1 - b1 - b2;
if (b0 >= 0 && b1 >= 0 && b2 >= 0)
{
// Line parameter for the point of intersection.
double DdE0 = line.Direction.Dot(edge0);
double DdE1 = line.Direction.Dot(edge1);
double DdDiff = line.Direction.Dot(diff);
LineParam = b1 * DdE0 + b2 * DdE1 - DdDiff;
// Barycentric coordinates for the point of intersection.
TriangleBaryCoords = new Vector3d(b0, b1, b2);
// The intersection point is inside or on the triangle.
LineClosest = line.Origin + LineParam * line.Direction;
TriangleClosest = triangle.V0 + b1 * edge0 + b2 * edge1;
DistanceSquared = 0;
return(0);
}
}
// Either (1) the line is not parallel to the triangle and the point of
// intersection of the line and the plane of the triangle is outside the
// triangle or (2) the line and triangle are parallel. Regardless, the
// closest point on the triangle is on an edge of the triangle. Compare
// the line to all three edges of the triangle.
double sqrDist = double.MaxValue;
for (int i0 = 2, i1 = 0; i1 < 3; i0 = i1++)
{
Segment3d segment = new Segment3d(triangle[i0], triangle[i1]);
DistLine3Segment3 queryLS = new DistLine3Segment3(line, segment);
double sqrDistTmp = queryLS.GetSquared();
if (sqrDistTmp < sqrDist)
{
LineClosest = queryLS.LineClosest;
TriangleClosest = queryLS.SegmentClosest;
sqrDist = sqrDistTmp;
LineParam = queryLS.LineParameter;
double ratio = queryLS.SegmentParameter / segment.Extent;
TriangleBaryCoords = Vector3d.Zero;
TriangleBaryCoords[i0] = (0.5) * (1 - ratio);
TriangleBaryCoords[i1] = 1 - TriangleBaryCoords[i0];
TriangleBaryCoords[3 - i0 - i1] = 0;
}
}
DistanceSquared = sqrDist;
return(DistanceSquared);
}
// build tree of boxes as sequential array
void build_sequential(DCurve3 curve)
{
int NV = curve.VertexCount;
int N = NV;
int boxCount = 0;
layers = 0;
layer_counts = new List <int>();
// count how many boxes in each layer, building up from initial segments
int bi = 0;
while (N > 1)
{
int layer_boxes = (N / 2) + (N % 2 == 0 ? 0 : 1);
boxCount += layer_boxes;
N = layer_boxes;
layer_counts.Add(layer_boxes);
bi += layer_boxes;
layers++;
}
boxes = new Box3d[boxCount];
bi = 0;
// make first layer
for (int si = 0; si < NV; si += 2)
{
Vector3d v1 = curve[(si + 1) % NV];
Segment3d seg1 = new Segment3d(curve[si], v1);
Box3d box = new Box3d(seg1);
if (si < NV - 1)
{
Segment3d seg2 = new Segment3d(v1, curve[(si + 2) % NV]);
Box3d box2 = new Box3d(seg2);
box = Box3d.Merge(ref box, ref box2);
}
boxes[bi++] = box;
}
// repeatedly build layers until we hit a single box
N = bi;
int prev_layer_start = 0;
bool done = false;
while (done == false)
{
int layer_start = bi;
for (int k = 0; k < N; k += 2)
{
Box3d mbox = Box3d.Merge(ref boxes[prev_layer_start + k], ref boxes[prev_layer_start + k + 1]);
boxes[bi++] = mbox;
}
N = (N / 2) + (N % 2 == 0 ? 0 : 1);
prev_layer_start = layer_start;
if (N == 1)
{
done = true;
}
}
}
// solidBox == false means fully contained segment does not intersect
public IntrSegment3Box3(Segment3d s, Box3d b, bool solidBox)
{
segment = s; box = b; this.solid = solidBox;
}
public DistRay3Segment3(Ray3d rayIn, Segment3d segmentIn)
{
this.ray = rayIn; this.segment = segmentIn;
}
public DistSegment3Triangle3(Segment3d SegmentIn, Triangle3d TriangleIn)
{
this.triangle = TriangleIn; this.segment = SegmentIn;
}
public Vector3d PointAt(int iSegment, double fSegT)
{
Segment3d seg = new Segment3d(vertices[iSegment], vertices[(iSegment + 1) % vertices.Count]);
return(seg.PointAt(fSegT));
}
/// <summary>
/// compute w/o allocating temporaries/etc
/// </summary>
public static double SquaredDistance(ref Ray3d ray, ref Segment3d segment,
out double rayT, out double segT)
{
Vector3d diff = ray.Origin - segment.Center;
double a01 = -ray.Direction.Dot(segment.Direction);
double b0 = diff.Dot(ray.Direction);
double b1 = -diff.Dot(segment.Direction);
double c = diff.LengthSquared;
double det = Math.Abs(1 - a01 * a01);
double s0, s1, sqrDist, extDet;
if (det >= MathUtil.ZeroTolerance)
{
// The Ray and Segment are not parallel.
s0 = a01 * b1 - b0;
s1 = a01 * b0 - b1;
extDet = segment.Extent * det;
if (s0 >= 0)
{
if (s1 >= -extDet)
{
if (s1 <= extDet) // region 0
{
// Minimum at interior points of Ray and Segment.
double invDet = (1) / det;
s0 *= invDet;
s1 *= invDet;
sqrDist = s0 * (s0 + a01 * s1 + (2) * b0) +
s1 * (a01 * s0 + s1 + (2) * b1) + c;
}
else // region 1
{
s1 = segment.Extent;
s0 = -(a01 * s1 + b0);
if (s0 > 0)
{
sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c;
}
else
{
s0 = 0;
sqrDist = s1 * (s1 + (2) * b1) + c;
}
}
}
else // region 5
{
s1 = -segment.Extent;
s0 = -(a01 * s1 + b0);
if (s0 > 0)
{
sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c;
}
else
{
s0 = 0;
sqrDist = s1 * (s1 + (2) * b1) + c;
}
}
}
else
{
if (s1 <= -extDet) // region 4
{
s0 = -(-a01 * segment.Extent + b0);
if (s0 > 0)
{
s1 = -segment.Extent;
sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c;
}
else
{
s0 = 0;
s1 = -b1;
if (s1 < -segment.Extent)
{
s1 = -segment.Extent;
}
else if (s1 > segment.Extent)
{
s1 = segment.Extent;
}
sqrDist = s1 * (s1 + (2) * b1) + c;
}
}
else if (s1 <= extDet) // region 3
{
s0 = 0;
s1 = -b1;
if (s1 < -segment.Extent)
{
s1 = -segment.Extent;
}
else if (s1 > segment.Extent)
{
s1 = segment.Extent;
}
sqrDist = s1 * (s1 + (2) * b1) + c;
}
else // region 2
{
s0 = -(a01 * segment.Extent + b0);
if (s0 > 0)
{
s1 = segment.Extent;
sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c;
}
else
{
s0 = 0;
s1 = -b1;
if (s1 < -segment.Extent)
{
s1 = -segment.Extent;
}
else if (s1 > segment.Extent)
{
s1 = segment.Extent;
}
sqrDist = s1 * (s1 + (2) * b1) + c;
}
}
}
}
else
{
// Ray and Segment are parallel.
if (a01 > 0)
{
// Opposite direction vectors.
s1 = -segment.Extent;
}
else
{
// Same direction vectors.
s1 = segment.Extent;
}
s0 = -(a01 * s1 + b0);
if (s0 > 0)
{
sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c;
}
else
{
s0 = 0;
sqrDist = s1 * (s1 + (2) * b1) + c;
}
}
rayT = s0;
segT = s1;
// Account for numerical round-off errors.
if (sqrDist < 0)
{
sqrDist = 0;
}
return(sqrDist);
}
static public double MinDistance(Ray3d r, Segment3d s) {
return new DistRay3Segment3(r, s).Get();
}
// build tree of boxes as sequential array
void build_sequential(DCurve3 curve)
{
int NV = curve.VertexCount;
int N = (curve.Closed) ? NV : NV - 1;
int boxCount = 0;
layers = 0;
layer_counts = new List <int>();
// count how many boxes in each layer, building up from initial segments
int bi = 0;
while (N > 1)
{
int layer_boxes = (N / 2) + (N % 2 == 0 ? 0 : 1);
boxCount += layer_boxes;
N = layer_boxes;
layer_counts.Add(layer_boxes);
bi += layer_boxes;
layers++;
}
// [RMS] this case happens if N = 1, previous loop is skipped and we have to
// hardcode initialization to this redundant box
if (layers == 0)
{
layers = 1;
boxCount = 1;
layer_counts = new List <int>()
{
1
};
}
boxes = new Box3d[boxCount];
bi = 0;
// make first layer
int NStop = (curve.Closed) ? NV : NV - 1;
for (int si = 0; si < NStop; si += 2)
{
Vector3d v1 = curve[(si + 1) % NV];
var seg1 = new Segment3d(curve[si], v1);
var box = new Box3d(seg1);
if (si < NV - 1)
{
var seg2 = new Segment3d(v1, curve[(si + 2) % NV]);
var box2 = new Box3d(seg2);
box = Box3d.Merge(ref box, ref box2);
}
boxes[bi++] = box;
}
// repeatedly build layers until we hit a single box
N = bi;
if (N == 1)
{
return;
}
int prev_layer_start = 0;
bool done = false;
while (done == false)
{
int layer_start = bi;
for (int k = 0; k < N; k += 2)
{
var mbox = Box3d.Merge(ref boxes[prev_layer_start + k], ref boxes[prev_layer_start + k + 1]);
boxes[bi++] = mbox;
}
N = (N / 2) + (N % 2 == 0 ? 0 : 1);
prev_layer_start = layer_start;
if (N == 1)
{
done = true;
}
}
}
static public double MinDistanceSegmentParam(Ray3d r, Segment3d s) {
return new DistRay3Segment3(r, s).Compute().SegmentParameter;
}
public DistLine3Segment3(Line3d LineIn, Segment3d SegmentIn)
{
this.segment = SegmentIn; this.line = LineIn;
}
static public double MinDistance(Line3d line, Segment3d segment)
{
return(new DistLine3Segment3(line, segment).Get());
}
