public int CompareTo(object obj)
{
NavGraphNode node = (NavGraphNode)obj;
if (Hcost < node.Hcost)
{
return(-1);
}
if (Hcost > node.Hcost)
{
return(1);
}
return(0);
}
public int[] GetPath(int startNodeIndex, int goalNodeIndex)
{
NavGraphNode startNode = Graph.GetNode(startNodeIndex);
NavGraphNode goalNode = Graph.GetNode(goalNodeIndex);
OpenList.Add(startNode);
startNode.Hcost = EstimateHeuristicCost(startNode, goalNode);
startNode.Gcost = 0;
NavGraphNode node = new NavGraphNode(-1);
while (OpenList.Count > 0)
{
node = GetFirstFreeNode();
if (node.Index == goalNode.Index)
{
return(CalculatePath(node));
}
List <NavGraphNode> connectedNodes = Graph.GetConnectedNodes(node.Index);
foreach (NavGraphNode connectedNode in connectedNodes)
{
if (!CloseList.Contains(connectedNode))
{
CalculateCostsForNode(node, connectedNode, goalNode);
if (!OpenList.Contains(connectedNode))
{
AddNodeAndSortList(connectedNode, OpenList);
}
}
}
AddNodeAndSortList(node, CloseList);
RemoveAndSortList(node, OpenList);
}
if (node.Index != goalNode.Index)
{
Debug.LogError("goal no found");
return(null);
}
return(CalculatePath(node));
}
private float EstimateHeuristicCost(NavGraphNode currentNode, NavGraphNode goalNode)
{
return((currentNode.Position - goalNode.Position).magnitude);
}
private void RemoveAndSortList(NavGraphNode node, List <NavGraphNode> nodeList)
{
OpenList.Remove(node);
OpenList.Sort();
}
private void AddNodeAndSortList(NavGraphNode connectedNode, List <NavGraphNode> nodeList)
{
nodeList.Add(connectedNode);
nodeList.Sort();
}
