public static double Distance(Trajectory2D trajectory, TPoint2D p)
{
double minDistance = double.MaxValue;
for (var i = 0; i < trajectory.Count - 1; i++)
{
var p1 = trajectory[i];
var p2 = trajectory[i + 1];
var A = p.X - p1.X;
var B = p.Y - p1.Y;
var C = p2.X - p1.X;
var D = p2.Y - p1.Y;
var dot = A * C + B * D;
var distanceSquared = C * C + D * D;
double param = -1;
if (distanceSquared != 0.0) //in case of 0 length line
{
param = dot / distanceSquared;
}
double closestX;
double closestY;
if (param < 0)
{
closestX = p1.X;
closestY = p1.Y;
}
else if (param > 1)
{
closestX = p2.X;
closestY = p2.Y;
}
else
{
closestX = p1.X + param * C;
closestY = p1.Y + param * D;
}
var lineDistance = Distance(p.X, p.Y, closestX, closestY);
if (lineDistance < minDistance)
{
minDistance = lineDistance;
}
}
return(minDistance);
}
public static double Distance(TPoint2D start, TPoint2D end, TPoint2D p)
{
return(Distance(new Trajectory2D {
start, end
}, p));
}
public static double Distance(TPoint2D p1, TPoint2D p2)
{
return(Distance(p1.X, p1.Y, p2.X, p2.Y));
}
public static double Dot(TPoint2D p1, TPoint2D p2)
{
return(p1.X * p2.X + p1.Y * p2.Y);
}
public static int Orient(TPoint2D start, TPoint2D end, TPoint2D point)
{
return(Orient(start.AsVector(), end.AsVector(), point.AsVector()));
}
public static double Angle(TPoint2D p1, TPoint2D p2)
{
var angle1 = Math.Atan2(p2.Y - p1.Y, p2.X - p1.X);
return(SimplifyRadians(angle1));
}
public static double Cross(TPoint2D p1, TPoint2D p2)
{
return(p1.X * p2.Y - p1.Y * p2.X);
}