public static Vector2d LineIntersectionPoint2d(Line2d l1, Line2d l2) { double x1 = l1.u.x, y1 = l1.u.y; double x2 = l2.u.x, y2 = l2.u.y; Vector2d d1 = (l1.v - l1.u).normalize(); Vector2d d2 = (l2.v - l2.u).normalize(); double dx = x2 - x1, dy = y2 - y1; double a1 = d1.x, a2 = d1.y, b1 = d2.x, b2 = d2.y; double t2 = (dx * a2 - dy * a1) / (a1 * b2 - b1 * a2); double t1 = 0; if (a1 != 0) { t1 = (dx + b1 * t2) / a1; } else { t1 = (dy + b2 * t2) / a2; } Vector2d vec1 = l1.u + d1 * t1; Vector2d vec2 = l2.u + d2 * t2; return(vec1); }
public static double PointDistToLine(Vector2d pt, Line2d line) { Vector2d footPoint = FindPointTolineFootPrint(pt, line); return((pt - footPoint).Length()); }