/// <summary> /// Method that switfly finds the best path from start to end. Doesn't reverse outcome /// </summary> /// <returns>The end breadcrump where each .next is a step back)</returns> private static BreadCrumb FindPathReversed(World world, Point3D start, Point3D end) { MinHeap<BreadCrumb> openList = new MinHeap<BreadCrumb>(256); BreadCrumb[, ,] brWorld = new BreadCrumb[world.Right, world.Top, world.Back]; BreadCrumb node; Point3D tmp; int cost; int diff; int count; BreadCrumb current = new BreadCrumb(start); current.cost = 0; BreadCrumb finish = new BreadCrumb(end); if (current.position.X < 0 || current.position.X > brWorld.GetUpperBound(0) || current.position.Y < 0 || current.position.Y > brWorld.GetUpperBound(0)) return null; brWorld[current.position.X, current.position.Y, current.position.Z] = current; openList.Add(current); count = 0; while (openList.Count > 0) { //Find best item and switch it to the 'closedList' current = openList.ExtractFirst(); current.onClosedList = true; //Find neighbours for (int i = 0; i < surrounding.Length; i++) { tmp = current.position + surrounding[i]; if (world.PositionIsFree(tmp) || (tmp.X==end.X && tmp.Y==end.Y && tmp.Z==end.Z)) { //Check if we've already examined a neighbour, if not create a new node for it. if (brWorld[tmp.X, tmp.Y, tmp.Z] == null) { node = new BreadCrumb(tmp); brWorld[tmp.X, tmp.Y, tmp.Z] = node; } else { node = brWorld[tmp.X, tmp.Y, tmp.Z]; } //If the node is not on the 'closedList' check it's new score, keep the best if (!node.onClosedList) { diff = 0; if (current.position.X != node.position.X) { diff += 1; } if (current.position.Y != node.position.Y) { diff += 1; } if (current.position.Z != node.position.Z) { diff += 1; } cost = current.cost + diff + node.position.GetDistanceSquared(end); if (cost < node.cost) { node.cost = cost; node.next = current; } //If the node wasn't on the openList yet, add it if (!node.onOpenList) { //Check to see if we're done if (node.Equals(finish)) { node.next = current; return node; } node.onOpenList = true; openList.Add(node); } } } } count++; if (count > 1000) return null; } return null; //no path found }
public void GetAStarData() { AStarWorld = null; AStarWorld = new AStar.World(Width, Height); foreach (var layer in Layers) { if (layer.GetType() == typeof(TileLayer)) { if (layer.Properties.Contains("Collision")) { TileLayer tl = (TileLayer)layer; for (int y = 0; y < Height; y++) { for (int x = 0; x < Width; x++) { if (tl.Tiles[x, y] != null) AStarWorld.MarkPosition(new AStar.Point3D(x, y, 0), true); } } } } } }
/// <summary> /// Method that switfly finds the best path from start to end. /// </summary> /// <returns>The starting breadcrumb traversable via .next to the end or null if there is no path</returns> public static BreadCrumb FindPath(World world, Point3D start, Point3D end) { //note we just flip start and end here so you don't have to. return FindPathReversed(world, end, start); }