private static int GetDistance(Vector2IHeapable NodeA, Vector2IHeapable NodeB) { int DstX = Mathf.Abs(NodeA.GridX - NodeB.GridX); int DstY = Mathf.Abs(NodeA.GridY - NodeB.GridY); if (DstX > DstY) { return(14 * DstY + 10 * (DstX - DstY)); } else { return(14 * DstX + 10 * (DstY - DstX)); } }
private static Vector2IHeapable[] SimplifyPath(List <Vector2IHeapable> path) { List <Vector2IHeapable> waypoints = new List <Vector2IHeapable>(); Vector2IHeapable DirectionOld = Vector2I.Zero; for (int i = 1; i < path.Count; i++) { Vector2IHeapable DirectionNew = new Vector2I(path[i - 1].GridX - path[i].GridX, path[i - 1].GridY - path[i].GridY); if (DirectionNew != DirectionOld) { waypoints.Add(path[i]); } DirectionOld = DirectionNew; } return(waypoints.ToArray()); }
private static Vector2IHeapable[] RetracePath(Vector2IHeapable StartNode, Vector2IHeapable EndNode) { List <Vector2IHeapable> Path = new List <Vector2IHeapable> { EndNode }; Vector2IHeapable CurrentNode = EndNode; while (CurrentNode != StartNode) { Path.Add(CurrentNode); CurrentNode = CurrentNode.NodeParent; } Vector2IHeapable[] Waypoints = SimplifyPath(Path); Array.Reverse(Waypoints); return(Waypoints); }
public static List <Vector2IHeapable> GetNeighbors( Vector2IHeapable TilePoint, Map map, Dictionary <Vector2I, Vector2IHeapable> dictionary, bool onlyGetPlus = false) { List <Vector2IHeapable> Neighbors = new List <Vector2IHeapable>(); for (int x = -1; x <= 1; x++) { for (int y = -1; y <= 1; y++) { if (x == 0 && y == 0) { continue; } if (onlyGetPlus) { if ((y == 1 && x == 1) || (y == -1 && x == 1) || (y == 1 && x == -1) || (y == -1 && x == -1)) { continue; } } var coord = new Vector2I(TilePoint.GridX + x, TilePoint.GridY + y); if (map.CoordinatesInMap(coord)) { Neighbors.Add(dictionary[coord]); } } } return(Neighbors); }
public static PathFindingResult FindPath(Vector2I StartPosition, Vector2I TargetPosition, Map map) { var dictionary = new Dictionary <Vector2I, Vector2IHeapable>(map.TotalCount); for (var x = 0; x < map.Width; x++) { for (var y = 0; y < map.Height; y++) { var vec2 = new Vector2I(x, y); dictionary.Add(vec2, vec2); } } bool IsPathSuccess = false; dictionary[StartPosition].IsWalkable = map[StartPosition].IsWalkable(); dictionary[StartPosition].Position = StartPosition; dictionary[StartPosition].NodeParent = null; dictionary[TargetPosition].IsWalkable = map[TargetPosition].IsWalkable(); dictionary[TargetPosition].Position = TargetPosition; dictionary[TargetPosition].NodeParent = null; var StartNode = dictionary[StartPosition]; var TargetNode = dictionary[TargetPosition]; Heap <Vector2IHeapable> OpenSet = new Heap <Vector2IHeapable>(map.TotalCount); HashSet <Vector2IHeapable> ClosedSet = new HashSet <Vector2IHeapable>(); if (StartNode.IsWalkable && TargetNode.IsWalkable) { OpenSet.Add(StartNode); while (OpenSet.Count > 0) { Vector2IHeapable CurrentNode = OpenSet.RemoveFirst(); ClosedSet.Add(CurrentNode); if (CurrentNode.Position == TargetNode.Position) { IsPathSuccess = true; ClosedSet.Add(TargetNode); break; } var neighbors = GetNeighbors(CurrentNode, map, dictionary, true); foreach (var neighbor in neighbors) { if (!map[neighbor].IsWalkable() || ClosedSet.Contains(neighbor)) { continue; } int NewMovementCostToNeighbor = CurrentNode.Gcost + GetDistance(CurrentNode, neighbor); if (NewMovementCostToNeighbor < neighbor.Gcost || !OpenSet.Contains(neighbor)) { neighbor.Gcost = NewMovementCostToNeighbor; neighbor.Hcost = GetDistance(neighbor, TargetNode); neighbor.NodeParent = CurrentNode; if (!OpenSet.Contains(neighbor)) { OpenSet.Add(neighbor); OpenSet.UpdateItem(neighbor); } } } } } IEnumerable <GridPosition> waypoints = new GridPosition[0]; if (IsPathSuccess) { waypoints = RetracePath(StartNode, TargetNode).Select(a => (GridPosition)a.Position); } return(new PathFindingResult { Path = new TilePath(waypoints), IsPathSuccess = IsPathSuccess }); }