elksharp stabilization
This commit is contained in:
master
394
src/__Libraries/StellaOps.ElkSharp/ElkEdgeRouterAStar8Dir.cs
Normal file
394
src/__Libraries/StellaOps.ElkSharp/ElkEdgeRouterAStar8Dir.cs
Normal file
@@ -0,0 +1,394 @@
|
||||
namespace StellaOps.ElkSharp;
|
||||
|
||||
internal static 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)
|
||||
private static readonly int[] DirCodes = [1, 1, 2, 2, 3, 3, 4, 4];
|
||||
|
||||
internal static List<ElkPoint>? Route(
|
||||
ElkPoint start,
|
||||
ElkPoint end,
|
||||
(double Left, double Top, double Right, double Bottom, string Id)[] obstacles,
|
||||
string sourceId,
|
||||
string targetId,
|
||||
AStarRoutingParams routingParams,
|
||||
IReadOnlyList<OrthogonalSoftObstacle> softObstacles,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
var xs = new SortedSet<double> { start.X, end.X };
|
||||
var ys = new SortedSet<double> { start.Y, end.Y };
|
||||
foreach (var ob in obstacles)
|
||||
{
|
||||
if (ob.Id == sourceId || ob.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);
|
||||
}
|
||||
|
||||
if (routingParams.IntermediateGridSpacing > 0d)
|
||||
{
|
||||
AddIntermediateLines(xs, routingParams.IntermediateGridSpacing);
|
||||
AddIntermediateLines(ys, routingParams.IntermediateGridSpacing);
|
||||
}
|
||||
|
||||
var xArr = xs.ToArray();
|
||||
var yArr = ys.ToArray();
|
||||
var xCount = xArr.Length;
|
||||
var yCount = yArr.Length;
|
||||
if (xCount < 2 || yCount < 2)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var startIx = Array.BinarySearch(xArr, start.X);
|
||||
var startIy = Array.BinarySearch(yArr, start.Y);
|
||||
var endIx = Array.BinarySearch(xArr, end.X);
|
||||
var endIy = Array.BinarySearch(yArr, end.Y);
|
||||
if (startIx < 0 || startIy < 0 || endIx < 0 || endIy < 0)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
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 (ob.Id == sourceId || ob.Id == targetId)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
if (ix1 == ix2)
|
||||
{
|
||||
if (x1 > ob.Left && x1 < ob.Right)
|
||||
{
|
||||
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 false;
|
||||
}
|
||||
|
||||
const int dirCount = 5;
|
||||
var stateCount = xCount * yCount * dirCount;
|
||||
var gScore = new double[stateCount];
|
||||
Array.Fill(gScore, double.MaxValue);
|
||||
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)
|
||||
{
|
||||
if (ob.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;
|
||||
if (Math.Abs(end.X - nodeLeft) < 2d || Math.Abs(end.X - nodeRight) < 2d)
|
||||
{
|
||||
blockedEntryDir = 2; // vertical side → block vertical
|
||||
}
|
||||
else if (Math.Abs(end.Y - nodeTop) < 2d || Math.Abs(end.Y - nodeBottom) < 2d)
|
||||
{
|
||||
blockedEntryDir = 1; // horizontal side → block horizontal
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
int StateId(int ix, int iy, int dir) => (ix * yCount + iy) * dirCount + dir;
|
||||
|
||||
double Heuristic(int ix, int iy)
|
||||
{
|
||||
var hdx = xArr[ix] - xArr[endIx];
|
||||
var hdy = yArr[iy] - yArr[endIy];
|
||||
return Math.Sqrt(hdx * hdx + hdy * hdy);
|
||||
}
|
||||
|
||||
var startState = StateId(startIx, startIy, 0);
|
||||
gScore[startState] = 0d;
|
||||
var openSet = new PriorityQueue<int, double>();
|
||||
openSet.Enqueue(startState, Heuristic(startIx, startIy));
|
||||
|
||||
var maxIterations = xCount * yCount * 12;
|
||||
var iterations = 0;
|
||||
var closed = new HashSet<int>();
|
||||
|
||||
while (openSet.Count > 0 && iterations++ < maxIterations)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
var current = openSet.Dequeue();
|
||||
if (!closed.Add(current))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var curDir = current % dirCount;
|
||||
var curIy = (current / dirCount) % yCount;
|
||||
var curIx = (current / dirCount) / yCount;
|
||||
|
||||
if (curIx == endIx && curIy == endIy)
|
||||
{
|
||||
return ReconstructPath(current, cameFrom, xArr, yArr, yCount, dirCount);
|
||||
}
|
||||
|
||||
for (var d = 0; d < 8; d++)
|
||||
{
|
||||
var nx = curIx + Dx[d];
|
||||
var ny = curIy + Dy[d];
|
||||
if (nx < 0 || nx >= xCount || ny < 0 || ny >= yCount)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var isDiagonal = Dx[d] != 0 && Dy[d] != 0;
|
||||
if (isDiagonal)
|
||||
{
|
||||
if (IsBlockedOrthogonal(curIx, curIy, nx, curIy)
|
||||
|| IsBlockedOrthogonal(curIx, curIy, curIx, ny))
|
||||
{
|
||||
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)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var bend = ComputeBendPenalty(curDir, newDir, routingParams.BendPenalty);
|
||||
|
||||
double dist;
|
||||
if (isDiagonal)
|
||||
{
|
||||
var ddx = xArr[nx] - xArr[curIx];
|
||||
var ddy = yArr[ny] - yArr[curIy];
|
||||
dist = Math.Sqrt(ddx * ddx + ddy * ddy) + routingParams.DiagonalPenalty;
|
||||
}
|
||||
else
|
||||
{
|
||||
dist = Math.Abs(xArr[nx] - xArr[curIx]) + Math.Abs(yArr[ny] - yArr[curIy]);
|
||||
}
|
||||
|
||||
var softCost = ComputeSoftObstacleCost(
|
||||
xArr[curIx], yArr[curIy], xArr[nx], yArr[ny],
|
||||
softObstacles, routingParams);
|
||||
|
||||
var tentativeG = gScore[current] + dist + bend + softCost;
|
||||
var neighborState = StateId(nx, ny, newDir);
|
||||
|
||||
if (tentativeG < gScore[neighborState])
|
||||
{
|
||||
gScore[neighborState] = tentativeG;
|
||||
cameFrom[neighborState] = current;
|
||||
openSet.Enqueue(neighborState, tentativeG + Heuristic(nx, ny));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return null;
|
||||
}
|
||||
|
||||
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,
|
||||
IReadOnlyList<OrthogonalSoftObstacle> softObstacles,
|
||||
AStarRoutingParams routingParams)
|
||||
{
|
||||
if (routingParams.SoftObstacleWeight <= 0d || softObstacles.Count == 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 cost = 0d;
|
||||
|
||||
foreach (var obstacle in softObstacles)
|
||||
{
|
||||
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.Start, obstacle.End,
|
||||
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,
|
||||
ElkPoint obStart, ElkPoint obEnd,
|
||||
double clearance)
|
||||
{
|
||||
var obIsH = Math.Abs(obStart.Y - obEnd.Y) < 2d;
|
||||
var obIsV = Math.Abs(obStart.X - obEnd.X) < 2d;
|
||||
|
||||
if (candidateIsH && obIsH)
|
||||
{
|
||||
var dist = Math.Abs(y1 - obStart.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));
|
||||
return overlapMax > overlapMin + 1d ? dist : -1d;
|
||||
}
|
||||
|
||||
if (candidateIsV && obIsV)
|
||||
{
|
||||
var dist = Math.Abs(x1 - obStart.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));
|
||||
return overlapMax > overlapMin + 1d ? dist : -1d;
|
||||
}
|
||||
|
||||
return -1d;
|
||||
}
|
||||
|
||||
private static List<ElkPoint> ReconstructPath(
|
||||
int endState, int[] cameFrom,
|
||||
double[] xArr, double[] yArr,
|
||||
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] });
|
||||
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 next = path[i + 1];
|
||||
var dx1 = Math.Sign(path[i].X - prev.X);
|
||||
var dy1 = Math.Sign(path[i].Y - prev.Y);
|
||||
var dx2 = Math.Sign(next.X - path[i].X);
|
||||
var dy2 = Math.Sign(next.Y - path[i].Y);
|
||||
if (dx1 != dx2 || dy1 != dy2)
|
||||
{
|
||||
simplified.Add(path[i]);
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user