From fd440f29ce00ab34cbf809c2bd14509bc23e88da Mon Sep 17 00:00:00 2001 From: Kamron Batman <3953314+kamronbatman@users.noreply.github.com> Date: Thu, 16 Sep 2021 23:38:51 -0700 Subject: [PATCH] fix: Fixes FastAStar (#788) * Fixes FastAStar algorithm, thanks @nullptr-w8 * Cleans up the code a little * Removes SlowAStar * Changes [Path recall runes to waypoints --- .../Engines/Pathing/FastAStarAlgorithm.cs | 145 ++++---- .../UOContent/Engines/Pathing/MovementPath.cs | 9 +- .../Engines/Pathing/SlowAStarAlgorithm.cs | 314 ------------------ .../Items/Misc/{Waypoint.cs => WayPoint.cs} | 1 + 4 files changed, 77 insertions(+), 392 deletions(-) delete mode 100644 Projects/UOContent/Engines/Pathing/SlowAStarAlgorithm.cs rename Projects/UOContent/Items/Misc/{Waypoint.cs => WayPoint.cs} (99%) diff --git a/Projects/UOContent/Engines/Pathing/FastAStarAlgorithm.cs b/Projects/UOContent/Engines/Pathing/FastAStarAlgorithm.cs index cad99c5f7..cd3e4cca4 100644 --- a/Projects/UOContent/Engines/Pathing/FastAStarAlgorithm.cs +++ b/Projects/UOContent/Engines/Pathing/FastAStarAlgorithm.cs @@ -24,22 +24,23 @@ namespace Server.PathAlgorithms.FastAStar private const int PlaneHeight = 20; public static PathAlgorithm Instance = new FastAStarAlgorithm(); - private static readonly Direction[] m_Path = new Direction[AreaSize * AreaSize]; - private static readonly PathNode[] m_Nodes = new PathNode[NodeCount]; - private static readonly BitArray m_Touched = new(NodeCount); - private static readonly BitArray m_OnOpen = new(NodeCount); - private static readonly int[] m_Successors = new int[8]; + private static readonly Direction[] _path = new Direction[AreaSize * AreaSize]; + private static readonly PathNode[] _nodes = new PathNode[NodeCount]; + private static readonly BitArray _touched = new(NodeCount); + private static readonly BitArray _onOpen = new(NodeCount); + private static readonly int[] _successors = new int[8]; - private static int m_xOffset, m_yOffset; - private static int m_OpenList; + private static int _xOffset; + private static int _yOffset; + private static int _openList; - private Point3D m_Goal; + private Point3D _goal; public int Heuristic(int x, int y, int z) { - x -= m_Goal.X - m_xOffset; - y -= m_Goal.Y - m_yOffset; - z -= m_Goal.Z; + x -= _goal.X - _xOffset; + y -= _goal.Y - _yOffset; + z -= _goal.Z; x *= 11; y *= 11; @@ -52,59 +53,59 @@ namespace Server.PathAlgorithms.FastAStar private void RemoveFromChain(int node) { - if (node < 0 || node >= NodeCount) + if (node is < 0 or >= NodeCount) { return; } - if (!m_Touched[node] || !m_OnOpen[node]) + if (!_touched[node] || !_onOpen[node]) { return; } - var prev = m_Nodes[node].prev; - var next = m_Nodes[node].next; + var prev = _nodes[node].prev; + var next = _nodes[node].next; - if (m_OpenList == node) + if (_openList == node) { - m_OpenList = next; + _openList = next; } if (prev != -1) { - m_Nodes[prev].next = next; + _nodes[prev].next = next; } if (next != -1) { - m_Nodes[next].prev = prev; + _nodes[next].prev = prev; } - m_Nodes[node].prev = -1; - m_Nodes[node].next = -1; + _nodes[node].prev = -1; + _nodes[node].next = -1; } private void AddToChain(int node) { - if (node < 0 || node >= NodeCount) + if (node is < 0 or >= NodeCount) { return; } RemoveFromChain(node); - if (m_OpenList != -1) + if (_openList != -1) { - m_Nodes[m_OpenList].prev = node; + _nodes[_openList].prev = node; } - m_Nodes[node].next = m_OpenList; - m_Nodes[node].prev = -1; + _nodes[node].next = _openList; + _nodes[node].prev = -1; - m_OpenList = node; + _openList = node; - m_Touched[node] = true; - m_OnOpen[node] = true; + _touched[node] = true; + _onOpen[node] = true; } public override Direction[] Find(Mobile m, Map map, Point3D start, Point3D goal) @@ -114,37 +115,38 @@ namespace Server.PathAlgorithms.FastAStar return null; } - m_Touched.SetAll(false); + _touched.SetAll(false); + _onOpen.SetAll(false); - m_Goal = goal; + _goal = goal; - m_xOffset = (start.X + goal.X - AreaSize) / 2; - m_yOffset = (start.Y + goal.Y - AreaSize) / 2; + _xOffset = (start.X + goal.X - AreaSize) / 2; + _yOffset = (start.Y + goal.Y - AreaSize) / 2; var fromNode = GetIndex(start.X, start.Y, start.Z); var destNode = GetIndex(goal.X, goal.Y, goal.Z); - m_OpenList = fromNode; + _openList = fromNode; - m_Nodes[m_OpenList].cost = 0; - m_Nodes[m_OpenList].total = Heuristic(start.X - m_xOffset, start.Y - m_yOffset, start.Z); - m_Nodes[m_OpenList].parent = -1; - m_Nodes[m_OpenList].next = -1; - m_Nodes[m_OpenList].prev = -1; - m_Nodes[m_OpenList].z = start.Z; + _nodes[_openList].cost = 0; + _nodes[_openList].total = Heuristic(start.X - _xOffset, start.Y - _yOffset, start.Z); + _nodes[_openList].parent = -1; + _nodes[_openList].next = -1; + _nodes[_openList].prev = -1; + _nodes[_openList].z = start.Z; - m_OnOpen[m_OpenList] = true; - m_Touched[m_OpenList] = true; + _onOpen[_openList] = true; + _touched[_openList] = true; var bc = m as BaseCreature; int backtrack = 0, depth = 0; - var path = m_Path; + var path = _path; - while (m_OpenList != -1) + while (_openList != -1) { - var bestNode = FindBest(m_OpenList); + var bestNode = FindBest(_openList); if (++depth > MaxDepth) { @@ -159,7 +161,7 @@ namespace Server.PathAlgorithms.FastAStar MoveImpl.Goal = goal; - var vals = m_Successors; + var vals = _successors; var count = GetSuccessors(bestNode, m, map); MoveImpl.AlwaysIgnoreDoors = false; @@ -175,30 +177,25 @@ namespace Server.PathAlgorithms.FastAStar { var newNode = vals[i]; - var wasTouched = m_Touched[newNode]; + var wasTouched = _touched[newNode]; if (wasTouched) { continue; } - var newCost = m_Nodes[bestNode].cost + 1; + var newCost = _nodes[bestNode].cost + 1; var newTotal = newCost + Heuristic( newNode % AreaSize, newNode / AreaSize % AreaSize, - m_Nodes[newNode].z + _nodes[newNode].z ); - if (m_Nodes[newNode].total <= newTotal) - { - continue; - } + _nodes[newNode].parent = bestNode; + _nodes[newNode].cost = newCost; + _nodes[newNode].total = newTotal; - m_Nodes[newNode].parent = bestNode; - m_Nodes[newNode].cost = newCost; - m_Nodes[newNode].total = newTotal; - - if (m_OnOpen[newNode]) + if (_onOpen[newNode]) { continue; } @@ -211,7 +208,7 @@ namespace Server.PathAlgorithms.FastAStar } var pathCount = 0; - var parent = m_Nodes[newNode].parent; + var parent = _nodes[newNode].parent; while (parent != -1) { @@ -222,7 +219,7 @@ namespace Server.PathAlgorithms.FastAStar newNode / AreaSize % AreaSize ); newNode = parent; - parent = m_Nodes[newNode].parent; + parent = _nodes[newNode].parent; if (newNode == fromNode) { @@ -246,8 +243,8 @@ namespace Server.PathAlgorithms.FastAStar private int GetIndex(int x, int y, int z) { - x -= m_xOffset; - y -= m_yOffset; + x -= _xOffset; + y -= _yOffset; z += PlaneOffset; z /= PlaneHeight; @@ -256,24 +253,24 @@ namespace Server.PathAlgorithms.FastAStar private int FindBest(int node) { - var least = m_Nodes[node].total; + var least = _nodes[node].total; var leastNode = node; while (node != -1) { - if (m_Nodes[node].total < least) + if (_nodes[node].total < least) { - least = m_Nodes[node].total; + least = _nodes[node].total; leastNode = node; } - node = m_Nodes[node].next; + node = _nodes[node].next; } RemoveFromChain(leastNode); - m_Touched[leastNode] = true; - m_OnOpen[leastNode] = false; + _touched[leastNode] = true; + _onOpen[leastNode] = false; return leastNode; } @@ -282,11 +279,11 @@ namespace Server.PathAlgorithms.FastAStar { var px = p % AreaSize; var py = p / AreaSize % AreaSize; - var pz = m_Nodes[p].z; + var pz = _nodes[p].z; - var p3D = new Point3D(px + m_xOffset, py + m_yOffset, pz); + var p3D = new Point3D(px + _xOffset, py + _yOffset, pz); - var vals = m_Successors; + var vals = _successors; var count = 0; for (var i = 0; i < 8; ++i) @@ -332,18 +329,18 @@ namespace Server.PathAlgorithms.FastAStar x += px; y += py; - if (x < 0 || x >= AreaSize || y < 0 || y >= AreaSize) + if (x is < 0 or >= AreaSize || y is < 0 or >= AreaSize) { continue; } if (CalcMoves.CheckMovement(m, map, p3D, (Direction)i, out var z)) { - var idx = GetIndex(x + m_xOffset, y + m_yOffset, z); + var idx = GetIndex(x + _xOffset, y + _yOffset, z); if (idx >= 0 && idx < NodeCount) { - m_Nodes[idx].z = z; + _nodes[idx].z = z; vals[count++] = idx; } } diff --git a/Projects/UOContent/Engines/Pathing/MovementPath.cs b/Projects/UOContent/Engines/Pathing/MovementPath.cs index 98663a060..438817c09 100644 --- a/Projects/UOContent/Engines/Pathing/MovementPath.cs +++ b/Projects/UOContent/Engines/Pathing/MovementPath.cs @@ -3,7 +3,6 @@ using System.Diagnostics; using Server.Items; using Server.PathAlgorithms; using Server.PathAlgorithms.FastAStar; -using Server.PathAlgorithms.SlowAStar; using Server.Spells; using Server.Targeting; @@ -89,18 +88,21 @@ namespace Server var y = from.Y; var z = from.Z; + WayPoint waypoint = null; + for (var i = 0; i < path.Directions.Length; ++i) { Movement.Movement.Offset(path.Directions[i], ref x, ref y); - new RecallRune().MoveToWorld(new Point3D(x, y, z + zOffset), from.Map); + waypoint = new WayPoint(waypoint); + waypoint.MoveToWorld(new Point3D(x, y, z + zOffset), from.Map); } } } public static void Path_OnTarget(Mobile from, object targeted) { - if (!(targeted is IPoint3D p)) + if (targeted is not IPoint3D p) { return; } @@ -108,7 +110,6 @@ namespace Server SpellHelper.GetSurfaceTop(ref p); Path(from, p, FastAStarAlgorithm.Instance, "Fast", 0); - Path(from, p, SlowAStarAlgorithm.Instance, "Slow", 2); OverrideAlgorithm = null; } } diff --git a/Projects/UOContent/Engines/Pathing/SlowAStarAlgorithm.cs b/Projects/UOContent/Engines/Pathing/SlowAStarAlgorithm.cs deleted file mode 100644 index 6299b1863..000000000 --- a/Projects/UOContent/Engines/Pathing/SlowAStarAlgorithm.cs +++ /dev/null @@ -1,314 +0,0 @@ -using System; -using Server.Mobiles; -using CalcMoves = Server.Movement.Movement; -using MoveImpl = Server.Movement.MovementImpl; - -namespace Server.PathAlgorithms.SlowAStar -{ - public struct PathNode - { - public int x, y, z; - public int g, h; - public int px, py, pz; - public int dir; - } - - public class SlowAStarAlgorithm : PathAlgorithm - { - private const int MaxDepth = 300; - private const int MaxNodes = MaxDepth * 16; - public static PathAlgorithm Instance = new SlowAStarAlgorithm(); - - private static readonly PathNode[] m_Closed = new PathNode[MaxNodes]; - private static readonly PathNode[] m_Open = new PathNode[MaxNodes]; - private static readonly PathNode[] m_Successors = new PathNode[8]; - private static readonly Direction[] m_Path = new Direction[MaxNodes]; - - private Point3D m_Goal; - - public int Heuristic(int x, int y, int z) - { - x -= m_Goal.X; - y -= m_Goal.Y; - z -= m_Goal.Z; - - x *= 11; - y *= 11; - - return x * x + y * y + z * z; - } - - public override bool CheckCondition(Mobile m, Map map, Point3D start, Point3D goal) => false; - - public override Direction[] Find(Mobile m, Map map, Point3D start, Point3D goal) - { - m_Goal = goal; - - var bc = m as BaseCreature; - - PathNode curNode; - - var goalNode = new PathNode(); - goalNode.x = goal.X; - goalNode.y = goal.Y; - goalNode.z = goal.Z; - - var startNode = new PathNode(); - startNode.x = start.X; - startNode.y = start.Y; - startNode.z = start.Z; - startNode.h = Heuristic(startNode.x, startNode.y, startNode.z); - - PathNode[] closed = m_Closed, open = m_Open, successors = m_Successors; - var path = m_Path; - - int closedCount = 0, openCount = 0; - var pathCount = 0; - var depth = 0; - - var iBacktrack = 0; - - open[openCount++] = startNode; - - while (openCount > 0) - { - curNode = open[0]; - var curF = curNode.g + curNode.h; - var popIndex = 0; - - for (var i = 1; i < openCount; ++i) - { - if (open[i].g + open[i].h < curF) - { - curNode = open[i]; - curF = curNode.g + curNode.h; - popIndex = i; - } - } - - if (curNode.x == goalNode.x && curNode.y == goalNode.y && (curNode.z - goalNode.z).Abs() < 16) - { - if (closedCount == MaxNodes) - { - break; - } - - closed[closedCount++] = curNode; - - var xBacktrack = curNode.px; - var yBacktrack = curNode.py; - var zBacktrack = curNode.pz; - - path[pathCount++] = (Direction)curNode.dir; - - // if (pathCount == MaxNodes) - // { - // break; - // } - - while (xBacktrack != startNode.x || yBacktrack != startNode.y || zBacktrack != startNode.z) - { - var found = false; - - for (var j = 0; !found && j < closedCount; ++j) - { - if (closed[j].x == xBacktrack && closed[j].y == yBacktrack && closed[j].z == zBacktrack) - { - if (pathCount == MaxNodes) - { - break; - } - - curNode = closed[j]; - path[pathCount++] = (Direction)curNode.dir; - xBacktrack = curNode.px; - yBacktrack = curNode.py; - zBacktrack = curNode.pz; - found = true; - } - } - - if (!found) - { - Console.WriteLine("bugaboo.."); - return null; - } - - if (pathCount == MaxNodes) - { - break; - } - } - - if (pathCount == MaxNodes) - { - break; - } - - var dirs = new Direction[pathCount]; - - while (pathCount > 0) - { - dirs[iBacktrack++] = path[--pathCount]; - } - - return dirs; - } - - --openCount; - - for (var i = popIndex; i < openCount; ++i) - { - open[i] = open[i + 1]; - } - - var sucCount = 0; - - if (bc != null) - { - MoveImpl.AlwaysIgnoreDoors = bc.CanOpenDoors; - MoveImpl.IgnoreMovableImpassables = bc.CanMoveOverObstacles; - } - - MoveImpl.Goal = goal; - - int x; - int y; - int z; - for (var i = 0; i < 8; ++i) - { - switch (i) - { - default: - x = 0; - y = -1; - break; - case 1: - x = 1; - y = -1; - break; - case 2: - x = 1; - y = 0; - break; - case 3: - x = 1; - y = 1; - break; - case 4: - x = 0; - y = 1; - break; - case 5: - x = -1; - y = 1; - break; - case 6: - x = -1; - y = 0; - break; - case 7: - x = -1; - y = -1; - break; - } - - if (CalcMoves.CheckMovement(m, map, new Point3D(curNode.x, curNode.y, curNode.z), (Direction)i, out z)) - { - successors[sucCount].x = x + curNode.x; - successors[sucCount].y = y + curNode.y; - successors[sucCount++].z = z; - } - } - - MoveImpl.AlwaysIgnoreDoors = false; - MoveImpl.IgnoreMovableImpassables = false; - MoveImpl.Goal = Point3D.Zero; - - if (sucCount == 0 || ++depth > MaxDepth) - { - break; - } - - for (var i = 0; i < sucCount; ++i) - { - x = successors[i].x; - y = successors[i].y; - z = successors[i].z; - - successors[i].g = curNode.g + 1; - - int openIndex = -1, closedIndex = -1; - - for (var j = 0; openIndex == -1 && j < openCount; ++j) - { - if (open[j].x == x && open[j].y == y && open[j].z == z) - { - openIndex = j; - } - } - - if (openIndex >= 0 && open[openIndex].g < successors[i].g) - { - continue; - } - - for (var j = 0; closedIndex == -1 && j < closedCount; ++j) - { - if (closed[j].x == x && closed[j].y == y && closed[j].z == z) - { - closedIndex = j; - } - } - - if (closedIndex >= 0 && closed[closedIndex].g < successors[i].g) - { - continue; - } - - if (openIndex >= 0) - { - --openCount; - - for (var j = openIndex; j < openCount; ++j) - { - open[j] = open[j + 1]; - } - } - - if (closedIndex >= 0) - { - --closedCount; - - for (var j = closedIndex; j < closedCount; ++j) - { - closed[j] = closed[j + 1]; - } - } - - successors[i].px = curNode.x; - successors[i].py = curNode.y; - successors[i].pz = curNode.z; - successors[i].dir = (int)GetDirection(curNode.x, curNode.y, x, y); - successors[i].h = Heuristic(x, y, z); - - if (openCount == MaxNodes) - { - break; - } - - open[openCount++] = successors[i]; - } - - if (openCount == MaxNodes || closedCount == MaxNodes) - { - break; - } - - closed[closedCount++] = curNode; - } - - return null; - } - } -} diff --git a/Projects/UOContent/Items/Misc/Waypoint.cs b/Projects/UOContent/Items/Misc/WayPoint.cs similarity index 99% rename from Projects/UOContent/Items/Misc/Waypoint.cs rename to Projects/UOContent/Items/Misc/WayPoint.cs index a7a222fb2..1836fb29b 100644 --- a/Projects/UOContent/Items/Misc/Waypoint.cs +++ b/Projects/UOContent/Items/Misc/WayPoint.cs @@ -13,6 +13,7 @@ namespace Server.Items Hue = 0x498; Visible = false; // this.Movable = false; + if (prev != null) { prev.NextPoint = this;