public List <PathFinderNode> FindPath(Vector2i start, Vector2i end)
{
bool found = false;
bool stop = false;
int heuristicEstimate = 2;
int closeNodeCounter = 0;
int directionCount = _diagonals ? 8 : 4;
// Instead of clearing the grid each time, I change node state values and simply ignore the other values.
// It's faster than clearing the grid (not much, but it is).
_openNodeValue += 2;
_closeNodeValue += 2;
_open.Clear();
_close.Clear();
int location = EncodeLocation(start.X, start.Y);
int endLocation = EncodeLocation(end.X, end.Y);
ushort locationX = 0, locationY = 0;
_calcGrid[location].goneCost = 0;
_calcGrid[location].cost = 0 + heuristicEstimate;
_calcGrid[location].parentX = (ushort)start.X;
_calcGrid[location].parentY = (ushort)start.Y;
_calcGrid[location].state = _openNodeValue;
_open.Push(location);
while (_open.Count > 0 && !stop)
{
location = _open.Pop();
// Is it in closed list? means this node was already processed
if (_calcGrid[location].state == _closeNodeValue)
{
continue;
}
DecodeLocation(location, ref locationX, ref locationY);
if (location == endLocation)
{
_calcGrid[location].state = _closeNodeValue;
found = true;
break;
}
if (closeNodeCounter > _searchLimit)
{
// Evaluated nodes exceeded limit : path not found
Console.WriteLine("Pathfinder searchLimit exceed");
return(null);
}
// Let's calculate each successors
for (int i = 0; i < directionCount; ++i)
{
ushort newLocationX = (ushort)(locationX + _directions[i][0]);
ushort newLocationY = (ushort)(locationY + _directions[i][1]);
int newLocation = EncodeLocation(newLocationX, newLocationY);
int newGoneCost;
// Outside the grid?
if (newLocationX >= _gridSizeX || newLocationY >= _gridSizeY)
{
continue;
}
// Not crossable?
if (_grid[newLocation] == 0)
{
continue;
}
if (_avoidDiagonalCross)
{
//
// +----+----+----+
// | | | 3 |
// | | | |
// +----+----+----+
// | | 2 |XXXX| Diagonals are allowed,
// | | |XXXX| but going through 1, 2 then 3 should be avoided,
// +----+----+----+ because there are contiguous uncrossable cells.
// | | 1 |XXXX| (A square object cannot go from 2 to 3 for example, it will have to bypass the corner).
// | | |XXXX|
// +----+----+----+
//
if (i > 3)
{
if (_grid[EncodeLocation(locationX + _directions[i][0], locationY)] == 0 ||
_grid[EncodeLocation(locationX, locationY + _directions[i][1])] == 0)
{
continue;
}
}
}
if (_heavyDiagonals && i > 3)
{
newGoneCost = _calcGrid[location].goneCost + (int)(_grid[newLocation] * 2.41f);
}
else
{
newGoneCost = _calcGrid[location].goneCost + _grid[newLocation];
}
//Is it open or closed?
if (_calcGrid[newLocation].state == _openNodeValue || _calcGrid[newLocation].state == _closeNodeValue)
{
// The current node has less code than the previous? then skip this node
if (_calcGrid[newLocation].goneCost <= newGoneCost)
{
continue;
}
}
_calcGrid[newLocation].parentX = locationX;
_calcGrid[newLocation].parentY = locationY;
_calcGrid[newLocation].goneCost = newGoneCost;
// Heuristic : manhattan distance
int heuristic = heuristicEstimate * (Math.Abs(newLocationX - end.X) + Math.Abs(newLocationY - end.Y));
_calcGrid[newLocation].cost = newGoneCost + heuristic;
//It is faster if we leave the open node in the priority queue
//When it is removed, it will be already closed, it will be ignored automatically
_open.Push(newLocation);
_calcGrid[newLocation].state = _openNodeValue;
}
closeNodeCounter++;
_calcGrid[location].state = _closeNodeValue;
}
if (found)
{
_close.Clear();
int posX = end.X;
int posY = end.Y;
PathFinderNodeFast tmpNode = _calcGrid[EncodeLocation(end.X, end.Y)];
PathFinderNode node = new PathFinderNode();
node.cost = tmpNode.cost;
node.goneCost = tmpNode.goneCost;
node.heuristic = 0;
node.parentX = tmpNode.parentX;
node.parentY = tmpNode.parentY;
node.x = end.X;
node.y = end.Y;
while (node.x != node.parentX || node.y != node.parentY)
{
_close.Add(node);
posX = node.parentX;
posY = node.parentY;
tmpNode = _calcGrid[EncodeLocation(posX, posY)];
node.cost = tmpNode.cost;
node.goneCost = tmpNode.goneCost;
node.heuristic = 0;
node.parentX = tmpNode.parentX;
node.parentY = tmpNode.parentY;
node.x = posX;
node.y = posY;
}
_close.Add(node);
// Path found
return(_close);
}
// Path not found
Console.WriteLine("Pathfinder path not found");
return(null);
}
/// <summary>
/// Generates a maze from one starting point.
/// All corridors will be connected to this point,
/// and no one will make any loop.
/// </summary>
/// <param name="seedX">Seed x.</param>
/// <param name="seedY">Seed y.</param>
public void Generate(int seedX, int seedY)
{
if (grid.sizeX <= 0 || grid.sizeY <= 0)
{
Log.Error("Invalid grid size " + grid);
return;
}
// By default, the grid is full of blocking cells (0).
//grid.Create();
Vector2i pos = new Vector2i(seedX, seedY);
int dir = -1;
List <Vector2i> startNodes = new List <Vector2i>();
startNodes.Add(new Vector2i(seedX, seedY));
// Initialize bits of the grid (do not touch the others because we could specify disabled cells before)
for (int i = 0; i < grid.data.Length; ++i)
{
grid.data[i] &= ~Maze.ANY_DIRS_BITS; // Clear directions
grid.data[i] |= Maze.UNVISITED_BIT; // Set unvisited bit
}
// While there is registered positions where we can start a new cooridor (start nodes)
while (startNodes.Count > 0)
{
// Choose at random from the available start nodes
int j = Random.Range(0, startNodes.Count);
pos = startNodes[j];
startNodes.RemoveAt(j);
// Randomize the maximum length of the corridor
int corridorLength = Random.Range(corridorLengthMin, corridorLengthMax);
// Generate the corridor by iteration
for (int i = 0; i < corridorLength; ++i)
{
int l = grid.EncodeLocation(pos.X, pos.Y);
List <int> availableDirs = UnvisitedDirections(pos.X, pos.Y);
// If there is at least one available direction
if (availableDirs.Count != 0)
{
// If there is more than one direction available
// Note: going back will never happen because the cell we are on will
// be be marked as visited, and so will not occur in the list.
if (availableDirs.Count > 1)
{
// Memorize the position to come back later
startNodes.Add(pos);
}
// Choose a random direction
dir = availableDirs[Random.Range(0, availableDirs.Count)];
// Add the available direction to cell's mask (from)
if ((grid.data[l] & Maze.UNVISITED_BIT) != 0)
{
grid.data[l] = 0;
}
grid.data[l] |= (1 << dir);
// Advance
pos.X += Dir.vec2i[dir].X;
pos.Y += Dir.vec2i[dir].Y;
// Add the available direction to cell's mask (to)
l = grid.EncodeLocation(pos.X, pos.Y);
if ((grid.data[l] & Maze.UNVISITED_BIT) != 0)
{
grid.data[l] = 0;
}
grid.data[l] |= (1 << Dir.opposite[dir]);
if (i + 1 == corridorLength)
{
// If it's the last iteration, add the current position in start nodes.
// If there are no direction available, the position will be discarded in further iterations.
startNodes.Add(pos);
}
}
else
{
// No directions available, finish corridor
break;
}
}
}
}
