private void CreatePath(int startField, int targetField)
{
while (startField != targetField && targetField > 0)
{
GridPathNode node = _grid[targetField];
gridPath.AddPosition(node.column, node.row);
// get next position
targetField = node.parentIndex;
}
}
private void AddToOpenList(GridPathNode newNode)
{
int count = _openList.Count;
for (int i = 0; i < count; i++)
{
if (newNode.f < _openList[i].f)
{
_openList.Insert(i, newNode);
return;
}
}
_openList.Add(newNode);
}
/// <summary>
/// will hardcode possible directions of all nodes into the nodes itself
/// </summary>
/// <param name="movementPossible"></param>
public void CalculatePossibleDirections(MovementPossibleCheck movementPossible = null)
{
for (int row = 0; row < _rowCount; row++)
{
//_closedList.Add(new List<GridPathNode>());
for (int column = 0; column < _columnCount; column++)
{
GridPathNode currentNode = _grid[row * _columnCount + column];
for (int i = 0; i < _checkLength; i++)
{
int checkRow = currentNode.row + _rowCheck[i];
int checkColumn = currentNode.column + _columnCheck[i];
int checkField = checkRow * _columnCount + checkColumn;
// if new position out of bounds, ignore it
if (checkRow < 0 || checkColumn < 0 || checkRow >= _rowCount || checkColumn >= _columnCount)
{
continue;
}
GridPathNode checkNode = _grid[checkField];
// movement not possible => ignore
if (movementPossible == null)
{
if (checkNode.walkable == false)
{
continue;
}
}
else
{
if (movementPossible(currentNode.column, currentNode.row, checkColumn, checkRow) == false)
{
continue;
}
}
currentNode.AddDirection(checkNode, _costOfMovement[i]);
}
}
}
}
// ======================================================================
// public methods
// ----------------------------------------------------------------------
public void SetDimensions(int columnCount, int rowCount)
{
_columnCount = columnCount;
_rowCount = rowCount;
// prepare closed list
_grid = new GridPathNode[rowCount * columnCount];
//_closedList.Clear();
for (int row = 0; row < _rowCount; row++)
{
//_closedList.Add(new List<GridPathNode>());
for (int column = 0; column < _columnCount; column++)
{
int index = row * _columnCount + column;
_grid[index] = new GridPathNode(column, row, index);
}
}
_fieldCount = _rowCount * _columnCount;
gridPath.SetDimensions(_columnCount, _rowCount);
}
// ======================================================================
// public methods
// ----------------------------------------------------------------------
public void SetDimensions(int columnCount, int rowCount)
{
_columnCount = columnCount;
_rowCount = rowCount;
// prepare closed list
_grid = new GridPathNode[rowCount * columnCount];
//_closedList.Clear();
for (int row = 0; row < _rowCount; row++)
{
//_closedList.Add(new List<GridPathNode>());
for (int column = 0; column < _columnCount; column++)
{
int index = row * _columnCount + column;
_grid[index] = new GridPathNode(column, row, index);
}
}
_fieldCount = _rowCount * _columnCount;
gridPath.SetDimensions(_columnCount, _rowCount);
}
private void AddToOpenList(GridPathNode newNode)
{
int count = _openList.Count;
for (int i = 0; i < count; i++)
{
if (newNode.f < _openList[i].f)
{
_openList.Insert(i, newNode);
return;
}
}
_openList.Add(newNode);
}
// calculates path from startPos to endPos
public bool FindPath(GridPosition startPos, GridPosition endPos, MovementPossibleCheck movementPossible = null, bool saveNearestPath = true, int maxNodeChecksWithoutProgress = 0)
{
// assume no path found
gridPath.Clear();
if (startPos.row < 0 || startPos.column < 0 || endPos.row < 0 || endPos.column < 0
|| startPos.column >= _columnCount || startPos.row >= _rowCount || endPos.column >= _columnCount || endPos.row >= _rowCount)
{
return false;
}
// reset open and closed list
_openList.Clear();
for (int field = 0; field < _fieldCount; field++)
{
GridPathNode node = _grid[field];
if (node.closed)
node.closed = false;
}
// set target values
int targetNodeIndex = endPos.row * _columnCount + endPos.column;
GridPathNode targetNode = _grid[targetNodeIndex];
// check if end point is walkable and then decide if using brute force or sophisticated
// brute force works better when no path will be found and maxNodeChecksWithoutProgress is not set
bool bruteForce = false;
bool movementFromStartToEndIsPossible = false;
if (movementPossible != null)
movementFromStartToEndIsPossible = movementPossible(startPos.column, startPos.row, endPos.column, endPos.row);
else
movementFromStartToEndIsPossible = targetNode.walkable;
// check if sophisticated or brute force approach
if (!movementFromStartToEndIsPossible && maxNodeChecksWithoutProgress <= 0)
{
// brute force
_heuristicValue = 1;
bruteForce = true;
}
else
{
// sophisticated
_heuristicValue = 10;
}
// set nearest point
_nearestDistance = int.MaxValue;
_lastTimeNearestPointPushedForward = 0;
// add the starting point to the open list
int startNodeIndex = startPos.row * _columnCount + startPos.column;
GridPathNode startNode = _grid[startNodeIndex];
_openList.Add(startNode);
startNode.h = GetHeuristic(startPos.row, startPos.column, endPos.row, endPos.column);
startNode.opened = true;
_nearestPoint = startNode; // starting point is also nearest point at the moment
// while not found and still nodes to check
while (targetNode.closed == false && _openList.Count > 0
// and while not the maximum of checks without real progress
&& (maxNodeChecksWithoutProgress <= 0 || _lastTimeNearestPointPushedForward < maxNodeChecksWithoutProgress))
{
// first node in the openList will become the current node
GridPathNode currentNode = _openList[0];
// save nearest field
if (currentNode.h < _nearestDistance)
{
_nearestDistance = currentNode.h;
_nearestPoint = currentNode;
_lastTimeNearestPointPushedForward = 0;
}
else
{
_lastTimeNearestPointPushedForward++;
}
// mark current node as closed
currentNode.closed = true;
currentNode.opened = false;
_openList.RemoveAt(0);
for (int i = 0; i < currentNode.directionCount; i++)
{
GridPathNode.GridMovement gridMovement = currentNode.directions[i];
GridPathNode checkNode = gridMovement.node;
// if node is already closed, ignore it
if (checkNode.closed)
continue;
// movement not possible => ignore
if (movementPossible == null)
{
if (checkNode.walkable == false)
continue;
}
else
{
if (movementPossible(currentNode.column, currentNode.row, checkNode.column, checkNode.row) == false)
continue;
}
// possible movement cost including path to currentNode
int g = currentNode.g + gridMovement.movementCost;
if (checkNode.opened == false)
{
checkNode.parentIndex = currentNode.index;
checkNode.h = GetHeuristic(checkNode.row, checkNode.column, endPos.row, endPos.column);
checkNode.g = g;
checkNode.f = checkNode.g + checkNode.h;
checkNode.opened = true;
if (bruteForce)
_openList.Add(checkNode);
else
AddToOpenList(checkNode);
}
else
{
// checkNode is on open list
// if path to this checkNode is better from the current Node, update
if (checkNode.g > g)
{
// update parent to the current Node
checkNode.parentIndex = currentNode.index;
// update path cost, checkNode.h already calculated
checkNode.g = g;
checkNode.f = checkNode.g + checkNode.h;
// do not bother to move node to correct spot in open list,
// this way it's faster
}
}
}
}
// clear opened values for next search
int openListCount = _openList.Count;
for (int i = 0; i < openListCount; i++)
{
GridPathNode node = _openList[i];
node.opened = false;
}
// if target found
if (targetNode.closed)
{
CreatePath(startNodeIndex, targetNodeIndex);
return true;
}
// save nearest path if wished
else if (saveNearestPath)
{
CreatePath(startNodeIndex, _nearestPoint.row * _columnCount + _nearestPoint.column);
return false;
}
// no path and no nearest point
else
{
return false;
}
}
// calculates path from startPos to endPos
public bool FindPath(GridPosition startPos, GridPosition endPos, MovementPossibleCheck movementPossible = null, bool saveNearestPath = true, int maxNodeChecksWithoutProgress = 0)
{
// assume no path found
gridPath.Clear();
if (startPos.row < 0 || startPos.column < 0 || endPos.row < 0 || endPos.column < 0 ||
startPos.column >= _columnCount || startPos.row >= _rowCount || endPos.column >= _columnCount || endPos.row >= _rowCount)
{
return(false);
}
// reset open and closed list
_openList.Clear();
for (int field = 0; field < _fieldCount; field++)
{
GridPathNode node = _grid[field];
if (node.closed)
{
node.closed = false;
}
}
// set target values
int targetNodeIndex = endPos.row * _columnCount + endPos.column;
GridPathNode targetNode = _grid[targetNodeIndex];
// check if end point is walkable and then decide if using brute force or sophisticated
// brute force works better when no path will be found and maxNodeChecksWithoutProgress is not set
bool bruteForce = false;
bool movementFromStartToEndIsPossible = false;
if (movementPossible != null)
{
movementFromStartToEndIsPossible = movementPossible(startPos.column, startPos.row, endPos.column, endPos.row);
}
else
{
movementFromStartToEndIsPossible = targetNode.walkable;
}
// check if sophisticated or brute force approach
if (!movementFromStartToEndIsPossible && maxNodeChecksWithoutProgress <= 0)
{
// brute force
_heuristicValue = 1;
bruteForce = true;
}
else
{
// sophisticated
_heuristicValue = 10;
}
// set nearest point
_nearestDistance = int.MaxValue;
_lastTimeNearestPointPushedForward = 0;
// add the starting point to the open list
int startNodeIndex = startPos.row * _columnCount + startPos.column;
GridPathNode startNode = _grid[startNodeIndex];
_openList.Add(startNode);
startNode.h = GetHeuristic(startPos.row, startPos.column, endPos.row, endPos.column);
startNode.opened = true;
_nearestPoint = startNode; // starting point is also nearest point at the moment
// while not found and still nodes to check
while (targetNode.closed == false && _openList.Count > 0
// and while not the maximum of checks without real progress
&& (maxNodeChecksWithoutProgress <= 0 || _lastTimeNearestPointPushedForward < maxNodeChecksWithoutProgress))
{
// first node in the openList will become the current node
GridPathNode currentNode = _openList[0];
// save nearest field
if (currentNode.h < _nearestDistance)
{
_nearestDistance = currentNode.h;
_nearestPoint = currentNode;
_lastTimeNearestPointPushedForward = 0;
}
else
{
_lastTimeNearestPointPushedForward++;
}
// mark current node as closed
currentNode.closed = true;
currentNode.opened = false;
_openList.RemoveAt(0);
for (int i = 0; i < currentNode.directionCount; i++)
{
GridPathNode.GridMovement gridMovement = currentNode.directions[i];
GridPathNode checkNode = gridMovement.node;
// if node is already closed, ignore it
if (checkNode.closed)
{
continue;
}
// movement not possible => ignore
if (movementPossible == null)
{
if (checkNode.walkable == false)
{
continue;
}
}
else
{
if (movementPossible(currentNode.column, currentNode.row, checkNode.column, checkNode.row) == false)
{
continue;
}
}
// possible movement cost including path to currentNode
int g = currentNode.g + gridMovement.movementCost;
if (checkNode.opened == false)
{
checkNode.parentIndex = currentNode.index;
checkNode.h = GetHeuristic(checkNode.row, checkNode.column, endPos.row, endPos.column);
checkNode.g = g;
checkNode.f = checkNode.g + checkNode.h;
checkNode.opened = true;
if (bruteForce)
{
_openList.Add(checkNode);
}
else
{
AddToOpenList(checkNode);
}
}
else
{
// checkNode is on open list
// if path to this checkNode is better from the current Node, update
if (checkNode.g > g)
{
// update parent to the current Node
checkNode.parentIndex = currentNode.index;
// update path cost, checkNode.h already calculated
checkNode.g = g;
checkNode.f = checkNode.g + checkNode.h;
// do not bother to move node to correct spot in open list,
// this way it's faster
}
}
}
}
// clear opened values for next search
int openListCount = _openList.Count;
for (int i = 0; i < openListCount; i++)
{
GridPathNode node = _openList[i];
node.opened = false;
}
// if target found
if (targetNode.closed)
{
CreatePath(startNodeIndex, targetNodeIndex);
return(true);
}
// save nearest path if wished
else if (saveNearestPath)
{
CreatePath(startNodeIndex, _nearestPoint.row * _columnCount + _nearestPoint.column);
return(false);
}
// no path and no nearest point
else
{
return(false);
}
}
private void CheckColliders(GridPathNode[] grid, int layerMask, int fromColumn, int fromRow, int toColumn, int toRow)
{
// save raycastsHitTriggers setting
bool beforeSetting = Physics2D.queriesHitTriggers;
Physics2D.queriesHitTriggers = false;
for (int row = fromRow; row <= toRow; row++)
{
for (int column = fromColumn; column <= toColumn; column++)
{
// calculate position
float posX = _startX + column * _fieldSizeX + _fieldSizeX * 0.5f;
float posY = _startY + row * _fieldSizeY + _fieldSizeY * 0.5f;
// check for colliders
Collider2D coll = Physics2D.OverlapCircle(new Vector2(posX, posY), _fieldSizeX * 0.45f, layerMask);
if (coll == null)
grid[row * _columnCount + column].SetWalkable(true);
else
grid[row * _columnCount + column].SetWalkable(false);
}
}
// restore raycastsHitTriggers setting
Physics2D.queriesHitTriggers = beforeSetting;
}
private void CheckColliders(GridPathNode[] grid, int layerMask)
{
CheckColliders(grid, layerMask, 0, 0, _columnCount - 1, _rowCount - 1);
}
