/// <summary> /// Finds the intersection of two lanes edges then proceeds to inscribe a circle of sufficient radii within /// the corner formed by this intersection. /// </summary> public bool Solve(out RoundedCorner results) { results = new RoundedCorner(); // Find Intersection between left and right road edges { var found = false; var iterators = new RoadSplineIterators(LeftBuffer, RightBuffer); do { int i = iterators.LeftIdx, j = iterators.RightIdx; if (!Utilities.GeometryUtility.LineSegmentIntersection2D( LeftBuffer[i - 1].pos.xz, LeftBuffer[i].pos.xz, RightBuffer[j + 1].pos.xz, RightBuffer[j].pos.xz, out m_EdgeIntersection)) { continue; } found = true; m_LeftIntersectIdx = i; m_RightIntersectIdx = j; break; } while (iterators.TryNextIteration()); if (!found) { LogNoEdgeIntersectionWarning(); return(false); } } // Inscribe a circle into the corner of the detected intersection { var foundCenter = false; Iterators = new RoadSplineIterators( LeftBuffer, RightBuffer, m_LeftIntersectIdx, m_RightIntersectIdx, m_EdgeIntersection); do { if (!InscribeCircleInCorner(out results)) { continue; } foundCenter = true; break; } while (Iterators.TryNextIteration()); if (!foundCenter) { LogCannotInscribeCircleWarning(CornerRadius); return(false); } } return(true); }
/// <summary> /// This function checks the edge case where the inscribed circle's radius can pivot on a particular point /// without exceeding the bounds established by the line segment spanning linePoint1 and linePoint2. /// /// Returns true if the edge case conditions are met. In addition, the inscribed circle information that meets /// said conditions will also be output. /// </summary> public static bool CheckPivotRadiusEdgeCase( float2 pivotPoint, float2 linePoint1, float2 linePoint2, float cornerRadius, out RoundedCorner results) { var p2 = linePoint2 - linePoint1; var pivot = pivotPoint - linePoint1; var angle = math.atan2(p2.y, p2.x); var sin = math.sin(angle); var cos = math.cos(angle); var rotation = new float2x2( cos, sin, -sin, cos); p2 = math.mul(rotation, p2); pivot = math.mul(rotation, pivot); var signedCornerRadius = math.sign(pivot.y) * cornerRadius; var pivotAngle = math.asin((pivot.y - signedCornerRadius) / signedCornerRadius); var tangentX = pivot.x + math.cos(pivotAngle) * cornerRadius; var newTangent = new float2(tangentX, 0f); var center = new float2(tangentX, signedCornerRadius); if (tangentX >= 0 && tangentX <= p2.x) { sin = math.sin(-angle); cos = math.cos(-angle); rotation = new float2x2( cos, sin, -sin, cos); newTangent = math.mul(rotation, newTangent); center = math.mul(rotation, center); newTangent += linePoint1; center += linePoint1; results = new RoundedCorner { LeftTangent = pivotPoint, RightTangent = newTangent, Center = center }; return(true); } results = new RoundedCorner(); return(false); }
/// <summary> /// Checks if the current subsection of line segments being iterated over satisfies one of 4 conditions /// indicating that a circle of a target radius could be inscribed between said segments. /// /// An interactive simulation of each of these 4 RoundedCornerSolver conditions can be visualized using the /// CornerSolverVisualizer script located within the SimViz TestProject. /// </summary> bool InscribeCircleInCorner(out RoundedCorner results) { results = new RoundedCorner(); int i = Iterators.LeftIdx, j = Iterators.RightIdx; var leftSample1 = i == m_LeftIntersectIdx ? m_EdgeIntersection : LeftBuffer[i - 1].pos.xz; var leftSample2 = LeftBuffer[i].pos.xz; var rightSample1 = j == m_RightIntersectIdx ? m_EdgeIntersection : RightBuffer[j + 1].pos.xz; var rightSample2 = RightBuffer[j].pos.xz; var leftVec = math.normalize(leftSample2 - leftSample1); var rightVec = math.normalize(rightSample2 - rightSample1); if (math.cross(new float3(leftVec.x, 0f, leftVec.y), new float3(rightVec.x, 0f, rightVec.y)).y <= 0f) { return(false); } // Check if corner is between line segments ls1->ls2 and rs1->rs2 Utilities.GeometryUtility.LineIntersection2D( leftSample1, leftSample2, rightSample1, rightSample2, out var intersection); var halfAngle = Utilities.GeometryUtility.AngleBetweenVectors(rightVec, leftVec) / 2f; var tan = math.tan(halfAngle); var leftMinRadius = math.distance(intersection, leftSample1) * tan; var leftMaxRadius = math.distance(intersection, leftSample2) * tan; var rightMinRadius = math.distance(intersection, rightSample1) * tan; var rightMaxRadius = math.distance(intersection, rightSample2) * tan; if (leftMinRadius <= CornerRadius && CornerRadius <= leftMaxRadius && rightMinRadius <= CornerRadius && CornerRadius <= rightMaxRadius) { var projectDist = CornerRadius / tan; results.LeftTangent = intersection + leftVec * projectDist; results.RightTangent = intersection + rightVec * projectDist; var halfVec = math.normalize(math.normalize(leftVec) + math.normalize(rightVec)); results.Center = intersection + halfVec * (CornerRadius / math.sin(halfAngle)); return(true); } float2 leftSample3 = new float2(), rightSample3 = new float2(); // Check if corner radius is pivoting on leftSample2 if (i + 1 < LeftBuffer.Length) { leftSample3 = LeftBuffer[i + 1].pos.xz; var leftVec2 = math.normalize(leftSample3 - leftSample2); Utilities.GeometryUtility.LineIntersection2D( leftSample2, leftSample3, rightSample1, rightSample2, out var intersectionLeft); var halfAngle2 = Utilities.GeometryUtility.AngleBetweenVectors(rightVec, leftVec2) / 2f; var tan2 = math.tan(halfAngle2); var leftMinRadius2 = math.distance(intersectionLeft, leftSample2) * tan2; if (CornerRadius > leftMaxRadius && CornerRadius < leftMinRadius2 && CheckPivotRadiusEdgeCase(leftSample2, rightSample1, rightSample2, CornerRadius, out var leftPivotResults)) { results = leftPivotResults; return(true); } } // Check if corner radius is pivoting on rightSample2 if (j - 1 >= 0) { rightSample3 = RightBuffer[j - 1].pos.xz; var rightVec2 = math.normalize(rightSample3 - rightSample2); Utilities.GeometryUtility.LineIntersection2D( leftSample1, leftSample2, rightSample2, rightSample3, out var intersection3); var halfAngle3 = Utilities.GeometryUtility.AngleBetweenVectors(rightVec2, leftVec) / 2f; var tan3 = math.tan(halfAngle3); var rightMinRadius3 = math.distance(intersection3, rightSample2) * tan3; if (CornerRadius > rightMaxRadius && CornerRadius < rightMinRadius3 && CheckPivotRadiusEdgeCase(rightSample2, leftSample1, leftSample2, CornerRadius, out var rightPivotResults)) { results = rightPivotResults.Reverse; return(true); } } if (i + 1 >= LeftBuffer.Length || j - 1 < 0) { return(false); } // Check if corner radius is pivoting simultaneously on leftSample2 and rightSample2 float leftMinRadius4, rightMinRadius4; var midPoint = (leftSample2 + rightSample2) / 2; var dist = math.distance(leftSample2, rightSample2) / 2; float angle1, angle2; { var vec = math.normalize(leftSample2 - rightSample2); var vec2 = math.normalize(rightSample3 - rightSample2); vec2 = new float2(-vec2.y, vec2.x); angle1 = Utilities.GeometryUtility.SignedAngleBetweenVectors(vec2, vec); var projDist = math.tan(angle1) * dist; var normal = new float2(vec.y, -vec.x); rightMinRadius4 = math.distance(rightSample2, midPoint + normal * projDist); } { var vec = math.normalize(rightSample2 - leftSample2); var vec2 = math.normalize(leftSample3 - leftSample2); vec2 = new float2(vec2.y, -vec2.x); angle2 = Utilities.GeometryUtility.SignedAngleBetweenVectors(vec, vec2); var projDist = math.tan(angle2) * dist; var normal = new float2(-vec.y, vec.x); leftMinRadius4 = math.distance(leftSample2, midPoint + normal * projDist); } if (!(angle1 > 0f) || !(angle2 > 0f) || !(leftMaxRadius < CornerRadius) || !(CornerRadius < leftMinRadius4) || !(rightMaxRadius < CornerRadius) || !(CornerRadius < rightMinRadius4)) { return(false); } results.LeftTangent = leftSample2; results.RightTangent = rightSample2; var midpoint = (leftSample2 + rightSample2) / 2; var midPointDist = math.distance(midpoint, rightSample2); var distToCenterFromMidPoint = math.sqrt((CornerRadius * CornerRadius) - (midPointDist * midPointDist)); var toPoint = math.normalize(rightSample2 - leftSample2); toPoint = new float2(-toPoint.y, toPoint.x); results.Center = midpoint + toPoint * distToCenterFromMidPoint; return(true); }