/// <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.Length(); float segment2Length = direction2.Length(); bool segment1IsAPoint = segment1Length < Geometry.Epsilon; bool segment2IsAPoint = segment2Length < Geometry.Epsilon; if (segment1IsAPoint && segment2IsAPoint) { return(segment1A.DistanceTo(segment2A)); } if (segment1IsAPoint) { direction2.Normalized(); return(PointSegment(segment1A, segment2A, segment2B, direction2, segment2Length)); } if (segment2IsAPoint) { direction1.Normalized(); return(PointSegment(segment2A, segment1A, segment1B, direction1, segment1Length)); } direction1.Normalized(); direction2.Normalized(); 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 = -direction1.Dot(from2ATo1A); if (segment2AProjection > -Geometry.Epsilon && segment2AProjection < segment1Length + Geometry.Epsilon) { float distanceSqr = from2ATo1A.LengthSquared() - segment2AProjection * segment2AProjection; // distanceSqr can be negative return(distanceSqr <= 0 ? 0 : Mathf.Sqrt(distanceSqr)); } Vector2 from1ATo2B = segment2B - segment1A; float segment2BProjection = direction1.Dot(from1ATo2B); if (segment2BProjection > -Geometry.Epsilon && segment2BProjection < segment1Length + Geometry.Epsilon) { float distanceSqr = from1ATo2B.LengthSquared() - segment2BProjection * segment2BProjection; // distanceSqr can be negative return(distanceSqr <= 0 ? 0 : Mathf.Sqrt(distanceSqr)); } if (segment2AProjection < 0 && segment2BProjection < 0) { if (segment2AProjection > segment2BProjection) { return(segment1A.DistanceTo(segment2A)); } return(segment1A.DistanceTo(segment2B)); } if (segment2AProjection > 0 && segment2BProjection > 0) { if (segment2AProjection < segment2BProjection) { return(segment1B.DistanceTo(segment2A)); } return(segment1B.DistanceTo(segment2B)); } float segment1AProjection = direction2.Dot(from2ATo1A); Vector2 segment2Point = segment2A + direction2 * segment1AProjection; return(segment1A.DistanceTo(segment2Point)); } // Collinear bool codirected = direction1.Dot(direction2) > 0; if (codirected) { // Codirected float segment2AProjection = -direction1.Dot(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 = direction1.Dot(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 = direction1.Dot(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 = -direction1.Dot(from2ATo1A); float segment2BProjection = direction1.Dot(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 = direction2.Dot(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 = direction2.Dot(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 = direction2.Dot(segment1Point - segment2A); segment1PointProjection = Mathf.Clamp(segment1PointProjection, 0, segment2Length); segment2Point = segment2A + direction2 * segment1PointProjection; } } return(segment1Point.DistanceTo(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 = -rayDirection.Dot(segmentAToOrigin); Vector2 originToSegmentB = segmentB - rayOrigin; float segmentBProjection = rayDirection.Dot(originToSegmentB); if (Mathf.Abs(perpDotA) > Geometry.Epsilon || Mathf.Abs(perpDotB) > Geometry.Epsilon) { // Not collinear if (segmentAProjection > -Geometry.Epsilon) { float distanceSqr = segmentAToOrigin.LengthSquared() - segmentAProjection * segmentAProjection; // distanceSqr can be negative return(distanceSqr <= 0 ? 0 : Mathf.Sqrt(distanceSqr)); } if (segmentBProjection > -Geometry.Epsilon) { float distanceSqr = originToSegmentB.LengthSquared() - segmentBProjection * segmentBProjection; // distanceSqr can be negative return(distanceSqr <= 0 ? 0 : Mathf.Sqrt(distanceSqr)); } if (segmentAProjection > segmentBProjection) { return(rayOrigin.DistanceTo(segmentA)); } return(rayOrigin.DistanceTo(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 = rayDirection.Dot(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 = -rayDirection.Dot(segmentAToOrigin); float segmentBProjection = -rayDirection.Dot(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(rayPoint.DistanceTo(segmentPoint)); } else { Vector2 rayPoint = rayOrigin + rayDirection * segmentBProjection; Vector2 segmentPoint = segmentB; return(rayPoint.DistanceTo(segmentPoint)); } } else if (!segmentAOnRay && segmentBOnRay) { if (segmentDistance < 0) { Vector2 rayPoint = rayOrigin; Vector2 segmentPoint = segmentA; return(rayPoint.DistanceTo(segmentPoint)); } else if (segmentDistance > 1 + Geometry.Epsilon) { Vector2 rayPoint = rayOrigin + rayDirection * segmentBProjection; Vector2 segmentPoint = segmentB; return(rayPoint.DistanceTo(segmentPoint)); } else { Vector2 rayPoint = rayOrigin; float originProjection = segmentDirection.Dot(segmentAToOrigin); Vector2 segmentPoint = segmentA + segmentDirection * originProjection / segmentDirection.LengthSquared(); return(rayPoint.DistanceTo(segmentPoint)); } } else { // Not on ray Vector2 rayPoint = rayOrigin; float originProjection = segmentDirection.Dot(segmentAToOrigin); float sqrSegmentLength = segmentDirection.LengthSquared(); if (originProjection < 0) { return(rayPoint.DistanceTo(segmentA)); } else if (originProjection > sqrSegmentLength) { return(rayPoint.DistanceTo(segmentB)); } else { Vector2 segmentPoint = segmentA + segmentDirection * originProjection / sqrSegmentLength; return(rayPoint.DistanceTo(segmentPoint)); } } } // Point intersection return(0); }