public virtual void InitializePathfindingSearch(Vector3 startPosition, Vector3 goalPosition) { this.StartPosition = startPosition; this.GoalPosition = goalPosition; this.StartNode = this.Quantize(this.StartPosition); this.GoalNode = this.Quantize(this.GoalPosition); //if it is not possible to quantize the positions and find the corresponding nodes, then we cannot proceed if (this.StartNode == null || this.GoalNode == null) return; //I need to do this because in Recast NavMesh graph, the edges of polygons are considered to be nodes and not the connections. //Theoretically the Quantize method should then return the appropriate edge, but instead it returns a polygon //Therefore, we need to create one explicit connection between the polygon and each edge of the corresponding polygon for the search algorithm to work ((NavMeshPoly)this.StartNode).AddConnectedPoly(this.StartPosition); ((NavMeshPoly)this.GoalNode).AddConnectedPoly(this.GoalPosition); this.InProgress = true; this.TotalExploredNodes = 0; this.TotalProcessingTime = Time.time; this.MaxOpenNodes = 0; var initialNode = new NodeRecord { gValue = 0, hValue = this.Heuristic.H(this.StartNode.Position, this.GoalNode.Position), node = this.StartNode }; initialNode.fValue = AStarPathfinding.F(initialNode); this.Open.Initialize(); this.Open.AddToOpen(initialNode); this.Closed.Initialize(); }
protected override void ProcessChildNode(NodeRecord bestNode, NavigationGraphEdge connectionEdge, int edgeIndex) { float g; float h; float f; var childNode = connectionEdge.ToNode; var childNodeRecord = this.NodeRecordArray.GetNodeRecord(childNode); if (childNodeRecord == null) { //this piece of code is used just because of the special start nodes and goal nodes added to the RAIN Navigation graph when a new search is performed. //Since these special goals were not in the original navigation graph, they will not be stored in the NodeRecordArray and we will have to add them //to a special structure //it's ok if you don't understand this, this is a hack and not part of the NodeArrayA* algorithm, just do NOT CHANGE THIS, or your algorithm will not work childNodeRecord = new NodeRecord { node = childNode, parent = bestNode, status = NodeStatus.Unvisited }; this.NodeRecordArray.AddSpecialCaseNode(childNodeRecord); } /* Here we calculate the cost so far, the heuristic value and the f value */ Vector3 childNodePosition = childNode.Position; g = bestNode.gValue + (childNodePosition - bestNode.node.Position).magnitude; h = base.Heuristic.H(childNodePosition, this.GoalNode.Position); f = AStarPathfinding.F(g, h); /* Set values of first node */ if (childNodeRecord.status == NodeStatus.Unvisited) { // Set values of child node childNodeRecord.gValue = g; childNodeRecord.hValue = h; childNodeRecord.fValue = f; childNodeRecord.parent = bestNode; this.Open.AddToOpen(childNodeRecord); } else if (childNodeRecord.status == NodeStatus.Open && f < childNodeRecord.fValue) { childNodeRecord.gValue = g; childNodeRecord.fValue = f; childNodeRecord.parent = bestNode; } else if (childNodeRecord.status == NodeStatus.Closed && f < childNodeRecord.fValue) { childNodeRecord.gValue = g; childNodeRecord.fValue = f; childNodeRecord.parent = bestNode; this.Open.AddToOpen(childNodeRecord); } this.MaxOpenNodes = Mathf.Max(this.MaxOpenNodes, this.Open.CountOpen()); }