287 lines
9.4 KiB
C#
287 lines
9.4 KiB
C#
namespace StellaOps.ElkSharp;
|
|
|
|
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)
|
|
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 obstacle in obstacles)
|
|
{
|
|
if (obstacle.Id == sourceId || obstacle.Id == targetId)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
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)
|
|
{
|
|
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 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);
|
|
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)
|
|
{
|
|
if (ix1 == ix2)
|
|
{
|
|
var minIy = Math.Min(iy1, iy2);
|
|
var maxIy = Math.Max(iy1, iy2);
|
|
for (var iy = minIy; iy < maxIy; iy++)
|
|
{
|
|
if (blockedSegments.IsVerticalBlocked(ix1, iy))
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
|
|
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 (blockedSegments.IsHorizontalBlocked(ix, iy1))
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
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);
|
|
|
|
var blockedEntryDir = 0;
|
|
if (routingParams.EnforceEntryAngle)
|
|
{
|
|
foreach (var obstacle in obstacles)
|
|
{
|
|
if (obstacle.Id != targetId)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
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;
|
|
}
|
|
else if (Math.Abs(end.Y - nodeTop) < 2d || Math.Abs(end.Y - nodeBottom) < 2d)
|
|
{
|
|
blockedEntryDir = 1;
|
|
}
|
|
|
|
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 bool[stateCount];
|
|
|
|
while (openSet.Count > 0 && iterations++ < maxIterations)
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
var current = openSet.Dequeue();
|
|
if (closed[current])
|
|
{
|
|
continue;
|
|
}
|
|
|
|
closed[current] = true;
|
|
|
|
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;
|
|
}
|
|
|
|
if (yArr[curIy] > disallowedBottomY || yArr[ny] > disallowedBottomY)
|
|
{
|
|
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];
|
|
if (blockedEntryDir > 0 && nx == endIx && ny == endIy && newDir == blockedEntryDir)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
double dist;
|
|
if (isDiagonal)
|
|
{
|
|
var ddx = xArr[nx] - xArr[curIx];
|
|
var ddy = yArr[ny] - yArr[curIy];
|
|
var diagonalStepLength = Math.Sqrt(ddx * ddx + ddy * ddy);
|
|
if (diagonalStepLength > maxDiagonalStepLength)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
dist = diagonalStepLength + routingParams.DiagonalPenalty;
|
|
}
|
|
else
|
|
{
|
|
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);
|
|
|
|
if (tentativeG < gScore[neighborState])
|
|
{
|
|
gScore[neighborState] = tentativeG;
|
|
cameFrom[neighborState] = current;
|
|
openSet.Enqueue(neighborState, tentativeG + Heuristic(nx, ny));
|
|
}
|
|
}
|
|
}
|
|
|
|
return null;
|
|
}
|
|
|
|
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 stateY = (state / dirCount) % yCount;
|
|
var stateX = (state / dirCount) / yCount;
|
|
path.Add(new ElkPoint { X = xArr[stateX], Y = yArr[stateY] });
|
|
state = cameFrom[state];
|
|
}
|
|
|
|
path.Reverse();
|
|
|
|
var simplified = new List<ElkPoint> { path[0] };
|
|
for (var i = 1; i < path.Count - 1; i++)
|
|
{
|
|
var previous = simplified[^1];
|
|
var next = path[i + 1];
|
|
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)
|
|
{
|
|
simplified.Add(path[i]);
|
|
}
|
|
}
|
|
|
|
simplified.Add(path[^1]);
|
|
return simplified;
|
|
}
|
|
}
|