/// <summary>
/// Returns the distance between the closest points on the line and the ray
/// </summary>
public static float LineRay(Vector2 lineOrigin, Vector2 lineDirection, Vector2 rayOrigin, Vector2 rayDirection)
{
Vector2 rayOriginToLineOrigin = lineOrigin - rayOrigin;
float denominator = VectorE.PerpDot(lineDirection, rayDirection);
float perpDotA = VectorE.PerpDot(lineDirection, rayOriginToLineOrigin);
if (Mathf.Abs(denominator) < Geometry.Epsilon)
{
// Parallel
float perpDotB = VectorE.PerpDot(rayDirection, rayOriginToLineOrigin);
if (Mathf.Abs(perpDotA) > Geometry.Epsilon || Mathf.Abs(perpDotB) > Geometry.Epsilon)
{
// Not collinear
float rayOriginProjection = Vector2.Dot(lineDirection, rayOriginToLineOrigin);
float distanceSqr = rayOriginToLineOrigin.sqrMagnitude - rayOriginProjection * rayOriginProjection;
// distanceSqr can be negative
return(distanceSqr <= 0 ? 0 : Mathf.Sqrt(distanceSqr));
}
// Collinear
return(0);
}
// Not parallel
float rayDistance = perpDotA / denominator;
if (rayDistance < -Geometry.Epsilon)
{
// No intersection
float rayOriginProjection = Vector2.Dot(lineDirection, rayOriginToLineOrigin);
Vector2 linePoint = lineOrigin - lineDirection * rayOriginProjection;
return(Vector2.Distance(linePoint, rayOrigin));
}
// Point intersection
return(0);
}
/// <summary>
/// Returns the distance between the closest points on the line and the ray
/// </summary>
public static float LineRay(Vector2 lineOrigin, Vector2 lineDirection, Vector2 rayOrigin, Vector2 rayDirection)
{
Vector2 rayOriginToLineOrigin = lineOrigin - rayOrigin;
float denominator = VectorE.PerpDot(lineDirection, rayDirection);
float perpDotA = VectorE.PerpDot(lineDirection, rayOriginToLineOrigin);
if (Mathf.Abs(denominator) < Geometry.Epsilon)
{
float perpDotB = VectorE.PerpDot(rayDirection, rayOriginToLineOrigin);
// Parallel
if (Mathf.Abs(perpDotA) > Geometry.Epsilon || Mathf.Abs(perpDotB) > Geometry.Epsilon)
{
// Not collinear
float dotA = Vector2.Dot(lineDirection, rayOriginToLineOrigin);
float distanceSqr = rayOriginToLineOrigin.sqrMagnitude - dotA * dotA;
return(Mathf.Sqrt(distanceSqr));
}
// Collinear
return(0);
}
// Not parallel
float rayDistance = perpDotA / denominator;
if (rayDistance < -Geometry.Epsilon)
{
// No intersection
Vector2 linepoint = lineOrigin - lineDirection * Vector2.Dot(lineDirection, rayOriginToLineOrigin);
return(Vector2.Distance(rayOrigin, linepoint));
}
// Point intersection
return(0);
}
/// <summary>
/// Computes an intersection of the lines
/// </summary>
public static IntersectionType LineLine(Vector2 originA, Vector2 directionA, Vector2 originB, Vector2 directionB,
out float distanceA, out float distanceB)
{
Vector2 originBToA = originA - originB;
float denominator = VectorE.PerpDot(directionA, directionB);
float perpDotA = VectorE.PerpDot(directionA, originBToA);
float perpDotB = VectorE.PerpDot(directionB, originBToA);
if (Mathf.Abs(denominator) < Geometry.Epsilon)
{
// Parallel
if (Mathf.Abs(perpDotA) > Geometry.Epsilon || Mathf.Abs(perpDotB) > Geometry.Epsilon)
{
// Not collinear
distanceA = 0;
distanceB = 0;
return(IntersectionType.None);
}
// Collinear
distanceA = 0;
distanceB = 0;
return(IntersectionType.Line);
}
// Not parallel
distanceA = perpDotB / denominator;
distanceB = perpDotA / denominator;
return(IntersectionType.Point);
}
/// <summary>
/// Computes an intersection of the line and the ray
/// </summary>
public static bool LineRay(Vector2 lineOrigin, Vector2 lineDirection, Vector2 rayOrigin, Vector2 rayDirection,
out IntersectionLineRay2 intersection)
{
Vector2 rayOriginToLineOrigin = lineOrigin - rayOrigin;
float denominator = VectorE.PerpDot(lineDirection, rayDirection);
float perpDotA = VectorE.PerpDot(lineDirection, rayOriginToLineOrigin);
if (Mathf.Abs(denominator) < Geometry.Epsilon)
{
// Parallel
float perpDotB = VectorE.PerpDot(rayDirection, rayOriginToLineOrigin);
if (Mathf.Abs(perpDotA) > Geometry.Epsilon || Mathf.Abs(perpDotB) > Geometry.Epsilon)
{
// Not collinear
intersection = IntersectionLineRay2.None();
return(false);
}
// Collinear
intersection = IntersectionLineRay2.Ray(rayOrigin);
return(true);
}
// Not parallel
float rayDistance = perpDotA / denominator;
if (rayDistance > -Geometry.Epsilon)
{
intersection = IntersectionLineRay2.Point(rayOrigin + rayDirection * rayDistance);
return(true);
}
intersection = IntersectionLineRay2.None();
return(false);
}
/// <summary>
/// Computes an intersection of the lines
/// </summary>
public static bool IntersectLineLine(Vector2 originA, Vector2 directionA, Vector2 originB, Vector2 directionB,
out IntersectionLineLine2 intersection)
{
intersection = new IntersectionLineLine2();
Vector2 originBToA = originA - originB;
float denominator = VectorE.PerpDot(directionA, directionB);
float perpDotB = VectorE.PerpDot(directionB, originBToA);
if (Mathf.Abs(denominator) < Epsilon)
{
// Parallel
float perpDotA = VectorE.PerpDot(directionA, originBToA);
if (Mathf.Abs(perpDotA) > Epsilon || Mathf.Abs(perpDotB) > Epsilon)
{
// Not collinear
intersection.type = IntersectionType.None;
return(false);
}
// Collinear
intersection.type = IntersectionType.Line;
intersection.point = originA;
return(true);
}
// Not parallel
intersection.type = IntersectionType.Point;
intersection.point = originA + directionA * (perpDotB / denominator);
return(true);
}
/// <summary>
/// Finds closest points on the lines
/// </summary>
public static void ClosestPointsOnLines(Vector2 originA, Vector2 directionA, Vector2 originB, Vector2 directionB,
out Vector2 pointA, out Vector2 pointB)
{
Vector2 originBToA = originA - originB;
float denominator = VectorE.PerpDot(directionA, directionB);
float perpDotB = VectorE.PerpDot(directionB, originBToA);
if (Mathf.Abs(denominator) < Epsilon)
{
// Parallel
if (Mathf.Abs(perpDotB) > Epsilon ||
Mathf.Abs(VectorE.PerpDot(directionA, originBToA)) > Epsilon)
{
// Not collinear
pointA = originA;
pointB = originB + directionB * Vector2.Dot(directionB, originBToA);
return;
}
// Collinear
pointA = pointB = originA;
return;
}
// Not parallel
pointA = pointB = originA + directionA * (perpDotB / denominator);
}
/// <summary>
/// Computes an intersection of the lines
/// </summary>
public static bool IntersectLineLine(Vector2 originA, Vector2 directionA, Vector2 originB, Vector2 directionB,
out Vector2 intersection)
{
float denominator = VectorE.PerpDot(directionA, directionB);
Vector2 originBToA = originA - originB;
float a = VectorE.PerpDot(directionA, originBToA);
float b = VectorE.PerpDot(directionB, originBToA);
if (Mathf.Abs(denominator) < Epsilon)
{
// Parallel
if (Mathf.Abs(a) > Epsilon || Mathf.Abs(b) > Epsilon)
{
// Not collinear
intersection = Vector2.zero;
return(false);
}
// Collinear
intersection = originA;
return(true);
}
float distanceA = b / denominator;
intersection = originA + distanceA * directionA;
return(true);
}
/// <summary>
/// Computes an intersection of the lines
/// </summary>
public static bool LineLine(Vector2 originA, Vector2 directionA, Vector2 originB, Vector2 directionB,
out IntersectionLineLine2 intersection)
{
Vector2 originBToA = originA - originB;
float denominator = VectorE.PerpDot(directionA, directionB);
float perpDotB = VectorE.PerpDot(directionB, originBToA);
if (Mathf.Abs(denominator) < Geometry.Epsilon)
{
// Parallel
float perpDotA = VectorE.PerpDot(directionA, originBToA);
if (Mathf.Abs(perpDotA) > Geometry.Epsilon || Mathf.Abs(perpDotB) > Geometry.Epsilon)
{
// Not collinear
intersection = IntersectionLineLine2.None();
return(false);
}
// Collinear
intersection = IntersectionLineLine2.Line(originA);
return(true);
}
// Not parallel
intersection = IntersectionLineLine2.Point(originA + directionA * (perpDotB / denominator));
return(true);
}
/// <summary>
/// Tests if the point lies on the segment
/// </summary>
/// <param name="side">
/// -1 if the point is to the left of the segment,
/// 0 if it is on the line,
/// 1 if it is to the right of the segment
/// </param>
public static bool PointSegment(Vector2 point, Vector2 segmentA, Vector2 segmentB, out int side)
{
Vector2 fromAToB = segmentB - segmentA;
float sqrSegmentLength = fromAToB.sqrMagnitude;
if (sqrSegmentLength < Geometry.Epsilon)
{
// The segment is a point
side = 0;
return(point == segmentA);
}
// Normalized direction gives more stable results
Vector2 segmentDirection = fromAToB.normalized;
Vector2 toPoint = point - segmentA;
float perpDot = VectorE.PerpDot(toPoint, segmentDirection);
if (perpDot < -Geometry.Epsilon)
{
side = -1;
return(false);
}
if (perpDot > Geometry.Epsilon)
{
side = 1;
return(false);
}
side = 0;
float pointProjection = Vector2.Dot(segmentDirection, toPoint);
return(pointProjection > -Geometry.Epsilon &&
pointProjection < Mathf.Sqrt(sqrSegmentLength) + Geometry.Epsilon);
}
/// <summary>
/// Tests if the point lies on the ray
/// </summary>
public static bool IntersectPointRay(Vector2 point, Vector2 origin, Vector2 direction)
{
Vector2 toPoint = point - origin;
float perpDot = VectorE.PerpDot(toPoint, direction);
return(-Epsilon < perpDot && perpDot < Epsilon &&
Vector2.Dot(toPoint, direction) > -Epsilon);
}
/// <summary>
/// Tests if the point lies on the ray
/// </summary>
public static bool PointRay(Vector2 point, Vector2 rayOrigin, Vector2 rayDirection)
{
Vector2 toPoint = point - rayOrigin;
float perpDot = VectorE.PerpDot(toPoint, rayDirection);
return(-Geometry.Epsilon < perpDot && perpDot < Geometry.Epsilon &&
Vector2.Dot(rayDirection, toPoint) > -Geometry.Epsilon);
}
/// <summary>
/// Tests if the point lies on the segment
/// </summary>
public static bool PointSegment(Vector2 point, Vector2 segmentA, Vector2 segmentB)
{
Vector2 segmentDirection = segmentB - segmentA;
Vector2 toPoint = point - segmentA;
float perpDot = VectorE.PerpDot(toPoint, segmentDirection);
if (-Geometry.Epsilon < perpDot && perpDot < Geometry.Epsilon)
{
float dotToPoint = Vector2.Dot(segmentDirection, toPoint);
return(dotToPoint > -Geometry.Epsilon &&
dotToPoint < segmentDirection.sqrMagnitude + Geometry.Epsilon);
}
return(false);
}
/// <summary>
/// Tests if the point lies on the segment
/// </summary>
public static bool IntersectPointSegment(Vector2 point, Vector2 segmentA, Vector2 segmentB)
{
Vector2 direction = segmentB - segmentA;
Vector2 toPoint = point - segmentA;
float perpDot = VectorE.PerpDot(toPoint, direction);
if (-Epsilon < perpDot && perpDot < Epsilon)
{
float dotToPoint = Vector2.Dot(toPoint, direction);
return(dotToPoint > -Epsilon &&
dotToPoint < Vector2.Dot(direction, direction) + Epsilon);
}
return(false);
}
/// <summary>
/// Calculates the signed area of the input polygon.
/// </summary>
/// <param name="polygon">Vertices of the polygon.</param>
public static float GetSignedArea(IList <Vector2> polygon)
{
if (polygon.Count < 3)
{
return(0);
}
float a = 0;
for (int i = 0; i < polygon.Count; i++)
{
a += VectorE.PerpDot(polygon.GetLooped(i - 1), polygon[i]);
}
return(a / 2);
}
/// <summary>
/// Computes an intersection of the line and the segment
/// </summary>
public static bool LineSegment(Vector2 lineOrigin, Vector2 lineDirection, Vector2 segmentA, Vector2 segmentB,
out IntersectionLineSegment2 intersection)
{
Vector2 segmentAToOrigin = lineOrigin - segmentA;
Vector2 segmentDirection = segmentB - segmentA;
float denominator = VectorE.PerpDot(lineDirection, segmentDirection);
float perpDotA = VectorE.PerpDot(lineDirection, segmentAToOrigin);
if (Mathf.Abs(denominator) < Geometry.Epsilon)
{
// Parallel
// Normalized direction gives more stable results
float perpDotB = VectorE.PerpDot(segmentDirection.normalized, segmentAToOrigin);
if (Mathf.Abs(perpDotA) > Geometry.Epsilon || Mathf.Abs(perpDotB) > Geometry.Epsilon)
{
// Not collinear
intersection = IntersectionLineSegment2.None();
return(false);
}
// Collinear
bool segmentIsAPoint = segmentDirection.sqrMagnitude < Geometry.Epsilon;
if (segmentIsAPoint)
{
intersection = IntersectionLineSegment2.Point(segmentA);
return(true);
}
bool codirected = Vector2.Dot(lineDirection, segmentDirection) > 0;
if (codirected)
{
intersection = IntersectionLineSegment2.Segment(segmentA, segmentB);
}
else
{
intersection = IntersectionLineSegment2.Segment(segmentB, segmentA);
}
return(true);
}
// Not parallel
float segmentDistance = perpDotA / denominator;
if (segmentDistance > -Geometry.Epsilon && segmentDistance < 1 + Geometry.Epsilon)
{
intersection = IntersectionLineSegment2.Point(segmentA + segmentDirection * segmentDistance);
return(true);
}
intersection = IntersectionLineSegment2.None();
return(false);
}
private static bool PointSegment(Vector2 point, Vector2 segmentA, Vector2 segmentDirection, float sqrSegmentLength)
{
float segmentLength = Mathf.Sqrt(sqrSegmentLength);
segmentDirection /= segmentLength;
Vector2 toPoint = point - segmentA;
float perpDot = VectorE.PerpDot(toPoint, segmentDirection);
if (-Geometry.Epsilon < perpDot && perpDot < Geometry.Epsilon)
{
float pointProjection = Vector2.Dot(segmentDirection, toPoint);
return(pointProjection > -Geometry.Epsilon &&
pointProjection < segmentLength + Geometry.Epsilon);
}
return(false);
}
/// <summary>
/// Tests if the point lies on the line
/// </summary>
/// <param name="side">
/// -1 if the point is to the left of the line,
/// 0 if it is on the line,
/// 1 if it is to the right of the line
/// </param>
public static bool IntersectPointLine(Vector2 point, Vector2 origin, Vector2 direction, out int side)
{
float perpDot = VectorE.PerpDot(point - origin, direction);
if (perpDot < -Epsilon)
{
side = -1;
return(false);
}
if (perpDot > Epsilon)
{
side = 1;
return(false);
}
side = 0;
return(true);
}
/// <summary>
/// Tests if the point lies on the line
/// </summary>
/// <param name="side">
/// -1 if the point is to the left of the line,
/// 0 if it is on the line,
/// 1 if it is to the right of the line
/// </param>
public static bool PointLine(Vector2 point, Vector2 lineOrigin, Vector2 lineDirection, out int side)
{
float perpDot = VectorE.PerpDot(point - lineOrigin, lineDirection);
if (perpDot < -Geometry.Epsilon)
{
side = -1;
return(false);
}
if (perpDot > Geometry.Epsilon)
{
side = 1;
return(false);
}
side = 0;
return(true);
}
/// <summary>
/// Tests if the point lies on the ray
/// </summary>
/// <param name="side">
/// -1 if the point is to the left of the ray,
/// 0 if it is on the line,
/// 1 if it is to the right of the ray
/// </param>
public static bool PointRay(Vector2 point, Vector2 rayOrigin, Vector2 rayDirection, out int side)
{
Vector2 toPoint = point - rayOrigin;
float perpDot = VectorE.PerpDot(toPoint, rayDirection);
if (perpDot < -Geometry.Epsilon)
{
side = -1;
return(false);
}
if (perpDot > Geometry.Epsilon)
{
side = 1;
return(false);
}
side = 0;
return(Vector2.Dot(rayDirection, toPoint) > -Geometry.Epsilon);
}
/// <summary>
/// Tests if the point lies on the ray
/// </summary>
/// <param name="side">
/// -1 if the point is to the left of the ray,
/// 0 if it is on the line,
/// 1 if it is to the right of the ray
/// </param>
public static bool IntersectPointRay(Vector2 point, Vector2 origin, Vector2 direction, out int side)
{
Vector2 toPoint = point - origin;
float perpDot = VectorE.PerpDot(toPoint, direction);
if (perpDot < -Epsilon)
{
side = -1;
return(false);
}
if (perpDot > Epsilon)
{
side = 1;
return(false);
}
side = 0;
return(Vector2.Dot(toPoint, direction) > -Epsilon);
}
/// <summary>
/// Tests if the point lies on the segment
/// </summary>
/// <param name="side">
/// -1 if the point is to the left of the segment,
/// 0 if it is on the line,
/// 1 if it is to the right of the segment
/// </param>
public static bool PointSegment(Vector2 point, Vector2 segmentA, Vector2 segmentB, out int side)
{
Vector2 segmentDirection = segmentB - segmentA;
Vector2 toPoint = point - segmentA;
float perpDot = VectorE.PerpDot(toPoint, segmentDirection);
if (perpDot < -Geometry.Epsilon)
{
side = -1;
return(false);
}
if (perpDot > Geometry.Epsilon)
{
side = 1;
return(false);
}
side = 0;
float dotToPoint = Vector2.Dot(segmentDirection, toPoint);
return(dotToPoint > -Geometry.Epsilon &&
dotToPoint < segmentDirection.sqrMagnitude + Geometry.Epsilon);
}
/// <summary>
/// Tests if the point lies on the segment
/// </summary>
/// <param name="side">
/// -1 if the point is to the left of the segment,
/// 0 if it is on the line,
/// 1 if it is to the right of the segment
/// </param>
public static bool IntersectPointSegment(Vector2 point, Vector2 segmentA, Vector2 segmentB, out int side)
{
Vector2 direction = segmentB - segmentA;
Vector2 toPoint = point - segmentA;
float perpDot = VectorE.PerpDot(toPoint, direction);
if (perpDot < -Epsilon)
{
side = -1;
return(false);
}
if (perpDot > Epsilon)
{
side = 1;
return(false);
}
side = 0;
float dotToPoint = Vector2.Dot(toPoint, direction);
return(dotToPoint > -Epsilon &&
dotToPoint < Vector2.Dot(direction, direction) + Epsilon);
}
/// <summary>
/// Returns the distance between the closest points on the lines defined by origin and direction
/// </summary>
public static float DistanceToLine(Vector2 originA, Vector2 directionA, Vector2 originB, Vector2 directionB)
{
if (Mathf.Abs(VectorE.PerpDot(directionA, directionB)) < Epsilon)
{
// Parallel
Vector2 originBToA = originA - originB;
if (Mathf.Abs(VectorE.PerpDot(directionA, originBToA)) > Epsilon ||
Mathf.Abs(VectorE.PerpDot(directionB, originBToA)) > Epsilon)
{
// Not collinear
float dotA = Vector2.Dot(directionA, originBToA);
float distanceSqr = originBToA.sqrMagnitude - dotA * dotA;
return(distanceSqr < 0 ? 0 : Mathf.Sqrt(distanceSqr));
}
// Collinear
return(0);
}
// Not parallel
return(0);
}
/// <summary>
/// Returns the distance between the closest points on the line and the segment
/// </summary>
public static float LineSegment(Vector2 lineOrigin, Vector2 lineDirection, Vector2 segmentA, Vector2 segmentB)
{
Vector2 segmentAToOrigin = lineOrigin - segmentA;
Vector2 segmentDirection = segmentB - segmentA;
float denominator = VectorE.PerpDot(lineDirection, segmentDirection);
float perpDotA = VectorE.PerpDot(lineDirection, segmentAToOrigin);
if (Mathf.Abs(denominator) < Geometry.Epsilon)
{
// Parallel
// Normalized direction gives more stable results
float perpDotB = VectorE.PerpDot(segmentDirection.normalized, segmentAToOrigin);
if (Mathf.Abs(perpDotA) > Geometry.Epsilon || Mathf.Abs(perpDotB) > Geometry.Epsilon)
{
// Not collinear
float segmentAProjection = Vector2.Dot(lineDirection, segmentAToOrigin);
float distanceSqr = segmentAToOrigin.sqrMagnitude - segmentAProjection * segmentAProjection;
// distanceSqr can be negative
return(distanceSqr <= 0 ? 0 : Mathf.Sqrt(distanceSqr));
}
// Collinear
return(0);
}
// Not parallel
float segmentDistance = perpDotA / denominator;
if (segmentDistance < -Geometry.Epsilon || segmentDistance > 1 + Geometry.Epsilon)
{
// No intersection
Vector2 segmentPoint = segmentA + segmentDirection * Mathf.Clamp01(segmentDistance);
float segmentPointProjection = Vector2.Dot(lineDirection, segmentPoint - lineOrigin);
Vector2 linePoint = lineOrigin + lineDirection * segmentPointProjection;
return(Vector2.Distance(linePoint, segmentPoint));
}
// Point intersection
return(0);
}
/// <summary>
/// Returns the distance between the closest points on the lines
/// </summary>
public static float LineLine(Vector2 originA, Vector2 directionA, Vector2 originB, Vector2 directionB)
{
if (Mathf.Abs(VectorE.PerpDot(directionA, directionB)) < Geometry.Epsilon)
{
// Parallel
Vector2 originBToA = originA - originB;
if (Mathf.Abs(VectorE.PerpDot(directionA, originBToA)) > Geometry.Epsilon ||
Mathf.Abs(VectorE.PerpDot(directionB, originBToA)) > Geometry.Epsilon)
{
// Not collinear
float originBProjection = Vector2.Dot(directionA, originBToA);
float distanceSqr = originBToA.sqrMagnitude - originBProjection * originBProjection;
// distanceSqr can be negative
return(distanceSqr <= 0 ? 0 : Mathf.Sqrt(distanceSqr));
}
// Collinear
return(0);
}
// Not parallel
return(0);
}
/// <summary>
/// Finds closest points on the line and the ray
/// </summary>
public static void ClosestPointsLineRay(Vector2 lineOrigin, Vector2 lineDirection, Vector2 rayOrigin, Vector2 rayDirection,
out Vector2 linePoint, out Vector2 rayPoint)
{
Vector2 rayOriginToLineOrigin = lineOrigin - rayOrigin;
float denominator = VectorE.PerpDot(lineDirection, rayDirection);
float perpDotA = VectorE.PerpDot(lineDirection, rayOriginToLineOrigin);
if (Mathf.Abs(denominator) < Epsilon)
{
float perpDotB = VectorE.PerpDot(rayDirection, rayOriginToLineOrigin);
// Parallel
if (Mathf.Abs(perpDotA) > Epsilon || Mathf.Abs(perpDotB) > Epsilon)
{
// Not collinear
float dotA = Vector2.Dot(lineDirection, rayOriginToLineOrigin);
linePoint = lineOrigin - dotA * lineDirection;
rayPoint = rayOrigin;
return;
}
// Collinear
linePoint = rayPoint = rayOrigin;
return;
}
// Not parallel
float rayDistance = perpDotA / denominator;
if (rayDistance < -Epsilon)
{
// No intersection
linePoint = lineOrigin - lineDirection * Vector2.Dot(lineDirection, rayOriginToLineOrigin);
rayPoint = rayOrigin;
return;
}
// Point intersection
linePoint = rayPoint = rayOrigin + rayDirection * rayDistance;
}
/// <summary>
/// Returns the distance between the closest points on the segments
/// </summary>
public static float SegmentSegment(Vector2 segment1A, Vector2 segment1B, Vector2 segment2A, Vector2 segment2B)
{
Vector2 from2ATo1A = segment1A - segment2A;
Vector2 direction1 = segment1B - segment1A;
Vector2 direction2 = segment2B - segment2A;
float segment1Length = direction1.magnitude;
float segment2Length = direction2.magnitude;
bool segment1IsAPoint = segment1Length < Geometry.Epsilon;
bool segment2IsAPoint = segment2Length < Geometry.Epsilon;
if (segment1IsAPoint && segment2IsAPoint)
{
return(Vector2.Distance(segment1A, segment2A));
}
if (segment1IsAPoint)
{
direction2.Normalize();
return(PointSegment(segment1A, segment2A, segment2B, direction2, segment2Length));
}
if (segment2IsAPoint)
{
direction1.Normalize();
return(PointSegment(segment2A, segment1A, segment1B, direction1, segment1Length));
}
direction1.Normalize();
direction2.Normalize();
float denominator = VectorE.PerpDot(direction1, direction2);
float perpDot1 = VectorE.PerpDot(direction1, from2ATo1A);
float perpDot2 = VectorE.PerpDot(direction2, from2ATo1A);
if (Mathf.Abs(denominator) < Geometry.Epsilon)
{
// Parallel
if (Mathf.Abs(perpDot1) > Geometry.Epsilon || Mathf.Abs(perpDot2) > Geometry.Epsilon)
{
// Not collinear
float segment2AProjection = -Vector2.Dot(direction1, from2ATo1A);
if (segment2AProjection > -Geometry.Epsilon &&
segment2AProjection < segment1Length + Geometry.Epsilon)
{
float distanceSqr = from2ATo1A.sqrMagnitude - segment2AProjection * segment2AProjection;
// distanceSqr can be negative
return(distanceSqr <= 0 ? 0 : Mathf.Sqrt(distanceSqr));
}
Vector2 from1ATo2B = segment2B - segment1A;
float segment2BProjection = Vector2.Dot(direction1, from1ATo2B);
if (segment2BProjection > -Geometry.Epsilon &&
segment2BProjection < segment1Length + Geometry.Epsilon)
{
float distanceSqr = from1ATo2B.sqrMagnitude - segment2BProjection * segment2BProjection;
// distanceSqr can be negative
return(distanceSqr <= 0 ? 0 : Mathf.Sqrt(distanceSqr));
}
if (segment2AProjection < 0 && segment2BProjection < 0)
{
if (segment2AProjection > segment2BProjection)
{
return(Vector2.Distance(segment1A, segment2A));
}
return(Vector2.Distance(segment1A, segment2B));
}
if (segment2AProjection > 0 && segment2BProjection > 0)
{
if (segment2AProjection < segment2BProjection)
{
return(Vector2.Distance(segment1B, segment2A));
}
return(Vector2.Distance(segment1B, segment2B));
}
float segment1AProjection = Vector2.Dot(direction2, from2ATo1A);
Vector2 segment2Point = segment2A + direction2 * segment1AProjection;
return(Vector2.Distance(segment1A, segment2Point));
}
// Collinear
bool codirected = Vector2.Dot(direction1, direction2) > 0;
if (codirected)
{
// Codirected
float segment2AProjection = -Vector2.Dot(direction1, from2ATo1A);
if (segment2AProjection > -Geometry.Epsilon)
{
// 1A------1B
// 2A------2B
return(SegmentSegmentCollinear(segment1A, segment1B, segment2A));
}
else
{
// 1A------1B
// 2A------2B
return(SegmentSegmentCollinear(segment2A, segment2B, segment1A));
}
}
else
{
// Contradirected
float segment2BProjection = Vector2.Dot(direction1, segment2B - segment1A);
if (segment2BProjection > -Geometry.Epsilon)
{
// 1A------1B
// 2B------2A
return(SegmentSegmentCollinear(segment1A, segment1B, segment2B));
}
else
{
// 1A------1B
// 2B------2A
return(SegmentSegmentCollinear(segment2B, segment2A, segment1A));
}
}
}
// Not parallel
float distance1 = perpDot2 / denominator;
float distance2 = perpDot1 / denominator;
if (distance1 < -Geometry.Epsilon || distance1 > segment1Length + Geometry.Epsilon ||
distance2 < -Geometry.Epsilon || distance2 > segment2Length + Geometry.Epsilon)
{
// No intersection
bool codirected = Vector2.Dot(direction1, direction2) > 0;
Vector2 from1ATo2B;
if (!codirected)
{
PTUtils.Swap(ref segment2A, ref segment2B);
direction2 = -direction2;
from1ATo2B = -from2ATo1A;
from2ATo1A = segment1A - segment2A;
distance2 = segment2Length - distance2;
}
else
{
from1ATo2B = segment2B - segment1A;
}
Vector2 segment1Point;
Vector2 segment2Point;
float segment2AProjection = -Vector2.Dot(direction1, from2ATo1A);
float segment2BProjection = Vector2.Dot(direction1, from1ATo2B);
bool segment2AIsAfter1A = segment2AProjection > -Geometry.Epsilon;
bool segment2BIsBefore1B = segment2BProjection < segment1Length + Geometry.Epsilon;
bool segment2AOnSegment1 = segment2AIsAfter1A && segment2AProjection < segment1Length + Geometry.Epsilon;
bool segment2BOnSegment1 = segment2BProjection > -Geometry.Epsilon && segment2BIsBefore1B;
if (segment2AOnSegment1 && segment2BOnSegment1)
{
if (distance2 < -Geometry.Epsilon)
{
segment1Point = segment1A + direction1 * segment2AProjection;
segment2Point = segment2A;
}
else
{
segment1Point = segment1A + direction1 * segment2BProjection;
segment2Point = segment2B;
}
}
else if (!segment2AOnSegment1 && !segment2BOnSegment1)
{
if (!segment2AIsAfter1A && !segment2BIsBefore1B)
{
segment1Point = distance1 < -Geometry.Epsilon ? segment1A : segment1B;
}
else
{
// Not on segment
segment1Point = segment2AIsAfter1A ? segment1B : segment1A;
}
float segment1PointProjection = Vector2.Dot(direction2, segment1Point - segment2A);
segment1PointProjection = Mathf.Clamp(segment1PointProjection, 0, segment2Length);
segment2Point = segment2A + direction2 * segment1PointProjection;
}
else if (segment2AOnSegment1)
{
if (distance2 < -Geometry.Epsilon)
{
segment1Point = segment1A + direction1 * segment2AProjection;
segment2Point = segment2A;
}
else
{
segment1Point = segment1B;
float segment1PointProjection = Vector2.Dot(direction2, segment1Point - segment2A);
segment1PointProjection = Mathf.Clamp(segment1PointProjection, 0, segment2Length);
segment2Point = segment2A + direction2 * segment1PointProjection;
}
}
else
{
if (distance2 > segment2Length + Geometry.Epsilon)
{
segment1Point = segment1A + direction1 * segment2BProjection;
segment2Point = segment2B;
}
else
{
segment1Point = segment1A;
float segment1PointProjection = Vector2.Dot(direction2, segment1Point - segment2A);
segment1PointProjection = Mathf.Clamp(segment1PointProjection, 0, segment2Length);
segment2Point = segment2A + direction2 * segment1PointProjection;
}
}
return(Vector2.Distance(segment1Point, segment2Point));
}
// Point intersection
return(0);
}
/// <summary>
/// Returns the distance between the closest points on the ray and the segment
/// </summary>
public static float RaySegment(Vector2 rayOrigin, Vector2 rayDirection, Vector2 segmentA, Vector2 segmentB)
{
Vector2 segmentAToOrigin = rayOrigin - segmentA;
Vector2 segmentDirection = segmentB - segmentA;
float denominator = VectorE.PerpDot(rayDirection, segmentDirection);
float perpDotA = VectorE.PerpDot(rayDirection, segmentAToOrigin);
// Normalized direction gives more stable results
float perpDotB = VectorE.PerpDot(segmentDirection.normalized, segmentAToOrigin);
if (Mathf.Abs(denominator) < Geometry.Epsilon)
{
// Parallel
float segmentAProjection = -Vector2.Dot(rayDirection, segmentAToOrigin);
Vector2 originToSegmentB = segmentB - rayOrigin;
float segmentBProjection = Vector2.Dot(rayDirection, originToSegmentB);
if (Mathf.Abs(perpDotA) > Geometry.Epsilon || Mathf.Abs(perpDotB) > Geometry.Epsilon)
{
// Not collinear
if (segmentAProjection > -Geometry.Epsilon)
{
float distanceSqr = segmentAToOrigin.sqrMagnitude - segmentAProjection * segmentAProjection;
// distanceSqr can be negative
return(distanceSqr <= 0 ? 0 : Mathf.Sqrt(distanceSqr));
}
if (segmentBProjection > -Geometry.Epsilon)
{
float distanceSqr = originToSegmentB.sqrMagnitude - segmentBProjection * segmentBProjection;
// distanceSqr can be negative
return(distanceSqr <= 0 ? 0 : Mathf.Sqrt(distanceSqr));
}
if (segmentAProjection > segmentBProjection)
{
return(Vector2.Distance(rayOrigin, segmentA));
}
return(Vector2.Distance(rayOrigin, segmentB));
}
// Collinear
if (segmentAProjection > -Geometry.Epsilon || segmentBProjection > -Geometry.Epsilon)
{
// Point or segment intersection
return(0);
}
// No intersection
return(segmentAProjection > segmentBProjection ? -segmentAProjection : -segmentBProjection);
}
// Not parallel
float rayDistance = perpDotB / denominator;
float segmentDistance = perpDotA / denominator;
if (rayDistance < -Geometry.Epsilon ||
segmentDistance < -Geometry.Epsilon || segmentDistance > 1 + Geometry.Epsilon)
{
// No intersection
bool codirected = Vector2.Dot(rayDirection, segmentDirection) > 0;
Vector2 segmentBToOrigin;
if (!codirected)
{
PTUtils.Swap(ref segmentA, ref segmentB);
segmentDirection = -segmentDirection;
segmentBToOrigin = segmentAToOrigin;
segmentAToOrigin = rayOrigin - segmentA;
segmentDistance = 1 - segmentDistance;
}
else
{
segmentBToOrigin = rayOrigin - segmentB;
}
float segmentAProjection = -Vector2.Dot(rayDirection, segmentAToOrigin);
float segmentBProjection = -Vector2.Dot(rayDirection, segmentBToOrigin);
bool segmentAOnRay = segmentAProjection > -Geometry.Epsilon;
bool segmentBOnRay = segmentBProjection > -Geometry.Epsilon;
if (segmentAOnRay && segmentBOnRay)
{
if (segmentDistance < 0)
{
Vector2 rayPoint = rayOrigin + rayDirection * segmentAProjection;
Vector2 segmentPoint = segmentA;
return(Vector2.Distance(rayPoint, segmentPoint));
}
else
{
Vector2 rayPoint = rayOrigin + rayDirection * segmentBProjection;
Vector2 segmentPoint = segmentB;
return(Vector2.Distance(rayPoint, segmentPoint));
}
}
else if (!segmentAOnRay && segmentBOnRay)
{
if (segmentDistance < 0)
{
Vector2 rayPoint = rayOrigin;
Vector2 segmentPoint = segmentA;
return(Vector2.Distance(rayPoint, segmentPoint));
}
else if (segmentDistance > 1 + Geometry.Epsilon)
{
Vector2 rayPoint = rayOrigin + rayDirection * segmentBProjection;
Vector2 segmentPoint = segmentB;
return(Vector2.Distance(rayPoint, segmentPoint));
}
else
{
Vector2 rayPoint = rayOrigin;
float originProjection = Vector2.Dot(segmentDirection, segmentAToOrigin);
Vector2 segmentPoint = segmentA + segmentDirection * originProjection / segmentDirection.sqrMagnitude;
return(Vector2.Distance(rayPoint, segmentPoint));
}
}
else
{
// Not on ray
Vector2 rayPoint = rayOrigin;
float originProjection = Vector2.Dot(segmentDirection, segmentAToOrigin);
float sqrSegmentLength = segmentDirection.sqrMagnitude;
if (originProjection < 0)
{
return(Vector2.Distance(rayPoint, segmentA));
}
else if (originProjection > sqrSegmentLength)
{
return(Vector2.Distance(rayPoint, segmentB));
}
else
{
Vector2 segmentPoint = segmentA + segmentDirection * originProjection / sqrSegmentLength;
return(Vector2.Distance(rayPoint, segmentPoint));
}
}
}
// Point intersection
return(0);
}
/// <summary>
/// Returns the distance between the closest points on the rays
/// </summary>
public static float RayRay(Vector2 originA, Vector2 directionA, Vector2 originB, Vector2 directionB)
{
Vector2 originBToA = originA - originB;
float denominator = VectorE.PerpDot(directionA, directionB);
float perpDotA = VectorE.PerpDot(directionA, originBToA);
float perpDotB = VectorE.PerpDot(directionB, originBToA);
bool codirected = Vector2.Dot(directionA, directionB) > 0;
if (Mathf.Abs(denominator) < Geometry.Epsilon)
{
// Parallel
float originBProjection = -Vector2.Dot(directionA, originBToA);
if (Mathf.Abs(perpDotA) > Geometry.Epsilon || Mathf.Abs(perpDotB) > Geometry.Epsilon)
{
// Not collinear
if (!codirected && originBProjection < Geometry.Epsilon)
{
return(Vector2.Distance(originA, originB));
}
float distanceSqr = originBToA.sqrMagnitude - originBProjection * originBProjection;
// distanceSqr can be negative
return(distanceSqr <= 0 ? 0 : Mathf.Sqrt(distanceSqr));
}
// Collinear
if (codirected)
{
// Ray intersection
return(0);
}
else
{
if (originBProjection < Geometry.Epsilon)
{
// No intersection
return(Vector2.Distance(originA, originB));
}
else
{
// Segment intersection
return(0);
}
}
}
// Not parallel
float distanceA = perpDotB / denominator;
float distanceB = perpDotA / denominator;
if (distanceA < -Geometry.Epsilon || distanceB < -Geometry.Epsilon)
{
// No intersection
if (codirected)
{
float originAProjection = Vector2.Dot(directionB, originBToA);
if (originAProjection > -Geometry.Epsilon)
{
Vector2 rayPointA = originA;
Vector2 rayPointB = originB + directionB * originAProjection;
return(Vector2.Distance(rayPointA, rayPointB));
}
float originBProjection = -Vector2.Dot(directionA, originBToA);
if (originBProjection > -Geometry.Epsilon)
{
Vector2 rayPointA = originA + directionA * originBProjection;
Vector2 rayPointB = originB;
return(Vector2.Distance(rayPointA, rayPointB));
}
return(Vector2.Distance(originA, originB));
}
else
{
if (distanceA > -Geometry.Epsilon)
{
float originBProjection = -Vector2.Dot(directionA, originBToA);
if (originBProjection > -Geometry.Epsilon)
{
Vector2 rayPointA = originA + directionA * originBProjection;
Vector2 rayPointB = originB;
return(Vector2.Distance(rayPointA, rayPointB));
}
}
else if (distanceB > -Geometry.Epsilon)
{
float originAProjection = Vector2.Dot(directionB, originBToA);
if (originAProjection > -Geometry.Epsilon)
{
Vector2 rayPointA = originA;
Vector2 rayPointB = originB + directionB * originAProjection;
return(Vector2.Distance(rayPointA, rayPointB));
}
}
return(Vector2.Distance(originA, originB));
}
}
// Point intersection
return(0);
}
/// <summary>
/// Computes an intersection of the rays
/// </summary>
public static bool IntersectRayRay(Vector2 originA, Vector2 directionA, Vector2 originB, Vector2 directionB,
out IntersectionRayRay2 intersection)
{
intersection = new IntersectionRayRay2();
Vector2 originBToA = originA - originB;
float denominator = VectorE.PerpDot(directionA, directionB);
float perpDotA = VectorE.PerpDot(directionA, originBToA);
float perpDotB = VectorE.PerpDot(directionB, originBToA);
if (Mathf.Abs(denominator) < Epsilon)
{
// Parallel
if (Mathf.Abs(perpDotA) > Epsilon || Mathf.Abs(perpDotB) > Epsilon)
{
// Not collinear
intersection.type = IntersectionType.None;
return(false);
}
// Collinear
bool codirected = Vector2.Dot(directionA, directionB) > 0;
float dotA = Vector2.Dot(directionA, originBToA);
if (codirected)
{
intersection.type = IntersectionType.Ray;
intersection.pointA = dotA > 0 ? originA : originB;
intersection.pointB = directionA;
return(true);
}
else
{
if (dotA > 0)
{
intersection.type = IntersectionType.None;
return(false);
}
else
{
intersection.type = IntersectionType.Segment;
intersection.pointA = originA;
intersection.pointB = originB;
return(true);
}
}
}
// The rays are skew and may intersect in a point
float distanceA = perpDotB / denominator;
if (distanceA < -Epsilon)
{
intersection.type = IntersectionType.None;
return(false);
}
float distanceB = perpDotA / denominator;
if (distanceB < -Epsilon)
{
intersection.type = IntersectionType.None;
return(false);
}
intersection.type = IntersectionType.Point;
intersection.pointA = originA + directionA * distanceA;
return(true);
}