public static List <GridCell> FindPath(GridController grid, Vector2Int start, Vector2Int end, bool diag = false)
{
List <GridCell> Path = null;
GridCell StartNode = grid.GetGridPoint(start);
GridCell TargetNode = grid.GetGridPoint(end);
if (TargetNode == null || StartNode == null)
{
return(null);
}
List <GridCell> OpenList = new List <GridCell>();
HashSet <GridCell> ClosedList = new HashSet <GridCell>();
OpenList.Add(StartNode);
while (OpenList.Count > 0)
{
GridCell CurrentNode = OpenList[0];
for (int i = 1; i < OpenList.Count; i++)
{
if (OpenList[i].FCost() < CurrentNode.FCost() || OpenList[i].FCost() == CurrentNode.FCost() && OpenList[i].hCost < CurrentNode.hCost)
{
CurrentNode = OpenList[i];
}
}
OpenList.Remove(CurrentNode);
ClosedList.Add(CurrentNode);
if (CurrentNode == TargetNode)
{
Path = GetFinalPath(StartNode, TargetNode);
}
foreach (GridCell NeighborNode in grid.GetNeighboringCells(CurrentNode.transform.position, diag))
{
if (NeighborNode.IsWall() || !NeighborNode.Walkable || ClosedList.Contains(NeighborNode))
{
continue;
}
int MoveCost = CurrentNode.gCost + GetManhattenDistance(CurrentNode, NeighborNode) + NeighborNode.weight;
if (MoveCost < NeighborNode.gCost || !OpenList.Contains(NeighborNode))
{
NeighborNode.gCost = MoveCost;
NeighborNode.hCost = GetManhattenDistance(NeighborNode, TargetNode);
NeighborNode.ParentNode = CurrentNode;
if (!OpenList.Contains(NeighborNode))
{
OpenList.Add(NeighborNode);
}
}
}
}
return(Path);
}
public List <PathFinderNode> FindPath(int StartX, int StartY, int EndX, int EndY)
{
PathFinderNode StartNode = grid[StartY - MapOffset.y, StartX - MapOffset.x];
PathFinderNode EndNode = grid[EndY - MapOffset.y, EndX - MapOffset.x];
OpenList = new List <PathFinderNode> {
StartNode
};
ClosedList = new List <PathFinderNode>();
for (int x = 0; x < width; x++)
{
for (int y = 0; y < height; y++)
{
PathFinderNode PathNode = grid[y, x];
PathNode.GCost = int.MaxValue;
PathNode.CalculateFCost();
PathNode.CameFromNode = null;
}
}
StartNode.GCost = 0;
StartNode.HCost = CalculateDistanceCost(StartNode, EndNode);
StartNode.CalculateFCost();
while (OpenList.Count > 0)
{
PathFinderNode CurrentNode = GetLowestFCostNode(OpenList);
if (CurrentNode == EndNode)
{
return(CalculatePath(EndNode));
}
OpenList.Remove(CurrentNode);
ClosedList.Add(CurrentNode);
foreach (PathFinderNode NeighborNode in GetNeighorNodes(CurrentNode))
{
if (ClosedList.Contains(NeighborNode))
{
continue;
}
if (!IsNodeOpen(NeighborNode))
{
if (!ClosedList.Contains(NeighborNode))
{
ClosedList.Add(NeighborNode);
}
continue;
}
else
{
// Debug.Log("Open Path");
}
int TentativeGCost = CurrentNode.GCost + 1;
if (TentativeGCost < NeighborNode.GCost)
{
NeighborNode.CameFromNode = CurrentNode;
NeighborNode.GCost = TentativeGCost;
NeighborNode.HCost = CalculateDistanceCost(NeighborNode, EndNode);
NeighborNode.CalculateFCost();
}
if (!OpenList.Contains(NeighborNode))
{
OpenList.Add(NeighborNode);
}
}
}
return(null);
}
public IList <ICell> FindPathOnMap(ICell cellStart, ICell cellEnd)
{
if (cellStart == null || cellEnd == null)
{
return(null);
}
List <ICell> OpenList = new List <ICell>();
List <ICell> ClosedList = new List <ICell>();
OpenList.Add(cellStart);
while (OpenList.Count > 0)
{
ICell CurrentNode = OpenList[0];
for (int i = 1; i < OpenList.Count; i++)
{
CellNode _target = OpenList[i].GetNode();
CellNode _current = CurrentNode.GetNode();
if (_target.FCost < _current.FCost || _target.FCost == _current.FCost && _target.ihCost < _current.ihCost)
{
CurrentNode = OpenList[i];
}
}
OpenList.Remove(CurrentNode);
ClosedList.Add(CurrentNode);
if (CurrentNode == cellEnd)
{
return(GetFinalPath(cellStart, cellEnd));
}
foreach (ICell NeighborNode in CurrentNode.GetNearestCells())
{
CellNode _target = NeighborNode.GetNode();
CellNode _current = CurrentNode.GetNode();
if (CellBase.CompareStates(NeighborNode.GetState(), CellBase.State.Obstacle) ||
ClosedList.Contains(NeighborNode))
{
continue;
}
int MoveCost = _current.igCost + GetManhattenDistance(_current, _target);
if (MoveCost < _target.igCost || !OpenList.Contains(NeighborNode))
{
NeighborNode.GetNode().igCost = MoveCost;
NeighborNode.GetNode().ihCost = GetManhattenDistance(_target, cellEnd.GetNode());
NeighborNode.GetNode().ParentNode = CurrentNode;
if (!OpenList.Contains(NeighborNode))
{
OpenList.Add(NeighborNode);
}
}
}
}
return(null);
}
public Vector3 GetValidPathFloorPos(Vector3 position)
{
// Save this value off, in case we need to use it to search for a valid location further along in this function.
Vector3 originalPosition = position;
// Find a position that is within the grid, at the same height as the grid.
float padding = m_cellSize / 4.0f;
Bounds gridBounds = GetGridBounds();
position.x = Mathf.Clamp(position.x, gridBounds.min.x + padding, gridBounds.max.x - padding);
position.y = Origin.y;
position.z = Mathf.Clamp(position.z, gridBounds.min.z + padding, gridBounds.max.z - padding);
// If this position is blocked, then look at all of the neighbors of this cell, and see if one of those cells is
// unblocked. If one of those neighbors is unblocked, then we return the position of that neighbor, to ensure that
// the agent is always pathing to and from valid positions.
int cellIndex = GetCellIndex(position);
/*if ( IsBlocked(cellIndex) )
* {
* // Find the closest unblocked neighbor, if one exists.
* int[] neighbors = null;
* int numNeighbors = GetNeighbors(cellIndex, ref neighbors);
* float closestDistSq = float.MaxValue;
* for ( int i = 0; i < numNeighbors; i++ )
* {
* int neighborIndex = neighbors[i];
* if ( !IsBlocked(neighborIndex) )
* {
* Vector3 neighborPos = GetCellCenter(neighborIndex);
* float distToCellSq = (neighborPos - originalPosition).sqrMagnitude;
* if ( distToCellSq < closestDistSq )
* {
* closestDistSq = distToCellSq;
* position = neighborPos;
* }
* }
* }
* }*/
BinaryHeap <NeighborNode> cellIndices = new BinaryHeap <NeighborNode>();
HashSet <NeighborNode> set = new HashSet <NeighborNode>();
int iterCount = 0, iterMax = 3 * (int)eNeighborDirection.kNumNeighbors;
while (IsBlocked(cellIndex) && (iterCount++) < iterMax)
{
int[] neighbors = null;
int numNeighbors = GetNeighbors(cellIndex, ref neighbors);
foreach (int neighborIndex in neighbors)
{
NeighborNode newNode = new NeighborNode(neighborIndex, 0.0f);
if (set.Contains(newNode))
{
continue;
}
set.Add(newNode);
Vector3 neighborPos = GetCellCenter(neighborIndex);
newNode.distance = (neighborPos - originalPosition).sqrMagnitude;
cellIndices.Add(newNode);
}
cellIndex = cellIndices.Remove().nodeIndex;
}
if (!IsBlocked(cellIndex))
{
position = GetCellCenter(cellIndex);
}
return(position);
}
