public int GetDistanceSquared(Point3D point) { int dx = this.X - point.X; int dy = this.Y - point.Y; int dz = this.Z - point.Z; return (dx * dx) + (dy * dy) + (dz * dz); }
/// <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 }
/// <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); }
/// <summary> /// Checks if a position is free or marked (and legal) /// </summary> /// <returns>true if the position is free</returns> public bool PositionIsFree(Point3D position) { return position.X >= 0 && position.X < worldBlocked.GetLength(0) && position.Y >= 0 && position.Y < worldBlocked.GetLength(1) && position.Z >= 0 && position.Z < worldBlocked.GetLength(2) && !worldBlocked[position.X, position.Y, position.Z]; }
/// <summary> /// Mark positions in the world als blocked (true) or unblocked (false) /// </summary> /// <param name="value">use true if you wan't to block the value</param> public void MarkPosition(Point3D position, bool value) { worldBlocked[position.X, position.Y, position.Z] = value; }