private static Point Intersect(double[][] l1, double[][] l2)
{
if (l1 == null || l2 == null || l1.Equals(l2))
return null;
var p1 = new Vector(l1[0][0], l1[0][1]);
var p2 = new Vector(l2[0][0], l2[0][1]);
var d1 = new Vector(l1[1][0], l1[1][1]) - p1;
var d2 = new Vector(l2[1][0], l2[1][1]) - p2;
var d1xd2 = Vector.CrossProduct(d1, d2);
if (d1xd2 == 0)
return null;
var d = p2 - p1;
var cross1 = Vector.CrossProduct(d, d1 / d1xd2);
if (cross1 < 0 || cross1 > 1)
return null;
var cross2 = Vector.CrossProduct(d, d2 / d1xd2);
if (cross2 < 0 || cross2 > 1)
return null;
return p1 + cross2 * d1;
}
private static double Distance(Vector p1, Vector p2, Vector p)
{
var d = Math.Pow(Distance(p1, p2), 2);
if (d == 0)
return Distance(p, p1);
var dot = Vector.DotProduct(p - p1, p2 - p1) / d;
if (dot < 0)
return Distance(p, p1);
if (dot > 1)
return Distance(p, p2);
return Distance(p, p1 + ((p2 - p1) * dot));
}