Next Gen STEP AP242 Native-ish BREP Geometric Modeling Kernel
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);
}