/// <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 Vector3d PointAt(int iSegment, double fSegT)
{
Segment3d seg = new Segment3d(vertices[iSegment], vertices[(iSegment + 1) % vertices.Count]);
return(seg.PointAt(fSegT));
}