2D Segment and Infinite Line Intersection Algorithm
I am trying to build logic to detect when lines might intersect by extending only one of the lines.
Here, we have have segments. A, B, C, D, E, F. Each segment will have "Two Points".
We always need to compare two segments. One can be extended and the other is constant in its current state.
if we compare A to C, we would get "false".
if we compare B to C, we would get "true"
if we compare D to C, we would get "false" since no matter how long you can extend D, it will still not intersect C
if we compare E to C, we would get "false" since no matter how long you can extend E, it will still not intersect C
if we compare F to C, we would get "true"
The below image is just an illustration of extending .
I think you are looking for this:
static void Main(string[] args)
{
var A = new Segment(Point.Cartesian(1, 1), Point.Cartesian(5, 2));
var B = new Segment(Point.Cartesian(7, 2), Point.Cartesian(7, 4));
if (A.AsRay.Intersect(B, out var point))
{
Console.WriteLine(point);
// (7, 2.5)
}
}
To make it happen in a way that makes sense to me I had to implement the following classes
Vector.cs
public readonly struct Vector
{
public Vector(double uX, double uY) : this()
{
UX=uX;
UY=uY;
}
public static readonly Vector Zero = new Vector(0, 0);
public static readonly Vector UnitX = new Vector(1, 0);
public static readonly Vector UnitY = new Vector(0, 1);
public static Vector Cartesian(double ux, double uy)
=> new Vector(ux, uy);
public static Vector Polar(double r, double θ)
=> new Vector(r*Math.Cos(θ), r*Math.Sin(θ));
public double UX { get; }
public double UY { get; }
public double SumSquares { get => UX*UX+UY*UY; }
public double Magnitude { get => Math.Sqrt(SumSquares); }
public Vector Unit()
{
double m2 = UX*UX+UY*UY;
if (m2>0)
{
return this/Math.Sqrt(m2);
}
return this;
}
}
Point.cs
public readonly struct Point
{
/// <summary>
/// Initializes a <see cref="Point"/> from <code>(x,y)</code> coordinates.
/// </summary>
/// <param name="x">The x.</param>
/// <param name="y">The y.</param>
public Point(double x, double y) : this()
{
X=x;
Y=y;
}
public static readonly Point Origin = new Point(0, 0);
public static Point Cartesian(double ux, double uy)
=> new Point(ux, uy);
public static Point Polar(double r, double θ)
=> new Point(r*Math.Cos(θ), r*Math.Sin(θ));
public double X { get; }
public double Y { get; }
/// <summary>
/// Find the point where two lines intersect, if it exists.
/// </summary>
/// <param name="g">The first line.</param>
/// <param name="h">The second line.</param>
public static bool Intersect(Line g, Line h, out Point point)
{
double d = g.A*h.B - h.A * g.B;
if (d!=0)
{
point = new Point(
(g.B*h.C - h.B*g.C)/d,
(h.A*g.C - g.A*h.C)/d);
return true;
}
point = Point.Origin;
return false;
}
public double DistanceTo(Point target)
=> Math.Sqrt((X-target.X)*(X-target.X) + (Y-target.Y)*(Y-target.Y));
}
Line.cs
public readonly struct Line
{
/// <summary>
/// Initializes a <see cref="Line"/> from the <code>(a,b,c)</code> coordinates.
/// Note that the equation of the line is <code>a*x+b*y+c=0</code>
/// </summary>
/// <param name="a">The a coefficient.</param>
/// <param name="b">The b coefficient.</param>
/// <param name="c">The c constant.</param>
public Line(double a, double b, double c) : this()
{
A=a;
B=b;
C=c;
}
public static readonly Line AlongX = new Line(0, 1, 0);
public static readonly Line AlongY = new Line(1, 0, 0);
public static readonly Line Infinity = new Line(0, 0, 1);
/// <summary>
/// Find the line that joins two points
/// </summary>
/// <param name="p">The first point.</param>
/// <param name="q">The second point.</param>
/// <returns></returns>
public static bool TryJoin(Point p, Point q, out Line line)
{
if (!p.Equals(q))
{
line = new Line(p.Y-q.Y,q.X-p.X, p.X * q.Y - p.Y * q.X);
return true;
}
line = Line.Infinity;
return false;
}
public double A { get; }
public double B { get; }
public double C { get; }
public Vector Direction { get => Vector.Cartesian(-B, A).Unit(); }
public Vector Normal { get => Vector.Cartesian(-A*C, -B*C).Unit(); }
public Line Normalized()
{
double m = Math.Sqrt(A*A+B*B);
if (m>0)
{
return new Line(A/m, B/m, C/m);
}
return this;
}
/// <summary>
/// The line origin is the point on the line closest to the origin.
/// </summary>
public Point Origin => Project(Point.Origin);
/// <summary>
/// Find a point on the line a specified distance from the
/// line origin.
/// </summary>
/// <param name="distance">The distance.</param>
public Point PointAlong(double distance)
{
double m2 = A*A+B*B;
double m = Math.Sqrt(m2);
return new Point(
-(B*distance*m+A*C)/m2,
(A*distance*m-B*C)/m2);
}
/// <summary>
/// Distances the along.
/// </summary>
/// <param name="point">The point.</param>
public double DistanceAlong(Point point)
=> (A*point.Y-B*point.X)/Math.Sqrt(A*A+B*B);
/// <summary>
/// Find point on line closest to target point
/// </summary>
/// <param name="target">The target point.</param>
/// <returns></returns>
public Point Project(Point target)
{
double m2 = A*A+B*B;
return new Point(
(B*(B*target.X-A*target.Y)-A*C)/m2,
(A*(A*target.Y-B*target.X)-B*C)/m2);
}
/// <summary>
/// Find perpendicular distance to a point
/// </summary>
/// <param name="point">The point.</param>
/// <param name="signed">signed distance flag.</param>
/// <returns>The distance to a point. The value might be negative
/// if point is "below" the line and the signed flag is turned on.</returns>
public double DistanceTo(Point point, bool signed = false)
{
var d = A*point.X+B*point.Y+C;
var m = Math.Sqrt( A*A+B*B );
return signed ? d/m : Math.Abs(d)/m;
}
/// <summary>
/// Determines whether this line contains a point.
/// </summary>
/// <param name="point">The point.</param>
/// <param name="tolerance">The length tolerance to use.</param>
public bool Contains(Point point, double tolerance = 1e-11)
{
return DistanceTo(point, false)<=tolerance;
}
}
Segment.cs
public readonly struct Segment
{
public Segment(Point start, Point end) : this()
{
Start=start;
End=end;
}
public Segment(Ray ray, double distance)
: this(ray.Start, ray.PointAlong(distance))
{ }
public Point Start { get; }
public Point End { get; }
/// <summary>
/// Gets the infinite line through the segment.
/// </summary>
public Line InfiniteLine
{
get
{
if (Line.TryJoin(Start, End, out var line))
{
return line;
}
return line;
}
}
public Ray AsRay => new Ray(Start, End);
public bool Contains(Point point, double tolerance = 1e-11)
{
var L = InfiniteLine;
if (L.Contains(point, tolerance))
{
point = L.Project(point);
var d = L.DistanceAlong(point);
var dA = L.DistanceAlong(Start);
var dB = L.DistanceAlong(End);
var t = (d-dA)/(dB-dA);
return t>=0 || t<=1;
}
return false;
}
}
Ray.cs
public readonly struct Ray
{
public Ray(Point start, Vector direction) : this()
{
Start=start;
Direction=direction.Unit();
}
public Ray(Point start, Point end) : this(start, end-start) { }
public Point Start { get; }
public Vector Direction { get; }
public Point PointAlong(double distance)
=> Start + distance * Direction;
public Ray Flip => new Ray(Start, -Direction);
public Line InfiniteLine
{
get
{
double d = Start.X * Direction.UY - Start.Y * Direction.UX;
return new Line(-Direction.UY, Direction.UX, d);
}
}
/// <summary>
/// Determines whether this ray contains a point.
/// </summary>
/// <param name="target">The target point.</param>
/// <param name="tolerance">The distance tolerance.</param>
public bool Contains(Point target, double tolerance = 1e-11)
{
var L = InfiniteLine;
if (L.Contains(target, tolerance))
{
double d = L.DistanceAlong(target);
double dA = L.DistanceAlong(Start);
return d<=dA;
}
return false;
}
/// <summary>
/// Determines whether this ray intersects a line segment, and return the
/// intersection point.
/// </summary>
/// <param name="target">The target segment.</param>
/// <param name="tolerance">The distance tolerance.</param>
public bool Intersect(Segment target, out Point point, double tolerance = 1e-11)
{
if (Point.Intersect(InfiniteLine, target.InfiniteLine, out point))
{
return Contains(point, tolerance)
&& target.Contains(point, tolerance);
}
return false;
}
}
Some notable capabilities are:
-
Point.Intersect(Line g, Line h, out Point point)find the intersection of two infinite lines. -
Line.TryJoin(Point p, Point q, out Line line)find line the joins two points. -
Line.DistanceAlong(Point point)distance along a line from the origin (point on line closest to coordinate origin) to the target point. -
Line.Project(Point target)find the point on line closest to target point. -
Line.Contains(Point point)check if a point lies on the line (within tolerance). -
Ray.Contains(Point point)check if a point lies on the ray and is in the direction the ray defines. -
Ray.Intersect(Segment target, out Point point)check if ray intersects a segment an returns the point of intersection.