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