Skip to content

Next Gen STEP AP242 Native-ish BREP Geometric Modeling Kernel

21.6 KB
Raw Blame
using Aetheris.Kernel.Core.Math;

namespace Aetheris.Kernel.Firmament.Piping;

/// <summary>
/// Deterministic bounded 3D orthogonal router. Candidate coordinates come from endpoints,
/// hard waypoints, and clearance-inflated obstacle faces; A* explores only adjacent visible
/// coordinate-grid nodes. Cost is length + 1000 mm per turn so fittings dominate small savings.
/// </summary>
public static class OrthogonalAutoRouter
{
    private const double Tol = 1e-7;
    private const double ElbowPenaltyMm = 1000d;
    private static readonly AxisDirection[] DirectionOrder =
    [
        new(0, 0, 1, 0, 0), new(1, 0, -1, 0, 0), new(2, 1, 0, 1, 0),
        new(3, 1, 0, -1, 0), new(4, 2, 0, 0, 1), new(5, 2, 0, 0, -1)
    ];

    public static RouteProposalIr Propose(RouteRequestIr request)
    {
        ArgumentNullException.ThrowIfNull(request);
        var diagnostics = Validate(request);
        if (diagnostics.Count > 0)
            return Failed(request, RouteFailureKind.EndpointDirectionImpossible, diagnostics);

        var inflated = request.KeepOuts.Select(x => x.Inflate(request.PipePolicy.RadiusMm + request.ClearanceMm)).ToArray();
        // Use farther coordinate planes for candidate corners without falsely treating a
        // valid nozzle inside that extra planning margin as collision geometry.
        const int verificationArcIntervals = 128;
        var verificationMargin = request.BendRadiusMm * (1 - Math.Cos(Math.PI / (4 * verificationArcIntervals))) + Tol;
        var candidateBounds = request.KeepOuts.Select(x => x.Inflate(request.PipePolicy.RadiusMm + request.ClearanceMm + request.BendRadiusMm + verificationMargin)).ToArray();
        var required = new[] { request.From.Position }.Concat(request.HardWaypoints).Append(request.To.Position).ToArray();
        var all = new List<Point3D>();
        AxisDirection? incoming = null;
        for (var leg = 0; leg < required.Length - 1; leg++)
        {
            var startRequired = leg == 0 ? Axis(request.From.OutwardDirection) : incoming;
            var endRequired = leg == required.Length - 2 ? Axis(-request.To.OutwardDirection) : null;
            var search = Search(required[leg], required[leg + 1], startRequired, endRequired, candidateBounds, inflated, required, request.BendRadiusMm);
            if (!search.Success)
            {
                var kind = request.HardWaypoints.Count > 0 && leg < required.Length - 2
                    ? RouteFailureKind.HardWaypointUnreachable : search.Failure;
                return Failed(request, kind, [$"piping-autoroute-{Code(kind)}:{request.Name}: {search.Diagnostic}"]);
            }
            if (all.Count > 0) all.RemoveAt(all.Count - 1);
            all.AddRange(search.Points);
            if (search.Points.Count >= 2) incoming = Axis(search.Points[^1] - search.Points[^2]);
        }

        var simplified = Simplify(all);
        var anchors = simplified.Select((point, i) => new RouteAnchorIr(
            $"proposal:{request.Name}:anchor:{i}", i, point,
            i == 0 || i == simplified.Count - 1 ? RouteAnchorKind.Endpoint
            : request.HardWaypoints.Any(w => Near(w, point)) ? RouteAnchorKind.HardWaypoint : RouteAnchorKind.AutoWaypoint)).ToArray();
        var length = Pairwise(simplified).Sum(x => Distance(x.A, x.B));
        var elbows = Math.Max(0, simplified.Count - 2);
        var minimum = MinimumMaterializedClearance(simplified, request.KeepOuts, request.PipePolicy.RadiusMm, request.BendRadiusMm);
        if (minimum + Tol < request.ClearanceMm)
            return Failed(request, RouteFailureKind.NoCollisionFreePath,
                [$"piping-autoroute-fitting-clearance:{request.Name}: realized elbow envelope has {minimum:G6}mm clearance, required {request.ClearanceMm:G6}mm"]);
        return new($"proposal:{request.Name}", request.Name, true, anchors, length, elbows, minimum,
            length + elbows * ElbowPenaltyMm, ["piping-autoroute-deterministic", "piping-route-simplified", "piping-endpoint-directions-respected"], null);
    }

    public static RouteIr Accept(RouteProposalIr proposal, RouteRequestIr request, string routeName)
    {
        if (!proposal.IsSuccess) throw new InvalidOperationException("Cannot accept an unsuccessful route proposal.");
        var anchors = proposal.Anchors.Select((x, i) => x with { StableId = $"route:{routeName}:anchor:{i}", Ordinal = i }).ToArray();
        return BuildRoute(routeName, request.Connection, request.From.StableId, request.To.StableId, anchors,
            request.BendRadiusMm, RouteProvenance.AutoGenerated, $"accepted:{proposal.StableId}", []);
    }

    public static (RouteIr? Route, IReadOnlyList<string> Diagnostics) Reroute(
        RouteIr original, RouteRequestIr environment, int fromAnchor, int toAnchor, IReadOnlyList<KeepOutIr>? additionalKeepOuts = null)
    {
        if (fromAnchor < 0 || toAnchor >= original.Anchors.Count || fromAnchor >= toAnchor)
            return (null, [$"piping-local-reroute-interval-invalid:{original.Name}:{fromAnchor}:{toAnchor}"]);
        if (original.LockedSegmentOrdinals.Any(i => i >= fromAnchor && i < toAnchor))
            return (null, [$"piping-local-reroute-crosses-lock:{original.Name}: locked segment prevents solution"]);

        var obstacles = environment.KeepOuts.Concat(additionalKeepOuts ?? []).DistinctBy(x => x.StableId).ToArray();
        var from = original.Anchors[fromAnchor].Point;
        var to = original.Anchors[toAnchor].Point;
        var startDirection = fromAnchor == 0 ? environment.From.OutwardDirection : original.Anchors[fromAnchor + 1].Point - from;
        var endOutward = toAnchor == original.Anchors.Count - 1 ? environment.To.OutwardDirection : -(to - original.Anchors[toAnchor - 1].Point);
        var request = environment with
        {
            StableId = environment.StableId + ":local",
            Name = environment.Name + "Local",
            From = environment.From with { Position = from, OutwardDirection = NormalizeAxis(startDirection) },
            To = environment.To with { Position = to, OutwardDirection = NormalizeAxis(endOutward) },
            KeepOuts = obstacles,
            HardWaypoints = []
        };
        var proposal = Propose(request);
        if (!proposal.IsSuccess)
            return (null, [$"piping-local-reroute-no-solution:{original.Name}:{string.Join("; ", proposal.Diagnostics)}"]);

        var replacement = proposal.Anchors.Select(x => x.Point).ToArray();
        var points = original.Anchors.Take(fromAnchor).Select(x => x.Point)
            .Concat(replacement)
            .Concat(original.Anchors.Skip(toAnchor + 1).Select(x => x.Point)).ToArray();
        var anchors = points.Select((p, i) => new RouteAnchorIr($"route:{original.Name}:anchor:{i}", i, p,
            i == 0 || i == points.Length - 1 ? RouteAnchorKind.Endpoint : RouteAnchorKind.AutoWaypoint)).ToArray();
        var shiftedLocks = original.LockedSegmentOrdinals.Where(i => i < fromAnchor)
            .Concat(original.LockedSegmentOrdinals.Where(i => i >= toAnchor).Select(i => i + replacement.Length - (toAnchor - fromAnchor + 1))).ToArray();
        return (BuildRoute(original.Name, original.Connection, original.StartInterface, original.EndInterface, anchors,
            environment.BendRadiusMm, RouteProvenance.LocallyRerouted, $"local-reroute:{fromAnchor}:{toAnchor}", shiftedLocks), []);
    }

    public static RouteIr BuildRoute(string name, string connection, string startInterface, string endInterface,
        IReadOnlyList<RouteAnchorIr> anchors, double bendRadius, RouteProvenance provenance, string source, IReadOnlyList<int> locks)
    {
        var segments = Pairwise(anchors).Select((pair, i) => new RouteSegmentIr($"route:{name}:segment:{i}", i,
            pair.A.StableId, pair.B.StableId, Distance(pair.A.Point, pair.B.Point), locks.Contains(i))).ToArray();
        var turns = Enumerable.Range(1, Math.Max(0, anchors.Count - 2)).Select(i =>
            new RouteTurnIr($"route:{name}:fitting:{i}", i, "Standard.Piping.Elbow90", bendRadius)).ToArray();
        return new($"route:{name}", name, connection, startInterface, endInterface, anchors, segments, turns, locks.Order().ToArray(), provenance, source);
    }

    public static IReadOnlyList<Point3D> Simplify(IReadOnlyList<Point3D> points)
    {
        var result = new List<Point3D>();
        foreach (var point in points)
        {
            if (result.Count > 0 && Near(result[^1], point)) continue;
            while (result.Count >= 2 && Collinear(result[^2], result[^1], point)) result.RemoveAt(result.Count - 1);
            result.Add(point);
        }
        return result;
    }

    public static double MinimumClearance(IReadOnlyList<Point3D> points, IReadOnlyList<KeepOutIr> obstacles, double pipeRadius)
    {
        if (obstacles.Count == 0) return 1_000_000_000_000d;
        var best = double.PositiveInfinity;
        foreach (var (a, b) in Pairwise(points))
            foreach (var obstacle in obstacles)
                best = Math.Min(best, SegmentBoxDistance(a, b, obstacle) - pipeRadius);
        return Math.Max(0, best);
    }

    /// <summary>
    /// Independently checks the realized straight cylinders and 90-degree torus envelopes.
    /// Arc sampling subtracts the maximum chord sagitta, making the reported distance a
    /// conservative lower bound rather than an optimistic sample estimate.
    /// </summary>
    public static double MinimumMaterializedClearance(IReadOnlyList<Point3D> points, IReadOnlyList<KeepOutIr> obstacles,
        double pipeRadius, double bendRadius)
    {
        if (obstacles.Count == 0) return 1_000_000_000_000d;
        var best = double.PositiveInfinity;
        for (var i = 0; i + 1 < points.Count; i++)
        {
            var direction = NormalizeAxis(points[i + 1] - points[i]);
            var start = i == 0 ? points[i] : points[i] + direction * bendRadius;
            var end = i == points.Count - 2 ? points[i + 1] : points[i + 1] - direction * bendRadius;
            foreach (var obstacle in obstacles)
                best = Math.Min(best, SegmentBoxDistance(start, end, obstacle) - pipeRadius);
        }

        const int arcIntervals = 128;
        var sagitta = bendRadius * (1 - Math.Cos(Math.PI / (4 * arcIntervals)));
        for (var i = 1; i + 1 < points.Count; i++)
        {
            var u = NormalizeAxis(points[i] - points[i - 1]);
            var v = NormalizeAxis(points[i + 1] - points[i]);
            var center = points[i] - u * bendRadius + v * bendRadius;
            foreach (var obstacle in obstacles)
                for (var sample = 0; sample <= arcIntervals; sample++)
                {
                    var angle = Math.PI * sample / (2 * arcIntervals);
                    var point = center - v * (bendRadius * Math.Cos(angle)) + u * (bendRadius * Math.Sin(angle));
                    best = Math.Min(best, PointBoxDistance(point, obstacle) - pipeRadius - sagitta);
                }
        }
        return Math.Max(0, best);
    }

    private static SearchResult Search(Point3D start, Point3D end, AxisDirection? startRequired, AxisDirection? endRequired,
        IReadOnlyList<KeepOutIr> candidateBounds, IReadOnlyList<KeepOutIr> blockedBounds, IReadOnlyList<Point3D> required, double bendRadius)
    {
        var margin = Math.Max(100d, bendRadius * 2d);
        var requiredX=required.Select(x=>x.X).ToArray();var requiredY=required.Select(x=>x.Y).ToArray();var requiredZ=required.Select(x=>x.Z).ToArray();
        var xs = Coordinates(requiredX, CandidateFaces(candidateBounds.SelectMany(x => new[] { x.Minimum.X, x.Maximum.X }),requiredX,bendRadius), margin);
        var ys = Coordinates(requiredY, CandidateFaces(candidateBounds.SelectMany(x => new[] { x.Minimum.Y, x.Maximum.Y }),requiredY,bendRadius), margin);
        var zs = Coordinates(requiredZ, CandidateFaces(candidateBounds.SelectMany(x => new[] { x.Minimum.Z, x.Maximum.Z }),requiredZ,bendRadius), margin);
        var nodes = new List<Point3D>();
        var index = new Dictionary<GridKey, int>();
        foreach (var x in xs) foreach (var y in ys) foreach (var z in zs)
        {
            var p = new Point3D(x, y, z);
            if (InsideAny(p, blockedBounds) && !Near(p, start) && !Near(p, end)) continue;
            index[new(Q(x), Q(y), Q(z))] = nodes.Count; nodes.Add(p);
        }
        if (!index.TryGetValue(Key(start), out var startNode) || !index.TryGetValue(Key(end), out var endNode))
            return new(false, [], RouteFailureKind.ClearanceTooLarge, "clearance-inflated obstacle contains an endpoint");

        var states = new Dictionary<State, Score>();
        var previous = new Dictionary<State, State>();
        var queue = new PriorityQueue<State, Priority>();
        var initial = new State(startNode, -1);
        states[initial] = new(0, 0, 0);
        queue.Enqueue(initial, new(Manhattan(start, end), 0, 0, startNode, -1));
        State? goal = null;
        while (queue.TryDequeue(out var current, out _))
        {
            var currentScore = states[current];
            var point = nodes[current.Node];
            if (current.Node == endNode && (endRequired is null || current.Direction == endRequired.Value.Order)) { goal = current; break; }
            foreach (var direction in DirectionOrder)
            {
                if (current.Direction < 0 && startRequired is not null && direction.Order != startRequired.Value.Order) continue;
                var neighborKey = Neighbor(point, direction, xs, ys, zs);
                if (neighborKey is null || !index.TryGetValue(neighborKey.Value, out var neighbor)) continue;
                var nextPoint = nodes[neighbor];
                if (SegmentBlocked(point, nextPoint, blockedBounds)) continue;
                var turn = current.Direction >= 0 && current.Direction != direction.Order ? 1 : 0;
                var length = Distance(point, nextPoint);
                var score = new Score(currentScore.Cost + length + turn * ElbowPenaltyMm, currentScore.Length + length, currentScore.Turns + turn);
                var next = new State(neighbor, direction.Order);
                if (states.TryGetValue(next, out var known) && known.CompareTo(score) <= 0) continue;
                states[next] = score; previous[next] = current;
                queue.Enqueue(next, new(score.Cost + Manhattan(nextPoint, end), score.Turns, score.Length, neighbor, direction.Order));
            }
        }
        if (goal is null)
            return new(false, [], startRequired is not null || endRequired is not null ? RouteFailureKind.EndpointDirectionImpossible : RouteFailureKind.RoutingRegionDisconnected,
                "candidate routing graph has no path satisfying obstacle, clearance, and endpoint-direction constraints");
        var path = new List<Point3D>(); var cursor = goal.Value;
        while (true) { path.Add(nodes[cursor.Node]); if (cursor.Equals(initial)) break; cursor = previous[cursor]; }
        path.Reverse();
        return new(true, path, default, string.Empty);
    }

    private static IReadOnlyList<string> Validate(RouteRequestIr r)
    {
        var d = new List<string>();
        if (r.ClearanceMm < 0 || !double.IsFinite(r.ClearanceMm)) d.Add($"piping-clearance-invalid:{r.Name}");
        if (Axis(r.From.OutwardDirection) is null) d.Add($"piping-port-direction-nonorthogonal:{r.From.Name}");
        if (Axis(r.To.OutwardDirection) is null) d.Add($"piping-port-direction-nonorthogonal:{r.To.Name}");
        return d;
    }

    private static RouteProposalIr Failed(RouteRequestIr request, RouteFailureKind kind, IReadOnlyList<string> diagnostics) =>
        new($"proposal:{request.Name}", request.Name, false, [], 0, 0, 0, double.PositiveInfinity, diagnostics, kind);
    private static string Code(RouteFailureKind kind) => kind switch
    {
        RouteFailureKind.NoCollisionFreePath => "no-collision-free-path",
        RouteFailureKind.EndpointDirectionImpossible => "endpoint-direction-impossible",
        RouteFailureKind.ClearanceTooLarge => "clearance-too-large",
        RouteFailureKind.HardWaypointUnreachable => "hard-waypoint-unreachable",
        RouteFailureKind.RoutingRegionDisconnected => "routing-region-disconnected",
        RouteFailureKind.LockedIntervalPreventsSolution => "locked-interval-prevents-solution",
        _ => "failed"
    };
    private static IReadOnlyList<double> Coordinates(IEnumerable<double> required, IEnumerable<double> obstacleFaces, double margin)
    {
        var values = required.Concat(obstacleFaces).DistinctBy(Q).Order().ToList();
        if (values.Count == 0) return [0];
        values.Add(values[0] - margin); values.Add(values.Max() + margin);
        return values.DistinctBy(Q).Order().ToArray();
    }
    private static IEnumerable<double> CandidateFaces(IEnumerable<double> faces,IReadOnlyList<double> required,double bendRadius)=>
        faces.Where(face=>required.All(value=>Math.Abs(face-value)>2*bendRadius+Tol));
    private static GridKey? Neighbor(Point3D p, AxisDirection d, IReadOnlyList<double> xs, IReadOnlyList<double> ys, IReadOnlyList<double> zs)
    {
        var values = d.Axis == 0 ? xs : d.Axis == 1 ? ys : zs;
        var coordinate = d.Axis == 0 ? p.X : d.Axis == 1 ? p.Y : p.Z;
        var i = values.ToList().FindIndex(x => Math.Abs(x - coordinate) <= Tol);
        var n = i + (d.Sign > 0 ? 1 : -1); if (n < 0 || n >= values.Count) return null;
        return d.Axis switch { 0 => new(Q(values[n]), Q(p.Y), Q(p.Z)), 1 => new(Q(p.X), Q(values[n]), Q(p.Z)), _ => new(Q(p.X), Q(p.Y), Q(values[n])) };
    }
    private static bool SegmentBlocked(Point3D a, Point3D b, IReadOnlyList<KeepOutIr> boxes) => boxes.Any(box => SegmentIntersectsInterior(a, b, box));
    private static bool SegmentIntersectsInterior(Point3D a, Point3D b, KeepOutIr box)
    {
        if (Math.Abs(a.X - b.X) > Tol) return Between(a.Y, box.Minimum.Y, box.Maximum.Y) && Between(a.Z, box.Minimum.Z, box.Maximum.Z) && IntervalInterior(a.X, b.X, box.Minimum.X, box.Maximum.X);
        if (Math.Abs(a.Y - b.Y) > Tol) return Between(a.X, box.Minimum.X, box.Maximum.X) && Between(a.Z, box.Minimum.Z, box.Maximum.Z) && IntervalInterior(a.Y, b.Y, box.Minimum.Y, box.Maximum.Y);
        return Between(a.X, box.Minimum.X, box.Maximum.X) && Between(a.Y, box.Minimum.Y, box.Maximum.Y) && IntervalInterior(a.Z, b.Z, box.Minimum.Z, box.Maximum.Z);
    }
    private static bool InsideAny(Point3D p, IReadOnlyList<KeepOutIr> boxes) => boxes.Any(b => Between(p.X,b.Minimum.X,b.Maximum.X)&&Between(p.Y,b.Minimum.Y,b.Maximum.Y)&&Between(p.Z,b.Minimum.Z,b.Maximum.Z));
    private static bool Between(double v, double min, double max) => v > min + Tol && v < max - Tol;
    private static bool IntervalInterior(double a,double b,double min,double max)=>Math.Max(Math.Min(a,b),min)<Math.Min(Math.Max(a,b),max)-Tol;
    private static double SegmentBoxDistance(Point3D a, Point3D b, KeepOutIr box)
    {
        var dx=IntervalDistance(Math.Min(a.X,b.X),Math.Max(a.X,b.X),box.Minimum.X,box.Maximum.X);
        var dy=IntervalDistance(Math.Min(a.Y,b.Y),Math.Max(a.Y,b.Y),box.Minimum.Y,box.Maximum.Y);
        var dz=IntervalDistance(Math.Min(a.Z,b.Z),Math.Max(a.Z,b.Z),box.Minimum.Z,box.Maximum.Z);
        return Math.Sqrt(dx*dx+dy*dy+dz*dz);
    }
    private static double PointBoxDistance(Point3D p,KeepOutIr box)
    {
        var dx=IntervalDistance(p.X,p.X,box.Minimum.X,box.Maximum.X);
        var dy=IntervalDistance(p.Y,p.Y,box.Minimum.Y,box.Maximum.Y);
        var dz=IntervalDistance(p.Z,p.Z,box.Minimum.Z,box.Maximum.Z);
        return Math.Sqrt(dx*dx+dy*dy+dz*dz);
    }
    private static double IntervalDistance(double aMin,double aMax,double bMin,double bMax)=>Math.Max(0,Math.Max(aMin,bMin)-Math.Min(aMax,bMax));
    private static AxisDirection? Axis(Vector3D v)
    {
        var components = new[] { Math.Abs(v.X), Math.Abs(v.Y), Math.Abs(v.Z) }; var axis = Array.IndexOf(components, components.Max());
        if (components[axis] <= Tol || components.Where((_,i)=>i!=axis).Any(x=>x>Tol)) return null;
        var sign = (axis==0?v.X:axis==1?v.Y:v.Z)>0?1:-1; return DirectionOrder.Single(x=>x.Axis==axis&&x.Sign==sign);
    }
    private static Vector3D NormalizeAxis(Vector3D v) { var a=Axis(v)??throw new ArgumentException("Direction must be axis aligned."); return new(a.X,a.Y,a.Z); }
    private static bool Collinear(Point3D a,Point3D b,Point3D c)=>(b-a).Cross(c-b).Length<=Tol;
    private static bool Near(Point3D a,Point3D b)=>Distance(a,b)<=Tol;
    private static double Distance(Point3D a,Point3D b)=>(b-a).Length;
    private static double Manhattan(Point3D a,Point3D b)=>Math.Abs(a.X-b.X)+Math.Abs(a.Y-b.Y)+Math.Abs(a.Z-b.Z);
    private static IEnumerable<(T A,T B)> Pairwise<T>(IReadOnlyList<T> values){for(var i=0;i+1<values.Count;i++)yield return(values[i],values[i+1]);}
    private static long Q(double x)=>(long)Math.Round(x*1_000_000,MidpointRounding.AwayFromZero);
    private static GridKey Key(Point3D p)=>new(Q(p.X),Q(p.Y),Q(p.Z));
    private readonly record struct GridKey(long X,long Y,long Z);
    private readonly record struct AxisDirection(int Order,int Axis,int X,int Y,int Z){public int Sign=>Axis==0?X:Axis==1?Y:Z;}
    private readonly record struct State(int Node,int Direction);
    private readonly record struct Score(double Cost,double Length,int Turns):IComparable<Score>{public int CompareTo(Score o){var c=Cost.CompareTo(o.Cost);if(c!=0)return c;c=Turns.CompareTo(o.Turns);return c!=0?c:Length.CompareTo(o.Length);}}
    private readonly record struct Priority(double EstimatedCost,int Turns,double Length,int Node,int Direction):IComparable<Priority>{public int CompareTo(Priority o){var c=EstimatedCost.CompareTo(o.EstimatedCost);if(c!=0)return c;c=Turns.CompareTo(o.Turns);if(c!=0)return c;c=Length.CompareTo(o.Length);if(c!=0)return c;c=Node.CompareTo(o.Node);return c!=0?c:Direction.CompareTo(o.Direction);}}
    private sealed record SearchResult(bool Success,IReadOnlyList<Point3D> Points,RouteFailureKind Failure,string Diagnostic);
}