public 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.TotalProcessedNodes = 0; this.TotalProcessingTime = 0.0f; this.MaxOpenNodes = 0; var initialNode = new NodeRecord { gValue = 0, hValue = this.Heuristic.H(this.StartNode, this.GoalNode), node = this.StartNode }; initialNode.fValue = AStarPathfinding.F(initialNode); this.Open.Initialize(); this.Open.AddToOpen(initialNode); this.Closed.Initialize(); }