Refactor ElkSharp hybrid routing and document speed path
This commit is contained in:
master
@@ -1,11 +1,11 @@
|
||||
namespace StellaOps.ElkSharp;
|
||||
|
||||
internal static class ElkEdgeRouterAStar8Dir
|
||||
internal static partial class ElkEdgeRouterAStar8Dir
|
||||
{
|
||||
// E, W, S, N, NE, SW, SE, NW
|
||||
private static readonly int[] Dx = [1, -1, 0, 0, 1, -1, 1, -1];
|
||||
private static readonly int[] Dy = [0, 0, 1, -1, -1, 1, 1, -1];
|
||||
// Direction codes: 1=horizontal, 2=vertical, 3=diagonal45(NE/SW), 4=diagonal135(SE/NW)
|
||||
// Direction codes: 1=horizontal, 2=vertical, 3=diagonal45 (NE/SW), 4=diagonal135 (SE/NW)
|
||||
private static readonly int[] DirCodes = [1, 1, 2, 2, 3, 3, 4, 4];
|
||||
|
||||
internal static List<ElkPoint>? Route(
|
||||
@@ -20,17 +20,17 @@ internal static class ElkEdgeRouterAStar8Dir
|
||||
{
|
||||
var xs = new SortedSet<double> { start.X, end.X };
|
||||
var ys = new SortedSet<double> { start.Y, end.Y };
|
||||
foreach (var ob in obstacles)
|
||||
foreach (var obstacle in obstacles)
|
||||
{
|
||||
if (ob.Id == sourceId || ob.Id == targetId)
|
||||
if (obstacle.Id == sourceId || obstacle.Id == targetId)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
xs.Add(ob.Left - routingParams.Margin);
|
||||
xs.Add(ob.Right + routingParams.Margin);
|
||||
ys.Add(ob.Top - routingParams.Margin);
|
||||
ys.Add(ob.Bottom + routingParams.Margin);
|
||||
xs.Add(obstacle.Left - routingParams.Margin);
|
||||
xs.Add(obstacle.Right + routingParams.Margin);
|
||||
ys.Add(obstacle.Top - routingParams.Margin);
|
||||
ys.Add(obstacle.Bottom + routingParams.Margin);
|
||||
}
|
||||
|
||||
if (routingParams.IntermediateGridSpacing > 0d)
|
||||
@@ -108,30 +108,27 @@ internal static class ElkEdgeRouterAStar8Dir
|
||||
var cameFrom = new int[stateCount];
|
||||
Array.Fill(cameFrom, -1);
|
||||
|
||||
// Side-aware entry angle: block moves parallel to the target's entry side
|
||||
// Vertical side (left/right) → block vertical (dir=2), force horizontal approach
|
||||
// Horizontal side (top/bottom) → block horizontal (dir=1), force vertical approach
|
||||
var blockedEntryDir = 0;
|
||||
if (routingParams.EnforceEntryAngle)
|
||||
{
|
||||
foreach (var ob in obstacles)
|
||||
foreach (var obstacle in obstacles)
|
||||
{
|
||||
if (ob.Id != targetId)
|
||||
if (obstacle.Id != targetId)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var nodeLeft = ob.Left + routingParams.Margin;
|
||||
var nodeRight = ob.Right - routingParams.Margin;
|
||||
var nodeTop = ob.Top + routingParams.Margin;
|
||||
var nodeBottom = ob.Bottom - routingParams.Margin;
|
||||
var nodeLeft = obstacle.Left + routingParams.Margin;
|
||||
var nodeRight = obstacle.Right - routingParams.Margin;
|
||||
var nodeTop = obstacle.Top + routingParams.Margin;
|
||||
var nodeBottom = obstacle.Bottom - routingParams.Margin;
|
||||
if (Math.Abs(end.X - nodeLeft) < 2d || Math.Abs(end.X - nodeRight) < 2d)
|
||||
{
|
||||
blockedEntryDir = 2; // vertical side → block vertical
|
||||
blockedEntryDir = 2;
|
||||
}
|
||||
else if (Math.Abs(end.Y - nodeTop) < 2d || Math.Abs(end.Y - nodeBottom) < 2d)
|
||||
{
|
||||
blockedEntryDir = 1; // horizontal side → block horizontal
|
||||
blockedEntryDir = 1;
|
||||
}
|
||||
|
||||
break;
|
||||
@@ -200,23 +197,16 @@ internal static class ElkEdgeRouterAStar8Dir
|
||||
continue;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (IsBlockedOrthogonal(curIx, curIy, nx, ny))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
var newDir = DirCodes[d];
|
||||
|
||||
// Side-aware entry angle: block parallel moves into end cell
|
||||
if (blockedEntryDir > 0 && nx == endIx && ny == endIy && newDir == blockedEntryDir)
|
||||
else if (IsBlockedOrthogonal(curIx, curIy, nx, ny))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var bend = ComputeBendPenalty(curDir, newDir, routingParams.BendPenalty);
|
||||
var newDir = DirCodes[d];
|
||||
if (blockedEntryDir > 0 && nx == endIx && ny == endIy && newDir == blockedEntryDir)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
double dist;
|
||||
if (isDiagonal)
|
||||
@@ -228,6 +218,7 @@ internal static class ElkEdgeRouterAStar8Dir
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
dist = diagonalStepLength + routingParams.DiagonalPenalty;
|
||||
}
|
||||
else
|
||||
@@ -235,10 +226,10 @@ internal static class ElkEdgeRouterAStar8Dir
|
||||
dist = Math.Abs(xArr[nx] - xArr[curIx]) + Math.Abs(yArr[ny] - yArr[curIy]);
|
||||
}
|
||||
|
||||
var bend = ComputeBendPenalty(curDir, newDir, routingParams.BendPenalty);
|
||||
var softCost = ComputeSoftObstacleCost(
|
||||
xArr[curIx], yArr[curIy], xArr[nx], yArr[ny],
|
||||
softObstacleInfos, routingParams);
|
||||
|
||||
var tentativeG = gScore[current] + dist + bend + softCost;
|
||||
var neighborState = StateId(nx, ny, newDir);
|
||||
|
||||
@@ -254,317 +245,33 @@ internal static class ElkEdgeRouterAStar8Dir
|
||||
return null;
|
||||
}
|
||||
|
||||
private static double ResolveMaxDiagonalStepLength(
|
||||
IReadOnlyCollection<(double Left, double Top, double Right, double Bottom, string Id)> obstacles)
|
||||
{
|
||||
if (obstacles.Count == 0)
|
||||
{
|
||||
return 256d;
|
||||
}
|
||||
|
||||
var averageWidth = obstacles.Average(obstacle => obstacle.Right - obstacle.Left);
|
||||
var averageHeight = obstacles.Average(obstacle => obstacle.Bottom - obstacle.Top);
|
||||
var averageShapeSize = (averageWidth + averageHeight) / 2d;
|
||||
return Math.Max(96d, averageShapeSize * 2d);
|
||||
}
|
||||
|
||||
private static double ComputeBendPenalty(int curDir, int newDir, double bendPenalty)
|
||||
{
|
||||
if (curDir == 0 || curDir == newDir)
|
||||
{
|
||||
return 0d;
|
||||
}
|
||||
|
||||
// H↔V = 90° bend, diag↔diag (opposite types) = 90° bend
|
||||
if ((curDir <= 2 && newDir <= 2) || (curDir >= 3 && newDir >= 3))
|
||||
{
|
||||
return bendPenalty;
|
||||
}
|
||||
|
||||
// ortho↔diag = 45° bend
|
||||
return bendPenalty / 2d;
|
||||
}
|
||||
|
||||
private static double ComputeSoftObstacleCost(
|
||||
double x1, double y1, double x2, double y2,
|
||||
SoftObstacleInfo[] softObstacles,
|
||||
AStarRoutingParams routingParams)
|
||||
{
|
||||
if (routingParams.SoftObstacleWeight <= 0d || softObstacles.Length == 0)
|
||||
{
|
||||
return 0d;
|
||||
}
|
||||
|
||||
var candidateStart = new ElkPoint { X = x1, Y = y1 };
|
||||
var candidateEnd = new ElkPoint { X = x2, Y = y2 };
|
||||
var candidateIsH = Math.Abs(y2 - y1) < 2d;
|
||||
var candidateIsV = Math.Abs(x2 - x1) < 2d;
|
||||
var candidateMinX = Math.Min(x1, x2);
|
||||
var candidateMaxX = Math.Max(x1, x2);
|
||||
var candidateMinY = Math.Min(y1, y2);
|
||||
var candidateMaxY = Math.Max(y1, y2);
|
||||
var expandedMinX = candidateMinX - routingParams.SoftObstacleClearance;
|
||||
var expandedMaxX = candidateMaxX + routingParams.SoftObstacleClearance;
|
||||
var expandedMinY = candidateMinY - routingParams.SoftObstacleClearance;
|
||||
var expandedMaxY = candidateMaxY + routingParams.SoftObstacleClearance;
|
||||
var cost = 0d;
|
||||
|
||||
foreach (var obstacle in softObstacles)
|
||||
{
|
||||
if (expandedMaxX < obstacle.MinX
|
||||
|| expandedMinX > obstacle.MaxX
|
||||
|| expandedMaxY < obstacle.MinY
|
||||
|| expandedMinY > obstacle.MaxY)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
if (ElkEdgeRoutingGeometry.SegmentsIntersect(candidateStart, candidateEnd, obstacle.Start, obstacle.End))
|
||||
{
|
||||
cost += 120d * routingParams.SoftObstacleWeight;
|
||||
continue;
|
||||
}
|
||||
|
||||
// Graduated proximity: closer = exponentially more expensive
|
||||
var dist = ComputeParallelDistance(
|
||||
x1, y1, x2, y2, candidateIsH, candidateIsV,
|
||||
obstacle,
|
||||
routingParams.SoftObstacleClearance);
|
||||
|
||||
if (dist >= 0d)
|
||||
{
|
||||
var factor = 1d - (dist / routingParams.SoftObstacleClearance);
|
||||
cost += 60d * factor * factor * routingParams.SoftObstacleWeight;
|
||||
}
|
||||
}
|
||||
|
||||
return cost;
|
||||
}
|
||||
|
||||
private static double ComputeParallelDistance(
|
||||
double x1, double y1, double x2, double y2,
|
||||
bool candidateIsH, bool candidateIsV,
|
||||
SoftObstacleInfo obstacle,
|
||||
double clearance)
|
||||
{
|
||||
if (candidateIsH && obstacle.IsHorizontal)
|
||||
{
|
||||
var dist = Math.Abs(y1 - obstacle.Start.Y);
|
||||
if (dist >= clearance)
|
||||
{
|
||||
return -1d;
|
||||
}
|
||||
|
||||
var overlapMin = Math.Max(Math.Min(x1, x2), obstacle.MinX);
|
||||
var overlapMax = Math.Min(Math.Max(x1, x2), obstacle.MaxX);
|
||||
return overlapMax > overlapMin + 1d ? dist : -1d;
|
||||
}
|
||||
|
||||
if (candidateIsV && obstacle.IsVertical)
|
||||
{
|
||||
var dist = Math.Abs(x1 - obstacle.Start.X);
|
||||
if (dist >= clearance)
|
||||
{
|
||||
return -1d;
|
||||
}
|
||||
|
||||
var overlapMin = Math.Max(Math.Min(y1, y2), obstacle.MinY);
|
||||
var overlapMax = Math.Min(Math.Max(y1, y2), obstacle.MaxY);
|
||||
return overlapMax > overlapMin + 1d ? dist : -1d;
|
||||
}
|
||||
|
||||
return -1d;
|
||||
}
|
||||
|
||||
private static BlockedSegments BuildBlockedSegments(
|
||||
private static List<ElkPoint> ReconstructPath(
|
||||
int endState,
|
||||
int[] cameFrom,
|
||||
double[] xArr,
|
||||
double[] yArr,
|
||||
(double Left, double Top, double Right, double Bottom, string Id)[] obstacles,
|
||||
string sourceId,
|
||||
string targetId)
|
||||
{
|
||||
var xCount = xArr.Length;
|
||||
var yCount = yArr.Length;
|
||||
var verticalBlocked = new bool[xCount * Math.Max(0, yCount - 1)];
|
||||
var horizontalBlocked = new bool[Math.Max(0, xCount - 1) * yCount];
|
||||
|
||||
foreach (var obstacle in obstacles)
|
||||
{
|
||||
if (obstacle.Id == sourceId || obstacle.Id == targetId)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var verticalXStart = Math.Max(0, LowerBoundExclusive(xArr, obstacle.Left));
|
||||
var verticalXEnd = Math.Min(xCount - 1, UpperBoundExclusive(xArr, obstacle.Right) - 1);
|
||||
if (verticalXStart <= verticalXEnd)
|
||||
{
|
||||
var verticalYStart = Math.Max(0, LowerBound(yArr, obstacle.Top) - 1);
|
||||
var verticalYEnd = Math.Min(yCount - 2, UpperBound(yArr, obstacle.Bottom) - 1);
|
||||
for (var ix = verticalXStart; ix <= verticalXEnd; ix++)
|
||||
{
|
||||
for (var iy = verticalYStart; iy <= verticalYEnd; iy++)
|
||||
{
|
||||
if (yArr[iy + 1] > obstacle.Top && yArr[iy] < obstacle.Bottom)
|
||||
{
|
||||
verticalBlocked[(ix * (yCount - 1)) + iy] = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
var horizontalYStart = Math.Max(0, LowerBoundExclusive(yArr, obstacle.Top));
|
||||
var horizontalYEnd = Math.Min(yCount - 1, UpperBoundExclusive(yArr, obstacle.Bottom) - 1);
|
||||
if (horizontalYStart <= horizontalYEnd)
|
||||
{
|
||||
var horizontalXStart = Math.Max(0, LowerBound(xArr, obstacle.Left) - 1);
|
||||
var horizontalXEnd = Math.Min(xCount - 2, UpperBound(xArr, obstacle.Right) - 1);
|
||||
for (var iy = horizontalYStart; iy <= horizontalYEnd; iy++)
|
||||
{
|
||||
for (var ix = horizontalXStart; ix <= horizontalXEnd; ix++)
|
||||
{
|
||||
if (xArr[ix + 1] > obstacle.Left && xArr[ix] < obstacle.Right)
|
||||
{
|
||||
horizontalBlocked[(ix * yCount) + iy] = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return new BlockedSegments(xCount, yCount, verticalBlocked, horizontalBlocked);
|
||||
}
|
||||
|
||||
private static SoftObstacleInfo[] BuildSoftObstacleInfos(IReadOnlyList<OrthogonalSoftObstacle> softObstacles)
|
||||
{
|
||||
if (softObstacles.Count == 0)
|
||||
{
|
||||
return [];
|
||||
}
|
||||
|
||||
var infos = new SoftObstacleInfo[softObstacles.Count];
|
||||
for (var i = 0; i < softObstacles.Count; i++)
|
||||
{
|
||||
var obstacle = softObstacles[i];
|
||||
infos[i] = new SoftObstacleInfo(
|
||||
obstacle.Start,
|
||||
obstacle.End,
|
||||
Math.Min(obstacle.Start.X, obstacle.End.X),
|
||||
Math.Max(obstacle.Start.X, obstacle.End.X),
|
||||
Math.Min(obstacle.Start.Y, obstacle.End.Y),
|
||||
Math.Max(obstacle.Start.Y, obstacle.End.Y),
|
||||
Math.Abs(obstacle.Start.Y - obstacle.End.Y) < 2d,
|
||||
Math.Abs(obstacle.Start.X - obstacle.End.X) < 2d);
|
||||
}
|
||||
|
||||
return infos;
|
||||
}
|
||||
|
||||
private static int LowerBound(double[] values, double target)
|
||||
{
|
||||
var low = 0;
|
||||
var high = values.Length;
|
||||
while (low < high)
|
||||
{
|
||||
var mid = low + ((high - low) / 2);
|
||||
if (values[mid] < target)
|
||||
{
|
||||
low = mid + 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
high = mid;
|
||||
}
|
||||
}
|
||||
|
||||
return low;
|
||||
}
|
||||
|
||||
private static int UpperBound(double[] values, double target)
|
||||
{
|
||||
var low = 0;
|
||||
var high = values.Length;
|
||||
while (low < high)
|
||||
{
|
||||
var mid = low + ((high - low) / 2);
|
||||
if (values[mid] <= target)
|
||||
{
|
||||
low = mid + 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
high = mid;
|
||||
}
|
||||
}
|
||||
|
||||
return low;
|
||||
}
|
||||
|
||||
private static int LowerBoundExclusive(double[] values, double target)
|
||||
{
|
||||
var low = 0;
|
||||
var high = values.Length;
|
||||
while (low < high)
|
||||
{
|
||||
var mid = low + ((high - low) / 2);
|
||||
if (values[mid] <= target)
|
||||
{
|
||||
low = mid + 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
high = mid;
|
||||
}
|
||||
}
|
||||
|
||||
return low;
|
||||
}
|
||||
|
||||
private static int UpperBoundExclusive(double[] values, double target)
|
||||
{
|
||||
var low = 0;
|
||||
var high = values.Length;
|
||||
while (low < high)
|
||||
{
|
||||
var mid = low + ((high - low) / 2);
|
||||
if (values[mid] < target)
|
||||
{
|
||||
low = mid + 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
high = mid;
|
||||
}
|
||||
}
|
||||
|
||||
return low;
|
||||
}
|
||||
|
||||
private static List<ElkPoint> ReconstructPath(
|
||||
int endState, int[] cameFrom,
|
||||
double[] xArr, double[] yArr,
|
||||
int yCount, int dirCount)
|
||||
int yCount,
|
||||
int dirCount)
|
||||
{
|
||||
var path = new List<ElkPoint>();
|
||||
var state = endState;
|
||||
while (state >= 0)
|
||||
{
|
||||
var sIy = (state / dirCount) % yCount;
|
||||
var sIx = (state / dirCount) / yCount;
|
||||
path.Add(new ElkPoint { X = xArr[sIx], Y = yArr[sIy] });
|
||||
var stateY = (state / dirCount) % yCount;
|
||||
var stateX = (state / dirCount) / yCount;
|
||||
path.Add(new ElkPoint { X = xArr[stateX], Y = yArr[stateY] });
|
||||
state = cameFrom[state];
|
||||
}
|
||||
|
||||
path.Reverse();
|
||||
|
||||
// Simplify: remove collinear points (same direction between consecutive segments)
|
||||
var simplified = new List<ElkPoint> { path[0] };
|
||||
for (var i = 1; i < path.Count - 1; i++)
|
||||
{
|
||||
var prev = simplified[^1];
|
||||
var previous = simplified[^1];
|
||||
var next = path[i + 1];
|
||||
var dx1 = Math.Sign(path[i].X - prev.X);
|
||||
var dy1 = Math.Sign(path[i].Y - prev.Y);
|
||||
var dx1 = Math.Sign(path[i].X - previous.X);
|
||||
var dy1 = Math.Sign(path[i].Y - previous.Y);
|
||||
var dx2 = Math.Sign(next.X - path[i].X);
|
||||
var dy2 = Math.Sign(next.Y - path[i].Y);
|
||||
if (dx1 != dx2 || dy1 != dy2)
|
||||
@@ -576,61 +283,4 @@ internal static class ElkEdgeRouterAStar8Dir
|
||||
simplified.Add(path[^1]);
|
||||
return simplified;
|
||||
}
|
||||
|
||||
private static void AddIntermediateLines(SortedSet<double> coords, double spacing)
|
||||
{
|
||||
var arr = coords.ToArray();
|
||||
for (var i = 0; i < arr.Length - 1; i++)
|
||||
{
|
||||
var gap = arr[i + 1] - arr[i];
|
||||
if (gap <= spacing * 2d)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var count = (int)(gap / spacing);
|
||||
var step = gap / (count + 1);
|
||||
for (var j = 1; j <= count; j++)
|
||||
{
|
||||
coords.Add(arr[i] + j * step);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private readonly record struct SoftObstacleInfo(
|
||||
ElkPoint Start,
|
||||
ElkPoint End,
|
||||
double MinX,
|
||||
double MaxX,
|
||||
double MinY,
|
||||
double MaxY,
|
||||
bool IsHorizontal,
|
||||
bool IsVertical);
|
||||
|
||||
private readonly record struct BlockedSegments(
|
||||
int XCount,
|
||||
int YCount,
|
||||
bool[] VerticalBlocked,
|
||||
bool[] HorizontalBlocked)
|
||||
{
|
||||
internal bool IsVerticalBlocked(int ix, int iy)
|
||||
{
|
||||
if (ix < 0 || ix >= XCount || iy < 0 || iy >= YCount - 1)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
return VerticalBlocked[(ix * (YCount - 1)) + iy];
|
||||
}
|
||||
|
||||
internal bool IsHorizontalBlocked(int ix, int iy)
|
||||
{
|
||||
if (ix < 0 || ix >= XCount - 1 || iy < 0 || iy >= YCount)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
return HorizontalBlocked[(ix * YCount) + iy];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user