public BlockInfo(int whatX, int whatY, int whatZ, MapNode.Status whatStatus)
{
x = whatX;
y = whatY;
z = whatZ;
status = whatStatus;
}
/// <summary>
/// Searches a path on a given map. The map data is altered in the process and a path is set on it.
/// </summary>
/// <param name="whatMap">The map to search a path on</param>
/// <returns>Whether a path could be found</returns>
public static SearchResult searchPathOnMap(WorldMap whatMap)
{
if (whatMap.goal == null || whatMap.start == null)
{
throw new ArgumentException("The provided map does either not have a start or a goal.", "whatMap");
}
SearchResult currentResult = SearchResult.None;
//Initialize open list
SortedDictionary <int, List <MapNode> > openList = new SortedDictionary <int, List <MapNode> >();
MapNode cache = whatMap.actualMap[whatMap.start.Item1][whatMap.start.Item2][whatMap.start.Item3];
cache.currentShortestPathToNode = 0;
openList.Add(cache.possibleDistanceToGoal, new List <MapNode>()
{
cache
});
//Very simple test whether it makes sense to search the map at all
if (cache.currentStatus == MapNode.Status.isClosed)
{
currentResult |= SearchResult.MapHasAlreadyBeenSearched;
cache = whatMap.actualMap[whatMap.goal.Item1][whatMap.goal.Item2][whatMap.goal.Item3];
for (int i = 0; i < CIRCLE_TIMEOUT && cache != null && cache != whatMap.start; i++)
{
cache = cache.currentPredesessorNode;
}
if (cache != null)
{
if (cache == whatMap.start)
{
currentResult |= SearchResult.FoundPath;
}
else
{
throw new Exception("Map has been altered to contain a circle as a path");
}
}
}
else
{
//The actual searching. Take the node with the possibly least remaining distance ( estimated ) and expand it
while (openList.Any())
{
cache = openList.First().Value.First();
//remove it from the open list and possibly the entire entry list if empty
openList[cache.possibleDistanceToGoal].Remove(cache);
if (!openList[cache.possibleDistanceToGoal].Any())
{
openList.Remove(cache.possibleDistanceToGoal);
}
//Have we found the goal?
if (cache == whatMap.goal)
{
currentResult = SearchResult.FoundPath;
break;
}
cache.currentStatus |= MapNode.Status.isClosed;
expandNode(cache, openList, whatMap);
}
//Create the path information on the map data ( This is an extra list for simplicity, the main data has been altered already above )
LinkedList <BlockInfo> path = new LinkedList <BlockInfo>();
if (currentResult.HasFlag(SearchResult.FoundPath))
{
cache = whatMap.actualMap[whatMap.goal.Item1][whatMap.goal.Item2][whatMap.goal.Item3];
while (cache.currentPredesessorNode != null)
{
path.AddFirst(new BlockInfo(cache.x, cache.y, cache.z, cache.currentStatus));
cache = cache.currentPredesessorNode;
}
//Add the start aswell even though its predecessor is null ( obviously )
path.AddFirst(new BlockInfo(cache.x, cache.y, cache.z, cache.currentStatus));
}
whatMap.path = path;
}
return(currentResult);
}
/// <summary>
/// Checks all neighbour nodes whether this node provides a shorter ( or new, thats trivially shorter ) path to it
/// and update the data on the map accordingly
/// </summary>
/// <param name="whatNode">The node to expand</param>
/// <param name="openList">A link to the current open list. As this function is just for code readability, this is plainly forwarded</param>
/// <param name="whatMap">The map that is currently searched on</param>
private static void expandNode(MapNode whatNode, SortedDictionary<int, List<MapNode>> openList, WorldMap whatMap)
{
foreach (Tuple<MapNode, int> reachableOpenNode in whatMap.getNeighbours(whatNode))
{
if (reachableOpenNode.Item1.currentShortestPathToNode == null || reachableOpenNode.Item1.currentShortestPathToNode > whatNode.currentShortestPathToNode + reachableOpenNode.Item2)
{
if (reachableOpenNode.Item1.currentStatus.HasFlag(MapNode.Status.isClosed))
throw new Exception("Node is closed but currently known path is not the shortest. If the algorithm is working properly, this is impossible. This could be due to rounding errors but still hasn't been confirmed, so no further check is implemented.");
//Remove the node from the open list because it needs to be put in another sublist. Remove current sublist if empty
if (reachableOpenNode.Item1.currentShortestPathToNode != null)
{
openList[reachableOpenNode.Item1.possibleDistanceToGoal].Remove(reachableOpenNode.Item1);
if (!openList[reachableOpenNode.Item1.possibleDistanceToGoal].Any())
openList.Remove(reachableOpenNode.Item1.possibleDistanceToGoal);
}
//Set the data on the updated node
reachableOpenNode.Item1.currentPredesessorNode = whatNode;
reachableOpenNode.Item1.currentShortestPathToNode = whatNode.currentShortestPathToNode + reachableOpenNode.Item2;
//Put the node in the correct sublist. Create a new sublist if it doesnt already exist
if (!openList.ContainsKey(reachableOpenNode.Item1.possibleDistanceToGoal))
openList.Add(reachableOpenNode.Item1.possibleDistanceToGoal, new List<MapNode>());
openList[reachableOpenNode.Item1.possibleDistanceToGoal].Add(reachableOpenNode.Item1);
}
}
}
public bool Equals(MapNode other)
{
if (other != null)
return x == other.x && y == other.y && z == other.z;
else return false;
}
/// <summary>
/// Gets the list of neighbours for a provided node
/// </summary>
/// <param name="whatNode">the node to search the neighbours for</param>
/// <returns>A list of neighbour nodes</returns>
public List <Tuple <MapNode, int> > getNeighbours(MapNode whatNode)
{
if (whatNode.x > xSizeCache - 1 || whatNode.y > ySizeCache - 1 || whatNode.z > zSizeCache - 1)
{
throw new ArgumentException("The provided node is not within this map", "whatNode");
}
List <Tuple <MapNode, int> > neighbours = new List <Tuple <MapNode, int> >();
MapNode cache;
AddedNeighbours addedCache = AddedNeighbours.None;
#region Straight neighbours
//Add straight x neighbours
if (whatNode.x != xSizeCache - 1)
{
cache = actualMap[whatNode.x + 1][whatNode.y][whatNode.z];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.BLOCK_DISTANCE));
addedCache |= AddedNeighbours.HigherX;
}
}
if (whatNode.x != 0)
{
cache = actualMap[whatNode.x - 1][whatNode.y][whatNode.z];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.BLOCK_DISTANCE));
addedCache |= AddedNeighbours.LowerX;
}
}
//Add straight y neighbours
if (whatNode.y != ySizeCache - 1)
{
cache = actualMap[whatNode.x][whatNode.y + 1][whatNode.z];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.BLOCK_DISTANCE));
addedCache |= AddedNeighbours.HigherY;
}
}
if (whatNode.y != 0)
{
cache = actualMap[whatNode.x][whatNode.y - 1][whatNode.z];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.BLOCK_DISTANCE));
addedCache |= AddedNeighbours.LowerY;
}
}
//Add straight z neighbours
if (whatNode.z != zSizeCache - 1)
{
cache = actualMap[whatNode.x][whatNode.y][whatNode.z + 1];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.BLOCK_DISTANCE));
addedCache |= AddedNeighbours.HigherZ;
}
}
if (whatNode.z != 0)
{
cache = actualMap[whatNode.x][whatNode.y][whatNode.z - 1];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.BLOCK_DISTANCE));
addedCache |= AddedNeighbours.LowerZ;
}
}
#endregion Straight neighbours
#region Diagonal XY plane
//Higher X higher Y
if (whatNode.x != xSizeCache - 1 && whatNode.y != ySizeCache - 1 &&
(canBarelyPassBlocks || addedCache.HasFlag(AddedNeighbours.HigherX | AddedNeighbours.HigherY)))
{
cache = actualMap[whatNode.x + 1][whatNode.y + 1][whatNode.z];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.APPROX_2D_DIAGONAL));
addedCache |= AddedNeighbours.Diagonal2DHigherXHigherY;
}
}
//Higher X lower Y
if (whatNode.x != xSizeCache - 1 && whatNode.y != 0 &&
(canBarelyPassBlocks || addedCache.HasFlag(AddedNeighbours.HigherX | AddedNeighbours.LowerY)))
{
cache = actualMap[whatNode.x + 1][whatNode.y - 1][whatNode.z];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.APPROX_2D_DIAGONAL));
addedCache |= AddedNeighbours.Diagonal2DHigherXLowerY;
}
}
//Lower X higher Y
if (whatNode.x != 0 && whatNode.y != ySizeCache - 1 &&
(canBarelyPassBlocks || addedCache.HasFlag(AddedNeighbours.LowerX | AddedNeighbours.HigherY)))
{
cache = actualMap[whatNode.x - 1][whatNode.y + 1][whatNode.z];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.APPROX_2D_DIAGONAL));
addedCache |= AddedNeighbours.Diagonal2DLowerXHigherY;
}
}
//Lower X lower Y
if (whatNode.x != 0 && whatNode.y != 0 &&
(canBarelyPassBlocks || addedCache.HasFlag(AddedNeighbours.LowerX | AddedNeighbours.LowerY)))
{
cache = actualMap[whatNode.x - 1][whatNode.y - 1][whatNode.z];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.APPROX_2D_DIAGONAL));
addedCache |= AddedNeighbours.Diagonal2DLowerXLowerY;
}
}
#endregion Diagonal XY plane
#region Diagonal XZ plane
//Higher X higher Z
if (whatNode.x != xSizeCache - 1 && whatNode.z != zSizeCache - 1 &&
(canBarelyPassBlocks || addedCache.HasFlag(AddedNeighbours.HigherX | AddedNeighbours.HigherZ)))
{
cache = actualMap[whatNode.x + 1][whatNode.y][whatNode.z + 1];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.APPROX_2D_DIAGONAL));
addedCache |= AddedNeighbours.Diagonal2DHigherXHigherZ;
}
}
//Higher X lower Z
if (whatNode.x != xSizeCache - 1 && whatNode.z != 0 &&
(canBarelyPassBlocks || addedCache.HasFlag(AddedNeighbours.HigherX | AddedNeighbours.LowerZ)))
{
cache = actualMap[whatNode.x + 1][whatNode.y][whatNode.z - 1];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.APPROX_2D_DIAGONAL));
addedCache |= AddedNeighbours.Diagonal2DHigherXLowerZ;
}
}
//Lower X higher Z
if (whatNode.x != 0 && whatNode.z != zSizeCache - 1 &&
(canBarelyPassBlocks || addedCache.HasFlag(AddedNeighbours.LowerX | AddedNeighbours.HigherZ)))
{
cache = actualMap[whatNode.x - 1][whatNode.y][whatNode.z + 1];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.APPROX_2D_DIAGONAL));
addedCache |= AddedNeighbours.Diagonal2DLowerXHigherZ;
}
}
//Lower X lower Z
if (whatNode.x != 0 && whatNode.z != 0 &&
(canBarelyPassBlocks || addedCache.HasFlag(AddedNeighbours.LowerX | AddedNeighbours.LowerZ)))
{
cache = actualMap[whatNode.x - 1][whatNode.y][whatNode.z - 1];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.APPROX_2D_DIAGONAL));
addedCache |= AddedNeighbours.Diagonal2DLowerXLowerZ;
}
}
#endregion Diagonal XZ plane
#region Diagonal YZ plane
//Higher Y higher Z
if (whatNode.y != xSizeCache - 1 && whatNode.z != zSizeCache - 1 &&
(canBarelyPassBlocks || addedCache.HasFlag(AddedNeighbours.HigherY | AddedNeighbours.HigherZ)))
{
cache = actualMap[whatNode.x][whatNode.y + 1][whatNode.z + 1];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.APPROX_2D_DIAGONAL));
addedCache |= AddedNeighbours.Diagonal2DHigherYHigherZ;
}
}
//Higher Y lower Z
if (whatNode.y != xSizeCache - 1 && whatNode.z != 0 &&
(canBarelyPassBlocks || addedCache.HasFlag(AddedNeighbours.HigherY | AddedNeighbours.LowerZ)))
{
cache = actualMap[whatNode.x][whatNode.y + 1][whatNode.z - 1];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.APPROX_2D_DIAGONAL));
addedCache |= AddedNeighbours.Diagonal2DHigherYLowerZ;
}
}
//Lower Y higher Z
if (whatNode.y != 0 && whatNode.z != zSizeCache - 1 &&
(canBarelyPassBlocks || addedCache.HasFlag(AddedNeighbours.LowerY | AddedNeighbours.HigherZ)))
{
cache = actualMap[whatNode.x][whatNode.y - 1][whatNode.z + 1];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.APPROX_2D_DIAGONAL));
addedCache |= AddedNeighbours.Diagonal2DLowerYHigherZ;
}
}
//Lower Y lower Z
if (whatNode.y != 0 && whatNode.z != 0 &&
(canBarelyPassBlocks || addedCache.HasFlag(AddedNeighbours.LowerY | AddedNeighbours.LowerZ)))
{
cache = actualMap[whatNode.x][whatNode.y - 1][whatNode.z - 1];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.APPROX_2D_DIAGONAL));
addedCache |= AddedNeighbours.Diagonal2DLowerYLowerZ;
}
}
#endregion Diagonal YZ plane
#region 3D walks higher x plane
//Higher X higher Y higher Z
if (whatNode.x != xSizeCache - 1 && whatNode.y != ySizeCache - 1 && whatNode.z != zSizeCache - 1 &&
(canBarelyPassBlocks || addedCache.HasFlag(
AddedNeighbours.Diagonal2DHigherXHigherY |
AddedNeighbours.Diagonal2DHigherXHigherZ |
AddedNeighbours.Diagonal2DHigherYHigherZ)))
{
cache = actualMap[whatNode.x + 1][whatNode.y + 1][whatNode.z + 1];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.APPROX_3D_DIAGONAL));
}
}
//Higher X higher Y lower Z
if (whatNode.x != xSizeCache - 1 && whatNode.y != ySizeCache - 1 && whatNode.z != 0 &&
(canBarelyPassBlocks || addedCache.HasFlag(
AddedNeighbours.Diagonal2DHigherXHigherY |
AddedNeighbours.Diagonal2DHigherXLowerZ |
AddedNeighbours.Diagonal2DHigherYLowerZ)))
{
cache = actualMap[whatNode.x + 1][whatNode.y + 1][whatNode.z - 1];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.APPROX_3D_DIAGONAL));
}
}
//Higher X lower Y higher Z
if (whatNode.x != xSizeCache - 1 && whatNode.y != 0 && whatNode.z != zSizeCache - 1 &&
(canBarelyPassBlocks || addedCache.HasFlag(
AddedNeighbours.Diagonal2DHigherXLowerY |
AddedNeighbours.Diagonal2DHigherXHigherZ |
AddedNeighbours.Diagonal2DLowerYHigherZ)))
{
cache = actualMap[whatNode.x + 1][whatNode.y - 1][whatNode.z + 1];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.APPROX_3D_DIAGONAL));
}
}
//Higher X lower Y lower Z
if (whatNode.x != xSizeCache - 1 && whatNode.y != 0 && whatNode.z != 0 &&
(canBarelyPassBlocks || addedCache.HasFlag(
AddedNeighbours.Diagonal2DHigherXLowerY |
AddedNeighbours.Diagonal2DHigherXLowerZ |
AddedNeighbours.Diagonal2DLowerYLowerZ)))
{
cache = actualMap[whatNode.x + 1][whatNode.y - 1][whatNode.z - 1];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.APPROX_3D_DIAGONAL));
}
}
#endregion 3D walks higher x plane
#region 3D walks lower x plane
//Lower X higher Y higher Z
if (whatNode.x != 0 && whatNode.y != ySizeCache - 1 && whatNode.z != zSizeCache - 1 &&
(canBarelyPassBlocks || addedCache.HasFlag(
AddedNeighbours.Diagonal2DLowerXHigherY |
AddedNeighbours.Diagonal2DLowerXHigherZ |
AddedNeighbours.Diagonal2DHigherYHigherZ)))
{
cache = actualMap[whatNode.x - 1][whatNode.y + 1][whatNode.z + 1];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.APPROX_3D_DIAGONAL));
}
}
//Lower X higher Y lower Z
if (whatNode.x != 0 && whatNode.y != ySizeCache - 1 && whatNode.z != 0 &&
(canBarelyPassBlocks || addedCache.HasFlag(
AddedNeighbours.Diagonal2DLowerXHigherY |
AddedNeighbours.Diagonal2DLowerXLowerZ |
AddedNeighbours.Diagonal2DHigherYLowerZ)))
{
cache = actualMap[whatNode.x - 1][whatNode.y + 1][whatNode.z - 1];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.APPROX_3D_DIAGONAL));
}
}
//Lower X lower Y higher Z
if (whatNode.x != 0 && whatNode.y != 0 && whatNode.z != zSizeCache - 1 &&
(canBarelyPassBlocks || addedCache.HasFlag(
AddedNeighbours.Diagonal2DLowerXLowerY |
AddedNeighbours.Diagonal2DLowerXHigherZ |
AddedNeighbours.Diagonal2DLowerYHigherZ)))
{
cache = actualMap[whatNode.x - 1][whatNode.y - 1][whatNode.z + 1];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.APPROX_3D_DIAGONAL));
}
}
//Lower X lower Y lower Z
if (whatNode.x != 0 && whatNode.y != 0 && whatNode.z != 0 &&
(canBarelyPassBlocks || addedCache.HasFlag(
AddedNeighbours.Diagonal2DLowerXLowerY |
AddedNeighbours.Diagonal2DLowerXLowerZ |
AddedNeighbours.Diagonal2DLowerYLowerZ)))
{
cache = actualMap[whatNode.x - 1][whatNode.y - 1][whatNode.z - 1];
if (!cache.currentStatus.HasFlag(MapNode.Status.isWall))
{
neighbours.Add(new Tuple <MapNode, int>(cache, MapNode.APPROX_3D_DIAGONAL));
}
}
#endregion 3D walks lower x plane
return(neighbours);
}
