Improve rendering
This commit is contained in:
master
159
src/__Libraries/StellaOps.ElkSharp/ElkEdgePostProcessorAStar.cs
Normal file
159
src/__Libraries/StellaOps.ElkSharp/ElkEdgePostProcessorAStar.cs
Normal file
@@ -0,0 +1,159 @@
|
||||
namespace StellaOps.ElkSharp;
|
||||
|
||||
internal static class ElkEdgePostProcessorAStar
|
||||
{
|
||||
internal static List<ElkPoint>? RerouteWithGridAStar(
|
||||
ElkPoint start, ElkPoint end,
|
||||
(double Left, double Top, double Right, double Bottom, string Id)[] obstacles,
|
||||
string sourceId, string targetId,
|
||||
double margin)
|
||||
{
|
||||
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 - margin);
|
||||
xs.Add(ob.Right + margin);
|
||||
ys.Add(ob.Top - margin);
|
||||
ys.Add(ob.Bottom + margin);
|
||||
}
|
||||
|
||||
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 IsBlocked(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)
|
||||
{
|
||||
var segX = x1;
|
||||
if (segX > ob.Left && segX < 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)
|
||||
{
|
||||
var segY = y1;
|
||||
if (segY > ob.Top && segY < 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;
|
||||
}
|
||||
|
||||
// A* with (ix, iy, direction) state; direction: 0=none, 1=horizontal, 2=vertical
|
||||
const double bendPenalty = 200d;
|
||||
var stateCount = xCount * yCount * 3;
|
||||
var gScore = new double[stateCount];
|
||||
Array.Fill(gScore, double.MaxValue);
|
||||
var cameFrom = new int[stateCount];
|
||||
Array.Fill(cameFrom, -1);
|
||||
|
||||
int StateId(int ix, int iy, int dir) => (ix * yCount + iy) * 3 + dir;
|
||||
double Heuristic(int ix, int iy) =>
|
||||
Math.Abs(xArr[ix] - xArr[endIx]) + Math.Abs(yArr[iy] - yArr[endIy]);
|
||||
|
||||
var startState = StateId(startIx, startIy, 0);
|
||||
gScore[startState] = 0d;
|
||||
var openSet = new PriorityQueue<int, double>();
|
||||
openSet.Enqueue(startState, Heuristic(startIx, startIy));
|
||||
|
||||
var dx = new[] { 1, -1, 0, 0 };
|
||||
var dy = new[] { 0, 0, 1, -1 };
|
||||
var dirs = new[] { 1, 1, 2, 2 };
|
||||
|
||||
var maxIterations = xCount * yCount * 6;
|
||||
var iterations = 0;
|
||||
var closed = new HashSet<int>();
|
||||
while (openSet.Count > 0 && iterations++ < maxIterations)
|
||||
{
|
||||
var current = openSet.Dequeue();
|
||||
|
||||
if (!closed.Add(current))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var curDir = current % 3;
|
||||
var curIy = (current / 3) % yCount;
|
||||
var curIx = (current / 3) / yCount;
|
||||
|
||||
if (curIx == endIx && curIy == endIy)
|
||||
{
|
||||
var path = new List<ElkPoint>();
|
||||
var state = current;
|
||||
while (state >= 0)
|
||||
{
|
||||
var sIy = (state / 3) % yCount;
|
||||
var sIx = (state / 3) / yCount;
|
||||
path.Add(new ElkPoint { X = xArr[sIx], Y = yArr[sIy] });
|
||||
state = cameFrom[state];
|
||||
}
|
||||
|
||||
path.Reverse();
|
||||
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];
|
||||
if (Math.Abs(prev.X - path[i].X) > 0.5d || Math.Abs(path[i].X - next.X) > 0.5d)
|
||||
{
|
||||
if (Math.Abs(prev.Y - path[i].Y) > 0.5d || Math.Abs(path[i].Y - next.Y) > 0.5d)
|
||||
{
|
||||
simplified.Add(path[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
simplified.Add(path[^1]);
|
||||
return simplified;
|
||||
}
|
||||
|
||||
for (var d = 0; d < 4; d++)
|
||||
{
|
||||
var nx = curIx + dx[d];
|
||||
var ny = curIy + dy[d];
|
||||
if (nx < 0 || nx >= xCount || ny < 0 || ny >= yCount) continue;
|
||||
if (IsBlocked(curIx, curIy, nx, ny)) continue;
|
||||
|
||||
var newDir = dirs[d];
|
||||
var bend = (curDir != 0 && curDir != newDir) ? bendPenalty : 0d;
|
||||
var dist = Math.Abs(xArr[nx] - xArr[curIx]) + Math.Abs(yArr[ny] - yArr[curIy]);
|
||||
var tentativeG = gScore[current] + dist + bend;
|
||||
var neighborState = StateId(nx, ny, newDir);
|
||||
|
||||
if (tentativeG < gScore[neighborState])
|
||||
{
|
||||
gScore[neighborState] = tentativeG;
|
||||
cameFrom[neighborState] = current;
|
||||
var f = tentativeG + Heuristic(nx, ny);
|
||||
openSet.Enqueue(neighborState, f);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return null;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user