public double DistToPoint(Point2D p, out Point2D footPos)
{
double minDist = this.points.Min(x => x.DistTo(p));
Point2D minPoint = this.points.First(x => x.DistTo(p) == minDist);
if (minPoint == this.points.First())
{
LineSeg seg1 = new LineSeg(this.points[0], this.points[1]);
LineSegPointRelation relation1 = new LineSegPointRelation(seg1, p);
if (relation1.Inner)
{
footPos = relation1.Foot;
return(relation1.Dist);
}
else
{
footPos = minPoint;
return(minDist);
}
}
else if (minPoint == this.points.Last())
{
int n = this.points.Count;
LineSeg seg1 = new LineSeg(this.points[n - 2], this.points[n - 1]);
LineSegPointRelation relation1 = new LineSegPointRelation(seg1, p);
if (relation1.Inner)
{
footPos = relation1.Foot;
return(relation1.Dist);
}
else
{
footPos = minPoint;
return(minDist);
}
}
else
{
int index = this.points.IndexOf(minPoint);
LineSeg seg1 = new LineSeg(this.points[index - 1], this.points[index]);
LineSeg seg2 = new LineSeg(this.points[index + 1], this.points[index]);
LineSegPointRelation relation1 = new LineSegPointRelation(seg1, p);
LineSegPointRelation relation2 = new LineSegPointRelation(seg2, p);
if (relation1.Inner)
{
footPos = relation1.Foot;
return(relation1.Dist);
}
else if (relation2.Inner)
{
footPos = relation2.Foot;
return(relation2.Dist);
}
else
{
footPos = minPoint;
return(minDist);
}
}
}
public LineSegIntersect(LineSeg l1, LineSeg l2)
{
A1 = l1.A;
B1 = l1.B;
A2 = l2.A;
B2 = l2.B;
}
public LineSegIntersect(LineSeg l1, LineSeg l2)
{
A = l1.p[0];
B = l1.p[1];
C = l2.p[0];
D = l2.p[1];
}
public LineSegPointRelation(LineSeg line, Point2D point)
{
A = line.p[0];
B = line.p[1];
C = point;
if (line.IsPointOn(point))
{
D = C;
lambda = (A.X - C.X) / (C.X - B.X);
if (double.IsNaN(lambda))
{
lambda = (A.Y - C.Y) / (C.Y - B.Y);
}
}
else
{
lambda = (-A.X * A.X + A.X * B.X + A.X * C.X - B.X * C.X - A.Y * A.Y + A.Y * B.Y + A.Y * C.Y - B.Y * C.Y) / (A.X * B.X - B.X * B.X - A.X * C.X + B.X * C.X + A.Y * B.Y - B.Y * B.Y - A.Y * C.Y + B.Y * C.Y); // from Mathematica
D = line.GetDivPoint(lambda);
}
}
public static double DistToPoint(this PointString poly, Vector p, out Vector footPos)
{
var points = poly.Points;
double minDist = points.Min(x => x.Dist(p));
var minPoint = points.First(x => x.Dist(p) == minDist);
if (minPoint.Equals(points.First()))
{
LineSeg seg1 = new LineSeg(points[0], points[1]);
LineSegPointRelation relation1 = new LineSegPointRelation(seg1, p);
if (relation1.Inner)
{
footPos = relation1.Foot;
return(relation1.Dist);
}
else
{
footPos = minPoint;
return(minDist);
}
}
else if (minPoint.Equals(points.Last()))
{
int n = points.Count;
LineSeg seg1 = new LineSeg(points[n - 2], points[n - 1]);
LineSegPointRelation relation1 = new LineSegPointRelation(seg1, p);
if (relation1.Inner)
{
footPos = relation1.Foot;
return(relation1.Dist);
}
else
{
footPos = minPoint;
return(minDist);
}
}
else
{
int index = points.IndexOf(minPoint);
LineSeg seg1 = new LineSeg(points[index - 1], points[index]);
LineSeg seg2 = new LineSeg(points[index + 1], points[index]);
LineSegPointRelation relation1 = new LineSegPointRelation(seg1, p);
LineSegPointRelation relation2 = new LineSegPointRelation(seg2, p);
if (relation1.Inner)
{
footPos = relation1.Foot;
return(relation1.Dist);
}
else if (relation2.Inner)
{
footPos = relation1.Foot;
return(relation2.Dist);
}
else
{
footPos = minPoint;
return(minDist);
}
}
}