https://pastein.ru/t/l3Z
Загрузка данных
public sealed class BundleRouter
{
private const float TurnEpsilon = 0.0005f;
private readonly struct LaneSortHint
{
public LaneSortHint(int side, float turnDistance, float barycenter)
{
Side = side;
TurnDistance = turnDistance;
Barycenter = barycenter;
}
public int Side { get; }
public float TurnDistance { get; }
public float Barycenter { get; }
}
private readonly struct FlowHintEntry
{
public FlowHintEntry(FlowRequest flow, LaneSortHint hint)
{
Flow = flow;
Hint = hint;
}
public FlowRequest Flow { get; }
public LaneSortHint Hint { get; }
}
private sealed class FlowRouteLookup
{
public GlobalRoute Route;
public Dictionary<int, int> EdgeToIndex;
}
private readonly struct LaneSmoothEntry
{
public LaneSmoothEntry(FlowRequest flow, float preferredOrder, int currentOrder)
{
Flow = flow;
PreferredOrder = preferredOrder;
CurrentOrder = currentOrder;
}
public FlowRequest Flow { get; }
public float PreferredOrder { get; }
public int CurrentOrder { get; }
}
public BundleRoutingResult Build(CorridorGraph graph, IReadOnlyList<GlobalRoute> globalRoutes,
RoutingConfig config)
{
BundleRoutingResult result = new BundleRoutingResult();
if (graph == null || globalRoutes == null || globalRoutes.Count == 0)
{
return result;
}
float lanePitch = config != null ? config.lanePitch : 0.25f;
result.UsedTurnAwareLaneOrdering = config != null && config.useTurnAwareLaneOrdering;
result.RequestedLaneAlignmentPasses = config != null ? Mathf.Max(0, config.laneAlignmentPasses) : 2;
Dictionary<FlowRequest, FlowRouteLookup> flowLookup = BuildFlowRouteLookup(globalRoutes);
Dictionary<int, List<FlowRequest>> edgeToFlows = BuildEdgeToFlowMap(globalRoutes);
foreach (KeyValuePair<int, List<FlowRequest>> pair in edgeToFlows)
{
int edgeId = pair.Key;
List<FlowRequest> flows = pair.Value;
if (flows.Count == 0)
{
continue;
}
CorridorEdge edge = graph.Edges[edgeId];
if (result.UsedTurnAwareLaneOrdering)
{
SortFlowsForEdge(flows, edgeId, edge, graph, flowLookup);
}
else
{
flows.Sort((left, right) =>
{
float leftKey = ComputeBarycenterKey(left, edge);
float rightKey = ComputeBarycenterKey(right, edge);
int compare = leftKey.CompareTo(rightKey);
if (compare != 0)
{
return compare;
}
string leftId = left != null && !string.IsNullOrWhiteSpace(left.Id) ? left.Id : "flow";
string rightId = right != null && !string.IsNullOrWhiteSpace(right.Id) ? right.Id : "flow";
return string.CompareOrdinal(leftId, rightId);
});
}
TrunkSegment trunk = new TrunkSegment { EdgeId = edgeId };
AssignLaneOffsets(trunk, flows, lanePitch);
result.TrunksByEdge[edgeId] = trunk;
}
// One lightweight pass aligns lane order across neighboring edges and removes obvious branch crossings.
int alignmentPasses = result.RequestedLaneAlignmentPasses;
for (int pass = 0; pass < alignmentPasses; pass++)
{
int changesBefore = result.LaneAlignmentChangeCount;
AlignNeighboringEdges(globalRoutes, result, lanePitch, forward: true);
AlignNeighboringEdges(globalRoutes, result, lanePitch, forward: false);
SmoothEdgeOrders(result, flowLookup, lanePitch);
result.AppliedLaneAlignmentPasses++;
if (result.LaneAlignmentChangeCount == changesBefore)
{
break;
}
}
return result;
}
private static Dictionary<FlowRequest, FlowRouteLookup> BuildFlowRouteLookup(IReadOnlyList<GlobalRoute> routes)
{
Dictionary<FlowRequest, FlowRouteLookup>
lookup = new Dictionary<FlowRequest, FlowRouteLookup>(routes.Count);
for (int i = 0; i < routes.Count; i++)
{
GlobalRoute route = routes[i];
if (route?.Flow == null)
{
continue;
}
Dictionary<int, int> edgeToIndex = new Dictionary<int, int>(route.EdgeIds.Count);
for (int edgeIndex = 0; edgeIndex < route.EdgeIds.Count; edgeIndex++)
{
int edgeId = route.EdgeIds[edgeIndex];
if (!edgeToIndex.ContainsKey(edgeId))
{
edgeToIndex[edgeId] = edgeIndex;
}
}
lookup[route.Flow] = new FlowRouteLookup
{
Route = route,
EdgeToIndex = edgeToIndex
};
}
return lookup;
}
private static Dictionary<int, List<FlowRequest>> BuildEdgeToFlowMap(IReadOnlyList<GlobalRoute> routes)
{
Dictionary<int, List<FlowRequest>> map = new Dictionary<int, List<FlowRequest>>();
for (int i = 0; i < routes.Count; i++)
{
GlobalRoute route = routes[i];
for (int edgeIndex = 0; edgeIndex < route.EdgeIds.Count; edgeIndex++)
{
int edgeId = route.EdgeIds[edgeIndex];
if (!map.TryGetValue(edgeId, out List<FlowRequest> flows))
{
flows = new List<FlowRequest>();
map[edgeId] = flows;
}
if (!flows.Contains(route.Flow))
{
flows.Add(route.Flow);
}
}
}
return map;
}
private static void SortFlowsForEdge(
List<FlowRequest> flows,
int edgeId,
CorridorEdge edge,
CorridorGraph graph,
Dictionary<FlowRequest, FlowRouteLookup> flowLookup)
{
List<FlowHintEntry> entries = new List<FlowHintEntry>(flows.Count);
for (int i = 0; i < flows.Count; i++)
{
FlowRequest flow = flows[i];
LaneSortHint hint = BuildLaneSortHint(flow, edgeId, edge, graph, flowLookup);
entries.Add(new FlowHintEntry(flow, hint));
}
entries.Sort(CompareFlowHintEntries);
for (int i = 0; i < entries.Count; i++)
{
flows[i] = entries[i].Flow;
}
}
private static LaneSortHint BuildLaneSortHint(
FlowRequest flow,
int edgeId,
CorridorEdge edge,
CorridorGraph graph,
Dictionary<FlowRequest, FlowRouteLookup> flowLookup)
{
float barycenter = ComputeBarycenterKey(flow, edge);
if (flow == null ||
!flowLookup.TryGetValue(flow, out FlowRouteLookup lookup) ||
lookup?.Route == null ||
lookup.EdgeToIndex == null ||
!lookup.EdgeToIndex.TryGetValue(edgeId, out int edgeIndex))
{
return new LaneSortHint(0, float.PositiveInfinity, barycenter);
}
int side = FindFirstSideTurn(lookup.Route, edgeIndex, edge.Axis, graph, out float turnDistance);
return new LaneSortHint(side, turnDistance, barycenter);
}
private static int CompareFlowHintEntries(FlowHintEntry left, FlowHintEntry right)
{
int compare = left.Hint.Side.CompareTo(right.Hint.Side);
if (compare != 0)
{
return compare;
}
if (left.Hint.Side < 0)
{
compare = left.Hint.TurnDistance.CompareTo(right.Hint.TurnDistance);
}
else if (left.Hint.Side > 0)
{
compare = right.Hint.TurnDistance.CompareTo(left.Hint.TurnDistance);
}
else
{
compare = left.Hint.Barycenter.CompareTo(right.Hint.Barycenter);
}
if (compare != 0)
{
return compare;
}
compare = left.Hint.Barycenter.CompareTo(right.Hint.Barycenter);
if (compare != 0)
{
return compare;
}
string leftId = left.Flow != null && !string.IsNullOrWhiteSpace(left.Flow.Id) ? left.Flow.Id : "flow";
string rightId = right.Flow != null && !string.IsNullOrWhiteSpace(right.Flow.Id) ? right.Flow.Id : "flow";
return string.CompareOrdinal(leftId, rightId);
}
private static int FindFirstSideTurn(
GlobalRoute route,
int edgeIndex,
EdgeAxis baseAxis,
CorridorGraph graph,
out float distanceToTurn)
{
distanceToTurn = float.PositiveInfinity;
if (route == null || graph == null || edgeIndex < 0 || edgeIndex >= route.EdgeIds.Count ||
route.NodeIds.Count <= edgeIndex + 1)
{
return 0;
}
float traveled = 0f;
for (int i = edgeIndex; i < route.EdgeIds.Count; i++)
{
int currentEdgeId = route.EdgeIds[i];
CorridorEdge currentEdge = graph.Edges[currentEdgeId];
if (currentEdge.Axis != baseAxis)
{
if (route.NodeIds.Count <= i + 1)
{
return 0;
}
Vector3 currentDirection = graph.Nodes[route.NodeIds[i + 1]] - graph.Nodes[route.NodeIds[i]];
float lateral = GetLateralComponent(currentDirection, baseAxis);
if (Mathf.Abs(lateral) <= TurnEpsilon)
{
return 0;
}
distanceToTurn = traveled;
return lateral > 0f ? 1 : -1;
}
traveled += currentEdge.Length;
}
return 0;
}
private static float GetLateralComponent(Vector3 direction, EdgeAxis axis)
{
return axis switch
{
EdgeAxis.X => direction.z,
EdgeAxis.Y => direction.x,
_ => direction.x
};
}
private static float ComputeBarycenterKey(FlowRequest flow, CorridorEdge edge)
{
if (flow == null || !flow.TryResolve(out SlotPose source, out SlotPose target))
{
return 0f;
}
float x = (source.Position.x + target.Position.x) * 0.5f;
float y = (source.Position.y + target.Position.y) * 0.5f;
float z = (source.Position.z + target.Position.z) * 0.5f;
return edge.Axis switch
{
EdgeAxis.X => y * 10f + z,
EdgeAxis.Y => x * 10f + z,
_ => x * 10f + y
};
}
private static void AlignNeighboringEdges(
IReadOnlyList<GlobalRoute> globalRoutes,
BundleRoutingResult result,
float lanePitch,
bool forward)
{
for (int i = 0; i < globalRoutes.Count; i++)
{
GlobalRoute route = globalRoutes[i];
if (route == null || route.EdgeIds.Count < 2)
{
continue;
}
if (forward)
{
for (int edgeIndex = 1; edgeIndex < route.EdgeIds.Count; edgeIndex++)
{
int previousEdge = route.EdgeIds[edgeIndex - 1];
int currentEdge = route.EdgeIds[edgeIndex];
if (!result.TrunksByEdge.TryGetValue(previousEdge, out TrunkSegment previousTrunk) ||
!result.TrunksByEdge.TryGetValue(currentEdge, out TrunkSegment currentTrunk))
{
continue;
}
if (AlignOrders(previousTrunk, currentTrunk, lanePitch))
{
result.LaneAlignmentChangeCount++;
}
}
}
else
{
for (int edgeIndex = route.EdgeIds.Count - 2; edgeIndex >= 0; edgeIndex--)
{
int currentEdge = route.EdgeIds[edgeIndex];
int nextEdge = route.EdgeIds[edgeIndex + 1];
if (!result.TrunksByEdge.TryGetValue(nextEdge, out TrunkSegment nextTrunk) ||
!result.TrunksByEdge.TryGetValue(currentEdge, out TrunkSegment currentTrunk))
{
continue;
}
if (AlignOrders(nextTrunk, currentTrunk, lanePitch))
{
result.LaneAlignmentChangeCount++;
}
}
}
}
}
private static void SmoothEdgeOrders(
BundleRoutingResult result,
Dictionary<FlowRequest, FlowRouteLookup> flowLookup,
float lanePitch)
{
foreach (KeyValuePair<int, TrunkSegment> pair in result.TrunksByEdge)
{
int edgeId = pair.Key;
TrunkSegment trunk = pair.Value;
if (trunk == null || trunk.Lanes.Count <= 1)
{
continue;
}
List<LaneSmoothEntry> entries = new List<LaneSmoothEntry>(trunk.Lanes.Count);
for (int i = 0; i < trunk.Lanes.Count; i++)
{
TrunkLane lane = trunk.Lanes[i];
float preferred = ComputePreferredOrder(edgeId, lane.Flow, lane.Order, result, flowLookup);
entries.Add(new LaneSmoothEntry(lane.Flow, preferred, lane.Order));
}
entries.Sort((left, right) =>
{
int compare = left.PreferredOrder.CompareTo(right.PreferredOrder);
if (compare != 0)
{
return compare;
}
compare = left.CurrentOrder.CompareTo(right.CurrentOrder);
if (compare != 0)
{
return compare;
}
string leftId = left.Flow != null && !string.IsNullOrWhiteSpace(left.Flow.Id)
? left.Flow.Id
: "flow";
string rightId = right.Flow != null && !string.IsNullOrWhiteSpace(right.Flow.Id)
? right.Flow.Id
: "flow";
return string.CompareOrdinal(leftId, rightId);
});
bool changed = false;
for (int i = 0; i < entries.Count; i++)
{
if (trunk.Lanes[i].Flow != entries[i].Flow)
{
changed = true;
break;
}
}
if (!changed)
{
continue;
}
result.LaneAlignmentChangeCount++;
trunk.Lanes.Clear();
int laneCount = entries.Count;
for (int i = 0; i < entries.Count; i++)
{
trunk.Lanes.Add(new TrunkLane
{
Flow = entries[i].Flow,
Order = i,
Offset = (i - (laneCount - 1) * 0.5f) * lanePitch
});
}
}
}
private static float ComputePreferredOrder(
int edgeId,
FlowRequest flow,
int currentOrder,
BundleRoutingResult result,
Dictionary<FlowRequest, FlowRouteLookup> flowLookup)
{
if (flow == null ||
!flowLookup.TryGetValue(flow, out FlowRouteLookup lookup) ||
lookup?.Route == null ||
lookup.EdgeToIndex == null ||
!lookup.EdgeToIndex.TryGetValue(edgeId, out int edgeIndex))
{
return currentOrder;
}
float sum = currentOrder;
int weight = 1;
if (edgeIndex > 0)
{
int previousEdge = lookup.Route.EdgeIds[edgeIndex - 1];
if (result.TrunksByEdge.TryGetValue(previousEdge, out TrunkSegment previousTrunk) &&
TryGetLaneOrder(previousTrunk, flow, out int previousOrder))
{
sum += previousOrder;
weight++;
}
}
if (edgeIndex + 1 < lookup.Route.EdgeIds.Count)
{
int nextEdge = lookup.Route.EdgeIds[edgeIndex + 1];
if (result.TrunksByEdge.TryGetValue(nextEdge, out TrunkSegment nextTrunk) &&
TryGetLaneOrder(nextTrunk, flow, out int nextOrder))
{
sum += nextOrder;
weight++;
}
}
return sum / Mathf.Max(1, weight);
}
private static bool TryGetLaneOrder(TrunkSegment trunk, FlowRequest flow, out int order)
{
order = 0;
if (trunk == null || flow == null)
{
return false;
}
for (int i = 0; i < trunk.Lanes.Count; i++)
{
if (trunk.Lanes[i].Flow == flow)
{
order = trunk.Lanes[i].Order;
return true;
}
}
return false;
}
private static void AssignLaneOffsets(TrunkSegment trunk, IReadOnlyList<FlowRequest> flows, float lanePitch)
{
trunk.Lanes.Clear();
int n = flows.Count;
for (int i = 0; i < flows.Count; i++)
{
float offset = (i - (n - 1) * 0.5f) * lanePitch;
trunk.Lanes.Add(new TrunkLane
{
Flow = flows[i],
Order = i,
Offset = offset
});
}
}
private static bool AlignOrders(TrunkSegment reference, TrunkSegment target, float lanePitch)
{
Dictionary<FlowRequest, int> referenceOrder = new Dictionary<FlowRequest, int>();
for (int i = 0; i < reference.Lanes.Count; i++)
{
referenceOrder[reference.Lanes[i].Flow] = reference.Lanes[i].Order;
}
List<TrunkLane> shared = new List<TrunkLane>();
for (int i = 0; i < target.Lanes.Count; i++)
{
if (referenceOrder.ContainsKey(target.Lanes[i].Flow))
{
shared.Add(target.Lanes[i]);
}
}
if (shared.Count <= 1)
{
return false;
}
shared.Sort((left, right) =>
{
int leftOrder = referenceOrder[left.Flow];
int rightOrder = referenceOrder[right.Flow];
return leftOrder.CompareTo(rightOrder);
});
bool changed = false;
int sharedCursor = 0;
for (int i = 0; i < target.Lanes.Count; i++)
{
if (!referenceOrder.ContainsKey(target.Lanes[i].Flow))
{
continue;
}
TrunkLane nextLane = shared[sharedCursor++];
if (target.Lanes[i].Flow != nextLane.Flow)
{
changed = true;
}
target.Lanes[i] = nextLane;
}
if (!changed)
{
return false;
}
int laneCount = target.Lanes.Count;
for (int i = 0; i < laneCount; i++)
{
TrunkLane lane = target.Lanes[i];
lane.Order = i;
lane.Offset = (i - (laneCount - 1) * 0.5f) * lanePitch;
target.Lanes[i] = lane;
}
return true;
}
}
public sealed class CorridorGraphBuilder
{
private const float Epsilon = 0.0001f;
public CorridorGraph Build(
IReadOnlyList<ModuleNode> modules,
IReadOnlyList<FlowRequest> flowRequests,
RoutingConfig config,
out List<Bounds> inflatedObstacles)
{
inflatedObstacles = BuildInflatedObstacles(modules, config.clearance);
List<Vector3> slotAnchors = CollectSlotAnchors(flowRequests);
Bounds workArea = ComputeWorkArea(modules, slotAnchors, config.workAreaPadding);
SortedSet<float> xs = new SortedSet<float>();
SortedSet<float> ys = new SortedSet<float>();
SortedSet<float> zs = new SortedSet<float>();
CollectGuideCoordinates(workArea, inflatedObstacles, slotAnchors, xs, ys, zs);
List<float> xGuides = ReduceGuideCoordinates(xs, config.maxGuideCoordsPerAxis, config.lanePitch * 0.5f);
List<float> yGuides = ReduceGuideCoordinates(ys, config.maxGuideCoordsPerAxis, config.lanePitch * 0.5f);
List<float> zGuides = ReduceGuideCoordinates(zs, config.maxGuideCoordsPerAxis, config.lanePitch * 0.5f);
long buildBudgetTicks = (long)(Mathf.Max(0.1f, config.maxGraphBuildTimeSeconds) * Stopwatch.Frequency);
long buildDeadline = Stopwatch.GetTimestamp() + buildBudgetTicks;
CorridorGraph graph = new CorridorGraph { WorkArea = workArea };
BuildNodes(graph, xGuides, yGuides, zGuides, inflatedObstacles, config, buildDeadline);
BuildEdges(graph, xGuides, yGuides, zGuides, inflatedObstacles, config, buildDeadline);
return graph;
}
private static List<Bounds> BuildInflatedObstacles(IReadOnlyList<ModuleNode> modules, float clearance)
{
List<Bounds> obstacles = new List<Bounds>(modules.Count);
for (int i = 0; i < modules.Count; i++)
{
if (modules[i] == null)
{
continue;
}
obstacles.Add(modules[i].GetInflatedBounds(clearance));
}
return obstacles;
}
private static List<Vector3> CollectSlotAnchors(IReadOnlyList<FlowRequest> flowRequests)
{
List<Vector3> anchors = new List<Vector3>(Mathf.Max(4, flowRequests.Count * 2));
for (int i = 0; i < flowRequests.Count; i++)
{
if (flowRequests[i] != null && flowRequests[i].TryResolve(out SlotPose src, out SlotPose dst))
{
anchors.Add(src.Position);
anchors.Add(dst.Position);
}
}
return anchors;
}
private static Bounds ComputeWorkArea(IReadOnlyList<ModuleNode> modules, IReadOnlyList<Vector3> anchors, float padding)
{
bool hasValue = false;
Vector3 min = Vector3.zero;
Vector3 max = Vector3.zero;
for (int i = 0; i < modules.Count; i++)
{
ModuleNode module = modules[i];
if (module == null)
{
continue;
}
IncludeBounds(ref hasValue, ref min, ref max, module.GetWorldBounds());
}
for (int i = 0; i < anchors.Count; i++)
{
IncludePoint(ref hasValue, ref min, ref max, anchors[i]);
}
if (!hasValue)
{
return new Bounds(Vector3.zero, Vector3.one * 20f);
}
Vector3 size = max - min;
Vector3 center = (min + max) * 0.5f;
Bounds bounds = new Bounds(center, size);
bounds.Expand(Mathf.Max(0f, padding) * 2f);
return bounds;
}
private static void IncludeBounds(ref bool hasValue, ref Vector3 min, ref Vector3 max, Bounds bounds)
{
IncludePoint(ref hasValue, ref min, ref max, bounds.min);
IncludePoint(ref hasValue, ref min, ref max, bounds.max);
}
private static void IncludePoint(ref bool hasValue, ref Vector3 min, ref Vector3 max, Vector3 point)
{
if (!hasValue)
{
hasValue = true;
min = point;
max = point;
return;
}
min = Vector3.Min(min, point);
max = Vector3.Max(max, point);
}
private static void CollectGuideCoordinates(
Bounds workArea,
IReadOnlyList<Bounds> inflatedObstacles,
IReadOnlyList<Vector3> slotAnchors,
SortedSet<float> xs,
SortedSet<float> ys,
SortedSet<float> zs)
{
xs.Add(Quantize(workArea.min.x));
xs.Add(Quantize(workArea.max.x));
ys.Add(Quantize(workArea.min.y));
ys.Add(Quantize(workArea.max.y));
zs.Add(Quantize(workArea.min.z));
zs.Add(Quantize(workArea.max.z));
for (int i = 0; i < inflatedObstacles.Count; i++)
{
Bounds obstacle = inflatedObstacles[i];
xs.Add(Quantize(obstacle.min.x));
xs.Add(Quantize(obstacle.max.x));
ys.Add(Quantize(obstacle.min.y));
ys.Add(Quantize(obstacle.max.y));
zs.Add(Quantize(obstacle.min.z));
zs.Add(Quantize(obstacle.max.z));
}
for (int i = 0; i < slotAnchors.Count; i++)
{
Vector3 anchor = slotAnchors[i];
xs.Add(Quantize(anchor.x));
ys.Add(Quantize(anchor.y));
zs.Add(Quantize(anchor.z));
}
}
private static void BuildNodes(
CorridorGraph graph,
IReadOnlyList<float> xs,
IReadOnlyList<float> ys,
IReadOnlyList<float> zs,
IReadOnlyList<Bounds> obstacles,
RoutingConfig config,
long buildDeadlineTicks)
{
for (int xIndex = 0; xIndex < xs.Count; xIndex++)
{
if (Stopwatch.GetTimestamp() >= buildDeadlineTicks)
{
graph.TruncatedByLimits = true;
return;
}
float x = xs[xIndex];
for (int yIndex = 0; yIndex < ys.Count; yIndex++)
{
float y = ys[yIndex];
for (int zIndex = 0; zIndex < zs.Count; zIndex++)
{
float z = zs[zIndex];
Vector3 point = new Vector3(x, y, z);
if (IsInsideAnyObstacle(point, obstacles))
{
continue;
}
graph.AddNode(point);
if (graph.Nodes.Count >= config.maxGraphNodes)
{
graph.TruncatedByLimits = true;
return;
}
}
}
}
}
private static void BuildEdges(
CorridorGraph graph,
IReadOnlyList<float> xs,
IReadOnlyList<float> ys,
IReadOnlyList<float> zs,
IReadOnlyList<Bounds> obstacles,
RoutingConfig config,
long buildDeadlineTicks)
{
for (int yIndex = 0; yIndex < ys.Count; yIndex++)
{
if (Stopwatch.GetTimestamp() >= buildDeadlineTicks)
{
graph.TruncatedByLimits = true;
return;
}
float y = ys[yIndex];
for (int zIndex = 0; zIndex < zs.Count; zIndex++)
{
float z = zs[zIndex];
int prevNode = -1;
Vector3 prevPoint = default;
for (int xIndex = 0; xIndex < xs.Count; xIndex++)
{
float x = xs[xIndex];
Vector3 point = new Vector3(x, y, z);
if (!graph.TryGetNode(point, out int nodeId))
{
continue;
}
if (prevNode >= 0 && !SegmentIntersectsAnyObstacle(prevPoint, point, obstacles))
{
AddEdgeWithCost(graph, prevNode, nodeId, prevPoint, point, obstacles, config);
}
prevNode = nodeId;
prevPoint = point;
if (graph.Edges.Count >= config.maxGraphEdges)
{
graph.TruncatedByLimits = true;
return;
}
}
}
}
for (int xIndex = 0; xIndex < xs.Count; xIndex++)
{
if (Stopwatch.GetTimestamp() >= buildDeadlineTicks)
{
graph.TruncatedByLimits = true;
return;
}
float x = xs[xIndex];
for (int zIndex = 0; zIndex < zs.Count; zIndex++)
{
float z = zs[zIndex];
int prevNode = -1;
Vector3 prevPoint = default;
for (int yIndex = 0; yIndex < ys.Count; yIndex++)
{
float y = ys[yIndex];
Vector3 point = new Vector3(x, y, z);
if (!graph.TryGetNode(point, out int nodeId))
{
continue;
}
if (prevNode >= 0 && !SegmentIntersectsAnyObstacle(prevPoint, point, obstacles))
{
AddEdgeWithCost(graph, prevNode, nodeId, prevPoint, point, obstacles, config);
}
prevNode = nodeId;
prevPoint = point;
if (graph.Edges.Count >= config.maxGraphEdges)
{
graph.TruncatedByLimits = true;
return;
}
}
}
}
for (int xIndex = 0; xIndex < xs.Count; xIndex++)
{
if (Stopwatch.GetTimestamp() >= buildDeadlineTicks)
{
graph.TruncatedByLimits = true;
return;
}
float x = xs[xIndex];
for (int yIndex = 0; yIndex < ys.Count; yIndex++)
{
float y = ys[yIndex];
int prevNode = -1;
Vector3 prevPoint = default;
for (int zIndex = 0; zIndex < zs.Count; zIndex++)
{
float z = zs[zIndex];
Vector3 point = new Vector3(x, y, z);
if (!graph.TryGetNode(point, out int nodeId))
{
continue;
}
if (prevNode >= 0 && !SegmentIntersectsAnyObstacle(prevPoint, point, obstacles))
{
AddEdgeWithCost(graph, prevNode, nodeId, prevPoint, point, obstacles, config);
}
prevNode = nodeId;
prevPoint = point;
if (graph.Edges.Count >= config.maxGraphEdges)
{
graph.TruncatedByLimits = true;
return;
}
}
}
}
}
private static void AddEdgeWithCost(
CorridorGraph graph,
int nodeA,
int nodeB,
Vector3 pointA,
Vector3 pointB,
IReadOnlyList<Bounds> obstacles,
RoutingConfig config)
{
float length = Vector3.Distance(pointA, pointB);
if (length < Epsilon)
{
return;
}
Vector3 midpoint = (pointA + pointB) * 0.5f;
float nearestDistance = NearestDistanceToObstacles(midpoint, obstacles);
float proximityPenalty = Mathf.Max(0f, config.proximityPenaltyDistance - nearestDistance);
float corridorWidth = Mathf.Max(config.lanePitch, nearestDistance * 2f);
int capacity = Mathf.Max(1, Mathf.FloorToInt(corridorWidth / Mathf.Max(0.01f, config.lanePitch)));
float baseCost = length + proximityPenalty;
graph.AddEdge(nodeA, nodeB, baseCost, capacity, proximityPenalty);
}
private static bool IsInsideAnyObstacle(Vector3 point, IReadOnlyList<Bounds> obstacles)
{
for (int i = 0; i < obstacles.Count; i++)
{
Bounds obstacle = obstacles[i];
if (point.x > obstacle.min.x + Epsilon &&
point.x < obstacle.max.x - Epsilon &&
point.y > obstacle.min.y + Epsilon &&
point.y < obstacle.max.y - Epsilon &&
point.z > obstacle.min.z + Epsilon &&
point.z < obstacle.max.z - Epsilon)
{
return true;
}
}
return false;
}
private static bool SegmentIntersectsAnyObstacle(Vector3 a, Vector3 b, IReadOnlyList<Bounds> obstacles)
{
for (int i = 0; i < obstacles.Count; i++)
{
if (SegmentIntersectsBoundsInterior(a, b, obstacles[i]))
{
return true;
}
}
return false;
}
private static bool SegmentIntersectsBoundsInterior(Vector3 a, Vector3 b, Bounds bounds)
{
Vector3 innerMin = bounds.min + Vector3.one * Epsilon;
Vector3 innerMax = bounds.max - Vector3.one * Epsilon;
if (innerMin.x >= innerMax.x || innerMin.y >= innerMax.y || innerMin.z >= innerMax.z)
{
return false;
}
float dx = Mathf.Abs(a.x - b.x);
float dy = Mathf.Abs(a.y - b.y);
float dz = Mathf.Abs(a.z - b.z);
if (dx > dy && dx > dz)
{
float y = (a.y + b.y) * 0.5f;
float z = (a.z + b.z) * 0.5f;
if (y <= innerMin.y || y >= innerMax.y || z <= innerMin.z || z >= innerMax.z)
{
return false;
}
float minX = Mathf.Min(a.x, b.x);
float maxX = Mathf.Max(a.x, b.x);
return maxX > innerMin.x && minX < innerMax.x;
}
if (dy > dx && dy > dz)
{
float x = (a.x + b.x) * 0.5f;
float z = (a.z + b.z) * 0.5f;
if (x <= innerMin.x || x >= innerMax.x || z <= innerMin.z || z >= innerMax.z)
{
return false;
}
float minY = Mathf.Min(a.y, b.y);
float maxY = Mathf.Max(a.y, b.y);
return maxY > innerMin.y && minY < innerMax.y;
}
if (dz > dx && dz > dy)
{
float x = (a.x + b.x) * 0.5f;
float y = (a.y + b.y) * 0.5f;
if (x <= innerMin.x || x >= innerMax.x || y <= innerMin.y || y >= innerMax.y)
{
return false;
}
float minZ = Mathf.Min(a.z, b.z);
float maxZ = Mathf.Max(a.z, b.z);
return maxZ > innerMin.z && minZ < innerMax.z;
}
return SegmentIntersectsBoundsGeneric(a, b, innerMin, innerMax);
}
private static bool SegmentIntersectsBoundsGeneric(Vector3 a, Vector3 b, Vector3 min, Vector3 max)
{
Vector3 d = b - a;
float tMin = 0f;
float tMax = 1f;
if (!ClipAxis(min.x, max.x, a.x, d.x, ref tMin, ref tMax) ||
!ClipAxis(min.y, max.y, a.y, d.y, ref tMin, ref tMax) ||
!ClipAxis(min.z, max.z, a.z, d.z, ref tMin, ref tMax))
{
return false;
}
return tMax > tMin;
}
private static bool ClipAxis(float min, float max, float origin, float direction, ref float tMin, ref float tMax)
{
if (Mathf.Abs(direction) < Epsilon)
{
return origin >= min && origin <= max;
}
float inv = 1f / direction;
float t1 = (min - origin) * inv;
float t2 = (max - origin) * inv;
if (t1 > t2)
{
float tmp = t1;
t1 = t2;
t2 = tmp;
}
tMin = Mathf.Max(tMin, t1);
tMax = Mathf.Min(tMax, t2);
return tMax >= tMin;
}
private static float NearestDistanceToObstacles(Vector3 point, IReadOnlyList<Bounds> obstacles)
{
if (obstacles.Count == 0)
{
return 999f;
}
float minDist = float.PositiveInfinity;
for (int i = 0; i < obstacles.Count; i++)
{
Bounds obstacle = obstacles[i];
float dx = Mathf.Max(obstacle.min.x - point.x, 0f, point.x - obstacle.max.x);
float dy = Mathf.Max(obstacle.min.y - point.y, 0f, point.y - obstacle.max.y);
float dz = Mathf.Max(obstacle.min.z - point.z, 0f, point.z - obstacle.max.z);
float dist = Mathf.Sqrt(dx * dx + dy * dy + dz * dz);
minDist = Mathf.Min(minDist, dist);
}
return minDist;
}
private static float Quantize(float value)
{
return Mathf.Round(value * 1000f) / 1000f;
}
private static List<float> ReduceGuideCoordinates(SortedSet<float> source, int maxCount, float minSpacing)
{
List<float> merged = new List<float>(source.Count);
float safeSpacing = Mathf.Max(0.001f, minSpacing);
bool hasLast = false;
float last = 0f;
foreach (float value in source)
{
if (!hasLast || Mathf.Abs(value - last) >= safeSpacing)
{
merged.Add(value);
hasLast = true;
last = value;
}
}
if (merged.Count <= maxCount || maxCount < 3)
{
return merged;
}
List<float> reduced = new List<float>(maxCount) { merged[0] };
HashSet<int> used = new HashSet<int> { 0, merged.Count - 1 };
float step = (merged.Count - 1f) / (maxCount - 1f);
for (int i = 1; i < maxCount - 1; i++)
{
int index = Mathf.RoundToInt(i * step);
index = Mathf.Clamp(index, 1, merged.Count - 2);
while (used.Contains(index) && index < merged.Count - 2)
{
index++;
}
if (used.Contains(index))
{
continue;
}
used.Add(index);
reduced.Add(merged[index]);
}
reduced.Add(merged[merged.Count - 1]);
reduced.Sort();
return reduced;
}
}
public sealed class DetailedGeometryBuilder
{
private const float Epsilon = 0.0001f;
private readonly struct SegmentLineKey
{
public SegmentLineKey(Axis axis, int a, int b)
{
Axis = axis;
A = a;
B = b;
}
public Axis Axis { get; }
public int A { get; }
public int B { get; }
}
private readonly struct SegmentRef
{
public SegmentRef(int pathIndex, int segmentIndex, Axis axis, float min, float max)
{
PathIndex = pathIndex;
SegmentIndex = segmentIndex;
Axis = axis;
Min = min;
Max = max;
}
public int PathIndex { get; }
public int SegmentIndex { get; }
public Axis Axis { get; }
public float Min { get; }
public float Max { get; }
}
private readonly struct SegmentId
{
public SegmentId(int pathIndex, int segmentIndex)
{
PathIndex = pathIndex;
SegmentIndex = segmentIndex;
}
public int PathIndex { get; }
public int SegmentIndex { get; }
}
public List<DetailedPath> Build(
CorridorGraph graph,
IReadOnlyList<GlobalRoute> routes,
BundleRoutingResult bundleResult,
IReadOnlyList<ModuleNode> modules,
RoutingConfig config)
{
if (graph == null || routes == null)
{
return new List<DetailedPath>();
}
List<DetailedPath> output = new List<DetailedPath>(routes.Count);
for (int i = 0; i < routes.Count; i++)
{
GlobalRoute route = routes[i];
if (route.Flow == null || !route.Flow.TryResolve(out SlotPose source, out SlotPose target))
{
continue;
}
DetailedPath detailedPath = BuildSinglePath(graph, route, source, target, bundleResult, config);
output.Add(detailedPath);
}
SeparateOverlappingSegments(output, config);
for (int i = 0; i < output.Count; i++)
{
NormalizeEndpointApproaches(output[i], config);
FinalizeDetailedPath(output[i], config);
AvoidModuleIntersections(output[i], modules, config);
}
return output;
}
public DetailedPath BuildPreviewPath(
CorridorGraph graph,
GlobalRoute route,
BundleRoutingResult bundleResult,
IReadOnlyList<ModuleNode> modules,
RoutingConfig config)
{
if (graph == null ||
route == null ||
route.Flow == null ||
!route.Flow.TryResolve(out SlotPose source, out SlotPose target))
{
return null;
}
DetailedPath detailedPath = BuildSinglePath(graph, route, source, target, bundleResult, config);
NormalizeEndpointApproaches(detailedPath, config);
FinalizeDetailedPath(detailedPath, config);
AvoidModuleIntersections(detailedPath, modules, config);
return detailedPath;
}
private static DetailedPath BuildSinglePath(
CorridorGraph graph,
GlobalRoute route,
SlotPose source,
SlotPose target,
BundleRoutingResult bundleResult,
RoutingConfig config)
{
DetailedPath detailed = new DetailedPath
{
Flow = route.Flow,
UsesCrossingFallback = route.UsesCrossingFallback
};
List<Vector3> corridor = BuildCorridorPolyline(graph, route, bundleResult, config);
Vector3 sourceFanout = source.Position + source.Normal * config.fanoutLength;
Vector3 targetFanout = target.Position + target.Normal * config.fanoutLength;
SanitizeEndpointCorridor(
corridor,
source,
route.Flow.Source.Module,
config,
fromStart: true);
SanitizeEndpointCorridor(
corridor,
target,
route.Flow.Target.Module,
config,
fromStart: false);
AppendUnique(detailed.Points, source.Position);
AppendUnique(detailed.Points, sourceFanout);
if (corridor.Count > 0)
{
AppendEndpointAwareConnection(
detailed.Points,
sourceFanout,
corridor[0],
source.Normal,
true,
config);
for (int i = 1; i < corridor.Count; i++)
{
AppendAxisAlignedConnection(
detailed.Points,
detailed.Points[detailed.Points.Count - 1],
corridor[i],
config.minSegmentLen);
}
AppendEndpointAwareConnection(
detailed.Points,
detailed.Points[detailed.Points.Count - 1],
targetFanout,
target.Normal,
false,
config);
}
else
{
AppendEndpointAwareConnection(
detailed.Points,
sourceFanout,
targetFanout,
source.Normal,
true,
config);
AppendEndpointAwareConnection(
detailed.Points,
detailed.Points[detailed.Points.Count - 1],
targetFanout,
target.Normal,
false,
config);
}
AppendUnique(detailed.Points, targetFanout);
AppendUnique(detailed.Points, target.Position);
PreCleanDetailedPoints(detailed.Points, config);
return detailed;
}
private static List<Vector3> BuildCorridorPolyline(
CorridorGraph graph,
GlobalRoute route,
BundleRoutingResult bundleResult,
RoutingConfig config)
{
List<Vector3> points = new List<Vector3>();
if (route.EdgeIds.Count == 0 || route.NodeIds.Count <= 1)
{
return points;
}
for (int edgeIndex = 0; edgeIndex < route.EdgeIds.Count; edgeIndex++)
{
int edgeId = route.EdgeIds[edgeIndex];
CorridorEdge edge = graph.Edges[edgeId];
int fromNode = route.NodeIds[edgeIndex];
int toNode = route.NodeIds[edgeIndex + 1];
Vector3 start = graph.Nodes[fromNode];
Vector3 end = graph.Nodes[toNode];
float offset = bundleResult != null ? bundleResult.GetOffset(route.Flow, edgeId) : 0f;
Vector3 shift = GetLaneShift(edge.Axis, offset);
Vector3 shiftedStart = start + shift;
Vector3 shiftedEnd = end + shift;
if (points.Count == 0)
{
AppendUnique(points, shiftedStart);
}
else
{
AppendAxisAlignedConnection(points, points[points.Count - 1], shiftedStart, config.minSegmentLen);
}
AppendAxisAlignedConnection(points, points[points.Count - 1], shiftedEnd, config.minSegmentLen);
}
SnapNearlyAxisAligned(points);
SimplifyCollinear(points);
CleanupLocalDetours(points, config.minSegmentLen, config.lanePitch);
SnapNearlyAxisAligned(points);
SimplifyCollinear(points);
return points;
}
private static void PreCleanDetailedPoints(List<Vector3> points, RoutingConfig config)
{
SnapNearlyAxisAligned(points);
SimplifyCollinear(points);
CleanupLocalDetours(points, config.minSegmentLen, config.lanePitch);
SnapNearlyAxisAligned(points);
SimplifyCollinear(points);
}
private static void NormalizeEndpointApproaches(DetailedPath path, RoutingConfig config)
{
if (path == null ||
path.Flow == null ||
path.Points.Count < 4 ||
!path.Flow.TryResolve(out SlotPose source, out SlotPose target))
{
return;
}
Vector3 sourceFanout = source.Position + source.Normal * config.fanoutLength;
Vector3 targetFanout = target.Position + target.Normal * config.fanoutLength;
float endpointGuard = Mathf.Max(config.minSegmentLen, config.lanePitch);
int sourceAnchorIndex = FindEndpointAnchorIndex(
path.Points,
source,
config.fanoutLength,
endpointGuard,
fromStart: true);
int targetAnchorIndex = FindEndpointAnchorIndex(
path.Points,
target,
config.fanoutLength,
endpointGuard,
fromStart: false);
List<Vector3> original = new List<Vector3>(path.Points);
List<Vector3> rebuilt = new List<Vector3>(original.Count + 6);
AppendUnique(rebuilt, source.Position);
AppendUnique(rebuilt, sourceFanout);
if (sourceAnchorIndex < targetAnchorIndex)
{
AppendEndpointAwareConnection(
rebuilt,
rebuilt[rebuilt.Count - 1],
original[sourceAnchorIndex],
source.Normal,
true,
config);
for (int i = sourceAnchorIndex + 1; i <= targetAnchorIndex; i++)
{
AppendAxisAlignedConnection(
rebuilt,
rebuilt[rebuilt.Count - 1],
original[i],
config.minSegmentLen);
}
}
else
{
AppendEndpointAwareConnection(
rebuilt,
rebuilt[rebuilt.Count - 1],
targetFanout,
source.Normal,
true,
config);
}
AppendEndpointAwareConnection(
rebuilt,
rebuilt[rebuilt.Count - 1],
targetFanout,
target.Normal,
false,
config);
AppendUnique(rebuilt, target.Position);
path.Points.Clear();
path.Points.AddRange(rebuilt);
SnapNearlyAxisAligned(path.Points);
SimplifyCollinear(path.Points);
}
private static int FindEndpointAnchorIndex(
List<Vector3> points,
SlotPose endpoint,
float fanoutDistance,
float guardDistance,
bool fromStart)
{
Vector3 normal = endpoint.Normal.normalized;
float minOutwardDistance = Mathf.Max(0f, fanoutDistance) + Mathf.Max(0f, guardDistance);
if (fromStart)
{
int fallback = Mathf.Min(points.Count - 1, 2);
for (int i = 1; i < points.Count - 1; i++)
{
float outwardDistance = Vector3.Dot(points[i] - endpoint.Position, normal);
if (outwardDistance >= minOutwardDistance - Epsilon)
{
return i;
}
}
return fallback;
}
int reverseFallback = Mathf.Max(0, points.Count - 3);
for (int i = points.Count - 2; i > 0; i--)
{
float outwardDistance = Vector3.Dot(points[i] - endpoint.Position, normal);
if (outwardDistance >= minOutwardDistance - Epsilon)
{
return i;
}
}
return reverseFallback;
}
private static void FinalizeDetailedPath(DetailedPath path, RoutingConfig config)
{
if (path == null)
{
return;
}
path.CornerMarkers.Clear();
SnapNearlyAxisAligned(path.Points);
SimplifyCollinear(path.Points);
CleanupLocalDetours(path.Points, config.minSegmentLen, config.lanePitch);
CleanupLaneTransitionDoglegs(path.Points, config.minSegmentLen, config.lanePitch);
CleanupSelfLoops(path.Points);
SnapNearlyAxisAligned(path.Points);
SimplifyCollinear(path.Points);
ApplyCornerRounding(path.Points, path.CornerMarkers, config.cornerRadius, config.minSegmentLen);
CleanupOppositeAxisSpikes(path.Points, Mathf.Max(config.minSegmentLen, config.lanePitch) * 0.35f);
CleanupPostRoundingHooks(path.Points, config.minSegmentLen, config.lanePitch);
CleanupSelfLoops(path.Points);
SnapNearlyAxisAligned(path.Points);
SimplifyCollinear(path.Points);
}
private static void AvoidModuleIntersections(
DetailedPath path,
IReadOnlyList<ModuleNode> modules,
RoutingConfig config)
{
if (path == null || path.Points.Count < 2 || modules == null || modules.Count == 0)
{
return;
}
List<Bounds> moduleBounds = BuildModuleBounds(modules);
if (moduleBounds.Count == 0)
{
return;
}
float lanePitch = config != null ? config.lanePitch : 0.25f;
float minSegmentLength = config != null ? config.minSegmentLen : 0.05f;
float padding = Mathf.Max(0.01f, lanePitch * 0.75f);
float planarTolerance = config != null ? Mathf.Max(0.001f, config.sameHeightTolerance) : 0.001f;
bool keepPlanar = IsPlanarPath(path.Points, planarTolerance);
int safety = 0;
bool changed;
do
{
changed = false;
for (int segmentIndex = 1; segmentIndex < path.Points.Count; segmentIndex++)
{
Vector3 from = path.Points[segmentIndex - 1];
Vector3 to = path.Points[segmentIndex];
if (!TryFindFirstModuleIntersection(from, to, moduleBounds, out Bounds blockingBounds))
{
continue;
}
if (!TryBuildModuleDetour(
from,
to,
blockingBounds,
moduleBounds,
padding,
minSegmentLength,
keepPlanar,
out List<Vector3> replacement))
{
continue;
}
path.Points.RemoveAt(segmentIndex);
path.Points.InsertRange(segmentIndex, replacement);
SnapNearlyAxisAligned(path.Points);
SimplifyCollinear(path.Points);
changed = true;
break;
}
}
while (changed && ++safety < 128);
SnapNearlyAxisAligned(path.Points);
SimplifyCollinear(path.Points);
}
private static List<Bounds> BuildModuleBounds(IReadOnlyList<ModuleNode> modules)
{
List<Bounds> bounds = new List<Bounds>(modules.Count);
for (int i = 0; i < modules.Count; i++)
{
ModuleNode module = modules[i];
if (module == null)
{
continue;
}
Bounds moduleBounds = module.GetWorldBounds();
if (moduleBounds.size.sqrMagnitude <= Epsilon)
{
continue;
}
bounds.Add(moduleBounds);
}
return bounds;
}
private static bool IsPlanarPath(List<Vector3> points, float tolerance)
{
if (points.Count <= 1)
{
return true;
}
float y = points[0].y;
for (int i = 1; i < points.Count; i++)
{
if (Mathf.Abs(points[i].y - y) > tolerance)
{
return false;
}
}
return true;
}
private static bool TryFindFirstModuleIntersection(
Vector3 from,
Vector3 to,
IReadOnlyList<Bounds> moduleBounds,
out Bounds blockingBounds)
{
for (int i = 0; i < moduleBounds.Count; i++)
{
Bounds bounds = moduleBounds[i];
if (SegmentIntersectsBoundsInterior(from, to, bounds))
{
blockingBounds = bounds;
return true;
}
}
blockingBounds = default;
return false;
}
private static bool TryBuildModuleDetour(
Vector3 from,
Vector3 to,
Bounds blockingBounds,
IReadOnlyList<Bounds> moduleBounds,
float padding,
float minSegmentLength,
bool keepPlanar,
out List<Vector3> replacement)
{
replacement = null;
TryGetAxisDirection(to - from, out Axis segmentAxis, out _, out _);
bool isAxisAlignedSegment = IsAxisAlignedSegment(from, to);
Axis[] detourAxes = keepPlanar
? new[] { Axis.X, Axis.Z }
: new[] { Axis.X, Axis.Y, Axis.Z };
List<float>[] detourCoordinates = BuildDetourCoordinates(
detourAxes,
blockingBounds,
moduleBounds,
padding);
int bestCollisions = int.MaxValue;
float bestLength = float.PositiveInfinity;
List<Vector3> bestCandidate = null;
for (int i = 0; i < detourAxes.Length; i++)
{
Axis detourAxis = detourAxes[i];
if (isAxisAlignedSegment && detourAxis == segmentAxis)
{
continue;
}
List<float> coordinates = detourCoordinates[i];
for (int coordinateIndex = 0; coordinateIndex < coordinates.Count; coordinateIndex++)
{
EvaluateModuleDetourCandidate(
from,
to,
detourAxis,
coordinates[coordinateIndex],
moduleBounds,
minSegmentLength,
ref bestCollisions,
ref bestLength,
ref bestCandidate);
}
}
for (int firstAxisIndex = 0; firstAxisIndex < detourAxes.Length; firstAxisIndex++)
{
Axis firstAxis = detourAxes[firstAxisIndex];
for (int secondAxisIndex = 0; secondAxisIndex < detourAxes.Length; secondAxisIndex++)
{
if (secondAxisIndex == firstAxisIndex)
{
continue;
}
Axis secondAxis = detourAxes[secondAxisIndex];
List<float> firstCoordinates = detourCoordinates[firstAxisIndex];
List<float> secondCoordinates = detourCoordinates[secondAxisIndex];
for (int firstCoordinateIndex = 0; firstCoordinateIndex < firstCoordinates.Count; firstCoordinateIndex++)
{
for (int secondCoordinateIndex = 0; secondCoordinateIndex < secondCoordinates.Count; secondCoordinateIndex++)
{
EvaluateCornerModuleDetourCandidate(
from,
to,
firstAxis,
firstCoordinates[firstCoordinateIndex],
secondAxis,
secondCoordinates[secondCoordinateIndex],
moduleBounds,
minSegmentLength,
ref bestCollisions,
ref bestLength,
ref bestCandidate);
}
}
}
}
if (bestCandidate == null || bestCollisions > 0)
{
return false;
}
replacement = bestCandidate;
return true;
}
private static List<float>[] BuildDetourCoordinates(
Axis[] detourAxes,
Bounds blockingBounds,
IReadOnlyList<Bounds> moduleBounds,
float padding)
{
List<float>[] coordinates = new List<float>[detourAxes.Length];
for (int axisIndex = 0; axisIndex < detourAxes.Length; axisIndex++)
{
Axis axis = detourAxes[axisIndex];
coordinates[axisIndex] = new List<float>();
Bounds expandedBlockingBounds = blockingBounds;
expandedBlockingBounds.Expand(padding * 2f);
AddDetourCoordinate(coordinates[axisIndex], GetAxis(expandedBlockingBounds.min, axis));
AddDetourCoordinate(coordinates[axisIndex], GetAxis(expandedBlockingBounds.max, axis));
for (int moduleIndex = 0; moduleIndex < moduleBounds.Count; moduleIndex++)
{
Bounds expandedBounds = moduleBounds[moduleIndex];
expandedBounds.Expand(padding * 2f);
AddDetourCoordinate(coordinates[axisIndex], GetAxis(expandedBounds.min, axis));
AddDetourCoordinate(coordinates[axisIndex], GetAxis(expandedBounds.max, axis));
}
}
return coordinates;
}
private static void AddDetourCoordinate(List<float> coordinates, float value)
{
const float duplicateTolerance = 0.0005f;
for (int i = 0; i < coordinates.Count; i++)
{
if (Mathf.Abs(coordinates[i] - value) <= duplicateTolerance)
{
return;
}
}
coordinates.Add(value);
}
private static void EvaluateModuleDetourCandidate(
Vector3 from,
Vector3 to,
Axis detourAxis,
float detourCoordinate,
IReadOnlyList<Bounds> moduleBounds,
float minSegmentLength,
ref int bestCollisions,
ref float bestLength,
ref List<Vector3> bestCandidate)
{
Vector3 first = SetAxis(from, detourAxis, detourCoordinate);
Vector3 second = SetAxis(to, detourAxis, detourCoordinate);
List<Vector3> candidate = new List<Vector3> { from };
AppendAxisAlignedConnection(candidate, candidate[candidate.Count - 1], first, minSegmentLength);
AppendAxisAlignedConnection(candidate, candidate[candidate.Count - 1], second, minSegmentLength);
AppendAxisAlignedConnection(candidate, candidate[candidate.Count - 1], to, minSegmentLength);
int collisionCount = CountModuleIntersections(candidate, moduleBounds, 0);
if (collisionCount > bestCollisions)
{
return;
}
float length = ComputePolylineLength(candidate);
if (collisionCount == bestCollisions && length >= bestLength)
{
return;
}
bestCollisions = collisionCount;
bestLength = length;
bestCandidate = new List<Vector3>(candidate.Count - 1);
for (int i = 1; i < candidate.Count; i++)
{
bestCandidate.Add(candidate[i]);
}
}
private static void EvaluateCornerModuleDetourCandidate(
Vector3 from,
Vector3 to,
Axis firstAxis,
float firstCoordinate,
Axis secondAxis,
float secondCoordinate,
IReadOnlyList<Bounds> moduleBounds,
float minSegmentLength,
ref int bestCollisions,
ref float bestLength,
ref List<Vector3> bestCandidate)
{
Vector3 first = SetAxis(from, firstAxis, firstCoordinate);
Vector3 corner = SetAxis(first, secondAxis, secondCoordinate);
Vector3 second = SetAxis(to, secondAxis, secondCoordinate);
second = SetAxis(second, firstAxis, firstCoordinate);
Vector3 third = SetAxis(to, firstAxis, firstCoordinate);
List<Vector3> candidate = new List<Vector3> { from };
AppendAxisAlignedConnection(candidate, candidate[candidate.Count - 1], first, minSegmentLength);
AppendAxisAlignedConnection(candidate, candidate[candidate.Count - 1], corner, minSegmentLength);
AppendAxisAlignedConnection(candidate, candidate[candidate.Count - 1], second, minSegmentLength);
AppendAxisAlignedConnection(candidate, candidate[candidate.Count - 1], third, minSegmentLength);
AppendAxisAlignedConnection(candidate, candidate[candidate.Count - 1], to, minSegmentLength);
int collisionCount = CountModuleIntersections(candidate, moduleBounds, 0);
if (collisionCount > bestCollisions)
{
return;
}
float length = ComputePolylineLength(candidate);
if (collisionCount == bestCollisions && length >= bestLength)
{
return;
}
bestCollisions = collisionCount;
bestLength = length;
bestCandidate = new List<Vector3>(candidate.Count - 1);
for (int i = 1; i < candidate.Count; i++)
{
bestCandidate.Add(candidate[i]);
}
}
private static int CountModuleIntersections(
List<Vector3> points,
IReadOnlyList<Bounds> moduleBounds,
int maxCount = int.MaxValue)
{
int count = 0;
for (int segmentIndex = 1; segmentIndex < points.Count; segmentIndex++)
{
Vector3 from = points[segmentIndex - 1];
Vector3 to = points[segmentIndex];
for (int moduleIndex = 0; moduleIndex < moduleBounds.Count; moduleIndex++)
{
if (SegmentIntersectsBoundsInterior(from, to, moduleBounds[moduleIndex]))
{
count++;
if (count > maxCount)
{
return count;
}
break;
}
}
}
return count;
}
private static float ComputePolylineLength(List<Vector3> points)
{
float length = 0f;
for (int i = 1; i < points.Count; i++)
{
length += Vector3.Distance(points[i - 1], points[i]);
}
return length;
}
private static bool SegmentIntersectsBoundsInterior(Vector3 from, Vector3 to, Bounds bounds)
{
Vector3 innerMin = bounds.min + Vector3.one * Epsilon;
Vector3 innerMax = bounds.max - Vector3.one * Epsilon;
if (innerMin.x >= innerMax.x || innerMin.y >= innerMax.y || innerMin.z >= innerMax.z)
{
return false;
}
Vector3 delta = to - from;
float tMin = 0f;
float tMax = 1f;
if (!ClipSegmentAxis(innerMin.x, innerMax.x, from.x, delta.x, ref tMin, ref tMax) ||
!ClipSegmentAxis(innerMin.y, innerMax.y, from.y, delta.y, ref tMin, ref tMax) ||
!ClipSegmentAxis(innerMin.z, innerMax.z, from.z, delta.z, ref tMin, ref tMax))
{
return false;
}
return tMax > tMin;
}
private static bool ClipSegmentAxis(
float min,
float max,
float origin,
float direction,
ref float tMin,
ref float tMax)
{
if (Mathf.Abs(direction) < Epsilon)
{
return origin >= min && origin <= max;
}
float inv = 1f / direction;
float t1 = (min - origin) * inv;
float t2 = (max - origin) * inv;
if (t1 > t2)
{
float tmp = t1;
t1 = t2;
t2 = tmp;
}
tMin = Mathf.Max(tMin, t1);
tMax = Mathf.Min(tMax, t2);
return tMax >= tMin;
}
private static void SeparateOverlappingSegments(List<DetailedPath> paths, RoutingConfig config)
{
if (paths == null || paths.Count <= 1)
{
return;
}
float lanePitch = Mathf.Max(0.001f, config.lanePitch);
float minSegmentLength = Mathf.Max(Epsilon * 10f, config.minSegmentLen * 0.5f);
float minOverlapLength = Mathf.Max(Epsilon * 10f, minSegmentLength * 0.25f);
Dictionary<SegmentLineKey, List<SegmentRef>> groups = new Dictionary<SegmentLineKey, List<SegmentRef>>();
for (int pathIndex = 0; pathIndex < paths.Count; pathIndex++)
{
DetailedPath path = paths[pathIndex];
if (path == null || path.Points.Count < 2)
{
continue;
}
for (int segmentIndex = 1; segmentIndex < path.Points.Count; segmentIndex++)
{
Vector3 start = path.Points[segmentIndex - 1];
Vector3 end = path.Points[segmentIndex];
if (!TryGetAxisDirection(end - start, out Axis axis, out _, out float length) ||
length < minSegmentLength)
{
continue;
}
SegmentLineKey key = BuildSegmentLineKey(start, end, axis);
if (!groups.TryGetValue(key, out List<SegmentRef> segments))
{
segments = new List<SegmentRef>();
groups[key] = segments;
}
float a = GetAxis(start, axis);
float b = GetAxis(end, axis);
segments.Add(new SegmentRef(
pathIndex,
segmentIndex,
axis,
Mathf.Min(a, b),
Mathf.Max(a, b)));
}
}
Dictionary<SegmentId, Vector3> offsets = new Dictionary<SegmentId, Vector3>();
foreach (KeyValuePair<SegmentLineKey, List<SegmentRef>> pair in groups)
{
AssignOverlapOffsets(pair.Value, lanePitch, minOverlapLength, offsets);
}
if (offsets.Count == 0)
{
return;
}
ExpandOffsetsAlongStraightRuns(paths, offsets);
ApplySegmentOffsets(paths, offsets, config);
}
private static SegmentLineKey BuildSegmentLineKey(Vector3 start, Vector3 end, Axis axis)
{
switch (axis)
{
case Axis.X:
return new SegmentLineKey(axis, QuantizeLineCoordinate(start.y), QuantizeLineCoordinate(start.z));
case Axis.Y:
return new SegmentLineKey(axis, QuantizeLineCoordinate(start.x), QuantizeLineCoordinate(start.z));
default:
return new SegmentLineKey(axis, QuantizeLineCoordinate(start.x), QuantizeLineCoordinate(start.y));
}
}
private static int QuantizeLineCoordinate(float value)
{
return Mathf.RoundToInt(value * 1000f);
}
private static void AssignOverlapOffsets(
List<SegmentRef> segments,
float lanePitch,
float minOverlapLength,
Dictionary<SegmentId, Vector3> offsets)
{
if (segments == null || segments.Count <= 1)
{
return;
}
List<int>[] adjacency = new List<int>[segments.Count];
for (int i = 0; i < segments.Count; i++)
{
for (int j = i + 1; j < segments.Count; j++)
{
if (segments[i].PathIndex == segments[j].PathIndex ||
GetOverlapLength(segments[i], segments[j]) < minOverlapLength)
{
continue;
}
if (adjacency[i] == null)
{
adjacency[i] = new List<int>();
}
if (adjacency[j] == null)
{
adjacency[j] = new List<int>();
}
adjacency[i].Add(j);
adjacency[j].Add(i);
}
}
bool[] visited = new bool[segments.Count];
List<int> stack = new List<int>();
List<int> component = new List<int>();
for (int i = 0; i < segments.Count; i++)
{
if (visited[i] || adjacency[i] == null)
{
continue;
}
stack.Clear();
component.Clear();
stack.Add(i);
visited[i] = true;
while (stack.Count > 0)
{
int current = stack[stack.Count - 1];
stack.RemoveAt(stack.Count - 1);
component.Add(current);
List<int> neighbors = adjacency[current];
for (int n = 0; n < neighbors.Count; n++)
{
int next = neighbors[n];
if (visited[next])
{
continue;
}
visited[next] = true;
stack.Add(next);
}
}
AssignComponentOffsets(segments, component, lanePitch, offsets);
}
}
private static float GetOverlapLength(SegmentRef first, SegmentRef second)
{
float min = Mathf.Max(first.Min, second.Min);
float max = Mathf.Min(first.Max, second.Max);
return Mathf.Max(0f, max - min);
}
private static void AssignComponentOffsets(
List<SegmentRef> segments,
List<int> component,
float lanePitch,
Dictionary<SegmentId, Vector3> offsets)
{
Dictionary<int, float> firstPositionByPath = new Dictionary<int, float>();
for (int i = 0; i < component.Count; i++)
{
SegmentRef segment = segments[component[i]];
if (!firstPositionByPath.TryGetValue(segment.PathIndex, out float existing) ||
segment.Min < existing)
{
firstPositionByPath[segment.PathIndex] = segment.Min;
}
}
if (firstPositionByPath.Count <= 1)
{
return;
}
List<int> orderedPaths = new List<int>(firstPositionByPath.Keys);
orderedPaths.Sort((left, right) =>
{
int compare = firstPositionByPath[left].CompareTo(firstPositionByPath[right]);
return compare != 0 ? compare : left.CompareTo(right);
});
Dictionary<int, Vector3> shiftByPath = new Dictionary<int, Vector3>(orderedPaths.Count);
for (int i = 0; i < orderedPaths.Count; i++)
{
float offset = (i - (orderedPaths.Count - 1) * 0.5f) * lanePitch;
Axis axis = segments[component[0]].Axis;
shiftByPath[orderedPaths[i]] = GetSeparationShift(axis, offset);
}
for (int i = 0; i < component.Count; i++)
{
SegmentRef segment = segments[component[i]];
if (!shiftByPath.TryGetValue(segment.PathIndex, out Vector3 shift) ||
shift.sqrMagnitude < Epsilon)
{
continue;
}
offsets[new SegmentId(segment.PathIndex, segment.SegmentIndex)] = shift;
}
}
private static Vector3 GetSeparationShift(Axis axis, float offset)
{
switch (axis)
{
case Axis.X:
return new Vector3(0f, 0f, offset);
case Axis.Y:
return new Vector3(offset, 0f, 0f);
default:
return new Vector3(offset, 0f, 0f);
}
}
private static void ExpandOffsetsAlongStraightRuns(
List<DetailedPath> paths,
Dictionary<SegmentId, Vector3> offsets)
{
if (offsets.Count == 0)
{
return;
}
List<KeyValuePair<SegmentId, Vector3>> seeds = new List<KeyValuePair<SegmentId, Vector3>>(offsets);
for (int i = 0; i < seeds.Count; i++)
{
SegmentId id = seeds[i].Key;
Vector3 shift = seeds[i].Value;
if (shift.sqrMagnitude < Epsilon ||
id.PathIndex < 0 ||
id.PathIndex >= paths.Count)
{
continue;
}
DetailedPath path = paths[id.PathIndex];
if (path == null ||
!TryGetSegmentLineKey(path.Points, id.SegmentIndex, out SegmentLineKey lineKey))
{
continue;
}
ExtendOffsetAlongStraightRun(path.Points, id.PathIndex, id.SegmentIndex - 1, -1, lineKey, shift, offsets);
ExtendOffsetAlongStraightRun(path.Points, id.PathIndex, id.SegmentIndex + 1, +1, lineKey, shift, offsets);
}
}
private static void ExtendOffsetAlongStraightRun(
List<Vector3> points,
int pathIndex,
int segmentIndex,
int step,
SegmentLineKey lineKey,
Vector3 shift,
Dictionary<SegmentId, Vector3> offsets)
{
for (int current = segmentIndex; current > 0 && current < points.Count; current += step)
{
if (!TryGetSegmentLineKey(points, current, out SegmentLineKey currentKey) ||
!currentKey.Equals(lineKey))
{
break;
}
SegmentId currentId = new SegmentId(pathIndex, current);
if (offsets.TryGetValue(currentId, out Vector3 existing))
{
if ((existing - shift).sqrMagnitude > Epsilon)
{
break;
}
continue;
}
offsets[currentId] = shift;
}
}
private static bool TryGetSegmentLineKey(
List<Vector3> points,
int segmentIndex,
out SegmentLineKey key)
{
key = default;
if (points == null || segmentIndex <= 0 || segmentIndex >= points.Count)
{
return false;
}
Vector3 start = points[segmentIndex - 1];
Vector3 end = points[segmentIndex];
if (!TryGetAxisDirection(end - start, out Axis axis, out _, out float length) ||
length < Epsilon)
{
return false;
}
key = BuildSegmentLineKey(start, end, axis);
return true;
}
private static void ApplySegmentOffsets(
List<DetailedPath> paths,
Dictionary<SegmentId, Vector3> offsets,
RoutingConfig config)
{
for (int pathIndex = 0; pathIndex < paths.Count; pathIndex++)
{
DetailedPath path = paths[pathIndex];
if (path == null || path.Points.Count < 2 || !HasSegmentOffset(pathIndex, path.Points.Count, offsets))
{
continue;
}
List<Vector3> original = new List<Vector3>(path.Points);
List<Vector3> rebuilt = new List<Vector3>(original.Count + 8) { original[0] };
for (int segmentIndex = 1; segmentIndex < original.Count; segmentIndex++)
{
offsets.TryGetValue(new SegmentId(pathIndex, segmentIndex), out Vector3 shift);
Vector3 shiftedStart = original[segmentIndex - 1] + shift;
Vector3 shiftedEnd = original[segmentIndex] + shift;
AppendAxisAlignedConnection(rebuilt, rebuilt[rebuilt.Count - 1], shiftedStart, config.minSegmentLen);
AppendAxisAlignedConnection(rebuilt, rebuilt[rebuilt.Count - 1], shiftedEnd, config.minSegmentLen);
}
AppendAxisAlignedConnection(rebuilt, rebuilt[rebuilt.Count - 1], original[original.Count - 1], config.minSegmentLen);
path.Points.Clear();
path.Points.AddRange(rebuilt);
SnapNearlyAxisAligned(path.Points);
SimplifyCollinear(path.Points);
}
}
private static bool HasSegmentOffset(
int pathIndex,
int pointCount,
Dictionary<SegmentId, Vector3> offsets)
{
for (int segmentIndex = 1; segmentIndex < pointCount; segmentIndex++)
{
if (offsets.ContainsKey(new SegmentId(pathIndex, segmentIndex)))
{
return true;
}
}
return false;
}
private static void SanitizeEndpointCorridor(
List<Vector3> corridor,
SlotPose endpoint,
ModuleNode module,
RoutingConfig config,
bool fromStart)
{
if (corridor == null ||
corridor.Count == 0 ||
module == null ||
endpoint.Normal.sqrMagnitude < 0.5f)
{
return;
}
Bounds bounds = module.GetWorldBounds();
Vector3 normal = endpoint.Normal.normalized;
float fanoutDistance = Mathf.Max(0f, config.fanoutLength);
float padding = Mathf.Max(config.minSegmentLen, config.lanePitch) * 1.25f;
int maxPoints = Mathf.Min(corridor.Count, 8);
for (int localIndex = 0; localIndex < maxPoints; localIndex++)
{
int index = fromStart ? localIndex : corridor.Count - 1 - localIndex;
Vector3 point = corridor[index];
float outwardDistance = Vector3.Dot(point - endpoint.Position, normal);
bool tangentiallyNear = IsTangentiallyNearBounds(point, bounds, normal, padding);
bool shouldClamp =
outwardDistance < fanoutDistance - Epsilon &&
(localIndex == 0 || tangentiallyNear || IsInsideExpandedBounds(point, bounds, padding));
if (shouldClamp)
{
corridor[index] = ClampToEndpointFanoutPlane(point, endpoint.Position, normal, fanoutDistance);
continue;
}
if (localIndex > 0 && outwardDistance >= fanoutDistance - Epsilon && !tangentiallyNear)
{
break;
}
}
}
private static Vector3 ClampToEndpointFanoutPlane(
Vector3 point,
Vector3 endpointPosition,
Vector3 endpointNormal,
float fanoutDistance)
{
float outwardDistance = Vector3.Dot(point - endpointPosition, endpointNormal);
if (outwardDistance >= fanoutDistance - Epsilon)
{
return point;
}
return point + endpointNormal * (fanoutDistance - outwardDistance);
}
private static bool IsTangentiallyNearBounds(Vector3 point, Bounds bounds, Vector3 normal, float padding)
{
float absX = Mathf.Abs(normal.x);
float absY = Mathf.Abs(normal.y);
float absZ = Mathf.Abs(normal.z);
if (absX >= absY && absX >= absZ)
{
return IsInRange(point.y, bounds.min.y - padding, bounds.max.y + padding) &&
IsInRange(point.z, bounds.min.z - padding, bounds.max.z + padding);
}
if (absY >= absX && absY >= absZ)
{
return IsInRange(point.x, bounds.min.x - padding, bounds.max.x + padding) &&
IsInRange(point.z, bounds.min.z - padding, bounds.max.z + padding);
}
return IsInRange(point.x, bounds.min.x - padding, bounds.max.x + padding) &&
IsInRange(point.y, bounds.min.y - padding, bounds.max.y + padding);
}
private static bool IsInsideExpandedBounds(Vector3 point, Bounds bounds, float padding)
{
return IsInRange(point.x, bounds.min.x - padding, bounds.max.x + padding) &&
IsInRange(point.y, bounds.min.y - padding, bounds.max.y + padding) &&
IsInRange(point.z, bounds.min.z - padding, bounds.max.z + padding);
}
private static bool IsInRange(float value, float min, float max)
{
return value >= min && value <= max;
}
private static Vector3 GetLaneShift(EdgeAxis axis, float offset)
{
return axis switch
{
EdgeAxis.X => new Vector3(0f, 0f, offset),
EdgeAxis.Y => new Vector3(offset, 0f, 0f),
_ => new Vector3(offset, 0f, 0f)
};
}
private static void AppendAxisAlignedConnection(List<Vector3> points, Vector3 from, Vector3 to, float minSegmentLength)
{
if ((to - from).sqrMagnitude < Epsilon)
{
return;
}
if (IsAxisAlignedSegment(from, to))
{
AppendUnique(points, to);
return;
}
AxisMask mask = GetAxisMask(from, to);
Axis[] order = ChooseBestAxisOrder(from, to, mask, minSegmentLength);
Vector3 current = from;
for (int i = 0; i < order.Length; i++)
{
current = SetAxis(current, order[i], GetAxis(to, order[i]));
AppendUnique(points, current);
}
}
private static void AppendEndpointAwareConnection(
List<Vector3> points,
Vector3 from,
Vector3 to,
Vector3 endpointNormal,
bool leavingEndpoint,
RoutingConfig config)
{
if ((to - from).sqrMagnitude < Epsilon)
{
return;
}
if (!TryGetNormalAxis(endpointNormal, out Axis normalAxis, out int normalDirection))
{
AppendAxisAlignedConnection(points, from, to, config.minSegmentLen);
return;
}
AxisMask mask = GetAxisMask(from, to);
float endpointClearance = Mathf.Max(config.minSegmentLen, config.lanePitch);
if ((mask & ToAxisMask(normalAxis)) == 0)
{
float endpointCoordinate = GetAxis(leavingEndpoint ? from : to, normalAxis);
float railCoordinate = endpointCoordinate + normalDirection * endpointClearance;
Vector3 railPoint = SetAxis(leavingEndpoint ? from : to, normalAxis, railCoordinate);
if (leavingEndpoint)
{
AppendUnique(points, railPoint);
AppendAxisAlignedConnection(points, railPoint, to, config.minSegmentLen);
}
else
{
AppendConnectionWithAxisOrder(
points,
from,
railPoint,
BuildForcedAxisOrder(GetAxisMask(from, railPoint), normalAxis, true, config.minSegmentLen, from, railPoint),
config.minSegmentLen);
AppendUnique(points, to);
}
return;
}
Axis[] order = BuildForcedAxisOrder(
mask,
normalAxis,
leavingEndpoint,
config.minSegmentLen,
from,
to);
AppendConnectionWithAxisOrder(points, from, to, order, config.minSegmentLen);
}
private static void AppendConnectionWithAxisOrder(
List<Vector3> points,
Vector3 from,
Vector3 to,
Axis[] order,
float minSegmentLength)
{
if (order == null || order.Length == 0)
{
AppendAxisAlignedConnection(points, from, to, minSegmentLength);
return;
}
Vector3 current = from;
for (int i = 0; i < order.Length; i++)
{
current = SetAxis(current, order[i], GetAxis(to, order[i]));
AppendUnique(points, current);
}
}
private static Axis[] BuildForcedAxisOrder(
AxisMask mask,
Axis forcedAxis,
bool forceFirst,
float minSegmentLength,
Vector3 from,
Vector3 to)
{
AxisMask forcedMask = ToAxisMask(forcedAxis);
if ((mask & forcedMask) == 0)
{
return ChooseBestAxisOrder(from, to, mask, minSegmentLength);
}
AxisMask remainingMask = mask & ~forcedMask;
Axis[] remainingOrder = remainingMask == AxisMask.None
? new Axis[0]
: ChooseBestAxisOrder(from, to, remainingMask, minSegmentLength);
List<Axis> order = new List<Axis>(remainingOrder.Length + 1);
if (forceFirst)
{
order.Add(forcedAxis);
order.AddRange(remainingOrder);
}
else
{
order.AddRange(remainingOrder);
order.Add(forcedAxis);
}
return order.ToArray();
}
private static Axis[] ChooseBestAxisOrder(Vector3 from, Vector3 to, AxisMask mask, float minSegmentLength)
{
List<Axis[]> candidates = BuildAxisOrderCandidates(mask);
if (candidates.Count == 0)
{
return new[] { Axis.X };
}
Axis[] best = candidates[0];
float bestScore = float.NegativeInfinity;
for (int i = 0; i < candidates.Count; i++)
{
Axis[] candidate = candidates[i];
float score = ScoreAxisOrder(from, to, candidate, minSegmentLength);
if (score > bestScore)
{
bestScore = score;
best = candidate;
}
}
return best;
}
private static float ScoreAxisOrder(Vector3 from, Vector3 to, Axis[] order, float minSegmentLength)
{
Vector3 current = from;
float minLen = float.PositiveInfinity;
int shortSegments = 0;
for (int i = 0; i < order.Length; i++)
{
Vector3 next = SetAxis(current, order[i], GetAxis(to, order[i]));
float len = Vector3.Distance(current, next);
minLen = Mathf.Min(minLen, len);
if (len < minSegmentLength)
{
shortSegments++;
}
current = next;
}
return minLen - shortSegments * 1000f;
}
private static List<Axis[]> BuildAxisOrderCandidates(AxisMask mask)
{
List<Axis> axes = new List<Axis>(3);
if ((mask & AxisMask.X) != 0)
{
axes.Add(Axis.X);
}
if ((mask & AxisMask.Y) != 0)
{
axes.Add(Axis.Y);
}
if ((mask & AxisMask.Z) != 0)
{
axes.Add(Axis.Z);
}
List<Axis[]> result = new List<Axis[]>();
if (axes.Count == 0)
{
return result;
}
Permute(axes, 0, result);
return result;
}
private static bool TryGetNormalAxis(Vector3 normal, out Axis axis, out int direction)
{
axis = Axis.X;
direction = 1;
float absX = Mathf.Abs(normal.x);
float absY = Mathf.Abs(normal.y);
float absZ = Mathf.Abs(normal.z);
if (absX < Epsilon && absY < Epsilon && absZ < Epsilon)
{
return false;
}
if (absX >= absY && absX >= absZ)
{
axis = Axis.X;
direction = normal.x >= 0f ? 1 : -1;
return true;
}
if (absY >= absZ)
{
axis = Axis.Y;
direction = normal.y >= 0f ? 1 : -1;
return true;
}
axis = Axis.Z;
direction = normal.z >= 0f ? 1 : -1;
return true;
}
private static AxisMask ToAxisMask(Axis axis)
{
return axis switch
{
Axis.X => AxisMask.X,
Axis.Y => AxisMask.Y,
_ => AxisMask.Z
};
}
private static void Permute(List<Axis> axes, int index, List<Axis[]> output)
{
if (index >= axes.Count)
{
output.Add(axes.ToArray());
return;
}
for (int i = index; i < axes.Count; i++)
{
(axes[index], axes[i]) = (axes[i], axes[index]);
Permute(axes, index + 1, output);
(axes[index], axes[i]) = (axes[i], axes[index]);
}
}
private static void ApplyCornerRounding(
List<Vector3> points,
List<Vector3> cornerMarkers,
float radius,
float minSegmentLength)
{
if (points.Count < 3 || radius <= 0f)
{
return;
}
cornerMarkers.Clear();
List<Vector3> rounded = new List<Vector3>(points.Count * 2) { points[0] };
for (int i = 1; i < points.Count - 1; i++)
{
Vector3 prev = points[i - 1];
Vector3 current = points[i];
Vector3 next = points[i + 1];
Vector3 inDir = current - prev;
Vector3 outDir = next - current;
float inLen = inDir.magnitude;
float outLen = outDir.magnitude;
if (inLen < Epsilon || outLen < Epsilon)
{
continue;
}
inDir /= inLen;
outDir /= outLen;
if (!IsAxisAlignedDirection(inDir) || !IsAxisAlignedDirection(outDir))
{
rounded.Add(current);
continue;
}
if (Vector3.Cross(inDir, outDir).sqrMagnitude < 0.001f)
{
rounded.Add(current);
continue;
}
float trim = Mathf.Min(radius, inLen * 0.45f, outLen * 0.45f);
if (trim < minSegmentLength * 0.2f)
{
rounded.Add(current);
continue;
}
Vector3 start = current - inDir * trim;
Vector3 end = current + outDir * trim;
rounded.Add(start);
rounded.Add(end);
cornerMarkers.Add(current);
}
rounded.Add(points[points.Count - 1]);
points.Clear();
points.AddRange(rounded);
}
private static void SimplifyCollinear(List<Vector3> points)
{
if (points.Count < 3)
{
return;
}
for (int i = points.Count - 2; i > 0; i--)
{
Vector3 a = points[i - 1];
Vector3 b = points[i];
Vector3 c = points[i + 1];
Vector3 ab = b - a;
if (ab.sqrMagnitude < Epsilon)
{
points.RemoveAt(i);
continue;
}
Vector3 bc = c - b;
if (bc.sqrMagnitude < Epsilon)
{
points.RemoveAt(i + 1);
continue;
}
Vector3 abDir = ab.normalized;
Vector3 bcDir = bc.normalized;
if (Vector3.Cross(abDir, bcDir).sqrMagnitude < 0.0005f && Vector3.Dot(abDir, bcDir) > 0.999f)
{
points.RemoveAt(i);
}
}
}
private static void SnapNearlyAxisAligned(List<Vector3> points)
{
if (points.Count < 2)
{
return;
}
const float epsilon = 0.0005f;
for (int i = 1; i < points.Count; i++)
{
Vector3 prev = points[i - 1];
Vector3 current = points[i];
float absDx = Mathf.Abs(current.x - prev.x);
float absDy = Mathf.Abs(current.y - prev.y);
float absDz = Mathf.Abs(current.z - prev.z);
float dominant = Mathf.Max(absDx, Mathf.Max(absDy, absDz));
if (dominant <= epsilon)
{
current = prev;
}
else if (dominant == absDx)
{
if (absDy <= epsilon || absDy < absDx * 0.02f)
{
current.y = prev.y;
}
if (absDz <= epsilon || absDz < absDx * 0.02f)
{
current.z = prev.z;
}
}
else if (dominant == absDy)
{
if (absDx <= epsilon || absDx < absDy * 0.02f)
{
current.x = prev.x;
}
if (absDz <= epsilon || absDz < absDy * 0.02f)
{
current.z = prev.z;
}
}
else
{
if (absDx <= epsilon || absDx < absDz * 0.02f)
{
current.x = prev.x;
}
if (absDy <= epsilon || absDy < absDz * 0.02f)
{
current.y = prev.y;
}
}
points[i] = current;
}
}
private static bool IsAxisAlignedSegment(Vector3 from, Vector3 to)
{
AxisMask mask = GetAxisMask(from, to);
int changes = 0;
if ((mask & AxisMask.X) != 0)
{
changes++;
}
if ((mask & AxisMask.Y) != 0)
{
changes++;
}
if ((mask & AxisMask.Z) != 0)
{
changes++;
}
return changes <= 1;
}
private static bool IsAxisAlignedDirection(Vector3 dir)
{
float absX = Mathf.Abs(dir.x);
float absY = Mathf.Abs(dir.y);
float absZ = Mathf.Abs(dir.z);
return Mathf.Abs(absX - 1f) < 0.0005f ||
Mathf.Abs(absY - 1f) < 0.0005f ||
Mathf.Abs(absZ - 1f) < 0.0005f;
}
private static AxisMask GetAxisMask(Vector3 a, Vector3 b)
{
AxisMask mask = AxisMask.None;
if (Mathf.Abs(a.x - b.x) > Epsilon)
{
mask |= AxisMask.X;
}
if (Mathf.Abs(a.y - b.y) > Epsilon)
{
mask |= AxisMask.Y;
}
if (Mathf.Abs(a.z - b.z) > Epsilon)
{
mask |= AxisMask.Z;
}
return mask;
}
private static float GetAxis(Vector3 value, Axis axis)
{
return axis switch
{
Axis.X => value.x,
Axis.Y => value.y,
_ => value.z
};
}
private static Vector3 SetAxis(Vector3 value, Axis axis, float newValue)
{
switch (axis)
{
case Axis.X:
value.x = newValue;
break;
case Axis.Y:
value.y = newValue;
break;
default:
value.z = newValue;
break;
}
return value;
}
private static void AppendUnique(List<Vector3> points, Vector3 point)
{
if (points.Count > 0 && (points[points.Count - 1] - point).sqrMagnitude < Epsilon)
{
return;
}
points.Add(point);
}
private static void CleanupLocalDetours(List<Vector3> points, float minSegmentLength, float lanePitch)
{
if (points.Count < 3)
{
return;
}
float threshold = Mathf.Max(minSegmentLength, lanePitch) * 1.4f;
int safety = 0;
bool changed;
do
{
changed = false;
changed |= CleanupOppositeAxisSpikes(points, threshold);
changed |= CleanupOppositeDoglegs(points, threshold);
if (changed)
{
SnapNearlyAxisAligned(points);
SimplifyCollinear(points);
}
}
while (changed && ++safety < 10);
}
private static void CleanupLaneTransitionDoglegs(List<Vector3> points, float minSegmentLength, float lanePitch)
{
if (points.Count < 4)
{
return;
}
float maxOuterLeg = Mathf.Max(minSegmentLength, lanePitch) * 3f;
float maxMiddleLeg = Mathf.Max(minSegmentLength, lanePitch) * 8f;
int safety = 0;
bool changed;
do
{
changed = CleanupLaneTransitionDoglegPass(points, maxOuterLeg, maxMiddleLeg);
if (changed)
{
SnapNearlyAxisAligned(points);
SimplifyCollinear(points);
}
}
while (changed && ++safety < 8);
}
private static bool CleanupLaneTransitionDoglegPass(
List<Vector3> points,
float maxOuterLeg,
float maxMiddleLeg)
{
bool changed = false;
for (int i = 1; i < points.Count - 2; i++)
{
Vector3 a = points[i - 1];
Vector3 b = points[i];
Vector3 c = points[i + 1];
Vector3 d = points[i + 2];
if (!TryGetAxisDirection(b - a, out Axis axisAB, out int dirAB, out float lenAB) ||
!TryGetAxisDirection(c - b, out Axis axisBC, out _, out float lenBC) ||
!TryGetAxisDirection(d - c, out Axis axisCD, out int dirCD, out float lenCD))
{
continue;
}
if (axisAB != axisCD || axisAB == axisBC || dirAB == dirCD)
{
continue;
}
if (Mathf.Min(lenAB, lenCD) > maxOuterLeg || lenBC > maxMiddleLeg)
{
continue;
}
if (!TryGetAxisDirection(d - a, out Axis axisAD, out _, out _) || axisAD != axisBC)
{
continue;
}
points.RemoveAt(i + 1);
points.RemoveAt(i);
InsertAxisAlignedBridge(points, i, a, d);
changed = true;
i = Mathf.Max(0, i - 2);
}
return changed;
}
private static void CleanupSelfLoops(List<Vector3> points)
{
if (points.Count < 4)
{
return;
}
int safety = 0;
bool changed;
do
{
changed = TryRemoveOneSelfLoop(points);
if (changed)
{
SnapNearlyAxisAligned(points);
SimplifyCollinear(points);
}
}
while (changed && ++safety < 32);
}
private static bool TryRemoveOneSelfLoop(List<Vector3> points)
{
for (int firstSegment = 0; firstSegment < points.Count - 2; firstSegment++)
{
Vector3 a0 = points[firstSegment];
Vector3 a1 = points[firstSegment + 1];
for (int secondSegment = firstSegment + 2; secondSegment < points.Count - 1; secondSegment++)
{
if (!TryFindAxisAlignedSegmentIntersection(
a0,
a1,
points[secondSegment],
points[secondSegment + 1],
out Vector3 intersection))
{
continue;
}
if ((firstSegment == 0 && ApproximatelyEqual(intersection, points[0])) ||
(secondSegment == points.Count - 2 && ApproximatelyEqual(intersection, points[points.Count - 1])))
{
continue;
}
int removedPointCount = secondSegment - firstSegment;
if (removedPointCount <= 0)
{
continue;
}
List<Vector3> rebuilt = new List<Vector3>(points.Count - removedPointCount + 1);
for (int i = 0; i <= firstSegment; i++)
{
AppendUnique(rebuilt, points[i]);
}
AppendUnique(rebuilt, intersection);
for (int i = secondSegment + 1; i < points.Count; i++)
{
AppendUnique(rebuilt, points[i]);
}
if (rebuilt.Count >= points.Count)
{
continue;
}
points.Clear();
points.AddRange(rebuilt);
return true;
}
}
return false;
}
private static bool TryFindAxisAlignedSegmentIntersection(
Vector3 a0,
Vector3 a1,
Vector3 b0,
Vector3 b1,
out Vector3 intersection)
{
intersection = default;
if (!TryGetAxisDirection(a1 - a0, out Axis axisA, out int dirA, out float lenA) ||
!TryGetAxisDirection(b1 - b0, out Axis axisB, out _, out float lenB) ||
lenA < Epsilon ||
lenB < Epsilon)
{
return false;
}
if (axisA == axisB)
{
return TryFindCollinearOverlapPoint(a0, a1, b0, b1, axisA, dirA, out intersection);
}
Axis thirdAxis = GetRemainingAxis(axisA, axisB);
if (Mathf.Abs(GetAxis(a0, thirdAxis) - GetAxis(b0, thirdAxis)) > 0.0005f)
{
return false;
}
Vector3 candidate = a0;
candidate = SetAxis(candidate, axisA, GetAxis(b0, axisA));
candidate = SetAxis(candidate, axisB, GetAxis(a0, axisB));
candidate = SetAxis(candidate, thirdAxis, GetAxis(a0, thirdAxis));
if (!IsPointOnAxisSegment(candidate, a0, a1) ||
!IsPointOnAxisSegment(candidate, b0, b1))
{
return false;
}
intersection = candidate;
return true;
}
private static bool TryFindCollinearOverlapPoint(
Vector3 a0,
Vector3 a1,
Vector3 b0,
Vector3 b1,
Axis axis,
int dirA,
out Vector3 intersection)
{
intersection = default;
if (!IsSameFixedLine(a0, b0, axis))
{
return false;
}
float aMin = Mathf.Min(GetAxis(a0, axis), GetAxis(a1, axis));
float aMax = Mathf.Max(GetAxis(a0, axis), GetAxis(a1, axis));
float bMin = Mathf.Min(GetAxis(b0, axis), GetAxis(b1, axis));
float bMax = Mathf.Max(GetAxis(b0, axis), GetAxis(b1, axis));
float overlapMin = Mathf.Max(aMin, bMin);
float overlapMax = Mathf.Min(aMax, bMax);
if (overlapMax < overlapMin - 0.0005f)
{
return false;
}
float coordinate = dirA >= 0 ? overlapMin : overlapMax;
intersection = SetAxis(a0, axis, coordinate);
return true;
}
private static bool IsSameFixedLine(Vector3 a, Vector3 b, Axis axis)
{
switch (axis)
{
case Axis.X:
return Mathf.Abs(a.y - b.y) <= 0.0005f && Mathf.Abs(a.z - b.z) <= 0.0005f;
case Axis.Y:
return Mathf.Abs(a.x - b.x) <= 0.0005f && Mathf.Abs(a.z - b.z) <= 0.0005f;
default:
return Mathf.Abs(a.x - b.x) <= 0.0005f && Mathf.Abs(a.y - b.y) <= 0.0005f;
}
}
private static bool ApproximatelyEqual(Vector3 a, Vector3 b)
{
return (a - b).sqrMagnitude <= 0.00000025f;
}
private static bool IsPointOnAxisSegment(Vector3 point, Vector3 a, Vector3 b)
{
if (!TryGetAxisDirection(b - a, out Axis axis, out _, out _))
{
return false;
}
if (!IsSameFixedLine(point, a, axis))
{
return false;
}
float value = GetAxis(point, axis);
float min = Mathf.Min(GetAxis(a, axis), GetAxis(b, axis)) - 0.0005f;
float max = Mathf.Max(GetAxis(a, axis), GetAxis(b, axis)) + 0.0005f;
return value >= min && value <= max;
}
private static Axis GetRemainingAxis(Axis first, Axis second)
{
if (first != Axis.X && second != Axis.X)
{
return Axis.X;
}
if (first != Axis.Y && second != Axis.Y)
{
return Axis.Y;
}
return Axis.Z;
}
private static bool CleanupOppositeAxisSpikes(List<Vector3> points, float maxSpikeLength)
{
bool changed = false;
for (int i = 1; i < points.Count - 1; i++)
{
if (!TryGetAxisDirection(points[i] - points[i - 1], out Axis axisAB, out int dirAB, out float lenAB) ||
!TryGetAxisDirection(points[i + 1] - points[i], out Axis axisBC, out int dirBC, out float lenBC))
{
continue;
}
if (axisAB != axisBC || dirAB == dirBC)
{
continue;
}
if (Mathf.Min(lenAB, lenBC) > maxSpikeLength)
{
continue;
}
points.RemoveAt(i);
changed = true;
i = Mathf.Max(0, i - 2);
}
return changed;
}
private static bool CleanupOppositeDoglegs(List<Vector3> points, float maxDetourLength)
{
bool changed = false;
for (int i = 1; i < points.Count - 2; i++)
{
Vector3 a = points[i - 1];
Vector3 b = points[i];
Vector3 c = points[i + 1];
Vector3 d = points[i + 2];
if (!TryGetAxisDirection(b - a, out Axis axisAB, out int dirAB, out float lenAB) ||
!TryGetAxisDirection(c - b, out Axis axisBC, out _, out float lenBC) ||
!TryGetAxisDirection(d - c, out Axis axisCD, out int dirCD, out float lenCD))
{
continue;
}
if (axisAB != axisCD || axisAB == axisBC || dirAB == dirCD)
{
continue;
}
if (Mathf.Min(lenAB, lenCD) > maxDetourLength || lenBC > maxDetourLength * 1.7f)
{
continue;
}
points.RemoveAt(i + 1);
points.RemoveAt(i);
InsertAxisAlignedBridge(points, i, a, d);
changed = true;
i = Mathf.Max(0, i - 2);
}
return changed;
}
private static void CleanupPostRoundingHooks(List<Vector3> points, float minSegmentLength, float lanePitch)
{
if (points.Count < 3)
{
return;
}
float maxLegLength = Mathf.Max(minSegmentLength, lanePitch) * 0.95f;
float maxMiddleLength = maxLegLength * 2f;
int safety = 0;
bool changed;
do
{
changed = false;
changed |= CleanupShortReversePairs(points, maxLegLength);
changed |= CleanupShortBacktrackingDoglegs(points, maxLegLength, maxMiddleLength);
if (changed)
{
SnapNearlyAxisAligned(points);
SimplifyCollinear(points);
}
}
while (changed && ++safety < 8);
}
private static bool CleanupShortReversePairs(List<Vector3> points, float maxLegLength)
{
bool changed = false;
for (int i = 1; i < points.Count - 1; i++)
{
Vector3 ab = points[i] - points[i - 1];
Vector3 bc = points[i + 1] - points[i];
float lenAB = ab.magnitude;
float lenBC = bc.magnitude;
if (lenAB < Epsilon)
{
points.RemoveAt(i);
changed = true;
i = Mathf.Max(0, i - 2);
continue;
}
if (lenBC < Epsilon)
{
points.RemoveAt(i + 1);
changed = true;
i = Mathf.Max(0, i - 2);
continue;
}
if (Mathf.Min(lenAB, lenBC) > maxLegLength)
{
continue;
}
float dot = Vector3.Dot(ab / lenAB, bc / lenBC);
if (dot > -0.84f)
{
continue;
}
points.RemoveAt(i);
changed = true;
i = Mathf.Max(0, i - 2);
}
return changed;
}
private static bool CleanupShortBacktrackingDoglegs(List<Vector3> points, float maxLegLength, float maxMiddleLength)
{
bool changed = false;
for (int i = 1; i < points.Count - 2; i++)
{
Vector3 a = points[i - 1];
Vector3 b = points[i];
Vector3 c = points[i + 1];
Vector3 d = points[i + 2];
Vector3 ab = b - a;
Vector3 bc = c - b;
Vector3 cd = d - c;
float lenAB = ab.magnitude;
float lenBC = bc.magnitude;
float lenCD = cd.magnitude;
if (lenAB < Epsilon || lenBC < Epsilon || lenCD < Epsilon)
{
continue;
}
if (lenBC > maxMiddleLength || Mathf.Min(lenAB, lenCD) > maxLegLength)
{
continue;
}
float outerDot = Vector3.Dot(ab / lenAB, cd / lenCD);
if (outerDot > -0.38f)
{
continue;
}
Vector3 ad = d - a;
float adLen = ad.magnitude;
if (adLen < Epsilon)
{
continue;
}
Vector3 adDir = ad / adLen;
float progressAB = Vector3.Dot(ab, adDir);
float progressCD = Vector3.Dot(cd, adDir);
if (progressAB * progressCD >= 0f)
{
continue;
}
points.RemoveAt(i + 1);
points.RemoveAt(i);
InsertAxisAlignedBridge(points, i, a, d);
changed = true;
i = Mathf.Max(0, i - 2);
}
return changed;
}
private static void InsertAxisAlignedBridge(List<Vector3> points, int insertIndex, Vector3 from, Vector3 to)
{
AxisMask mask = GetAxisMask(from, to);
if (mask == AxisMask.None || IsAxisAlignedSegment(from, to))
{
return;
}
Axis[] order = ChooseBestAxisOrder(from, to, mask, 0f);
Vector3 current = from;
for (int i = 0; i < order.Length - 1; i++)
{
current = SetAxis(current, order[i], GetAxis(to, order[i]));
points.Insert(insertIndex + i, current);
}
}
private static bool TryGetAxisDirection(Vector3 segment, out Axis axis, out int dir, out float length)
{
axis = Axis.X;
dir = 0;
length = 0f;
float absX = Mathf.Abs(segment.x);
float absY = Mathf.Abs(segment.y);
float absZ = Mathf.Abs(segment.z);
float dominant = Mathf.Max(absX, Mathf.Max(absY, absZ));
if (dominant < Epsilon)
{
return false;
}
float crossTolerance = dominant * 0.02f + 0.0005f;
if (dominant == absX)
{
if (absY > crossTolerance || absZ > crossTolerance)
{
return false;
}
axis = Axis.X;
dir = segment.x >= 0f ? 1 : -1;
length = absX;
return true;
}
if (dominant == absY)
{
if (absX > crossTolerance || absZ > crossTolerance)
{
return false;
}
axis = Axis.Y;
dir = segment.y >= 0f ? 1 : -1;
length = absY;
return true;
}
if (absX > crossTolerance || absY > crossTolerance)
{
return false;
}
axis = Axis.Z;
dir = segment.z >= 0f ? 1 : -1;
length = absZ;
return true;
}
[System.Flags]
private enum AxisMask
{
None = 0,
X = 1,
Y = 2,
Z = 4
}
private enum Axis
{
X,
Y,
Z
}
}
public sealed class GlobalRouter
{
private const int AxisStateCount = 4;
private enum AxisState : byte
{
None = 0,
X = 1,
Y = 2,
Z = 3
}
private readonly struct RouteConstraint
{
public RouteConstraint(bool usePlanar, float planeY, float tolerance)
{
UsePlanar = usePlanar;
PlaneY = planeY;
Tolerance = tolerance;
}
public bool UsePlanar { get; }
public float PlaneY { get; }
public float Tolerance { get; }
}
public GlobalRoute RouteFlow(
CorridorGraph graph,
FlowRequest flow,
RoutingConfig config,
int[] edgeUsage,
float[] edgeHistory,
Dictionary<int, List<int>> crossingMap,
out float routeCost,
long deadlineTicks = long.MaxValue)
{
routeCost = 0f;
if (flow == null || graph == null || graph.Nodes.Count == 0 || graph.Edges.Count == 0)
{
return null;
}
if (!flow.TryResolve(out SlotPose src, out SlotPose dst))
{
return null;
}
RouteConstraint constraint = BuildRouteConstraint(flow, src, dst, config);
Vector3 startAnchor = src.Position + src.Normal * Mathf.Max(config.clearance, config.fanoutLength);
Vector3 endAnchor = dst.Position + dst.Normal * Mathf.Max(config.clearance, config.fanoutLength);
if (constraint.UsePlanar)
{
startAnchor.y = constraint.PlaneY;
endAnchor.y = constraint.PlaneY;
}
int startNode = FindClosestNode(graph, startAnchor, constraint, src.Position, src.Normal);
int targetNode = FindClosestNode(graph, endAnchor, constraint, dst.Position, dst.Normal);
if (startNode < 0 || targetNode < 0)
{
return null;
}
int stateCount = graph.Nodes.Count * AxisStateCount;
float[] gScore = new float[stateCount];
int[] previousState = new int[stateCount];
int[] previousEdge = new int[stateCount];
byte[] closed = new byte[stateCount];
for (int i = 0; i < stateCount; i++)
{
gScore[i] = float.PositiveInfinity;
previousState[i] = -1;
previousEdge[i] = -1;
}
int startState = EncodeState(startNode, AxisState.None);
float startHeuristic = Heuristic(graph.Nodes[startNode], graph.Nodes[targetNode], config);
float maxPrunedHeuristic = startHeuristic * Mathf.Max(1f, config.heuristicPruneMultiplier) + Mathf.Max(0f, config.heuristicPrunePadding);
gScore[startState] = 0f;
MinHeap open = new MinHeap();
open.Push(startState, startHeuristic);
int expandedNodes = 0;
int goalState = -1;
while (open.Count > 0)
{
if (Stopwatch.GetTimestamp() >= deadlineTicks)
{
return null;
}
int currentState = open.PopMin();
if (closed[currentState] != 0)
{
continue;
}
closed[currentState] = 1;
expandedNodes++;
if (expandedNodes > config.maxExpandedNodesPerFlow)
{
return null;
}
int currentNode = DecodeNode(currentState);
if (currentNode == targetNode)
{
goalState = currentState;
break;
}
AxisState currentAxis = DecodeAxis(currentState);
List<int> incidentEdges = graph.NodeToEdges[currentNode];
for (int edgeIndex = 0; edgeIndex < incidentEdges.Count; edgeIndex++)
{
int edgeId = incidentEdges[edgeIndex];
CorridorEdge edge = graph.Edges[edgeId];
int neighborNode = graph.GetOtherNode(edgeId, currentNode);
if (constraint.UsePlanar)
{
if (edge.Axis == EdgeAxis.Y)
{
continue;
}
if (Mathf.Abs(graph.Nodes[neighborNode].y - constraint.PlaneY) > constraint.Tolerance)
{
continue;
}
}
AxisState nextAxis = AxisFromEdge(edge.Axis);
int nextState = EncodeState(neighborNode, nextAxis);
if (closed[nextState] != 0)
{
continue;
}
float heuristic = Heuristic(graph.Nodes[neighborNode], graph.Nodes[targetNode], config);
if (config.useHeuristicPruning && heuristic > maxPrunedHeuristic)
{
continue;
}
float bendPenalty = currentAxis != AxisState.None && currentAxis != nextAxis ? config.wBend : 0f;
float presentCongestion = (float)edgeUsage[edgeId] / Mathf.Max(1, edge.Capacity);
int overflow = Mathf.Max(0, edgeUsage[edgeId] + 1 - edge.Capacity);
float crossingEstimate = EstimateCrossingPenalty(edgeId, edgeUsage, edgeHistory, crossingMap);
float weightedCrossingPenalty = config.wCrossing * crossingEstimate * (1f + crossingEstimate * 0.08f);
float crossingHotspotPenalty = config.wCrossing * EstimateCrossingHotspotPenalty(edgeId, crossingMap);
float stepCost =
config.wLength * edge.Length +
edge.BaseCost +
bendPenalty +
config.wCongestion * presentCongestion +
config.wHistory * edgeHistory[edgeId] +
config.wOverflow * overflow +
weightedCrossingPenalty +
crossingHotspotPenalty;
float tentativeG = gScore[currentState] + stepCost;
if (tentativeG >= gScore[nextState])
{
continue;
}
gScore[nextState] = tentativeG;
previousState[nextState] = currentState;
previousEdge[nextState] = edgeId;
open.Push(nextState, tentativeG + heuristic);
}
}
if (goalState < 0)
{
return null;
}
GlobalRoute route = ReconstructRoute(flow, graph, goalState, previousState, previousEdge);
if (route == null)
{
return null;
}
int crossings = CountCrossings(route.EdgeIds, edgeUsage, crossingMap);
route.CrossingCount = crossings;
route.UsesCrossingFallback = crossings > 0;
if (!config.mustRenderAll && crossings > config.maxCrossingFallback)
{
return null;
}
routeCost = gScore[goalState];
return route;
}
private static GlobalRoute ReconstructRoute(
FlowRequest flow,
CorridorGraph graph,
int goalState,
IReadOnlyList<int> previousState,
IReadOnlyList<int> previousEdge)
{
List<int> reversedEdges = new List<int>();
List<int> reversedNodes = new List<int> { DecodeNode(goalState) };
int currentState = goalState;
while (previousState[currentState] >= 0)
{
reversedEdges.Add(previousEdge[currentState]);
currentState = previousState[currentState];
reversedNodes.Add(DecodeNode(currentState));
}
GlobalRoute route = new GlobalRoute { Flow = flow };
reversedEdges.Reverse();
reversedNodes.Reverse();
route.EdgeIds.AddRange(reversedEdges);
route.NodeIds.AddRange(reversedNodes);
float totalLength = 0f;
int bends = 0;
EdgeAxis? previousAxis = null;
for (int i = 0; i < route.EdgeIds.Count; i++)
{
CorridorEdge edge = graph.Edges[route.EdgeIds[i]];
totalLength += edge.Length;
if (previousAxis.HasValue && previousAxis.Value != edge.Axis)
{
bends++;
}
previousAxis = edge.Axis;
}
route.Length = totalLength;
route.Bends = bends;
return route;
}
private static RouteConstraint BuildRouteConstraint(FlowRequest flow, SlotPose src, SlotPose dst, RoutingConfig config)
{
if (config == null || !config.enforcePlanarRoutingOnSameHeight)
{
return new RouteConstraint(false, 0f, 0f);
}
float tolerance = Mathf.Max(0.001f, config.sameHeightTolerance);
float sourceHeight = src.Position.y;
float targetHeight = dst.Position.y;
if (flow?.Source?.Module != null)
{
sourceHeight = flow.Source.Module.GetWorldBounds().center.y;
}
if (flow?.Target?.Module != null)
{
targetHeight = flow.Target.Module.GetWorldBounds().center.y;
}
if (Mathf.Abs(sourceHeight - targetHeight) > tolerance)
{
return new RouteConstraint(false, 0f, 0f);
}
float planeY = (sourceHeight + targetHeight) * 0.5f;
float planeTolerance = Mathf.Max(tolerance, config.planarNodeTolerance);
return new RouteConstraint(true, planeY, planeTolerance);
}
private static AxisState AxisFromEdge(EdgeAxis axis)
{
return axis switch
{
EdgeAxis.X => AxisState.X,
EdgeAxis.Y => AxisState.Y,
_ => AxisState.Z
};
}
private static int EncodeState(int nodeId, AxisState axis)
{
return nodeId * AxisStateCount + (int)axis;
}
private static int DecodeNode(int state)
{
return state / AxisStateCount;
}
private static AxisState DecodeAxis(int state)
{
return (AxisState)(state % AxisStateCount);
}
private static int FindClosestNode(
CorridorGraph graph,
Vector3 point,
RouteConstraint constraint,
Vector3 endpointPosition,
Vector3 endpointNormal)
{
int preferredNode = FindClosestNode(
graph,
point,
constraint,
endpointPosition,
endpointNormal,
requireEndpointSide: true);
if (preferredNode >= 0)
{
return preferredNode;
}
return FindClosestNode(
graph,
point,
constraint,
endpointPosition,
endpointNormal,
requireEndpointSide: false);
}
private static int FindClosestNode(
CorridorGraph graph,
Vector3 point,
RouteConstraint constraint,
Vector3 endpointPosition,
Vector3 endpointNormal,
bool requireEndpointSide)
{
if (graph.TryGetNode(point, out int exactNode))
{
Vector3 exactPoint = graph.Nodes[exactNode];
if ((!constraint.UsePlanar || Mathf.Abs(exactPoint.y - constraint.PlaneY) <= constraint.Tolerance) &&
IsOnEndpointOuterSide(exactPoint, endpointPosition, endpointNormal, requireEndpointSide))
{
return exactNode;
}
}
int bestNode = -1;
float bestSqrDistance = float.PositiveInfinity;
for (int i = 0; i < graph.Nodes.Count; i++)
{
Vector3 node = graph.Nodes[i];
if (constraint.UsePlanar && Mathf.Abs(node.y - constraint.PlaneY) > constraint.Tolerance)
{
continue;
}
if (!IsOnEndpointOuterSide(node, endpointPosition, endpointNormal, requireEndpointSide))
{
continue;
}
float sqr = (node - point).sqrMagnitude;
if (sqr < bestSqrDistance)
{
bestSqrDistance = sqr;
bestNode = i;
if (bestSqrDistance <= 0.000001f)
{
break;
}
}
}
return bestNode;
}
private static bool IsOnEndpointOuterSide(
Vector3 point,
Vector3 endpointPosition,
Vector3 endpointNormal,
bool requireEndpointSide)
{
if (!requireEndpointSide || endpointNormal.sqrMagnitude < 0.5f)
{
return true;
}
Vector3 normal = endpointNormal.normalized;
return Vector3.Dot(point - endpointPosition, normal) >= -0.001f;
}
private static float EstimateCrossingPenalty(
int edgeId,
int[] edgeUsage,
float[] edgeHistory,
Dictionary<int, List<int>> crossingMap)
{
if (!crossingMap.TryGetValue(edgeId, out List<int> crossingEdges))
{
return 0f;
}
float conflicts = 0f;
for (int i = 0; i < crossingEdges.Count; i++)
{
int crossingEdgeId = crossingEdges[i];
int usage = edgeUsage[crossingEdgeId];
if (usage <= 0)
{
continue;
}
conflicts += 1f + 0.35f * (usage - 1f);
conflicts += edgeHistory[crossingEdgeId] * 0.05f;
}
return conflicts;
}
private static int CountCrossings(IReadOnlyList<int> routeEdges, int[] edgeUsage, Dictionary<int, List<int>> crossingMap)
{
int crossings = 0;
for (int i = 0; i < routeEdges.Count; i++)
{
if (!crossingMap.TryGetValue(routeEdges[i], out List<int> crossingEdges))
{
continue;
}
for (int crossingIndex = 0; crossingIndex < crossingEdges.Count; crossingIndex++)
{
int usage = edgeUsage[crossingEdges[crossingIndex]];
if (usage > 0)
{
crossings += usage;
}
}
}
return crossings;
}
private static float EstimateCrossingHotspotPenalty(int edgeId, Dictionary<int, List<int>> crossingMap)
{
if (!crossingMap.TryGetValue(edgeId, out List<int> crossingEdges))
{
return 0f;
}
return crossingEdges.Count * 0.03f;
}
private static float Heuristic(Vector3 current, Vector3 target, RoutingConfig config)
{
return (Mathf.Abs(target.x - current.x) + Mathf.Abs(target.y - current.y) + Mathf.Abs(target.z - current.z)) * config.wLength;
}
private sealed class MinHeap
{
private readonly List<HeapEntry> items = new List<HeapEntry>(256);
public int Count => items.Count;
public void Push(int state, float score)
{
items.Add(new HeapEntry(state, score));
SiftUp(items.Count - 1);
}
public int PopMin()
{
HeapEntry root = items[0];
int lastIndex = items.Count - 1;
items[0] = items[lastIndex];
items.RemoveAt(lastIndex);
if (items.Count > 0)
{
SiftDown(0);
}
return root.State;
}
private void SiftUp(int index)
{
while (index > 0)
{
int parent = (index - 1) >> 1;
if (items[parent].Score <= items[index].Score)
{
break;
}
(items[parent], items[index]) = (items[index], items[parent]);
index = parent;
}
}
private void SiftDown(int index)
{
int count = items.Count;
while (true)
{
int left = (index << 1) + 1;
if (left >= count)
{
break;
}
int right = left + 1;
int smallest = left;
if (right < count && items[right].Score < items[left].Score)
{
smallest = right;
}
if (items[index].Score <= items[smallest].Score)
{
break;
}
(items[index], items[smallest]) = (items[smallest], items[index]);
index = smallest;
}
}
private readonly struct HeapEntry
{
public HeapEntry(int state, float score)
{
State = state;
Score = score;
}
public int State { get; }
public float Score { get; }
}
}
}
public sealed class NegotiatedCongestionSolver
{
private readonly GlobalRouter globalRouter = new GlobalRouter();
public NegotiationResult Solve(
CorridorGraph graph,
IReadOnlyList<FlowRequest> flowRequests,
RoutingConfig config,
int maxIterationsOverride = -1)
{
NegotiationResult bestResult = new NegotiationResult
{
EdgeUsage = new int[graph.Edges.Count],
EdgeOverflow = new int[graph.Edges.Count],
EdgeHistory = new float[graph.Edges.Count]
};
if (graph == null || graph.Edges.Count == 0 || flowRequests.Count == 0)
{
return bestResult;
}
Dictionary<int, List<int>> crossingMap = BuildCrossingMap(graph, config);
float[] edgeHistory = new float[graph.Edges.Count];
bool crossingMapDisabled =
config.disableCrossingMapWhenLarge &&
config.crossingMapMaxEdges > 0 &&
graph.Edges.Count > config.crossingMapMaxEdges;
int iterations = maxIterationsOverride > 0 ? maxIterationsOverride : Mathf.Max(1, config.maxNegotiationIters);
float bestScore = float.PositiveInfinity;
long now = Stopwatch.GetTimestamp();
long solveBudgetTicks = (long)(Mathf.Max(0.5f, config.maxSolveTimeSeconds) * Stopwatch.Frequency);
long solveDeadline = now + solveBudgetTicks;
List<FlowRequest> sortedFlows = SortFlows(graph, flowRequests, config, solveDeadline);
bool crossingWarningAdded = false;
int crossingOptimizationItersUsed = 0;
for (int iter = 0; iter < iterations; iter++)
{
if (Stopwatch.GetTimestamp() >= solveDeadline)
{
bestResult.Warnings.Add($"Routing stopped by solve timeout ({config.maxSolveTimeSeconds:F1}s).");
break;
}
bool prioritizeCrossings =
config.optimizeCrossingsAfterOverflow &&
crossingMap.Count > 0 &&
crossingOptimizationItersUsed > 0;
IterationResult iteration = ExecuteIteration(
graph,
sortedFlows,
config,
edgeHistory,
crossingMap,
solveDeadline,
prioritizeCrossings);
float score = ComputeScore(iteration);
if (score < bestScore)
{
bestScore = score;
bestResult = CopyToNegotiationResult(iteration, edgeHistory, iter + 1);
ApplyCrossingMetadata(bestResult, crossingMap, crossingMapDisabled, graph.Edges.Count, config, crossingOptimizationItersUsed);
if (crossingMapDisabled && !crossingWarningAdded)
{
bestResult.Warnings.Add(
$"Crossing-map skipped for performance (edges={graph.Edges.Count}, limit={config.crossingMapMaxEdges}).");
crossingWarningAdded = true;
}
}
UpdateHistoryCosts(edgeHistory, iteration.EdgeOverflow);
if (iteration.Metrics.OverflowCount > 0)
{
crossingOptimizationItersUsed = 0;
continue;
}
if (iteration.Metrics.CrossingCount <= 0 || !config.optimizeCrossingsAfterOverflow || crossingMap.Count == 0)
{
break;
}
crossingOptimizationItersUsed++;
if (crossingOptimizationItersUsed >= Mathf.Max(0, config.maxCrossingOptimizationIters))
{
bestResult.Warnings.Add(
$"Crossing optimization stopped after {crossingOptimizationItersUsed} iteration(s) (limit={config.maxCrossingOptimizationIters}).");
break;
}
}
ApplyCrossingMetadata(bestResult, crossingMap, crossingMapDisabled, graph.Edges.Count, config, crossingOptimizationItersUsed);
return bestResult;
}
public async UniTask SolveIncrementalAsync(
CorridorGraph graph,
IReadOnlyList<FlowRequest> flowRequests,
RoutingConfig config,
Action<NegotiationProgress> onProgress,
CancellationToken cancellationToken,
int maxIterationsOverride = -1)
{
NegotiationResult bestResult = new NegotiationResult
{
EdgeUsage = graph != null ? new int[graph.Edges.Count] : Array.Empty<int>(),
EdgeOverflow = graph != null ? new int[graph.Edges.Count] : Array.Empty<int>(),
EdgeHistory = graph != null ? new float[graph.Edges.Count] : Array.Empty<float>()
};
if (graph == null || graph.Edges.Count == 0 || flowRequests == null || flowRequests.Count == 0)
{
onProgress?.Invoke(new NegotiationProgress
{
Phase = NegotiationProgressPhase.Completed,
BestResult = bestResult,
IsFinal = true,
Message = "No routable flows."
});
return;
}
Dictionary<int, List<int>> crossingMap = BuildCrossingMap(graph, config);
float[] edgeHistory = new float[graph.Edges.Count];
bool crossingMapDisabled =
config.disableCrossingMapWhenLarge &&
config.crossingMapMaxEdges > 0 &&
graph.Edges.Count > config.crossingMapMaxEdges;
int iterations = maxIterationsOverride > 0 ? maxIterationsOverride : Mathf.Max(1, config.maxNegotiationIters);
float bestScore = float.PositiveInfinity;
long now = Stopwatch.GetTimestamp();
long solveBudgetTicks = (long)(Mathf.Max(0.5f, config.maxSolveTimeSeconds) * Stopwatch.Frequency);
long solveDeadline = now + solveBudgetTicks;
List<FlowRequest> sortedFlows = SortFlows(graph, flowRequests, config, solveDeadline);
bool crossingWarningAdded = false;
int crossingOptimizationItersUsed = 0;
onProgress?.Invoke(new NegotiationProgress
{
Phase = NegotiationProgressPhase.Started,
BestResult = bestResult,
Iteration = 0,
CompletedFlows = 0,
TotalFlows = sortedFlows.Count,
Message = "Incremental routing started."
});
await UniTask.Yield(cancellationToken);
for (int iter = 0; iter < iterations; iter++)
{
cancellationToken.ThrowIfCancellationRequested();
if (Stopwatch.GetTimestamp() >= solveDeadline)
{
bestResult.Warnings.Add($"Routing stopped by solve timeout ({config.maxSolveTimeSeconds:F1}s).");
break;
}
bool prioritizeCrossings =
config.optimizeCrossingsAfterOverflow &&
crossingMap.Count > 0 &&
crossingOptimizationItersUsed > 0;
onProgress?.Invoke(new NegotiationProgress
{
Phase = NegotiationProgressPhase.IterationStarted,
BestResult = bestResult,
Iteration = iter + 1,
CompletedFlows = 0,
TotalFlows = sortedFlows.Count,
Message = $"Negotiation iteration {iter + 1} started."
});
await UniTask.Yield(cancellationToken);
IterationResult iteration = new IterationResult
{
EdgeUsage = new int[graph.Edges.Count],
EdgeOverflow = new int[graph.Edges.Count]
};
await ExecuteIterationIncrementalAsync(
graph,
sortedFlows,
config,
edgeHistory,
crossingMap,
solveDeadline,
prioritizeCrossings,
iteration,
onProgress,
iter + 1,
cancellationToken);
float score = ComputeScore(iteration);
if (score < bestScore)
{
bestScore = score;
bestResult = CopyToNegotiationResult(iteration, edgeHistory, iter + 1);
ApplyCrossingMetadata(bestResult, crossingMap, crossingMapDisabled, graph.Edges.Count, config, crossingOptimizationItersUsed);
if (crossingMapDisabled && !crossingWarningAdded)
{
bestResult.Warnings.Add(
$"Crossing-map skipped for performance (edges={graph.Edges.Count}, limit={config.crossingMapMaxEdges}).");
crossingWarningAdded = true;
}
}
UpdateHistoryCosts(edgeHistory, iteration.EdgeOverflow);
onProgress?.Invoke(new NegotiationProgress
{
Phase = NegotiationProgressPhase.IterationCompleted,
Routes = new List<GlobalRoute>(bestResult.Routes),
BestResult = bestResult,
Iteration = iter + 1,
CompletedFlows = iteration.Routes.Count,
TotalFlows = sortedFlows.Count,
Message = $"Negotiation iteration {iter + 1} completed."
});
await UniTask.Yield(cancellationToken);
if (iteration.Metrics.OverflowCount > 0)
{
crossingOptimizationItersUsed = 0;
continue;
}
if (iteration.Metrics.CrossingCount <= 0 || !config.optimizeCrossingsAfterOverflow || crossingMap.Count == 0)
{
break;
}
crossingOptimizationItersUsed++;
if (crossingOptimizationItersUsed >= Mathf.Max(0, config.maxCrossingOptimizationIters))
{
bestResult.Warnings.Add(
$"Crossing optimization stopped after {crossingOptimizationItersUsed} iteration(s) (limit={config.maxCrossingOptimizationIters}).");
break;
}
}
ApplyCrossingMetadata(bestResult, crossingMap, crossingMapDisabled, graph.Edges.Count, config, crossingOptimizationItersUsed);
onProgress?.Invoke(new NegotiationProgress
{
Phase = NegotiationProgressPhase.Completed,
Routes = new List<GlobalRoute>(bestResult.Routes),
BestResult = bestResult,
Iteration = bestResult.IterationsUsed,
CompletedFlows = bestResult.Routes.Count,
TotalFlows = sortedFlows.Count,
IsFinal = true,
Message = "Incremental routing completed."
});
}
private IterationResult ExecuteIteration(
CorridorGraph graph,
IReadOnlyList<FlowRequest> sortedFlows,
RoutingConfig config,
float[] edgeHistory,
Dictionary<int, List<int>> crossingMap,
long solveDeadlineTicks,
bool prioritizeCrossings)
{
IterationResult result = new IterationResult
{
EdgeUsage = new int[graph.Edges.Count],
EdgeOverflow = new int[graph.Edges.Count]
};
List<(GlobalRoute route, float cost)> routed = new List<(GlobalRoute route, float cost)>();
for (int i = 0; i < sortedFlows.Count; i++)
{
if (Stopwatch.GetTimestamp() >= solveDeadlineTicks)
{
result.FailedFlows.Add(sortedFlows[i]);
continue;
}
FlowRequest flow = sortedFlows[i];
long routeBudgetTicks = (long)(Mathf.Max(0.01f, config.maxRouteTimeSeconds) * Stopwatch.Frequency);
long routeDeadline = Stopwatch.GetTimestamp() + routeBudgetTicks;
if (routeDeadline > solveDeadlineTicks)
{
routeDeadline = solveDeadlineTicks;
}
GlobalRoute route = globalRouter.RouteFlow(
graph,
flow,
config,
result.EdgeUsage,
edgeHistory,
crossingMap,
out float routeCost,
routeDeadline);
if (route == null)
{
result.FailedFlows.Add(flow);
continue;
}
routed.Add((route, routeCost));
IncrementUsage(result.EdgeUsage, route.EdgeIds, +1);
}
if (Stopwatch.GetTimestamp() < solveDeadlineTicks)
{
ApplyRipUpAndReroute(
graph,
routed,
result,
config,
edgeHistory,
crossingMap,
solveDeadlineTicks,
prioritizeCrossings);
}
for (int edgeId = 0; edgeId < graph.Edges.Count; edgeId++)
{
int overflow = Mathf.Max(0, result.EdgeUsage[edgeId] - graph.Edges[edgeId].Capacity);
result.EdgeOverflow[edgeId] = overflow;
result.Metrics.OverflowCount += overflow;
}
for (int i = 0; i < routed.Count; i++)
{
GlobalRoute route = routed[i].route;
int crossingCount = CountCrossings(route.EdgeIds, result.EdgeUsage, crossingMap);
route.CrossingCount = crossingCount;
route.UsesCrossingFallback = crossingCount > 0;
result.Routes.Add(route);
result.Metrics.TotalLength += route.Length;
result.Metrics.TotalBends += route.Bends;
result.Metrics.CrossingCount += crossingCount;
}
result.Metrics.RoutedCount = result.Routes.Count;
result.Metrics.FailedCount = result.FailedFlows.Count;
return result;
}
private async UniTask ExecuteIterationIncrementalAsync(
CorridorGraph graph,
IReadOnlyList<FlowRequest> sortedFlows,
RoutingConfig config,
float[] edgeHistory,
Dictionary<int, List<int>> crossingMap,
long solveDeadlineTicks,
bool prioritizeCrossings,
IterationResult result,
Action<NegotiationProgress> onProgress,
int iterationIndex,
CancellationToken cancellationToken)
{
List<(GlobalRoute route, float cost)> routed = new List<(GlobalRoute route, float cost)>();
for (int i = 0; i < sortedFlows.Count; i++)
{
cancellationToken.ThrowIfCancellationRequested();
FlowRequest flow = sortedFlows[i];
if (Stopwatch.GetTimestamp() >= solveDeadlineTicks)
{
result.FailedFlows.Add(flow);
onProgress?.Invoke(new NegotiationProgress
{
Phase = NegotiationProgressPhase.RouteCommitted,
RoutedPreviewCount = routed.Count,
LatestFlow = flow,
Iteration = iterationIndex,
CompletedFlows = i + 1,
TotalFlows = sortedFlows.Count,
Message = "Route skipped because solve timeout was reached."
});
await UniTask.Yield(cancellationToken);
continue;
}
long routeBudgetTicks = (long)(Mathf.Max(0.01f, config.maxRouteTimeSeconds) * Stopwatch.Frequency);
long routeDeadline = Stopwatch.GetTimestamp() + routeBudgetTicks;
if (routeDeadline > solveDeadlineTicks)
{
routeDeadline = solveDeadlineTicks;
}
GlobalRoute route = globalRouter.RouteFlow(
graph,
flow,
config,
result.EdgeUsage,
edgeHistory,
crossingMap,
out float routeCost,
routeDeadline);
if (route == null)
{
result.FailedFlows.Add(flow);
onProgress?.Invoke(new NegotiationProgress
{
Phase = NegotiationProgressPhase.RouteCommitted,
RoutedPreviewCount = routed.Count,
LatestFlow = flow,
Iteration = iterationIndex,
CompletedFlows = i + 1,
TotalFlows = sortedFlows.Count,
Message = "Flow failed to route in this iteration."
});
await UniTask.Yield(cancellationToken);
continue;
}
routed.Add((route, routeCost));
IncrementUsage(result.EdgeUsage, route.EdgeIds, +1);
onProgress?.Invoke(new NegotiationProgress
{
Phase = NegotiationProgressPhase.RouteCommitted,
RoutedPreviewCount = routed.Count,
LatestRoute = route,
LatestFlow = flow,
Iteration = iterationIndex,
CompletedFlows = i + 1,
TotalFlows = sortedFlows.Count,
Message = $"Routed {i + 1}/{sortedFlows.Count}."
});
await UniTask.Yield(cancellationToken);
}
if (Stopwatch.GetTimestamp() < solveDeadlineTicks)
{
await ApplyRipUpAndRerouteIncrementalAsync(
graph,
routed,
result,
config,
edgeHistory,
crossingMap,
solveDeadlineTicks,
prioritizeCrossings,
onProgress,
iterationIndex,
sortedFlows.Count,
cancellationToken);
}
for (int edgeId = 0; edgeId < graph.Edges.Count; edgeId++)
{
int overflow = Mathf.Max(0, result.EdgeUsage[edgeId] - graph.Edges[edgeId].Capacity);
result.EdgeOverflow[edgeId] = overflow;
result.Metrics.OverflowCount += overflow;
}
for (int i = 0; i < routed.Count; i++)
{
GlobalRoute route = routed[i].route;
int crossingCount = CountCrossings(route.EdgeIds, result.EdgeUsage, crossingMap);
route.CrossingCount = crossingCount;
route.UsesCrossingFallback = crossingCount > 0;
result.Routes.Add(route);
result.Metrics.TotalLength += route.Length;
result.Metrics.TotalBends += route.Bends;
result.Metrics.CrossingCount += crossingCount;
}
result.Metrics.RoutedCount = result.Routes.Count;
result.Metrics.FailedCount = result.FailedFlows.Count;
}
private void ApplyRipUpAndReroute(
CorridorGraph graph,
List<(GlobalRoute route, float cost)> routed,
IterationResult result,
RoutingConfig config,
float[] edgeHistory,
Dictionary<int, List<int>> crossingMap,
long solveDeadlineTicks,
bool prioritizeCrossings)
{
if (routed.Count == 0)
{
return;
}
bool hasOverflow = HasOverflow(graph, result.EdgeUsage);
bool crossingMode = !hasOverflow && prioritizeCrossings && crossingMap.Count > 0;
float ripUpRatio = hasOverflow ? config.ripUpPercent : config.crossingRipUpPercent;
if (ripUpRatio <= 0f)
{
return;
}
int rerouteCount = Mathf.CeilToInt(routed.Count * ripUpRatio);
rerouteCount = Mathf.Clamp(rerouteCount, 1, routed.Count);
routed.Sort((left, right) =>
{
float leftScore = ComputeOverflowContribution(left.route, graph, result.EdgeUsage);
float rightScore = ComputeOverflowContribution(right.route, graph, result.EdgeUsage);
int leftCrossings = ComputeCrossingContribution(left.route, result.EdgeUsage, crossingMap);
int rightCrossings = ComputeCrossingContribution(right.route, result.EdgeUsage, crossingMap);
float leftRank = crossingMode
? leftCrossings * Mathf.Max(1f, config.crossingRerouteBoost) + leftScore * 4f + left.route.Bends * 0.2f
: leftScore * 100f + leftCrossings * 0.5f + left.route.Bends;
float rightRank = crossingMode
? rightCrossings * Mathf.Max(1f, config.crossingRerouteBoost) + rightScore * 4f + right.route.Bends * 0.2f
: rightScore * 100f + rightCrossings * 0.5f + right.route.Bends;
return rightRank.CompareTo(leftRank);
});
List<(GlobalRoute route, float cost)> selected = routed.GetRange(0, rerouteCount);
for (int i = 0; i < selected.Count; i++)
{
IncrementUsage(result.EdgeUsage, selected[i].route.EdgeIds, -1);
}
for (int i = 0; i < selected.Count; i++)
{
FlowRequest flow = selected[i].route.Flow;
if (Stopwatch.GetTimestamp() >= solveDeadlineTicks)
{
result.FailedFlows.Add(flow);
continue;
}
long routeBudgetTicks = (long)(Mathf.Max(0.01f, config.maxRouteTimeSeconds) * Stopwatch.Frequency);
long routeDeadline = Stopwatch.GetTimestamp() + routeBudgetTicks;
if (routeDeadline > solveDeadlineTicks)
{
routeDeadline = solveDeadlineTicks;
}
GlobalRoute rerouted = globalRouter.RouteFlow(
graph,
flow,
config,
result.EdgeUsage,
edgeHistory,
crossingMap,
out float cost,
routeDeadline);
if (rerouted == null)
{
result.FailedFlows.Add(flow);
continue;
}
selected[i] = (rerouted, cost);
IncrementUsage(result.EdgeUsage, rerouted.EdgeIds, +1);
}
for (int i = 0; i < selected.Count; i++)
{
routed[i] = selected[i];
}
}
private async UniTask ApplyRipUpAndRerouteIncrementalAsync(
CorridorGraph graph,
List<(GlobalRoute route, float cost)> routed,
IterationResult result,
RoutingConfig config,
float[] edgeHistory,
Dictionary<int, List<int>> crossingMap,
long solveDeadlineTicks,
bool prioritizeCrossings,
Action<NegotiationProgress> onProgress,
int iterationIndex,
int totalFlows,
CancellationToken cancellationToken)
{
if (routed.Count == 0)
{
return;
}
bool hasOverflow = HasOverflow(graph, result.EdgeUsage);
bool crossingMode = !hasOverflow && prioritizeCrossings && crossingMap.Count > 0;
float ripUpRatio = hasOverflow ? config.ripUpPercent : config.crossingRipUpPercent;
if (ripUpRatio <= 0f)
{
return;
}
int rerouteCount = Mathf.CeilToInt(routed.Count * ripUpRatio);
rerouteCount = Mathf.Clamp(rerouteCount, 1, routed.Count);
routed.Sort((left, right) =>
{
float leftScore = ComputeOverflowContribution(left.route, graph, result.EdgeUsage);
float rightScore = ComputeOverflowContribution(right.route, graph, result.EdgeUsage);
int leftCrossings = ComputeCrossingContribution(left.route, result.EdgeUsage, crossingMap);
int rightCrossings = ComputeCrossingContribution(right.route, result.EdgeUsage, crossingMap);
float leftRank = crossingMode
? leftCrossings * Mathf.Max(1f, config.crossingRerouteBoost) + leftScore * 4f + left.route.Bends * 0.2f
: leftScore * 100f + leftCrossings * 0.5f + left.route.Bends;
float rightRank = crossingMode
? rightCrossings * Mathf.Max(1f, config.crossingRerouteBoost) + rightScore * 4f + right.route.Bends * 0.2f
: rightScore * 100f + rightCrossings * 0.5f + right.route.Bends;
return rightRank.CompareTo(leftRank);
});
for (int i = 0; i < rerouteCount; i++)
{
IncrementUsage(result.EdgeUsage, routed[i].route.EdgeIds, -1);
}
for (int i = 0; i < rerouteCount; i++)
{
cancellationToken.ThrowIfCancellationRequested();
GlobalRoute previousRoute = routed[i].route;
FlowRequest flow = previousRoute.Flow;
if (Stopwatch.GetTimestamp() >= solveDeadlineTicks)
{
IncrementUsage(result.EdgeUsage, previousRoute.EdgeIds, +1);
continue;
}
long routeBudgetTicks = (long)(Mathf.Max(0.01f, config.maxRouteTimeSeconds) * Stopwatch.Frequency);
long routeDeadline = Stopwatch.GetTimestamp() + routeBudgetTicks;
if (routeDeadline > solveDeadlineTicks)
{
routeDeadline = solveDeadlineTicks;
}
GlobalRoute rerouted = globalRouter.RouteFlow(
graph,
flow,
config,
result.EdgeUsage,
edgeHistory,
crossingMap,
out float cost,
routeDeadline);
if (rerouted == null)
{
IncrementUsage(result.EdgeUsage, previousRoute.EdgeIds, +1);
continue;
}
routed[i] = (rerouted, cost);
IncrementUsage(result.EdgeUsage, rerouted.EdgeIds, +1);
onProgress?.Invoke(new NegotiationProgress
{
Phase = NegotiationProgressPhase.RouteCommitted,
RoutedPreviewCount = routed.Count,
LatestRoute = rerouted,
LatestFlow = flow,
Iteration = iterationIndex,
CompletedFlows = Mathf.Min(totalFlows, totalFlows - rerouteCount + i + 1),
TotalFlows = totalFlows,
Message = $"Rerouted {i + 1}/{rerouteCount} selected flow(s)."
});
await UniTask.Yield(cancellationToken);
}
}
private static void IncrementUsage(int[] edgeUsage, IReadOnlyList<int> edges, int delta)
{
for (int i = 0; i < edges.Count; i++)
{
int edgeId = edges[i];
edgeUsage[edgeId] = Mathf.Max(0, edgeUsage[edgeId] + delta);
}
}
private static float ComputeOverflowContribution(GlobalRoute route, CorridorGraph graph, int[] edgeUsage)
{
float score = 0f;
for (int i = 0; i < route.EdgeIds.Count; i++)
{
int edgeId = route.EdgeIds[i];
CorridorEdge edge = graph.Edges[edgeId];
int overflow = Mathf.Max(0, edgeUsage[edgeId] - edge.Capacity);
score += overflow;
}
return score * 10f + route.Bends;
}
private static bool HasOverflow(CorridorGraph graph, int[] edgeUsage)
{
for (int edgeId = 0; edgeId < graph.Edges.Count; edgeId++)
{
if (edgeUsage[edgeId] > graph.Edges[edgeId].Capacity)
{
return true;
}
}
return false;
}
private static int ComputeCrossingContribution(
GlobalRoute route,
int[] edgeUsage,
Dictionary<int, List<int>> crossingMap)
{
if (route == null || crossingMap == null || crossingMap.Count == 0)
{
return 0;
}
int crossings = 0;
for (int i = 0; i < route.EdgeIds.Count; i++)
{
int edgeId = route.EdgeIds[i];
if (!crossingMap.TryGetValue(edgeId, out List<int> crossingEdges))
{
continue;
}
for (int crossingIndex = 0; crossingIndex < crossingEdges.Count; crossingIndex++)
{
int crossingEdgeId = crossingEdges[crossingIndex];
int usage = edgeUsage[crossingEdgeId];
if (usage > 0)
{
crossings += usage;
}
}
}
return crossings;
}
private static int CountCrossings(
IReadOnlyList<int> routeEdges,
int[] edgeUsage,
Dictionary<int, List<int>> crossingMap)
{
if (routeEdges == null || crossingMap == null || crossingMap.Count == 0)
{
return 0;
}
int crossings = 0;
for (int i = 0; i < routeEdges.Count; i++)
{
int edgeId = routeEdges[i];
if (!crossingMap.TryGetValue(edgeId, out List<int> crossingEdges))
{
continue;
}
for (int crossingIndex = 0; crossingIndex < crossingEdges.Count; crossingIndex++)
{
int usage = edgeUsage[crossingEdges[crossingIndex]];
if (usage > 0)
{
crossings += usage;
}
}
}
return crossings;
}
private List<FlowRequest> SortFlows(
CorridorGraph graph,
IReadOnlyList<FlowRequest> flowRequests,
RoutingConfig config,
long solveDeadlineTicks)
{
List<(FlowRequest flow, float length)> sorted = new List<(FlowRequest flow, float length)>(flowRequests.Count);
int edgeCount = graph != null ? graph.Edges.Count : 0;
int[] emptyUsage = edgeCount > 0 ? new int[edgeCount] : Array.Empty<int>();
float[] emptyHistory = edgeCount > 0 ? new float[edgeCount] : Array.Empty<float>();
Dictionary<int, List<int>> emptyCrossingMap = new Dictionary<int, List<int>>();
for (int i = 0; i < flowRequests.Count; i++)
{
FlowRequest flow = flowRequests[i];
if (flow == null)
{
continue;
}
float length = float.PositiveInfinity;
if (graph != null && edgeCount > 0 && config != null && Stopwatch.GetTimestamp() < solveDeadlineTicks)
{
long routeBudgetTicks = (long)(Mathf.Max(0.01f, config.maxRouteTimeSeconds) * Stopwatch.Frequency);
long routeDeadline = Stopwatch.GetTimestamp() + routeBudgetTicks;
if (routeDeadline > solveDeadlineTicks)
{
routeDeadline = solveDeadlineTicks;
}
GlobalRoute estimate = globalRouter.RouteFlow(
graph,
flow,
config,
emptyUsage,
emptyHistory,
emptyCrossingMap,
out _,
routeDeadline);
if (estimate != null)
{
length = estimate.Length;
}
}
sorted.Add((flow, length));
}
sorted.Sort((left, right) =>
{
int lengthCompare = left.length.CompareTo(right.length);
if (lengthCompare != 0)
{
return lengthCompare;
}
string leftId = string.IsNullOrWhiteSpace(left.flow.Id) ? string.Empty : left.flow.Id;
string rightId = string.IsNullOrWhiteSpace(right.flow.Id) ? string.Empty : right.flow.Id;
return string.CompareOrdinal(leftId, rightId);
});
List<FlowRequest> result = new List<FlowRequest>(sorted.Count);
for (int i = 0; i < sorted.Count; i++)
{
result.Add(sorted[i].flow);
}
return result;
}
private static void ApplyCrossingMetadata(
NegotiationResult result,
Dictionary<int, List<int>> crossingMap,
bool crossingMapSkippedByLimit,
int graphEdgeCount,
RoutingConfig config,
int crossingOptimizationIterationsUsed)
{
if (result == null)
{
return;
}
result.Metrics.CrossingMapEnabled = crossingMap != null && crossingMap.Count > 0;
result.Metrics.CrossingMapSkippedByLimit = crossingMapSkippedByLimit;
result.Metrics.CrossingMapEdges = graphEdgeCount;
result.Metrics.CrossingOptimizationIterationsUsed = crossingOptimizationIterationsUsed;
result.Metrics.RequestedCrossingOptimizationIterations = config != null
? Mathf.Max(0, config.maxCrossingOptimizationIters)
: 0;
}
private static Dictionary<int, List<int>> BuildCrossingMap(CorridorGraph graph, RoutingConfig config)
{
if (config.disableCrossingMapWhenLarge &&
config.crossingMapMaxEdges > 0 &&
graph.Edges.Count > config.crossingMapMaxEdges)
{
return new Dictionary<int, List<int>>();
}
Dictionary<int, List<int>> map = new Dictionary<int, List<int>>();
for (int i = 0; i < graph.Edges.Count; i++)
{
map[i] = new List<int>();
}
for (int i = 0; i < graph.Edges.Count; i++)
{
CorridorEdge first = graph.Edges[i];
Vector3 a1 = graph.Nodes[first.NodeA];
Vector3 a2 = graph.Nodes[first.NodeB];
for (int j = i + 1; j < graph.Edges.Count; j++)
{
CorridorEdge second = graph.Edges[j];
if (first.Axis == second.Axis)
{
continue;
}
Vector3 b1 = graph.Nodes[second.NodeA];
Vector3 b2 = graph.Nodes[second.NodeB];
if (!EdgesCrossInterior(first.Axis, a1, a2, second.Axis, b1, b2))
{
continue;
}
map[i].Add(j);
map[j].Add(i);
}
}
return map;
}
private static bool EdgesCrossInterior(
EdgeAxis axisA,
Vector3 a1,
Vector3 a2,
EdgeAxis axisB,
Vector3 b1,
Vector3 b2)
{
const float epsilon = 0.0001f;
if (ShareEndpoint(a1, a2, b1, b2, epsilon))
{
return false;
}
if (axisA == EdgeAxis.X && axisB == EdgeAxis.Y)
{
return CrossXY(a1, a2, b1, b2, epsilon);
}
if (axisA == EdgeAxis.Y && axisB == EdgeAxis.X)
{
return CrossXY(b1, b2, a1, a2, epsilon);
}
if (axisA == EdgeAxis.X && axisB == EdgeAxis.Z)
{
return CrossXZ(a1, a2, b1, b2, epsilon);
}
if (axisA == EdgeAxis.Z && axisB == EdgeAxis.X)
{
return CrossXZ(b1, b2, a1, a2, epsilon);
}
if (axisA == EdgeAxis.Y && axisB == EdgeAxis.Z)
{
return CrossYZ(a1, a2, b1, b2, epsilon);
}
if (axisA == EdgeAxis.Z && axisB == EdgeAxis.Y)
{
return CrossYZ(b1, b2, a1, a2, epsilon);
}
return false;
}
private static bool CrossXY(Vector3 x0, Vector3 x1, Vector3 y0, Vector3 y1, float epsilon)
{
if (Mathf.Abs(x0.z - y0.z) > epsilon)
{
return false;
}
float x = y0.x;
float y = x0.y;
return IsInsideRange(x, x0.x, x1.x, epsilon) &&
IsInsideRange(y, y0.y, y1.y, epsilon);
}
private static bool CrossXZ(Vector3 x0, Vector3 x1, Vector3 z0, Vector3 z1, float epsilon)
{
if (Mathf.Abs(x0.y - z0.y) > epsilon)
{
return false;
}
float x = z0.x;
float z = x0.z;
return IsInsideRange(x, x0.x, x1.x, epsilon) &&
IsInsideRange(z, z0.z, z1.z, epsilon);
}
private static bool CrossYZ(Vector3 y0, Vector3 y1, Vector3 z0, Vector3 z1, float epsilon)
{
if (Mathf.Abs(y0.x - z0.x) > epsilon)
{
return false;
}
float y = z0.y;
float z = y0.z;
return IsInsideRange(y, y0.y, y1.y, epsilon) &&
IsInsideRange(z, z0.z, z1.z, epsilon);
}
private static bool IsInsideRange(float value, float a, float b, float epsilon)
{
float min = Mathf.Min(a, b);
float max = Mathf.Max(a, b);
return value > min + epsilon && value < max - epsilon;
}
private static bool ShareEndpoint(Vector3 a0, Vector3 a1, Vector3 b0, Vector3 b1, float epsilon)
{
return ApproximatelyEqual(a0, b0, epsilon) ||
ApproximatelyEqual(a0, b1, epsilon) ||
ApproximatelyEqual(a1, b0, epsilon) ||
ApproximatelyEqual(a1, b1, epsilon);
}
private static bool ApproximatelyEqual(Vector3 a, Vector3 b, float epsilon)
{
return (a - b).sqrMagnitude <= epsilon * epsilon;
}
private static float ComputeScore(IterationResult iteration)
{
return iteration.Metrics.OverflowCount * 10000f +
iteration.Metrics.FailedCount * 1000f +
iteration.Metrics.CrossingCount * 250f +
iteration.Metrics.TotalLength;
}
private static void UpdateHistoryCosts(float[] edgeHistory, int[] overflow)
{
for (int i = 0; i < edgeHistory.Length; i++)
{
if (overflow[i] > 0)
{
edgeHistory[i] += overflow[i];
}
}
}
private static NegotiationResult CopyToNegotiationResult(IterationResult iteration, float[] historySnapshot, int iterationsUsed)
{
NegotiationResult copy = new NegotiationResult
{
EdgeUsage = (int[])iteration.EdgeUsage.Clone(),
EdgeOverflow = (int[])iteration.EdgeOverflow.Clone(),
EdgeHistory = (float[])historySnapshot.Clone(),
IterationsUsed = iterationsUsed
};
copy.Routes.AddRange(iteration.Routes);
copy.FailedFlows.AddRange(iteration.FailedFlows);
copy.Metrics = new RoutingMetrics
{
RoutedCount = iteration.Metrics.RoutedCount,
FailedCount = iteration.Metrics.FailedCount,
TotalLength = iteration.Metrics.TotalLength,
TotalBends = iteration.Metrics.TotalBends,
OverflowCount = iteration.Metrics.OverflowCount,
CrossingCount = iteration.Metrics.CrossingCount,
ModuleCollisionCount = iteration.Metrics.ModuleCollisionCount
};
if (copy.Metrics.OverflowCount > 0)
{
copy.Warnings.Add($"Negotiated congestion converged with overflow={copy.Metrics.OverflowCount}.");
}
if (copy.Metrics.FailedCount > 0)
{
copy.Warnings.Add($"Failed to route {copy.Metrics.FailedCount} flow(s).");
}
return copy;
}
private sealed class IterationResult
{
public readonly List<GlobalRoute> Routes = new List<GlobalRoute>();
public readonly List<FlowRequest> FailedFlows = new List<FlowRequest>();
public readonly RoutingMetrics Metrics = new RoutingMetrics();
public int[] EdgeUsage = Array.Empty<int>();
public int[] EdgeOverflow = Array.Empty<int>();
}
}
public sealed class RouteValidator
{
private const float Epsilon = 0.0001f;
public RouteValidationReport Validate(
IReadOnlyList<ModuleNode> modules,
IReadOnlyList<DetailedPath> paths,
RoutingConfig config = null)
{
RouteValidationReport report = new RouteValidationReport();
if (paths == null)
{
return report;
}
int checks = 0;
int checkBudget = config != null ? Mathf.Max(1000, config.maxValidationChecks) : int.MaxValue;
bool truncated = false;
ValidateModuleCollisions(modules, paths, report, ref checks, checkBudget, ref truncated);
if (!truncated)
{
ValidateFlowCrossings(paths, report, ref checks, checkBudget, ref truncated);
}
if (report.ModuleCollisionCount > 0)
{
report.Errors.Add($"Detected {report.ModuleCollisionCount} module collision segment(s).");
}
if (report.FlowCrossingCount > 0)
{
report.Warnings.Add($"Detected {report.FlowCrossingCount} flow crossing segment(s).");
}
if (truncated)
{
report.Warnings.Add($"Validation truncated by performance limit (maxValidationChecks={checkBudget}).");
}
return report;
}
private static void ValidateModuleCollisions(
IReadOnlyList<ModuleNode> modules,
IReadOnlyList<DetailedPath> paths,
RouteValidationReport report,
ref int checks,
int checkBudget,
ref bool truncated)
{
if (modules == null)
{
return;
}
for (int pathIndex = 0; pathIndex < paths.Count; pathIndex++)
{
IReadOnlyList<Vector3> points = paths[pathIndex].Points;
for (int pointIndex = 1; pointIndex < points.Count; pointIndex++)
{
Vector3 a = points[pointIndex - 1];
Vector3 b = points[pointIndex];
for (int moduleIndex = 0; moduleIndex < modules.Count; moduleIndex++)
{
if (++checks > checkBudget)
{
truncated = true;
return;
}
ModuleNode module = modules[moduleIndex];
if (module == null)
{
continue;
}
Bounds bounds = module.GetWorldBounds();
if (SegmentIntersectsBoundsInterior(a, b, bounds))
{
report.ModuleCollisionCount++;
report.ModuleCollisionSegments.Add(new SegmentIssue(a, b));
break;
}
}
}
}
}
private static void ValidateFlowCrossings(
IReadOnlyList<DetailedPath> paths,
RouteValidationReport report,
ref int checks,
int checkBudget,
ref bool truncated)
{
for (int i = 0; i < paths.Count; i++)
{
IReadOnlyList<Vector3> aPoints = paths[i].Points;
for (int j = i + 1; j < paths.Count; j++)
{
IReadOnlyList<Vector3> bPoints = paths[j].Points;
for (int a = 1; a < aPoints.Count; a++)
{
Vector3 a0 = aPoints[a - 1];
Vector3 a1 = aPoints[a];
for (int b = 1; b < bPoints.Count; b++)
{
if (++checks > checkBudget)
{
truncated = true;
return;
}
Vector3 b0 = bPoints[b - 1];
Vector3 b1 = bPoints[b];
if (ShareEndpoint(a0, a1, b0, b1))
{
continue;
}
if (!SegmentsIntersect3D(a0, a1, b0, b1))
{
continue;
}
report.FlowCrossingCount++;
report.FlowCrossingSegments.Add(new SegmentIssue(a0, a1));
report.FlowCrossingSegments.Add(new SegmentIssue(b0, b1));
}
}
}
}
}
private static bool SegmentIntersectsBoundsInterior(Vector3 a, Vector3 b, Bounds bounds)
{
Vector3 innerMin = bounds.min + Vector3.one * Epsilon;
Vector3 innerMax = bounds.max - Vector3.one * Epsilon;
if (innerMin.x >= innerMax.x || innerMin.y >= innerMax.y || innerMin.z >= innerMax.z)
{
return false;
}
return SegmentIntersectsAabb(a, b, innerMin, innerMax);
}
private static bool SegmentIntersectsAabb(Vector3 a, Vector3 b, Vector3 min, Vector3 max)
{
Vector3 d = b - a;
float tMin = 0f;
float tMax = 1f;
if (!ClipAxis(min.x, max.x, a.x, d.x, ref tMin, ref tMax) ||
!ClipAxis(min.y, max.y, a.y, d.y, ref tMin, ref tMax) ||
!ClipAxis(min.z, max.z, a.z, d.z, ref tMin, ref tMax))
{
return false;
}
return tMax > tMin;
}
private static bool ClipAxis(float min, float max, float origin, float direction, ref float tMin, ref float tMax)
{
if (Mathf.Abs(direction) < Epsilon)
{
return origin >= min && origin <= max;
}
float inv = 1f / direction;
float t1 = (min - origin) * inv;
float t2 = (max - origin) * inv;
if (t1 > t2)
{
float tmp = t1;
t1 = t2;
t2 = tmp;
}
tMin = Mathf.Max(tMin, t1);
tMax = Mathf.Min(tMax, t2);
return tMax >= tMin;
}
private static bool ShareEndpoint(Vector3 a0, Vector3 a1, Vector3 b0, Vector3 b1)
{
return ApproximatelyEqual(a0, b0) ||
ApproximatelyEqual(a0, b1) ||
ApproximatelyEqual(a1, b0) ||
ApproximatelyEqual(a1, b1);
}
private static bool SegmentsIntersect3D(Vector3 p1, Vector3 q1, Vector3 p2, Vector3 q2)
{
float sqDist = SegmentSegmentSqrDistance(p1, q1, p2, q2);
return sqDist <= Epsilon * Epsilon;
}
private static float SegmentSegmentSqrDistance(Vector3 p1, Vector3 q1, Vector3 p2, Vector3 q2)
{
Vector3 d1 = q1 - p1;
Vector3 d2 = q2 - p2;
Vector3 r = p1 - p2;
float a = Vector3.Dot(d1, d1);
float e = Vector3.Dot(d2, d2);
float f = Vector3.Dot(d2, r);
float s;
float t;
if (a <= Epsilon && e <= Epsilon)
{
return (p1 - p2).sqrMagnitude;
}
if (a <= Epsilon)
{
s = 0f;
t = Mathf.Clamp01(f / e);
}
else
{
float c = Vector3.Dot(d1, r);
if (e <= Epsilon)
{
t = 0f;
s = Mathf.Clamp01(-c / a);
}
else
{
float b = Vector3.Dot(d1, d2);
float denom = a * e - b * b;
if (denom != 0f)
{
s = Mathf.Clamp01((b * f - c * e) / denom);
}
else
{
s = 0f;
}
t = (b * s + f) / e;
if (t < 0f)
{
t = 0f;
s = Mathf.Clamp01(-c / a);
}
else if (t > 1f)
{
t = 1f;
s = Mathf.Clamp01((b - c) / a);
}
}
}
Vector3 c1 = p1 + d1 * s;
Vector3 c2 = p2 + d2 * t;
return (c1 - c2).sqrMagnitude;
}
private static bool ApproximatelyEqual(Vector3 a, Vector3 b)
{
return (a - b).sqrMagnitude < 0.0004f;
}
}
public enum EdgeAxis
{
X,
Y,
Z
}
[Serializable]
public struct CorridorEdge
{
public int Id;
public int NodeA;
public int NodeB;
public float Length;
public int Capacity;
public float BaseCost;
public float ProximityPenalty;
public EdgeAxis Axis;
}
public sealed class CorridorGraph
{
private readonly Dictionary<Vector3Int, int> nodeLookup = new Dictionary<Vector3Int, int>();
private readonly Dictionary<long, int> edgeLookup = new Dictionary<long, int>();
public readonly List<Vector3> Nodes = new List<Vector3>();
public readonly List<CorridorEdge> Edges = new List<CorridorEdge>();
public readonly List<List<int>> NodeToEdges = new List<List<int>>();
public Bounds WorkArea { get; set; }
public bool TruncatedByLimits { get; set; }
public int AddNode(Vector3 position)
{
Vector3Int key = Quantize(position);
if (nodeLookup.TryGetValue(key, out int existingId))
{
return existingId;
}
int nodeId = Nodes.Count;
Nodes.Add(position);
NodeToEdges.Add(new List<int>(8));
nodeLookup[key] = nodeId;
return nodeId;
}
public bool TryGetNode(Vector3 position, out int nodeId)
{
return nodeLookup.TryGetValue(Quantize(position), out nodeId);
}
public int AddEdge(int nodeA, int nodeB, float baseCost, int capacity, float proximityPenalty)
{
if (nodeA == nodeB)
{
return -1;
}
long key = PairKey(nodeA, nodeB);
if (edgeLookup.TryGetValue(key, out int existingId))
{
return existingId;
}
Vector3 a = Nodes[nodeA];
Vector3 b = Nodes[nodeB];
EdgeAxis axis = DetermineAxis(a, b);
float length = Vector3.Distance(a, b);
CorridorEdge edge = new CorridorEdge
{
Id = Edges.Count,
NodeA = nodeA,
NodeB = nodeB,
Length = length,
Capacity = Mathf.Max(1, capacity),
BaseCost = Mathf.Max(0.001f, baseCost),
ProximityPenalty = Mathf.Max(0f, proximityPenalty),
Axis = axis
};
Edges.Add(edge);
edgeLookup[key] = edge.Id;
NodeToEdges[nodeA].Add(edge.Id);
NodeToEdges[nodeB].Add(edge.Id);
return edge.Id;
}
public bool TryGetEdgeId(int nodeA, int nodeB, out int edgeId)
{
return edgeLookup.TryGetValue(PairKey(nodeA, nodeB), out edgeId);
}
public int GetOtherNode(int edgeId, int nodeId)
{
CorridorEdge edge = Edges[edgeId];
if (edge.NodeA == nodeId)
{
return edge.NodeB;
}
return edge.NodeA;
}
private static EdgeAxis DetermineAxis(Vector3 a, Vector3 b)
{
Vector3 delta = b - a;
float absX = Mathf.Abs(delta.x);
float absY = Mathf.Abs(delta.y);
float absZ = Mathf.Abs(delta.z);
if (absX >= absY && absX >= absZ)
{
return EdgeAxis.X;
}
if (absY >= absX && absY >= absZ)
{
return EdgeAxis.Y;
}
return EdgeAxis.Z;
}
private static Vector3Int Quantize(Vector3 value)
{
return new Vector3Int(
Mathf.RoundToInt(value.x * 1000f),
Mathf.RoundToInt(value.y * 1000f),
Mathf.RoundToInt(value.z * 1000f));
}
private static long PairKey(int a, int b)
{
int min = Mathf.Min(a, b);
int max = Mathf.Max(a, b);
return ((long)min << 32) | (uint)max;
}
}
[Serializable]
public sealed class GlobalRoute
{
public FlowRequest Flow;
public readonly List<int> NodeIds = new List<int>();
public readonly List<int> EdgeIds = new List<int>();
public float Length;
public int Bends;
public int CrossingCount;
public bool UsesCrossingFallback;
}
[Serializable]
public sealed class RoutingMetrics
{
public int RoutedCount;
public int FailedCount;
public float TotalLength;
public int TotalBends;
public int OverflowCount;
public int CrossingCount;
public int ModuleCollisionCount;
public bool CrossingMapEnabled;
public bool CrossingMapSkippedByLimit;
public int CrossingMapEdges;
public int CrossingOptimizationIterationsUsed;
public int RequestedCrossingOptimizationIterations;
}
[Serializable]
public sealed class NegotiationResult
{
public readonly List<GlobalRoute> Routes = new List<GlobalRoute>();
public readonly List<FlowRequest> FailedFlows = new List<FlowRequest>();
public readonly List<string> Warnings = new List<string>();
public int[] EdgeUsage = Array.Empty<int>();
public int[] EdgeOverflow = Array.Empty<int>();
public float[] EdgeHistory = Array.Empty<float>();
public int IterationsUsed;
public RoutingMetrics Metrics = new RoutingMetrics();
}
public enum NegotiationProgressPhase
{
Started,
IterationStarted,
RouteCommitted,
IterationCompleted,
Completed
}
public sealed class NegotiationProgress
{
public NegotiationProgressPhase Phase;
public IReadOnlyList<GlobalRoute> Routes = Array.Empty<GlobalRoute>();
public NegotiationResult BestResult;
public GlobalRoute LatestRoute;
public FlowRequest LatestFlow;
public int RoutedPreviewCount;
public int Iteration;
public int CompletedFlows;
public int TotalFlows;
public bool IsFinal;
public string Message;
}
[Serializable]
public sealed class TrunkLane
{
public FlowRequest Flow;
public int Order;
public float Offset;
}
[Serializable]
public sealed class TrunkSegment
{
public int EdgeId;
public readonly List<TrunkLane> Lanes = new List<TrunkLane>();
}
[Serializable]
public sealed class BundleRoutingResult
{
public readonly Dictionary<int, TrunkSegment> TrunksByEdge = new Dictionary<int, TrunkSegment>();
public bool UsedTurnAwareLaneOrdering;
public int RequestedLaneAlignmentPasses;
public int AppliedLaneAlignmentPasses;
public int LaneAlignmentChangeCount;
public float GetOffset(FlowRequest flow, int edgeId)
{
if (!TrunksByEdge.TryGetValue(edgeId, out TrunkSegment segment))
{
return 0f;
}
for (int i = 0; i < segment.Lanes.Count; i++)
{
if (segment.Lanes[i].Flow == flow)
{
return segment.Lanes[i].Offset;
}
}
return 0f;
}
public int GetOrder(FlowRequest flow, int edgeId)
{
if (!TrunksByEdge.TryGetValue(edgeId, out TrunkSegment segment))
{
return 0;
}
for (int i = 0; i < segment.Lanes.Count; i++)
{
if (segment.Lanes[i].Flow == flow)
{
return segment.Lanes[i].Order;
}
}
return 0;
}
}
[Serializable]
public sealed class DetailedPath
{
public FlowRequest Flow;
public readonly List<Vector3> Points = new List<Vector3>();
public readonly List<Vector3> CornerMarkers = new List<Vector3>();
public bool UsesCrossingFallback;
}
[Serializable]
public readonly struct SegmentIssue
{
public SegmentIssue(Vector3 start, Vector3 end)
{
Start = start;
End = end;
}
public Vector3 Start { get; }
public Vector3 End { get; }
}
[Serializable]
public sealed class RouteValidationReport
{
public readonly List<string> Errors = new List<string>();
public readonly List<string> Warnings = new List<string>();
public readonly List<SegmentIssue> ModuleCollisionSegments = new List<SegmentIssue>();
public readonly List<SegmentIssue> FlowCrossingSegments = new List<SegmentIssue>();
public int ModuleCollisionCount;
public int FlowCrossingCount;
}
public class FlowRoutingSystem : MonoBehaviour
{
[Header("Scene Data")]
[SerializeField] private RoutingConfig config;
[SerializeField] private bool autoCollectModules = true;
[SerializeField] private List<ModuleNode> modules = new List<ModuleNode>();
[SerializeField] private List<FlowRequest> flowRequests = new List<FlowRequest>();
[Header("Random Generation")]
[Min(1)]
[SerializeField] private int randomConnectionCount = 20;
[SerializeField] private int randomSeed = 12345;
[Header("Runtime Report")]
[TextArea(5, 20)]
[SerializeField] private string lastReport = "No routing has been run yet.";
[Tooltip("Optional vertical visualization offset for all debug gizmos.")]
[SerializeField] private float gizmoHeight = 0f;
[Header("Line Renderer Output")]
[SerializeField] private bool renderFlowsWithLineRenderers = true;
[Min(0.001f)]
[SerializeField] private float lineRendererWidth = 0.035f;
[SerializeField] private Material lineRendererMaterial;
[SerializeField] private float lineRendererHeightOffset = 0.02f;
[Header("Incremental Generation")]
[SerializeField] private bool renderIncrementalProgress = true;
[Min(0f)]
[SerializeField] private float incrementalPreviewIntervalSeconds = 0.05f;
[Min(2)]
[SerializeField] private int incrementalPreviewMaxNodes = 256;
[Min(0f)]
[SerializeField] private float incrementalReportIntervalSeconds = 0.1f;
private readonly CorridorGraphBuilder corridorGraphBuilder = new CorridorGraphBuilder();
private readonly NegotiatedCongestionSolver congestionSolver = new NegotiatedCongestionSolver();
private readonly BundleRouter bundleRouter = new BundleRouter();
private readonly DetailedGeometryBuilder detailedGeometryBuilder = new DetailedGeometryBuilder();
private readonly RouteValidator routeValidator = new RouteValidator();
private readonly Dictionary<FlowRequest, LineRenderer> generatedLineRenderers =
new Dictionary<FlowRequest, LineRenderer>();
private CorridorGraph corridorGraph;
private List<Bounds> inflatedObstacles = new List<Bounds>();
private NegotiationResult negotiationResult;
private BundleRoutingResult bundleResult;
private List<DetailedPath> detailedPaths = new List<DetailedPath>();
private RouteValidationReport validationReport;
private long lastPipelineDurationMs;
private long lastGraphBuildMs;
private long lastSolveMs;
private long lastBundleMs;
private long lastDetailedGeometryMs;
private long lastValidationMs;
private long lastLineRenderMs;
private Transform lineRendererRoot;
private CancellationTokenSource activeRoutingCts;
private bool isRoutingInProgress;
private bool routingCancelRequested;
private long lastIncrementalPreviewTicks;
private long lastIncrementalReportTicks;
public RoutingConfig Config => config;
public IList<ModuleNode> Modules => modules;
public IList<FlowRequest> FlowRequests => flowRequests;
public string LastReport => lastReport;
public bool IsRoutingInProgress => isRoutingInProgress;
public int RandomConnectionCount { get => randomConnectionCount; set => randomConnectionCount = Mathf.Max(1, value); }
public int RandomSeed { get => randomSeed; set => randomSeed = value; }
private void OnDestroy()
{
CancelActiveRouting(false);
DestroyGeneratedLineRenderers();
}
[ContextMenu("Build Corridors Only")]
public void BuildCorridorsOnly()
{
CancelActiveRouting(false);
using (ModuleNode.UseRoutingSpace(transform))
{
if (!PrepareSceneData())
{
return;
}
long started = System.Diagnostics.Stopwatch.GetTimestamp();
ResetStageTimings();
long stageStarted = System.Diagnostics.Stopwatch.GetTimestamp();
corridorGraph = corridorGraphBuilder.Build(modules, flowRequests, config, out inflatedObstacles);
lastGraphBuildMs = TicksToMs(System.Diagnostics.Stopwatch.GetTimestamp() - stageStarted);
lastPipelineDurationMs = (long)((System.Diagnostics.Stopwatch.GetTimestamp() - started) * 1000.0 / System.Diagnostics.Stopwatch.Frequency);
negotiationResult = null;
bundleResult = null;
detailedPaths.Clear();
validationReport = null;
DestroyGeneratedLineRenderers();
lastIncrementalPreviewTicks = 0L;
lastIncrementalReportTicks = 0L;
lastReport = $"Corridor graph built: nodes={corridorGraph.Nodes.Count}, edges={corridorGraph.Edges.Count}.";
Debug.Log($"[FlowRoutingSystem] {lastReport}", this);
}
}
[ContextMenu("Run One Negotiation Iteration")]
public void RunOneNegotiationIteration()
{
CancelActiveRouting(false);
using (ModuleNode.UseRoutingSpace(transform))
{
if (!PrepareSceneData())
{
return;
}
long started = System.Diagnostics.Stopwatch.GetTimestamp();
ResetStageTimings();
long stageStarted = System.Diagnostics.Stopwatch.GetTimestamp();
EnsureCorridorsBuilt();
lastGraphBuildMs = TicksToMs(System.Diagnostics.Stopwatch.GetTimestamp() - stageStarted);
stageStarted = System.Diagnostics.Stopwatch.GetTimestamp();
negotiationResult = congestionSolver.Solve(corridorGraph, flowRequests, config, 1);
lastSolveMs = TicksToMs(System.Diagnostics.Stopwatch.GetTimestamp() - stageStarted);
stageStarted = System.Diagnostics.Stopwatch.GetTimestamp();
bundleResult = bundleRouter.Build(corridorGraph, negotiationResult.Routes, config);
lastBundleMs = TicksToMs(System.Diagnostics.Stopwatch.GetTimestamp() - stageStarted);
stageStarted = System.Diagnostics.Stopwatch.GetTimestamp();
detailedPaths = detailedGeometryBuilder.Build(corridorGraph, negotiationResult.Routes, bundleResult, modules, config);
lastDetailedGeometryMs = TicksToMs(System.Diagnostics.Stopwatch.GetTimestamp() - stageStarted);
stageStarted = System.Diagnostics.Stopwatch.GetTimestamp();
validationReport = routeValidator.Validate(modules, detailedPaths, config);
lastValidationMs = TicksToMs(System.Diagnostics.Stopwatch.GetTimestamp() - stageStarted);
negotiationResult.Metrics.ModuleCollisionCount = validationReport.ModuleCollisionCount;
stageStarted = System.Diagnostics.Stopwatch.GetTimestamp();
RebuildGeneratedLineRenderers();
lastLineRenderMs = TicksToMs(System.Diagnostics.Stopwatch.GetTimestamp() - stageStarted);
lastPipelineDurationMs = (long)((System.Diagnostics.Stopwatch.GetTimestamp() - started) * 1000.0 / System.Diagnostics.Stopwatch.Frequency);
PublishReport("One-iteration route report");
}
}
[ContextMenu("Generate Random Connections")]
public void GenerateRandomConnections()
{
GenerateRandomConnections(randomConnectionCount, randomSeed);
}
public void GenerateRandomConnections(int count, int seed)
{
CancelActiveRouting(false);
using (ModuleNode.UseRoutingSpace(transform))
{
if (!PrepareSceneData())
{
return;
}
System.Random random = new System.Random(seed);
List<ModuleNode> sourceCandidates = new List<ModuleNode>();
List<ModuleNode> targetCandidates = new List<ModuleNode>();
CollectModuleCandidates(sourceCandidates, targetCandidates);
if (sourceCandidates.Count == 0 || targetCandidates.Count == 0)
{
Debug.LogWarning("[FlowRoutingSystem] No module candidates found for random generation.", this);
return;
}
flowRequests.Clear();
for (int i = 0; i < count; i++)
{
ModuleNode source = sourceCandidates[random.Next(sourceCandidates.Count)];
ModuleNode target = targetCandidates[random.Next(targetCandidates.Count)];
if (source == target && sourceCandidates.Count > 1)
{
int safety = 0;
while (target == source && safety++ < 8)
{
target = targetCandidates[random.Next(targetCandidates.Count)];
}
}
FlowRequest request = new FlowRequest
{
Id = $"flow_{i + 1:D3}",
FlowClass = "random",
DebugColor = Color.HSVToRGB((i * 0.6180339f) % 1f, 0.65f, 0.95f)
};
request.Source.Module = source;
request.Target.Module = target;
flowRequests.Add(request);
}
RecomputeAutoSlotsFromConnectivity();
corridorGraph = null;
DestroyGeneratedLineRenderers();
lastReport = $"Generated {flowRequests.Count} random flow request(s) with seed={seed}.";
Debug.Log($"[FlowRoutingSystem] {lastReport}", this);
}
}
[ContextMenu("Generate And Draw Flows")]
public void GenerateAndDrawFlows()
{
if (Application.isPlaying)
{
GenerateAndDrawFlowsIncrementally();
return;
}
GenerateAndDrawFlowsImmediate();
}
[ContextMenu("Generate And Draw Flows Immediate")]
public void GenerateAndDrawFlowsImmediate()
{
CancelActiveRouting(false);
using (ModuleNode.UseRoutingSpace(transform))
{
if (!PrepareSceneData())
{
return;
}
long started = System.Diagnostics.Stopwatch.GetTimestamp();
ResetStageTimings();
long stageStarted = System.Diagnostics.Stopwatch.GetTimestamp();
EnsureCorridorsBuilt();
lastGraphBuildMs = TicksToMs(System.Diagnostics.Stopwatch.GetTimestamp() - stageStarted);
stageStarted = System.Diagnostics.Stopwatch.GetTimestamp();
negotiationResult = congestionSolver.Solve(corridorGraph, flowRequests, config);
lastSolveMs = TicksToMs(System.Diagnostics.Stopwatch.GetTimestamp() - stageStarted);
stageStarted = System.Diagnostics.Stopwatch.GetTimestamp();
bundleResult = bundleRouter.Build(corridorGraph, negotiationResult.Routes, config);
lastBundleMs = TicksToMs(System.Diagnostics.Stopwatch.GetTimestamp() - stageStarted);
stageStarted = System.Diagnostics.Stopwatch.GetTimestamp();
detailedPaths = detailedGeometryBuilder.Build(corridorGraph, negotiationResult.Routes, bundleResult, modules, config);
lastDetailedGeometryMs = TicksToMs(System.Diagnostics.Stopwatch.GetTimestamp() - stageStarted);
stageStarted = System.Diagnostics.Stopwatch.GetTimestamp();
validationReport = routeValidator.Validate(modules, detailedPaths, config);
lastValidationMs = TicksToMs(System.Diagnostics.Stopwatch.GetTimestamp() - stageStarted);
negotiationResult.Metrics.ModuleCollisionCount = validationReport.ModuleCollisionCount;
stageStarted = System.Diagnostics.Stopwatch.GetTimestamp();
RebuildGeneratedLineRenderers();
lastLineRenderMs = TicksToMs(System.Diagnostics.Stopwatch.GetTimestamp() - stageStarted);
lastPipelineDurationMs = (long)((System.Diagnostics.Stopwatch.GetTimestamp() - started) * 1000.0 / System.Diagnostics.Stopwatch.Frequency);
foreach (string warning in validationReport.Warnings)
{
negotiationResult.Warnings.Add(warning);
}
foreach (string error in validationReport.Errors)
{
negotiationResult.Warnings.Add($"ERROR: {error}");
}
PublishReport("Full route generation report");
}
}
[ContextMenu("Generate And Draw Flows Incrementally")]
public void GenerateAndDrawFlowsIncrementally()
{
if (!Application.isPlaying)
{
Debug.LogWarning(
"[FlowRoutingSystem] Incremental routing outside Play Mode is started by the custom inspector editor runner.",
this);
return;
}
CancelActiveRouting(false);
GenerateAndDrawFlowsIncrementallyAsync().Forget();
}
public async UniTask GenerateAndDrawFlowsIncrementallyAsync()
{
CancelActiveRouting(false);
routingCancelRequested = false;
isRoutingInProgress = true;
activeRoutingCts = CancellationTokenSource.CreateLinkedTokenSource(this.GetCancellationTokenOnDestroy());
CancellationToken cancellationToken = activeRoutingCts.Token;
using (ModuleNode.UseRoutingSpace(transform))
{
if (!PrepareSceneData())
{
isRoutingInProgress = false;
DisposeActiveRoutingCts();
return;
}
long started = System.Diagnostics.Stopwatch.GetTimestamp();
ResetStageTimings();
negotiationResult = null;
bundleResult = null;
detailedPaths.Clear();
validationReport = null;
DestroyGeneratedLineRenderers();
lastReport = "Incremental route generation started.";
try
{
await UniTask.Yield(cancellationToken);
long stageStarted = System.Diagnostics.Stopwatch.GetTimestamp();
EnsureCorridorsBuilt();
lastGraphBuildMs = TicksToMs(System.Diagnostics.Stopwatch.GetTimestamp() - stageStarted);
if (routingCancelRequested)
{
FinishCancelledRouting();
return;
}
lastReport = $"Corridor graph built: nodes={corridorGraph.Nodes.Count}, edges={corridorGraph.Edges.Count}. Routing flows incrementally...";
await UniTask.Yield(cancellationToken);
stageStarted = System.Diagnostics.Stopwatch.GetTimestamp();
await congestionSolver.SolveIncrementalAsync(
corridorGraph,
flowRequests,
config,
progress => HandleIncrementalRoutingProgress(progress, started),
cancellationToken);
lastSolveMs = TicksToMs(System.Diagnostics.Stopwatch.GetTimestamp() - stageStarted);
if (negotiationResult == null)
{
negotiationResult = new NegotiationResult();
}
stageStarted = System.Diagnostics.Stopwatch.GetTimestamp();
bundleResult = bundleRouter.Build(corridorGraph, negotiationResult.Routes, config);
lastBundleMs = TicksToMs(System.Diagnostics.Stopwatch.GetTimestamp() - stageStarted);
stageStarted = System.Diagnostics.Stopwatch.GetTimestamp();
detailedPaths = detailedGeometryBuilder.Build(corridorGraph, negotiationResult.Routes, bundleResult, modules, config);
lastDetailedGeometryMs = TicksToMs(System.Diagnostics.Stopwatch.GetTimestamp() - stageStarted);
stageStarted = System.Diagnostics.Stopwatch.GetTimestamp();
validationReport = routeValidator.Validate(modules, detailedPaths, config);
lastValidationMs = TicksToMs(System.Diagnostics.Stopwatch.GetTimestamp() - stageStarted);
negotiationResult.Metrics.ModuleCollisionCount = validationReport.ModuleCollisionCount;
stageStarted = System.Diagnostics.Stopwatch.GetTimestamp();
RebuildGeneratedLineRenderers();
lastLineRenderMs = TicksToMs(System.Diagnostics.Stopwatch.GetTimestamp() - stageStarted);
lastPipelineDurationMs = (long)((System.Diagnostics.Stopwatch.GetTimestamp() - started) * 1000.0 / System.Diagnostics.Stopwatch.Frequency);
foreach (string warning in validationReport.Warnings)
{
negotiationResult.Warnings.Add(warning);
}
foreach (string error in validationReport.Errors)
{
negotiationResult.Warnings.Add($"ERROR: {error}");
}
isRoutingInProgress = false;
DisposeActiveRoutingCts();
PublishReport("Full route generation report");
}
catch (System.OperationCanceledException)
{
FinishCancelledRouting();
}
}
}
[ContextMenu("Clear")]
public void Clear()
{
CancelActiveRouting(false);
flowRequests.Clear();
corridorGraph = null;
inflatedObstacles.Clear();
negotiationResult = null;
bundleResult = null;
detailedPaths.Clear();
validationReport = null;
lastPipelineDurationMs = 0L;
ResetStageTimings();
DestroyGeneratedLineRenderers();
lastReport = "Cleared flow requests, cached routes, and debug overlays.";
Debug.Log($"[FlowRoutingSystem] {lastReport}", this);
}
public void RefreshModuleList()
{
using (ModuleNode.UseRoutingSpace(transform))
{
EnsureModuleList();
RecomputeAutoSlotsFromConnectivity();
corridorGraph = null;
}
}
private void EnsureCorridorsBuilt()
{
if (corridorGraph == null)
{
corridorGraph = corridorGraphBuilder.Build(modules, flowRequests, config, out inflatedObstacles);
}
}
private bool PrepareSceneData()
{
if (config == null)
{
Debug.LogError("[FlowRoutingSystem] RoutingConfig is not assigned.", this);
return false;
}
EnsureModuleList();
if (modules.Count == 0)
{
Debug.LogError("[FlowRoutingSystem] No ModuleNode found in scene.", this);
return false;
}
flowRequests.RemoveAll(item => item == null);
RecomputeAutoSlotsFromConnectivity();
corridorGraph = null;
return true;
}
private void RebuildGeneratedLineRenderers()
{
ClearGeneratedLineRenderers(destroyRoot: false);
if (!renderFlowsWithLineRenderers || detailedPaths == null || detailedPaths.Count == 0)
{
return;
}
for (int i = 0; i < detailedPaths.Count; i++)
{
UpsertGeneratedLineRenderer(detailedPaths[i], i);
}
}
private void HandleIncrementalRoutingProgress(NegotiationProgress progress, long started)
{
if (progress == null)
{
return;
}
if (progress.BestResult != null)
{
negotiationResult = progress.BestResult;
}
long now = System.Diagnostics.Stopwatch.GetTimestamp();
if (renderIncrementalProgress && progress.LatestRoute != null && ShouldUpdateIncrementalPreview(progress, now))
{
RenderIncrementalRoute(progress.LatestRoute, progress.CompletedFlows - 1);
}
if (!ShouldUpdateIncrementalReport(progress, now))
{
return;
}
long elapsedMs = (long)((now - started) * 1000.0 / System.Diagnostics.Stopwatch.Frequency);
int routedCount = progress.RoutedPreviewCount > 0
? progress.RoutedPreviewCount
: (progress.Routes != null ? progress.Routes.Count : (negotiationResult?.Routes.Count ?? 0));
lastReport =
"Incremental route generation in progress\n" +
$"Iteration: {Mathf.Max(0, progress.Iteration)}\n" +
$"Routed preview: {routedCount}/{progress.TotalFlows}\n" +
$"Elapsed: {elapsedMs} ms\n" +
progress.Message;
}
private bool ShouldUpdateIncrementalPreview(NegotiationProgress progress, long now)
{
if (progress.IsFinal || progress.CompletedFlows >= progress.TotalFlows)
{
lastIncrementalPreviewTicks = now;
return true;
}
float interval = Mathf.Max(0f, incrementalPreviewIntervalSeconds);
if (interval <= 0f || lastIncrementalPreviewTicks == 0L)
{
lastIncrementalPreviewTicks = now;
return true;
}
long intervalTicks = (long)(interval * System.Diagnostics.Stopwatch.Frequency);
if (now - lastIncrementalPreviewTicks < intervalTicks)
{
return false;
}
lastIncrementalPreviewTicks = now;
return true;
}
private bool ShouldUpdateIncrementalReport(NegotiationProgress progress, long now)
{
if (progress.IsFinal || progress.Phase == NegotiationProgressPhase.IterationCompleted)
{
lastIncrementalReportTicks = now;
return true;
}
float interval = Mathf.Max(0f, incrementalReportIntervalSeconds);
if (interval <= 0f || lastIncrementalReportTicks == 0L)
{
lastIncrementalReportTicks = now;
return true;
}
long intervalTicks = (long)(interval * System.Diagnostics.Stopwatch.Frequency);
if (now - lastIncrementalReportTicks < intervalTicks)
{
return false;
}
lastIncrementalReportTicks = now;
return true;
}
private void RenderIncrementalRoute(GlobalRoute route, int routeIndex)
{
if (route == null || corridorGraph == null)
{
return;
}
DetailedPath previewPath = BuildLightweightPreviewPath(route);
UpsertPreviewDetailedPath(previewPath);
if (renderFlowsWithLineRenderers)
{
UpsertGeneratedLineRenderer(previewPath, Mathf.Max(0, routeIndex));
}
}
private DetailedPath BuildLightweightPreviewPath(GlobalRoute route)
{
DetailedPath path = new DetailedPath
{
Flow = route.Flow,
UsesCrossingFallback = route.UsesCrossingFallback
};
if (route.NodeIds.Count == 0 || corridorGraph == null)
{
return path;
}
int maxNodes = Mathf.Max(2, incrementalPreviewMaxNodes);
int stride = Mathf.Max(1, Mathf.CeilToInt(route.NodeIds.Count / (float)maxNodes));
for (int i = 0; i < route.NodeIds.Count; i += stride)
{
int nodeId = route.NodeIds[i];
if (nodeId >= 0 && nodeId < corridorGraph.Nodes.Count)
{
path.Points.Add(corridorGraph.Nodes[nodeId]);
}
}
int lastNodeId = route.NodeIds[route.NodeIds.Count - 1];
if (lastNodeId >= 0 && lastNodeId < corridorGraph.Nodes.Count)
{
Vector3 lastPoint = corridorGraph.Nodes[lastNodeId];
if (path.Points.Count == 0 || (path.Points[path.Points.Count - 1] - lastPoint).sqrMagnitude > 0.000001f)
{
path.Points.Add(lastPoint);
}
}
return path;
}
private void UpsertPreviewDetailedPath(DetailedPath path)
{
if (path == null || path.Flow == null)
{
return;
}
for (int i = 0; i < detailedPaths.Count; i++)
{
if (detailedPaths[i]?.Flow == path.Flow)
{
detailedPaths[i] = path;
return;
}
}
detailedPaths.Add(path);
}
private void UpsertGeneratedLineRenderer(DetailedPath path, int index)
{
if (!renderFlowsWithLineRenderers || path == null || path.Points.Count < 2)
{
return;
}
Transform root = EnsureLineRendererRoot();
LineRenderer line = null;
if (path.Flow != null)
{
generatedLineRenderers.TryGetValue(path.Flow, out line);
}
if (line == null)
{
GameObject lineObject = new GameObject(GetLineObjectName(path, index));
lineObject.transform.SetParent(root, false);
line = lineObject.AddComponent<LineRenderer>();
if (path.Flow != null)
{
generatedLineRenderers[path.Flow] = line;
}
}
else
{
line.gameObject.name = GetLineObjectName(path, index);
}
line.transform.localPosition = Vector3.zero;
line.transform.localRotation = Quaternion.identity;
line.transform.localScale = Vector3.one;
line.useWorldSpace = false;
line.sharedMaterial = GetLineRendererMaterial();
line.widthMultiplier = Mathf.Max(0.001f, lineRendererWidth);
line.numCornerVertices = 4;
line.numCapVertices = 2;
line.positionCount = path.Points.Count;
Color color = path.Flow != null ? path.Flow.DebugColor : new Color(0.3f, 0.85f, 1f, 1f);
line.startColor = color;
line.endColor = color;
for (int pointIndex = 0; pointIndex < path.Points.Count; pointIndex++)
{
line.SetPosition(pointIndex, ToLineRendererPosition(path.Points[pointIndex]));
}
}
public void CancelActiveRouting()
{
CancelActiveRouting(true);
}
private void CancelActiveRouting(bool updateReport)
{
routingCancelRequested = true;
if (activeRoutingCts != null)
{
activeRoutingCts.Cancel();
if (!isRoutingInProgress)
{
DisposeActiveRoutingCts();
}
}
if (isRoutingInProgress)
{
isRoutingInProgress = false;
if (updateReport)
{
lastReport = "Incremental route generation cancelled.";
}
}
}
private void FinishCancelledRouting()
{
isRoutingInProgress = false;
DisposeActiveRoutingCts();
routingCancelRequested = false;
lastReport = "Incremental route generation cancelled.";
}
private void DisposeActiveRoutingCts()
{
if (activeRoutingCts == null)
{
return;
}
activeRoutingCts.Dispose();
activeRoutingCts = null;
}
private Transform EnsureLineRendererRoot()
{
const string rootName = "Generated Flow Lines";
if (lineRendererRoot != null)
{
return lineRendererRoot;
}
Transform existing = transform.Find(rootName);
if (existing != null)
{
lineRendererRoot = existing;
lineRendererRoot.localPosition = Vector3.zero;
lineRendererRoot.localRotation = Quaternion.identity;
lineRendererRoot.localScale = Vector3.one;
return lineRendererRoot;
}
GameObject rootObject = new GameObject(rootName);
rootObject.transform.SetParent(transform, false);
lineRendererRoot = rootObject.transform;
lineRendererRoot.localPosition = Vector3.zero;
lineRendererRoot.localRotation = Quaternion.identity;
lineRendererRoot.localScale = Vector3.one;
return lineRendererRoot;
}
private void DestroyGeneratedLineRenderers()
{
ClearGeneratedLineRenderers(destroyRoot: true);
}
private void ClearGeneratedLineRenderers(bool destroyRoot)
{
const string rootName = "Generated Flow Lines";
generatedLineRenderers.Clear();
Transform root = lineRendererRoot != null ? lineRendererRoot : transform.Find(rootName);
if (root == null)
{
lineRendererRoot = null;
return;
}
#if UNITY_EDITOR
ClearEditorSelectionIfDestroying(root);
#endif
if (destroyRoot)
{
lineRendererRoot = null;
if (Application.isPlaying)
{
Destroy(root.gameObject);
}
else
{
DestroyImmediate(root.gameObject);
}
return;
}
for (int i = root.childCount - 1; i >= 0; i--)
{
GameObject child = root.GetChild(i).gameObject;
if (Application.isPlaying)
{
Destroy(child);
}
else
{
DestroyImmediate(child);
}
}
}
#if UNITY_EDITOR
private static void ClearEditorSelectionIfDestroying(Transform root)
{
if (root == null || Selection.activeObject == null)
{
return;
}
GameObject selectedGameObject = Selection.activeObject as GameObject;
if (selectedGameObject == null && Selection.activeObject is Component selectedComponent)
{
selectedGameObject = selectedComponent.gameObject;
}
if (selectedGameObject != null && selectedGameObject.transform.IsChildOf(root))
{
Selection.activeObject = null;
}
}
#endif
private Material GetLineRendererMaterial()
{
if (lineRendererMaterial != null)
{
return lineRendererMaterial;
}
Shader shader = Shader.Find("Sprites/Default");
if (shader == null)
{
shader = Shader.Find("Universal Render Pipeline/Unlit");
}
if (shader == null)
{
shader = Shader.Find("Unlit/Color");
}
lineRendererMaterial = shader != null
? new Material(shader) { name = "Generated Flow Line Material" }
: null;
return lineRendererMaterial;
}
private static string GetLineObjectName(DetailedPath path, int index)
{
string flowId = path.Flow != null && !string.IsNullOrWhiteSpace(path.Flow.Id)
? path.Flow.Id
: $"flow_{index + 1:D3}";
return $"Flow Line - {flowId}";
}
private void EnsureModuleList()
{
if (autoCollectModules)
{
modules.Clear();
ModuleNode[] discovered =
#if UNITY_2022_2_OR_NEWER
FindObjectsByType<ModuleNode>(FindObjectsInactive.Include, FindObjectsSortMode.None);
#else
FindObjectsOfType<ModuleNode>(true);
#endif
for (int i = 0; i < discovered.Length; i++)
{
modules.Add(discovered[i]);
}
modules.Sort((left, right) =>
{
string leftName = left != null ? left.name : string.Empty;
string rightName = right != null ? right.name : string.Empty;
return string.CompareOrdinal(leftName, rightName);
});
}
else
{
modules.RemoveAll(item => item == null);
}
}
private void CollectModuleCandidates(
List<ModuleNode> sourceCandidates,
List<ModuleNode> targetCandidates)
{
for (int moduleIndex = 0; moduleIndex < modules.Count; moduleIndex++)
{
ModuleNode module = modules[moduleIndex];
if (module == null)
{
continue;
}
sourceCandidates.Add(module);
targetCandidates.Add(module);
}
}
private void RecomputeAutoSlotsFromConnectivity()
{
Dictionary<ModuleNode, List<FlowRequest>> outputsByModule = new Dictionary<ModuleNode, List<FlowRequest>>();
Dictionary<ModuleNode, List<FlowRequest>> inputsByModule = new Dictionary<ModuleNode, List<FlowRequest>>();
for (int i = 0; i < flowRequests.Count; i++)
{
FlowRequest flow = flowRequests[i];
if (flow == null)
{
continue;
}
AddFlowGroup(outputsByModule, flow.Source.Module, flow);
AddFlowGroup(inputsByModule, flow.Target.Module, flow);
}
Dictionary<FlowRequest, int> sourceIndexByFlow = new Dictionary<FlowRequest, int>(flowRequests.Count);
Dictionary<FlowRequest, int> sourceTotalByFlow = new Dictionary<FlowRequest, int>(flowRequests.Count);
Dictionary<FlowRequest, int> targetIndexByFlow = new Dictionary<FlowRequest, int>(flowRequests.Count);
Dictionary<FlowRequest, int> targetTotalByFlow = new Dictionary<FlowRequest, int>(flowRequests.Count);
foreach (KeyValuePair<ModuleNode, List<FlowRequest>> pair in outputsByModule)
{
ModuleNode module = pair.Key;
List<FlowRequest> groupedFlows = pair.Value;
groupedFlows.Sort((left, right) => CompareFlowOrderForModule(module, left, right, SlotKind.Out));
int total = groupedFlows.Count;
for (int i = 0; i < groupedFlows.Count; i++)
{
FlowRequest flow = groupedFlows[i];
sourceIndexByFlow[flow] = i;
sourceTotalByFlow[flow] = total;
}
}
foreach (KeyValuePair<ModuleNode, List<FlowRequest>> pair in inputsByModule)
{
ModuleNode module = pair.Key;
List<FlowRequest> groupedFlows = pair.Value;
groupedFlows.Sort((left, right) => CompareFlowOrderForModule(module, left, right, SlotKind.In));
int total = groupedFlows.Count;
for (int i = 0; i < groupedFlows.Count; i++)
{
FlowRequest flow = groupedFlows[i];
targetIndexByFlow[flow] = i;
targetTotalByFlow[flow] = total;
}
}
for (int i = 0; i < flowRequests.Count; i++)
{
FlowRequest flow = flowRequests[i];
if (flow == null)
{
continue;
}
int sourceIndex = sourceIndexByFlow.TryGetValue(flow, out int sourceSlotIndex) ? sourceSlotIndex : 0;
int sourceTotal = sourceTotalByFlow.TryGetValue(flow, out int sourceSlotTotal) ? sourceSlotTotal : 0;
int targetIndex = targetIndexByFlow.TryGetValue(flow, out int targetSlotIndex) ? targetSlotIndex : 0;
int targetTotal = targetTotalByFlow.TryGetValue(flow, out int targetSlotTotal) ? targetSlotTotal : 0;
flow.AssignAutoSlots(sourceIndex, sourceTotal, targetIndex, targetTotal);
}
}
private static void AddFlowGroup(Dictionary<ModuleNode, List<FlowRequest>> groups, ModuleNode module, FlowRequest flow)
{
if (module == null || flow == null)
{
return;
}
if (!groups.TryGetValue(module, out List<FlowRequest> list))
{
list = new List<FlowRequest>();
groups[module] = list;
}
list.Add(flow);
}
private static int CompareFlowOrderForModule(ModuleNode module, FlowRequest left, FlowRequest right, SlotKind slotKind)
{
ComputeModuleSortKeys(module, left, slotKind, out float leftPrimary, out float leftSecondary);
ComputeModuleSortKeys(module, right, slotKind, out float rightPrimary, out float rightSecondary);
int compare = leftPrimary.CompareTo(rightPrimary);
if (compare != 0)
{
return compare;
}
compare = leftSecondary.CompareTo(rightSecondary);
if (compare != 0)
{
return compare;
}
string leftId = left != null && !string.IsNullOrWhiteSpace(left.Id) ? left.Id : "flow";
string rightId = right != null && !string.IsNullOrWhiteSpace(right.Id) ? right.Id : "flow";
return string.CompareOrdinal(leftId, rightId);
}
private static void ComputeModuleSortKeys(ModuleNode module, FlowRequest flow, SlotKind slotKind, out float primary, out float secondary)
{
primary = 0f;
secondary = 0f;
if (module == null || flow == null)
{
return;
}
Vector3 counterpartCenter = GetCounterpartCenter(module, flow, slotKind);
SlotSide side = slotKind == SlotKind.Out ? module.DefaultOutputSide : module.DefaultInputSide;
switch (side)
{
case SlotSide.Left:
case SlotSide.Right:
primary = counterpartCenter.z;
secondary = counterpartCenter.y;
break;
case SlotSide.Top:
case SlotSide.Bottom:
primary = counterpartCenter.x;
secondary = counterpartCenter.y;
break;
case SlotSide.Up:
case SlotSide.Down:
primary = counterpartCenter.z;
secondary = counterpartCenter.x;
break;
}
}
private static Vector3 GetCounterpartCenter(ModuleNode module, FlowRequest flow, SlotKind slotKind)
{
ModuleNode counterpart = slotKind == SlotKind.Out ? flow.Target.Module : flow.Source.Module;
if (counterpart != null)
{
return counterpart.GetWorldBounds().center;
}
if (module != null)
{
return module.GetWorldBounds().center;
}
return Vector3.zero;
}
private void PublishReport(string title)
{
List<string> warnings = new List<string>();
if (corridorGraph != null && corridorGraph.TruncatedByLimits)
{
warnings.Add("Corridor graph was truncated by performance limits (maxGraphNodes/maxGraphEdges).");
}
if (negotiationResult != null && negotiationResult.Warnings.Count > 0)
{
warnings.AddRange(negotiationResult.Warnings);
}
StringBuilder builder = new StringBuilder();
builder.AppendLine(title);
builder.AppendLine($"Modules: {modules.Count}, Flows: {flowRequests.Count}");
builder.AppendLine($"Corridor graph: nodes={corridorGraph?.Nodes.Count ?? 0}, edges={corridorGraph?.Edges.Count ?? 0}");
if (lastPipelineDurationMs > 0)
{
builder.AppendLine($"Elapsed: {lastPipelineDurationMs} ms");
builder.AppendLine(
$"Stage timings: graph={lastGraphBuildMs} ms, solve={lastSolveMs} ms, bundle={lastBundleMs} ms, detailed={lastDetailedGeometryMs} ms, validation={lastValidationMs} ms, lineRender={lastLineRenderMs} ms");
}
if (bundleResult != null)
{
builder.AppendLine(
$"Bundling: turnAware={(bundleResult.UsedTurnAwareLaneOrdering ? "on" : "off")}, alignmentPasses={bundleResult.AppliedLaneAlignmentPasses}/{bundleResult.RequestedLaneAlignmentPasses}, laneOrderChanges={bundleResult.LaneAlignmentChangeCount}");
}
if (negotiationResult != null)
{
RoutingMetrics metrics = negotiationResult.Metrics;
string crossingMapStatus = metrics.CrossingMapSkippedByLimit
? $"skipped by limit (edges={metrics.CrossingMapEdges}, limit={config.crossingMapMaxEdges})"
: (metrics.CrossingMapEnabled ? $"active (edges={metrics.CrossingMapEdges})" : "empty/off");
builder.AppendLine(
$"Crossing optimization: map={crossingMapStatus}, postOverflow={(config.optimizeCrossingsAfterOverflow ? "on" : "off")}, iterations={metrics.CrossingOptimizationIterationsUsed}/{metrics.RequestedCrossingOptimizationIterations}");
if (config.optimizeCrossingsAfterOverflow && metrics.CrossingMapSkippedByLimit)
{
builder.AppendLine("- NOTE: Optimize Crossings After Overflow is ignored because the crossing map was skipped by performance limits.");
}
if (!config.optimizeCrossingsAfterOverflow && metrics.RequestedCrossingOptimizationIterations > 0)
{
builder.AppendLine("- NOTE: Max Crossing Optimization Iters is ignored while Optimize Crossings After Overflow is disabled.");
}
}
if (negotiationResult != null)
{
builder.AppendLine($"Negotiation iterations: {negotiationResult.IterationsUsed}");
builder.AppendLine($"Routed: {negotiationResult.Metrics.RoutedCount}, Failed: {negotiationResult.Metrics.FailedCount}");
builder.AppendLine($"Length: {negotiationResult.Metrics.TotalLength:F2}, Bends: {negotiationResult.Metrics.TotalBends}");
builder.AppendLine($"Overflow: {negotiationResult.Metrics.OverflowCount}, Crossings: {negotiationResult.Metrics.CrossingCount}");
}
if (validationReport != null)
{
builder.AppendLine($"Module collisions: {validationReport.ModuleCollisionCount}");
builder.AppendLine($"Flow crossings: {validationReport.FlowCrossingCount}");
}
if (warnings.Count > 0)
{
builder.AppendLine("Warnings:");
for (int i = 0; i < warnings.Count; i++)
{
builder.AppendLine($"- {warnings[i]}");
}
}
lastReport = builder.ToString().TrimEnd();
Debug.Log($"[FlowRoutingSystem]\n{lastReport}", this);
}
private void ResetStageTimings()
{
lastGraphBuildMs = 0L;
lastSolveMs = 0L;
lastBundleMs = 0L;
lastDetailedGeometryMs = 0L;
lastValidationMs = 0L;
lastLineRenderMs = 0L;
}
private static long TicksToMs(long ticks)
{
return (long)(ticks * 1000.0 / System.Diagnostics.Stopwatch.Frequency);
}
private void OnDrawGizmos()
{
if (config == null)
{
return;
}
DrawObstacles();
DrawCorridorGraph();
DrawGlobalRoutes();
DrawTrunks();
DrawDetailedPaths();
DrawConflicts();
}
private void DrawObstacles()
{
if (!config.showObstacles || inflatedObstacles == null)
{
return;
}
Gizmos.color = new Color(0.95f, 0.35f, 0.2f, 0.35f);
for (int i = 0; i < inflatedObstacles.Count; i++)
{
DrawBounds(inflatedObstacles[i]);
}
}
private void DrawCorridorGraph()
{
if (!config.showCorridorGraph || corridorGraph == null)
{
return;
}
int step = GetGizmoStride(corridorGraph.Edges.Count);
for (int edgeId = 0; edgeId < corridorGraph.Edges.Count; edgeId += step)
{
CorridorEdge edge = corridorGraph.Edges[edgeId];
Vector3 a = ToV3(corridorGraph.Nodes[edge.NodeA]);
Vector3 b = ToV3(corridorGraph.Nodes[edge.NodeB]);
if (config.showCongestion && negotiationResult != null && negotiationResult.EdgeUsage.Length == corridorGraph.Edges.Count)
{
int usage = negotiationResult.EdgeUsage[edgeId];
int overflow = negotiationResult.EdgeOverflow[edgeId];
float t = Mathf.Clamp01((float)usage / Mathf.Max(1, edge.Capacity));
Gizmos.color = overflow > 0
? Color.red
: Color.Lerp(new Color(0.2f, 0.95f, 0.2f, 0.9f), new Color(1f, 0.85f, 0.15f, 0.9f), t);
}
else
{
Gizmos.color = new Color(0.5f, 0.5f, 0.5f, 0.6f);
}
Gizmos.DrawLine(a, b);
}
}
private void DrawGlobalRoutes()
{
if (!config.showGlobalRoutes || negotiationResult == null)
{
return;
}
int segmentBudget = Mathf.Max(1, config.maxGizmoSegments);
int drawnSegments = 0;
for (int routeIndex = 0; routeIndex < negotiationResult.Routes.Count; routeIndex++)
{
GlobalRoute route = negotiationResult.Routes[routeIndex];
if (route.NodeIds.Count <= 1)
{
continue;
}
Gizmos.color = new Color(route.Flow.DebugColor.r, route.Flow.DebugColor.g, route.Flow.DebugColor.b, 0.35f);
for (int i = 1; i < route.NodeIds.Count; i++)
{
if (drawnSegments++ >= segmentBudget)
{
return;
}
Vector3 a = ToV3(corridorGraph.Nodes[route.NodeIds[i - 1]]);
Vector3 b = ToV3(corridorGraph.Nodes[route.NodeIds[i]]);
Gizmos.DrawLine(a, b);
}
}
}
private void DrawTrunks()
{
if (!config.showTrunks || bundleResult == null || corridorGraph == null)
{
return;
}
Gizmos.color = new Color(0.1f, 0.75f, 1f, 0.85f);
foreach (KeyValuePair<int, TrunkSegment> pair in bundleResult.TrunksByEdge)
{
CorridorEdge edge = corridorGraph.Edges[pair.Key];
Vector3 a = corridorGraph.Nodes[edge.NodeA];
Vector3 b = corridorGraph.Nodes[edge.NodeB];
Vector3 center = (a + b) * 0.5f;
Vector3 axisNormal = GetTrunkNormal(edge.Axis);
float span = pair.Value.Lanes.Count * config.lanePitch * 0.5f;
Gizmos.DrawLine(ToV3(center - axisNormal * span), ToV3(center + axisNormal * span));
}
}
private void DrawDetailedPaths()
{
if (!config.showDetailedPaths || detailedPaths == null)
{
return;
}
int segmentBudget = Mathf.Max(1, config.maxGizmoSegments);
int drawnSegments = 0;
for (int i = 0; i < detailedPaths.Count; i++)
{
DetailedPath path = detailedPaths[i];
Gizmos.color = path.Flow != null ? path.Flow.DebugColor : new Color(0.3f, 0.85f, 1f, 1f);
for (int pointIndex = 1; pointIndex < path.Points.Count; pointIndex++)
{
if (drawnSegments++ >= segmentBudget)
{
return;
}
Gizmos.DrawLine(ToV3(path.Points[pointIndex - 1]), ToV3(path.Points[pointIndex]));
}
if (config.showRoundedCorners)
{
Gizmos.color = Color.white;
for (int markerIndex = 0; markerIndex < path.CornerMarkers.Count; markerIndex++)
{
Gizmos.DrawSphere(ToV3(path.CornerMarkers[markerIndex]), config.lanePitch * 0.2f);
}
}
DrawPathOrderLabels(path, i + 1);
}
}
private void DrawPathOrderLabels(DetailedPath path, int order)
{
#if UNITY_EDITOR
if (!config.showPathOrderLabels || path == null || path.Points.Count == 0)
{
return;
}
string label = $"#{order}";
Vector3 labelLift = Vector3.up * Mathf.Max(0.08f, config.lanePitch * 0.35f);
GUIStyle style = new GUIStyle(EditorStyles.boldLabel)
{
alignment = TextAnchor.MiddleCenter
};
Color color = path.Flow != null ? path.Flow.DebugColor : Color.white;
style.normal.textColor = new Color(color.r, color.g, color.b, 1f);
Handles.Label(ToV3(path.Points[0]) + labelLift, label, style);
if (path.Points.Count > 1)
{
Handles.Label(ToV3(path.Points[path.Points.Count - 1]) + labelLift, label, style);
}
#endif
}
private void DrawConflicts()
{
if (!config.showConflicts || validationReport == null)
{
return;
}
int segmentBudget = Mathf.Max(1, config.maxGizmoSegments);
int drawnSegments = 0;
Gizmos.color = new Color(1f, 0f, 0f, 0.9f);
for (int i = 0; i < validationReport.ModuleCollisionSegments.Count; i++)
{
if (drawnSegments++ >= segmentBudget)
{
return;
}
SegmentIssue issue = validationReport.ModuleCollisionSegments[i];
Gizmos.DrawLine(ToV3(issue.Start), ToV3(issue.End));
}
Gizmos.color = new Color(1f, 0.5f, 0f, 0.85f);
for (int i = 0; i < validationReport.FlowCrossingSegments.Count; i++)
{
if (drawnSegments++ >= segmentBudget)
{
return;
}
SegmentIssue issue = validationReport.FlowCrossingSegments[i];
Gizmos.DrawLine(ToV3(issue.Start), ToV3(issue.End));
}
}
private Vector3 ToV3(Vector3 point)
{
return transform.TransformPoint(point + Vector3.up * gizmoHeight);
}
private Vector3 ToLineRendererPosition(Vector3 point)
{
return point + Vector3.up * lineRendererHeightOffset;
}
private static Vector3 GetTrunkNormal(EdgeAxis axis)
{
return axis switch
{
EdgeAxis.X => Vector3.forward,
EdgeAxis.Y => Vector3.right,
_ => Vector3.right
};
}
private void DrawBounds(Bounds bounds)
{
Matrix4x4 previous = Gizmos.matrix;
Gizmos.matrix = transform.localToWorldMatrix;
Gizmos.DrawWireCube(bounds.center + Vector3.up * gizmoHeight, bounds.size);
Gizmos.matrix = previous;
}
private int GetGizmoStride(int count)
{
int maxSegments = Mathf.Max(1, config.maxGizmoSegments);
return Mathf.Max(1, Mathf.CeilToInt(count / (float)maxSegments));
}
}
[CustomEditor(typeof(FlowRoutingSystem))]
[CanEditMultipleObjects]
public class FlowRoutingSystemEditor : UnityEditor.Editor
{
public override void OnInspectorGUI()
{
serializedObject.Update();
DrawDefaultInspector();
serializedObject.ApplyModifiedProperties();
EditorGUILayout.Space(8f);
EditorGUILayout.LabelField("Routing Tools", EditorStyles.boldLabel);
if (GUILayout.Button("Generate Random Connections"))
{
RunForTargets("Generate Random Connections", system => system.GenerateRandomConnections());
}
if (GUILayout.Button("Build Corridors Only"))
{
RunForTargets("Build Corridors Only", system => system.BuildCorridorsOnly());
}
if (GUILayout.Button("Run One Negotiation Iteration"))
{
RunForTargets("Run One Negotiation Iteration", system => system.RunOneNegotiationIteration());
}
if (GUILayout.Button("Generate And Draw Flows"))
{
RunIncrementalForTargets("Generate And Draw Flows");
}
if (GUILayout.Button("Cancel Incremental Generation"))
{
RunForTargets("Cancel Incremental Generation", system =>
{
system.CancelActiveRouting();
});
}
if (GUILayout.Button("Clear"))
{
RunForTargets("Clear Routing Data", system => system.Clear());
}
EditorGUILayout.Space(6f);
EditorGUILayout.HelpBox(
"Use Build Corridors Only and Run One Negotiation Iteration for step-by-step debug. " +
"Generate And Draw Flows runs the route pipeline incrementally and paints LineRenderers as routes arrive.",
MessageType.Info);
}
private void RunIncrementalForTargets(string undoLabel)
{
for (int i = 0; i < targets.Length; i++)
{
FlowRoutingSystem system = targets[i] as FlowRoutingSystem;
if (system == null)
{
continue;
}
Undo.RecordObject(system, undoLabel);
if (Application.isPlaying)
{
system.GenerateAndDrawFlowsIncrementally();
}
else
{
system.GenerateAndDrawFlowsIncrementallyAsync().Forget();
}
EditorUtility.SetDirty(system);
}
}
private void RunForTargets(string undoLabel, System.Action<FlowRoutingSystem> action)
{
for (int i = 0; i < targets.Length; i++)
{
FlowRoutingSystem system = targets[i] as FlowRoutingSystem;
if (system == null)
{
continue;
}
Undo.RecordObject(system, undoLabel);
action(system);
EditorUtility.SetDirty(system);
}
}
}
[Serializable]
public class FlowEndpoint
{
[SerializeField] private ModuleNode module;
[SerializeField, HideInInspector] private int autoSlotIndex;
[SerializeField, HideInInspector] private int autoSlotCount;
public ModuleNode Module
{
get => module;
set => module = value;
}
public int AutoSlotIndex
{
get => autoSlotIndex;
set => autoSlotIndex = Mathf.Max(0, value);
}
public int AutoSlotCount
{
get => autoSlotCount;
set => autoSlotCount = Mathf.Max(0, value);
}
public void AssignAutoSlot(int slotIndex, int slotCount)
{
autoSlotIndex = Mathf.Max(0, slotIndex);
autoSlotCount = Mathf.Max(0, slotCount);
}
public bool TryGetPose(SlotKind slotKind, out SlotPose pose)
{
pose = default;
return module != null && autoSlotCount > 0 && module.TryGetSlotPose(slotKind, autoSlotIndex, autoSlotCount, out pose);
}
}
public class ModuleNode : MonoBehaviour
{
private static Transform activeRoutingSpace;
[SerializeField] private bool includeChildrenInBounds = true;
[SerializeField] private SlotSide defaultInputSide = SlotSide.Left;
[SerializeField] private SlotSide defaultOutputSide = SlotSide.Right;
public Vector3 Size => GetWorldBounds().size;
public SlotSide DefaultInputSide => defaultInputSide;
public SlotSide DefaultOutputSide => defaultOutputSide;
public static IDisposable UseRoutingSpace(Transform routingSpace)
{
Transform previous = activeRoutingSpace;
activeRoutingSpace = routingSpace;
return new RoutingSpaceScope(previous);
}
public Bounds GetWorldBounds()
{
if (TryGetComponentBounds(out Bounds bounds))
{
return bounds;
}
Vector3 position = activeRoutingSpace != null
? activeRoutingSpace.InverseTransformPoint(transform.position)
: transform.position;
return new Bounds(position, Vector3.zero);
}
public Bounds GetInflatedBounds(float clearance)
{
Bounds inflated = GetWorldBounds();
inflated.Expand(Mathf.Max(0f, clearance) * 2f);
return inflated;
}
public Rect GetWorldRectAabb()
{
Bounds bounds = GetWorldBounds();
return Rect.MinMaxRect(bounds.min.x, bounds.min.z, bounds.max.x, bounds.max.z);
}
public Rect GetInflatedRect(float clearance)
{
Bounds bounds = GetInflatedBounds(clearance);
return Rect.MinMaxRect(bounds.min.x, bounds.min.z, bounds.max.x, bounds.max.z);
}
public bool TryGetSlotPose(SlotKind slotKind, int slotIndex, int totalSlots, out SlotPose pose)
{
pose = default;
if (slotIndex < 0 || totalSlots <= 0 || slotIndex >= totalSlots)
{
return false;
}
Bounds bounds = GetWorldBounds();
Vector3 point;
Vector3 normal;
float linearT = (slotIndex + 1f) / (totalSlots + 1f);
GetGridCoordinates(slotIndex, totalSlots, out float u, out float v);
SlotSide side = slotKind == SlotKind.In ? defaultInputSide : defaultOutputSide;
switch (side)
{
case SlotSide.Left:
point = new Vector3(
bounds.min.x,
bounds.center.y,
Mathf.Lerp(bounds.min.z, bounds.max.z, linearT));
normal = Vector3.left;
break;
case SlotSide.Right:
point = new Vector3(
bounds.max.x,
bounds.center.y,
Mathf.Lerp(bounds.min.z, bounds.max.z, linearT));
normal = Vector3.right;
break;
case SlotSide.Top:
point = new Vector3(
Mathf.Lerp(bounds.min.x, bounds.max.x, linearT),
bounds.center.y,
bounds.max.z);
normal = Vector3.forward;
break;
case SlotSide.Bottom:
point = new Vector3(
Mathf.Lerp(bounds.min.x, bounds.max.x, linearT),
bounds.center.y,
bounds.min.z);
normal = Vector3.back;
break;
case SlotSide.Up:
point = new Vector3(
Mathf.Lerp(bounds.min.x, bounds.max.x, u),
bounds.max.y,
Mathf.Lerp(bounds.min.z, bounds.max.z, v));
normal = Vector3.up;
break;
default: // Down
point = new Vector3(
Mathf.Lerp(bounds.min.x, bounds.max.x, u),
bounds.min.y,
Mathf.Lerp(bounds.min.z, bounds.max.z, v));
normal = Vector3.down;
break;
}
pose = new SlotPose(point, normal);
return true;
}
private static void GetGridCoordinates(int slotIndex, int totalSlots, out float u, out float v)
{
int safeTotal = Mathf.Max(1, totalSlots);
int columns = Mathf.Max(1, Mathf.CeilToInt(Mathf.Sqrt(safeTotal)));
int rows = Mathf.Max(1, Mathf.CeilToInt(safeTotal / (float)columns));
int row = slotIndex / columns;
int column = slotIndex % columns;
u = (column + 1f) / (columns + 1f);
v = (row + 1f) / (rows + 1f);
}
private bool TryGetComponentBounds(out Bounds bounds)
{
bool hasBounds = false;
bounds = default;
Renderer[] renderers = includeChildrenInBounds
? GetComponentsInChildren<Renderer>(true)
: GetComponents<Renderer>();
for (int i = 0; i < renderers.Length; i++)
{
if (renderers[i] == null)
{
continue;
}
EncapsulateBounds(ref hasBounds, ref bounds, GetRendererBounds(renderers[i]));
}
Collider[] colliders = includeChildrenInBounds
? GetComponentsInChildren<Collider>(true)
: GetComponents<Collider>();
for (int i = 0; i < colliders.Length; i++)
{
if (colliders[i] == null)
{
continue;
}
EncapsulateBounds(ref hasBounds, ref bounds, GetColliderBounds(colliders[i]));
}
return hasBounds;
}
private static Bounds GetRendererBounds(Renderer renderer)
{
if (activeRoutingSpace == null)
{
return renderer.bounds;
}
return TransformLocalBounds(renderer.transform, renderer.localBounds, activeRoutingSpace);
}
private static Bounds GetColliderBounds(Collider collider)
{
if (activeRoutingSpace == null)
{
return collider.bounds;
}
if (collider is BoxCollider box)
{
return TransformLocalBounds(box.transform, new Bounds(box.center, box.size), activeRoutingSpace);
}
return TransformWorldBounds(collider.bounds, activeRoutingSpace);
}
private static Bounds TransformLocalBounds(Transform source, Bounds localBounds, Transform targetSpace)
{
Vector3 min = localBounds.min;
Vector3 max = localBounds.max;
Bounds result = default;
bool hasPoint = false;
EncapsulateCorner(ref hasPoint, ref result, source.TransformPoint(new Vector3(min.x, min.y, min.z)), targetSpace);
EncapsulateCorner(ref hasPoint, ref result, source.TransformPoint(new Vector3(min.x, min.y, max.z)), targetSpace);
EncapsulateCorner(ref hasPoint, ref result, source.TransformPoint(new Vector3(min.x, max.y, min.z)), targetSpace);
EncapsulateCorner(ref hasPoint, ref result, source.TransformPoint(new Vector3(min.x, max.y, max.z)), targetSpace);
EncapsulateCorner(ref hasPoint, ref result, source.TransformPoint(new Vector3(max.x, min.y, min.z)), targetSpace);
EncapsulateCorner(ref hasPoint, ref result, source.TransformPoint(new Vector3(max.x, min.y, max.z)), targetSpace);
EncapsulateCorner(ref hasPoint, ref result, source.TransformPoint(new Vector3(max.x, max.y, min.z)), targetSpace);
EncapsulateCorner(ref hasPoint, ref result, source.TransformPoint(new Vector3(max.x, max.y, max.z)), targetSpace);
return result;
}
private static Bounds TransformWorldBounds(Bounds worldBounds, Transform targetSpace)
{
Vector3 min = worldBounds.min;
Vector3 max = worldBounds.max;
Bounds result = default;
bool hasPoint = false;
EncapsulateCorner(ref hasPoint, ref result, new Vector3(min.x, min.y, min.z), targetSpace);
EncapsulateCorner(ref hasPoint, ref result, new Vector3(min.x, min.y, max.z), targetSpace);
EncapsulateCorner(ref hasPoint, ref result, new Vector3(min.x, max.y, min.z), targetSpace);
EncapsulateCorner(ref hasPoint, ref result, new Vector3(min.x, max.y, max.z), targetSpace);
EncapsulateCorner(ref hasPoint, ref result, new Vector3(max.x, min.y, min.z), targetSpace);
EncapsulateCorner(ref hasPoint, ref result, new Vector3(max.x, min.y, max.z), targetSpace);
EncapsulateCorner(ref hasPoint, ref result, new Vector3(max.x, max.y, min.z), targetSpace);
EncapsulateCorner(ref hasPoint, ref result, new Vector3(max.x, max.y, max.z), targetSpace);
return result;
}
private static void EncapsulateCorner(ref bool hasPoint, ref Bounds bounds, Vector3 worldPoint, Transform targetSpace)
{
Vector3 point = targetSpace.InverseTransformPoint(worldPoint);
if (!hasPoint)
{
bounds = new Bounds(point, Vector3.zero);
hasPoint = true;
return;
}
bounds.Encapsulate(point);
}
private static void EncapsulateBounds(ref bool hasBounds, ref Bounds bounds, Bounds value)
{
if (!hasBounds)
{
bounds = value;
hasBounds = true;
return;
}
bounds.Encapsulate(value);
}
private readonly struct RoutingSpaceScope : IDisposable
{
private readonly Transform previous;
public RoutingSpaceScope(Transform previous)
{
this.previous = previous;
}
public void Dispose()
{
activeRoutingSpace = previous;
}
}
}
[CreateAssetMenu(menuName = "Routing/Lightweight PCB Routing Config", fileName = "RoutingConfig")]
public class RoutingConfig : ScriptableObject
{
[Header("Geometry")]
[Min(0f)] public float clearance = 0.4f;
[Min(0.02f)] public float lanePitch = 0.25f;
[Min(0.02f)] public float minSegmentLen = 0.2f;
[Min(0f)] public float cornerRadius = 0.2f;
[Min(0f)] public float fanoutLength = 0.6f;
[Min(0f)] public float workAreaPadding = 2f;
[Min(0f)] public float proximityPenaltyDistance = 1.5f;
[Header("Routing Modes")]
public bool enforcePlanarRoutingOnSameHeight = true;
[Min(0f)] public float sameHeightTolerance = 0.05f;
[Min(0f)] public float planarNodeTolerance = 0.08f;
[Header("Bundling")]
public bool useTurnAwareLaneOrdering = true;
[Min(0)] public int laneAlignmentPasses = 2;
[Header("Cost Weights")]
[Min(0f)] public float wLength = 1f;
[Min(0f)] public float wBend = 0.8f;
[Min(0f)] public float wCongestion = 4f;
[Min(0f)] public float wOverflow = 10f;
[Min(0f)] public float wCrossing = 8f;
[Min(0f)] public float wHistory = 1.2f;
[Header("Negotiation")]
[Min(1)] public int maxNegotiationIters = 16;
[Range(0f, 1f)] public float ripUpPercent = 0.35f;
[Min(0)] public int maxCrossingFallback = 12;
public bool mustRenderAll = true;
[Header("Crossing Optimization")]
public bool optimizeCrossingsAfterOverflow = true;
[Range(0f, 1f)] public float crossingRipUpPercent = 0.28f;
[Min(0)] public int maxCrossingOptimizationIters = 4;
[Min(0f)] public float crossingRerouteBoost = 5f;
[Header("Performance & Timeouts")]
[Min(8)] public int maxGuideCoordsPerAxis = 40;
[Min(1000)] public int maxGraphNodes = 80000;
[Min(1000)] public int maxGraphEdges = 220000;
[Min(0.1f)] public float maxGraphBuildTimeSeconds = 6f;
[Min(1000)] public int maxExpandedNodesPerFlow = 35000;
[Min(0.05f)] public float maxRouteTimeSeconds = 0.15f;
[Min(0.5f)] public float maxSolveTimeSeconds = 30f;
public bool useHeuristicPruning = true;
[Min(1f)] public float heuristicPruneMultiplier = 2.4f;
[Min(0f)] public float heuristicPrunePadding = 4f;
public bool disableCrossingMapWhenLarge = true;
[Min(0)] public int crossingMapMaxEdges = 3000;
[Min(1000)] public int maxValidationChecks = 120000;
[Header("Debug Flags")]
public bool showObstacles = true;
public bool showCorridorGraph = true;
public bool showCongestion = true;
public bool showGlobalRoutes = true;
public bool showTrunks = true;
public bool showDetailedPaths = true;
public bool showPathOrderLabels = true;
public bool showRoundedCorners = false;
public bool showConflicts = true;
[Min(100)] public int maxGizmoSegments = 8000;
}
public enum SlotKind
{
In,
Out
}
public enum SlotSide
{
Left,
Right,
Top,
Bottom,
Up,
Down
}
public readonly struct SlotPose
{
public SlotPose(Vector3 position, Vector3 normal)
{
Position = position;
Normal = normal.sqrMagnitude > 0f ? normal.normalized : Vector3.right;
}
public Vector3 Position { get; }
public Vector3 Normal { get; }
}
