git.stella-ops.org/src/__Libraries/StellaOps.ElkSharp/ElkShapeBoundaries.Intersections.cs

namespace StellaOps.ElkSharp;

internal static partial class ElkShapeBoundaries
{
    internal static bool TryProjectGatewayDiagonalBoundary(
        ElkPositionedNode node,
        ElkPoint anchor,
        ElkPoint fallbackBoundary,
        out ElkPoint boundaryPoint)
    {
        boundaryPoint = default!;
        if (!IsGatewayShape(node))
        {
            return false;
        }

        var candidates = new List<ElkPoint>();
        var projectedAnchor = ProjectOntoShapeBoundary(node, anchor);
        AddGatewayCandidate(node, candidates, projectedAnchor);
        AddGatewayCandidate(node, candidates, fallbackBoundary);
        AddGatewayCandidate(node, candidates, ProjectOntoShapeBoundary(node, fallbackBoundary));

        foreach (var vertex in BuildGatewayBoundaryPoints(node))
        {
            AddGatewayCandidate(node, candidates, vertex);
        }

        var centerX = node.X + (node.Width / 2d);
        var centerY = node.Y + (node.Height / 2d);
        var directionX = Math.Sign(centerX - anchor.X);
        var directionY = Math.Sign(centerY - anchor.Y);
        var diagonalDirections = new HashSet<(int X, int Y)>();
        if (directionX != 0 && directionY != 0)
        {
            diagonalDirections.Add((directionX, directionY));
        }

        var fallbackDirectionX = Math.Sign(fallbackBoundary.X - anchor.X);
        var fallbackDirectionY = Math.Sign(fallbackBoundary.Y - anchor.Y);
        if (fallbackDirectionX != 0 && fallbackDirectionY != 0)
        {
            diagonalDirections.Add((fallbackDirectionX, fallbackDirectionY));
        }

        foreach (var diagonalDirection in diagonalDirections)
        {
            if (TryIntersectGatewayRay(
                    node,
                    anchor.X,
                    anchor.Y,
                    diagonalDirection.X,
                    diagonalDirection.Y,
                    out var rayBoundary))
            {
                AddGatewayCandidate(node, candidates, rayBoundary);
            }
        }

        var bestCandidate = default(ElkPoint?);
        var bestScore = double.PositiveInfinity;
        foreach (var candidate in candidates)
        {
            var score = ScoreGatewayBoundaryCandidate(node, anchor, projectedAnchor, candidate);
            if (score >= bestScore)
            {
                continue;
            }

            bestScore = score;
            bestCandidate = candidate;
        }

        if (bestCandidate is null)
        {
            return false;
        }

        boundaryPoint = PreferGatewayEdgeInteriorBoundary(node, bestCandidate, anchor);
        return true;
    }

    internal static ElkPoint IntersectPolygonBoundary(
        double originX,
        double originY,
        double deltaX,
        double deltaY,
        IReadOnlyList<ElkPoint> polygon)
    {
        var bestScale = double.PositiveInfinity;
        ElkPoint? bestPoint = null;
        for (var index = 0; index < polygon.Count; index++)
        {
            var start = polygon[index];
            var end = polygon[(index + 1) % polygon.Count];
            if (!TryIntersectRayWithSegment(originX, originY, deltaX, deltaY, start, end, out var scale, out var point))
            {
                continue;
            }

            if (scale < bestScale)
            {
                bestScale = scale;
                bestPoint = point;
            }
        }

        return bestPoint ?? new ElkPoint
        {
            X = originX + deltaX,
            Y = originY + deltaY,
        };
    }

    internal static bool TryIntersectRayWithSegment(
        double originX,
        double originY,
        double deltaX,
        double deltaY,
        ElkPoint segmentStart,
        ElkPoint segmentEnd,
        out double scale,
        out ElkPoint point)
    {
        scale = double.PositiveInfinity;
        point = default!;

        var segmentDeltaX = segmentEnd.X - segmentStart.X;
        var segmentDeltaY = segmentEnd.Y - segmentStart.Y;
        var denominator = Cross(deltaX, deltaY, segmentDeltaX, segmentDeltaY);
        if (Math.Abs(denominator) <= 0.001d)
        {
            return false;
        }

        var relativeX = segmentStart.X - originX;
        var relativeY = segmentStart.Y - originY;
        var rayScale = Cross(relativeX, relativeY, segmentDeltaX, segmentDeltaY) / denominator;
        var segmentScale = Cross(relativeX, relativeY, deltaX, deltaY) / denominator;
        if (rayScale < 0d || segmentScale < 0d || segmentScale > 1d)
        {
            return false;
        }

        scale = rayScale;
        point = new ElkPoint
        {
            X = originX + (deltaX * rayScale),
            Y = originY + (deltaY * rayScale),
        };
        return true;
    }

    internal static double Cross(double ax, double ay, double bx, double by)
    {
        return (ax * by) - (ay * bx);
    }

    private static bool TryIntersectGatewayRay(
        ElkPositionedNode node,
        double originX,
        double originY,
        double deltaX,
        double deltaY,
        out ElkPoint boundaryPoint)
    {
        var polygon = BuildGatewayBoundaryPoints(node);
        var bestScale = double.PositiveInfinity;
        ElkPoint? bestPoint = null;
        for (var index = 0; index < polygon.Count; index++)
        {
            var start = polygon[index];
            var end = polygon[(index + 1) % polygon.Count];
            if (!TryIntersectRayWithSegment(originX, originY, deltaX, deltaY, start, end, out var scale, out var point))
            {
                continue;
            }

            if (scale < bestScale)
            {
                bestScale = scale;
                bestPoint = point;
            }
        }

        boundaryPoint = bestPoint ?? default!;
        return bestPoint is not null;
    }

    private static void AddGatewayCandidate(
        ElkPositionedNode node,
        ICollection<ElkPoint> candidates,
        ElkPoint candidate)
    {
        if (!IsPointOnGatewayBoundary(node, candidate, 2d))
        {
            return;
        }

        if (candidates.Any(existing =>
                Math.Abs(existing.X - candidate.X) <= CoordinateTolerance
                && Math.Abs(existing.Y - candidate.Y) <= CoordinateTolerance))
        {
            return;
        }

        candidates.Add(candidate);
    }

    private static double ScoreGatewayBoundaryCandidate(
        ElkPositionedNode node,
        ElkPoint anchor,
        ElkPoint projectedAnchor,
        ElkPoint candidate)
    {
        var towardCenterX = (node.X + (node.Width / 2d)) - anchor.X;
        var towardCenterY = (node.Y + (node.Height / 2d)) - anchor.Y;
        var candidateDeltaX = candidate.X - anchor.X;
        var candidateDeltaY = candidate.Y - anchor.Y;
        var towardDot = (candidateDeltaX * towardCenterX) + (candidateDeltaY * towardCenterY);
        if (towardDot <= 0d)
        {
            return double.PositiveInfinity;
        }

        var absDx = Math.Abs(candidateDeltaX);
        var absDy = Math.Abs(candidateDeltaY);
        var isDiagonal = absDx >= 3d && absDy >= 3d;
        var diagonalPenalty = isDiagonal
            ? Math.Abs(absDx - absDy)
            : 10_000d;
        var projectedDistance = Math.Abs(candidate.X - projectedAnchor.X) + Math.Abs(candidate.Y - projectedAnchor.Y);
        var segmentLength = Math.Sqrt((candidateDeltaX * candidateDeltaX) + (candidateDeltaY * candidateDeltaY));
        var candidateNearVertex = IsNearGatewayVertex(node, candidate, GatewayVertexTolerance);
        var projectedNearVertex = IsNearGatewayVertex(node, projectedAnchor, GatewayVertexTolerance);
        var vertexPenalty = candidateNearVertex
            ? projectedNearVertex
                ? 4d
                : 24d
            : 0d;

        return diagonalPenalty + (segmentLength * 0.05d) + (projectedDistance * 0.1d) + vertexPenalty;
    }
}