public override PathNode FindPath(Vector2 startPos, Vector2 destPos)
{
MapHandler.GetInstance().ResetClosedList();
MapHandler.GetInstance().ResetOpenList();
m_lstClosed = new List <AStarNode>();
m_lstOpen = new List <AStarNode>();
// Insert start node in the open list.
AStarNode node = new AStarNode();
node.SetGCost(0);
node.SetHCost(HeuristicCostStimation(startPos, destPos));
node.SetNodePosition(startPos);
if (MapHandler.GetInstance().IsTileSolid(startPos) || MapHandler.GetInstance().IsTileSolid(destPos))
{
return(null);
}
// Place node in the open list.
m_lstOpen.Add(node);
while (m_lstOpen.Count > 0)
{
m_lstOpen.Sort();
node = m_lstOpen[0];
if (node.GetNodePosition().Equals(destPos))
{
MapHandler.GetInstance().SaveClosedList(m_lstClosed);
MapHandler.GetInstance().SaveOpenList(m_lstOpen);
return(ReconstructPath(node));
}
// Remove from open list.
m_lstOpen.Remove(node);
// Add to closed list
m_lstClosed.Add(node);
bool tentativeIsBeter = false;
double tentativeGScore = 0;
// Get Adjacents tiles
List <Vector2> adjacents = MapHandler.GetInstance().GetAdjacentsToTile(node.GetNodePosition());
// For each neighbour node N in "node"
foreach (Vector2 pos in adjacents)
{
AStarNode neighbour = new AStarNode();
neighbour.SetNodePosition(pos);
MapHandler.GetInstance().SetTileColor(neighbour.GetNodePosition(), Color.Gray);
//If it is in the close set then we skip it
if (m_lstClosed.Contains(neighbour))
{
continue;
}
// We no longer get solid tiles as valid adjecent.
//if (MapHandler.GetInstance().IsTileSolid(pos))
// continue;
tentativeGScore = node.GetGCost() + DistanceBetweenNodes(node, neighbour);
if (!m_lstOpen.Contains(neighbour))
{
m_lstOpen.Add(neighbour);
tentativeIsBeter = true;
}
else if (tentativeGScore < neighbour.GetGCost())
{
tentativeIsBeter = true;
}
else
{
tentativeIsBeter = false;
}
if (tentativeIsBeter)
{
neighbour.SetPrevNode(node);
neighbour.SetGCost(tentativeGScore);
neighbour.SetHCost(HeuristicCostStimation(neighbour.GetNodePosition(), destPos));
}
}
}
MapHandler.GetInstance().SaveOpenList(m_lstOpen);
MapHandler.GetInstance().SaveClosedList(m_lstClosed);
return(ReconstructPath((AStarNode)m_lstClosed[m_lstClosed.Count - 1]));
}
private double DistanceBetweenNodes(AStarNode n1, AStarNode n2)
{
double dist = Vector2.Distance(n1.GetNodePosition(), n2.GetNodePosition());
return(dist);
}