stella-ops.org/git.stella-ops.org
1
0
Fork 0
Code Issues Pull Requests Actions 2 Releases Wiki Activity

Refactor ElkSharp routing sources into partial modules

Browse Source
This commit is contained in:
master
2026-03-28 11:56:35 +02:00
parent 7be4e855d6
commit 7057819f4d
34 changed files with 33377 additions and 21402 deletions
Download Patch File Download Diff File Expand all files Collapse all files

334
src/__Libraries/StellaOps.ElkSharp/ElkEdgeRouterAStar8Dir.cs
View File

@@ -48,6 +48,15 @@ internal static class ElkEdgeRouterAStar8Dir
return null;
}
var graphMaxY = obstacles.Length > 0
? obstacles.Max(obstacle => obstacle.Bottom)
: double.MaxValue;
var disallowedBottomY = graphMaxY + 4d;
var maxDiagonalStepLength = ResolveMaxDiagonalStepLength(obstacles);
var blockedSegments = BuildBlockedSegments(xArr, yArr, obstacles, sourceId, targetId);
var softObstacleInfos = BuildSoftObstacleInfos(softObstacles);
var startIx = Array.BinarySearch(xArr, start.X);
var startIy = Array.BinarySearch(yArr, start.Y);
var endIx = Array.BinarySearch(xArr, end.X);
@@ -59,41 +68,34 @@ internal static class ElkEdgeRouterAStar8Dir
bool IsBlockedOrthogonal(int ix1, int iy1, int ix2, int iy2)
{
var x1 = xArr[ix1];
var y1 = yArr[iy1];
var x2 = xArr[ix2];
var y2 = yArr[iy2];
foreach (var ob in obstacles)
if (ix1 == ix2)
{
if (ob.Id == sourceId || ob.Id == targetId)
var minIy = Math.Min(iy1, iy2);
var maxIy = Math.Max(iy1, iy2);
for (var iy = minIy; iy < maxIy; iy++)
{
continue;
if (blockedSegments.IsVerticalBlocked(ix1, iy))
{
return true;
}
}
if (ix1 == ix2)
return false;
}
if (iy1 == iy2)
{
var minIx = Math.Min(ix1, ix2);
var maxIx = Math.Max(ix1, ix2);
for (var ix = minIx; ix < maxIx; ix++)
{
if (x1 > ob.Left && x1 < ob.Right)
if (blockedSegments.IsHorizontalBlocked(ix, iy1))
{
var minY = Math.Min(y1, y2);
var maxY = Math.Max(y1, y2);
if (maxY > ob.Top && minY < ob.Bottom)
{
return true;
}
}
}
else if (iy1 == iy2)
{
if (y1 > ob.Top && y1 < ob.Bottom)
{
var minX = Math.Min(x1, x2);
var maxX = Math.Max(x1, x2);
if (maxX > ob.Left && minX < ob.Right)
{
return true;
}
return true;
}
}
return false;
}
return false;
@@ -152,18 +154,20 @@ internal static class ElkEdgeRouterAStar8Dir
var maxIterations = xCount * yCount * 12;
var iterations = 0;
var closed = new HashSet<int>();
var closed = new bool[stateCount];
while (openSet.Count > 0 && iterations++ < maxIterations)
{
cancellationToken.ThrowIfCancellationRequested();
var current = openSet.Dequeue();
if (!closed.Add(current))
if (closed[current])
{
continue;
}
closed[current] = true;
var curDir = current % dirCount;
var curIy = (current / dirCount) % yCount;
var curIx = (current / dirCount) / yCount;
@@ -182,6 +186,11 @@ internal static class ElkEdgeRouterAStar8Dir
continue;
}
if (yArr[curIy] > disallowedBottomY || yArr[ny] > disallowedBottomY)
{
continue;
}
var isDiagonal = Dx[d] != 0 && Dy[d] != 0;
if (isDiagonal)
{
@@ -214,7 +223,12 @@ internal static class ElkEdgeRouterAStar8Dir
{
var ddx = xArr[nx] - xArr[curIx];
var ddy = yArr[ny] - yArr[curIy];
dist = Math.Sqrt(ddx * ddx + ddy * ddy) + routingParams.DiagonalPenalty;
var diagonalStepLength = Math.Sqrt(ddx * ddx + ddy * ddy);
if (diagonalStepLength > maxDiagonalStepLength)
{
continue;
}
dist = diagonalStepLength + routingParams.DiagonalPenalty;
}
else
{
@@ -223,7 +237,7 @@ internal static class ElkEdgeRouterAStar8Dir
var softCost = ComputeSoftObstacleCost(
xArr[curIx], yArr[curIy], xArr[nx], yArr[ny],
softObstacles, routingParams);
softObstacleInfos, routingParams);
var tentativeG = gScore[current] + dist + bend + softCost;
var neighborState = StateId(nx, ny, newDir);
@@ -240,6 +254,20 @@ 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)
@@ -259,10 +287,10 @@ internal static class ElkEdgeRouterAStar8Dir
private static double ComputeSoftObstacleCost(
double x1, double y1, double x2, double y2,
IReadOnlyList<OrthogonalSoftObstacle> softObstacles,
SoftObstacleInfo[] softObstacles,
AStarRoutingParams routingParams)
{
if (routingParams.SoftObstacleWeight <= 0d || softObstacles.Count == 0)
if (routingParams.SoftObstacleWeight <= 0d || softObstacles.Length == 0)
{
return 0d;
}
@@ -271,10 +299,26 @@ internal static class ElkEdgeRouterAStar8Dir
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;
@@ -284,7 +328,7 @@ internal static class ElkEdgeRouterAStar8Dir
// Graduated proximity: closer = exponentially more expensive
var dist = ComputeParallelDistance(
x1, y1, x2, y2, candidateIsH, candidateIsV,
obstacle.Start, obstacle.End,
obstacle,
routingParams.SoftObstacleClearance);
if (dist >= 0d)
@@ -300,41 +344,202 @@ internal static class ElkEdgeRouterAStar8Dir
private static double ComputeParallelDistance(
double x1, double y1, double x2, double y2,
bool candidateIsH, bool candidateIsV,
ElkPoint obStart, ElkPoint obEnd,
SoftObstacleInfo obstacle,
double clearance)
{
var obIsH = Math.Abs(obStart.Y - obEnd.Y) < 2d;
var obIsV = Math.Abs(obStart.X - obEnd.X) < 2d;
if (candidateIsH && obIsH)
if (candidateIsH && obstacle.IsHorizontal)
{
var dist = Math.Abs(y1 - obStart.Y);
var dist = Math.Abs(y1 - obstacle.Start.Y);
if (dist >= clearance)
{
return -1d;
}
var overlapMin = Math.Max(Math.Min(x1, x2), Math.Min(obStart.X, obEnd.X));
var overlapMax = Math.Min(Math.Max(x1, x2), Math.Max(obStart.X, obEnd.X));
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 && obIsV)
if (candidateIsV && obstacle.IsVertical)
{
var dist = Math.Abs(x1 - obStart.X);
var dist = Math.Abs(x1 - obstacle.Start.X);
if (dist >= clearance)
{
return -1d;
}
var overlapMin = Math.Max(Math.Min(y1, y2), Math.Min(obStart.Y, obEnd.Y));
var overlapMax = Math.Min(Math.Max(y1, y2), Math.Max(obStart.Y, obEnd.Y));
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(
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,
@@ -391,4 +596,41 @@ internal static class ElkEdgeRouterAStar8Dir
}
}
}
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];
}
}
}