private List <NavMesh2DNode> RunPathWorker(NavMesh2DNode start, NavMesh2DNode end)
{
AStarData startData = new AStarData
{
cameFrom = null,
fScore = 0,
gScore = 0,
hScore = 0
};
Dictionary <NavMesh2DNode, NodeDataPair> openList = new Dictionary <NavMesh2DNode, NodeDataPair>();
Dictionary <NavMesh2DNode, NodeDataPair> closedList = new Dictionary <NavMesh2DNode, NodeDataPair>();
openList.Add(start, new NodeDataPair
{
n = start,
d = startData
});
while (openList.Count != 0)
{
NodeDataPair current = null;
{
float lowestFscore = float.MaxValue;
foreach (NodeDataPair n in openList.Values)
{
if (n.d.fScore < lowestFscore)
{
lowestFscore = n.d.fScore;
current = n;
}
}
}
openList.Remove(current.n);
closedList.Add(current.n, current);
if (closedList.ContainsKey(end))
{
//found a path
List <NavMesh2DNode> pathFound = new List <NavMesh2DNode>();
ReconstructPath(current, ref pathFound);
pathFound.Reverse();
//Path path = new Path(startNode, endNode);
//FindGoingTo(endNode, null);
//ReconstructPath(startNode, ref path);
//path.Smooth();
return(pathFound);
}
foreach (NavMesh2DConnection connection in current.n.connections)
{
NavMesh2DNode connectedMesh2DNode = GetNode(connection.connectedNodeIndex);
if (closedList.ContainsKey(connectedMesh2DNode))
{
continue;
}
NodeDataPair pairAlreadyInOpenlist = null;
if (openList.ContainsKey(connectedMesh2DNode))
{
pairAlreadyInOpenlist = openList[connectedMesh2DNode];
}
if (pairAlreadyInOpenlist == null)
{ //if the neighbor doesn't exist
AStarData nextNodeData = new AStarData();
nextNodeData.gScore = current.d.gScore + Vector3.Distance(current.n.position, connectedMesh2DNode.position);
nextNodeData.hScore = CalculateHeuristic(connectedMesh2DNode.position, end.position);//connection.Key == endNode ? Vector3.Distance(current.position,endNode.position) * 10 : Vector3.Distance(connection.Key.position,endNode.position); //heucistics will be used to find
//paths that are less dangerous, etc. danger being the heucistic.
nextNodeData.fScore = nextNodeData.gScore + nextNodeData.hScore;
nextNodeData.cameFrom = current;
openList.Add(connectedMesh2DNode, new NodeDataPair
{
n = connectedMesh2DNode,
d = nextNodeData
});
}
else if (current.d.gScore + Vector3.Distance(current.n.position, connectedMesh2DNode.position) < pairAlreadyInOpenlist.d.gScore)
{ //if the neighbor exists and has bigger g score
pairAlreadyInOpenlist.d.gScore = current.d.gScore + Vector3.Distance(current.n.position, connectedMesh2DNode.position);
pairAlreadyInOpenlist.d.hScore = CalculateHeuristic(pairAlreadyInOpenlist.n.position, end.position); //connection.Key == endNode ? Vector3.Distance(current.position,endNode.position) * 10 : Vector3.Distance(connection.Key.position,endNode.position);//Vector3.Distance(connection.Key.position,endNode.position);
pairAlreadyInOpenlist.d.fScore = pairAlreadyInOpenlist.d.gScore + pairAlreadyInOpenlist.d.hScore;
pairAlreadyInOpenlist.d.cameFrom = current;
}
}
}
return(null);
}
private List<NavMesh2DNode> RunPathWorker(NavMesh2DNode start, NavMesh2DNode end)
{
AStarData startData = new AStarData
{
cameFrom = null,
fScore = 0,
gScore = 0,
hScore = 0
};
Dictionary<NavMesh2DNode, NodeDataPair> openList = new Dictionary<NavMesh2DNode, NodeDataPair>();
Dictionary<NavMesh2DNode, NodeDataPair> closedList = new Dictionary<NavMesh2DNode, NodeDataPair>();
openList.Add(start, new NodeDataPair
{
n = start,
d = startData
});
while (openList.Count != 0)
{
NodeDataPair current = null;
{
float lowestFscore = float.MaxValue;
foreach (NodeDataPair n in openList.Values)
{
if (n.d.fScore < lowestFscore)
{
lowestFscore = n.d.fScore;
current = n;
}
}
}
openList.Remove(current.n);
closedList.Add(current.n, current);
if (closedList.ContainsKey(end))
{
//found a path
List<NavMesh2DNode> pathFound = new List<NavMesh2DNode>();
ReconstructPath(current, ref pathFound);
pathFound.Reverse();
//Path path = new Path(startNode, endNode);
//FindGoingTo(endNode, null);
//ReconstructPath(startNode, ref path);
//path.Smooth();
return pathFound;
}
foreach (NavMesh2DConnection connection in current.n.connections)
{
NavMesh2DNode connectedMesh2DNode = GetNode(connection.connectedNodeIndex);
if (closedList.ContainsKey(connectedMesh2DNode))
continue;
NodeDataPair pairAlreadyInOpenlist = null;
if (openList.ContainsKey(connectedMesh2DNode))
pairAlreadyInOpenlist = openList[connectedMesh2DNode];
if (pairAlreadyInOpenlist == null)
{ //if the neighbor doesn't exist
AStarData nextNodeData = new AStarData();
nextNodeData.gScore = current.d.gScore + Vector3.Distance(current.n.position, connectedMesh2DNode.position);
nextNodeData.hScore = CalculateHeuristic(connectedMesh2DNode.position, end.position);//connection.Key == endNode ? Vector3.Distance(current.position,endNode.position) * 10 : Vector3.Distance(connection.Key.position,endNode.position); //heucistics will be used to find
//paths that are less dangerous, etc. danger being the heucistic.
nextNodeData.fScore = nextNodeData.gScore + nextNodeData.hScore;
nextNodeData.cameFrom = current;
openList.Add(connectedMesh2DNode, new NodeDataPair
{
n = connectedMesh2DNode,
d = nextNodeData
});
}
else if (current.d.gScore + Vector3.Distance(current.n.position, connectedMesh2DNode.position) < pairAlreadyInOpenlist.d.gScore)
{ //if the neighbor exists and has bigger g score
pairAlreadyInOpenlist.d.gScore = current.d.gScore + Vector3.Distance(current.n.position, connectedMesh2DNode.position);
pairAlreadyInOpenlist.d.hScore = CalculateHeuristic(pairAlreadyInOpenlist.n.position, end.position);//connection.Key == endNode ? Vector3.Distance(current.position,endNode.position) * 10 : Vector3.Distance(connection.Key.position,endNode.position);//Vector3.Distance(connection.Key.position,endNode.position);
pairAlreadyInOpenlist.d.fScore = pairAlreadyInOpenlist.d.gScore + pairAlreadyInOpenlist.d.hScore;
pairAlreadyInOpenlist.d.cameFrom = current;
}
}
}
return null;
}
/// <summary>
/// Attempts to path from one Node to another. Use ClosestNodeTo and ActualClosestNodeTo to get the nodes.
/// </summary>
/// <param name="start">The start node</param>
/// <param name="end">The end node</param>
/// <returns>A list of nodes that form a path from the node start to the end node.</returns>
public List <NavMesh2DNode> GetPath(NavMesh2DNode start, NavMesh2DNode end)
{
return(RunPathWorker(start, end));
}
/// <summary>
/// Attempts to path from one Node to another. Use ClosestNodeTo and ActualClosestNodeTo to get the nodes.
/// </summary>
/// <param name="start">The start node</param>
/// <param name="end">The end node</param>
/// <returns>A list of nodes that form a path from the node start to the end node.</returns>
public List<NavMesh2DNode> GetPath(NavMesh2DNode start, NavMesh2DNode end)
{
return RunPathWorker(start, end);
}
