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(); }