void DebugGrid()
{
for (int r = 0; r < _navGrid.rows; ++r)
{
for (int c = 0; c < _navGrid.cols; ++c)
{
NavGrid.Cell cell = _cellGrid[r, c];
if (cell.cost < int.MaxValue)
{
Debug.Log("(" + cell.row.ToString() + "," + cell.col.ToString() + ") = " + cell.cost.ToString());
}
}
}
}
public void DebugDrawOnRun()
{
for (int i = 0; i < _path.Count; ++i)
{
NavGrid.Cell cell = _path[i];
if (cell == _currentPathStartCell)
{
Gizmos.color = Color.yellow;
}
else if (cell == _currentPathEndCell)
{
Gizmos.color = Color.red;
}
else
{
Gizmos.color = Color.green;
}
Gizmos.DrawSphere(cell.GetPosition(), 0.55f);
}
}
bool EvaluateNeighbor(NavGrid.Cell parentCell, NavGrid.Cell neighborCell)
{
int r = neighborCell.row;
int c = neighborCell.col;
if (neighborCell.isObstacle)
{
return(false);
}
else
{
int costFromParent = (r == parentCell.row || c == parentCell.col)?2:3; // diagonals are more expensive
int newCost = parentCell.cost + costFromParent;
if (newCost < neighborCell.cost)
{
neighborCell.cost = newCost;
int heuristic = GetHeuristic(r, c, _currentPathEndCell.row, c);
int priority = neighborCell.cost + heuristic;
if (neighborCell.isInOpenList)
{
_unvisitedCellsFPQ.Remove(neighborCell);
}
_unvisitedCellsFPQ.Enqueue(neighborCell, priority);
neighborCell.isInOpenList = true;
neighborCell.visited++;
neighborCell.parent = parentCell;
if (neighborCell == _currentPathEndCell)
{
return(true);
}
}
}
return(false);
}
void GenerateAStarPath(Vector3 start, Vector3 destination)
{
NavGrid.Cell startCell = _navGrid.FindFirstCellThatContainsPoint(start);
if (startCell == null || startCell.isObstacle)
{
if (DEBUG)
{
Debug.Log("Error: Start cell is not on grid or in obstacle");
}
return;
}
NavGrid.Cell destinationCell = _navGrid.FindFirstCellThatContainsPoint(destination);
if (destinationCell == null || destinationCell.isObstacle)
{
if (DEBUG)
{
Debug.Log("Error: Destination cell is not on grid or in obstacle");
}
return;
}
_unvisitedCellsFPQ.Clear();
_navGrid.Refresh(false);
_path.Clear();
_totalPointsInPath = 0;
_currentPathIndex = 0;
_currentPathStartCell = startCell;
_currentPathEndCell = destinationCell;
startCell.cost = 0;
_unvisitedCellsFPQ.Enqueue(startCell, 0);
int numCellsVisited = 0;
while (_unvisitedCellsFPQ.Count > 0)
{
NavGrid.Cell c = _unvisitedCellsFPQ.Dequeue();
c.isInOpenList = false;
numCellsVisited++;
bool hasReachedDestination = false;
int numNeighbors = c.neighbors.Count;
for (int i = 0; i < numNeighbors; ++i)
{
if (EvaluateNeighbor(c, c.neighbors[i]))
{
hasReachedDestination = true;
break;
}
}
if (hasReachedDestination)
{
break;
}
}
// Calculate Path
NavGrid.Cell pathFinder = destinationCell;
_path.Add(pathFinder);
while (pathFinder != startCell)
{
pathFinder = pathFinder.parent;
_path.Add(pathFinder);
}
_path.Reverse();
_totalPointsInPath = _path.Count;
//DebugGrid();
//Debug.Break();
}