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); }