/// <summary>
/// 计算父节点G,H,F的数据.
/// </summary>
/// <param name="node">Node.</param>
/// <param name="parentNode">Parent node.</param>
protected void RecordParentNodeAndPathCosts(NNode node, NNode parentNode)
{
node.G = parentNode.G + World.GetGCost(parentNode.Index, node.Index);
node.H = World.GetHCost(node.Index, m_goalNode.Index);
node.F = node.G + node.H;
node.Parent = parentNode;
}
public void StartANewPlan(int startNodeIndex, int goalNodeIndex)
{
if (startNodeIndex == NNode.kInvalidIndex || goalNodeIndex == NNode.kInvalidIndex)
{
m_planStatus = NPlanner.ePlanStatus.kPlanFailed;
return;
}
// 清楚数据
m_nodePool.Clear();
m_openNodes.Clear();
m_solution.Clear();
m_expandedNodes.Clear();
// 设置起始位置
m_startNode = GetNode(startNodeIndex).Item;
m_goalNode = GetNode(goalNodeIndex).Item;
// 初始化成功条件赋值终点位置
m_reachedGoalNodeSuccessCondition.Awake(m_goalNode);
m_successCondition = m_reachedGoalNodeSuccessCondition;
// 初始化G,H,F 将初始点加入路径列表设置状态为正在寻路中
m_startNode.G = 0.0f;
m_startNode.H = World.GetHCost(m_startNode.Index, m_goalNode.Index);
m_startNode.F = m_startNode.G + m_startNode.H;
m_startNode.Parent = null;
OpenNode(m_startNode);
m_planStatus = ePlanStatus.kPlanning;
}
/// <summary>
/// 替换队列中某一个node的数据
/// </summary>
/// <param name="item">The node whose item value is to be set.</param>
/// <exception cref="ArgumentException">
/// The node being returned is invalid.
/// </exception>
/// <remarks>
/// This method is necessary to modify the value of a value type stored
/// in the Pool. It copies the value of the node's Item field into the
/// Pool.
/// This method is an O(1) operation.
/// </remarks>
public void SetItemValue(NNode item)
{
if ((item.NodeIndex < 0) || (item.NodeIndex > pool.Length))
{
throw new ArgumentException("Invalid item node.");
}
pool[item.NodeIndex].Item = item.Item;
}
/// <summary>
/// 将节点添加路径队列
/// </summary>
protected void ConstructSolution()
{
for (NNode nextNode = m_goalNode; nextNode != m_startNode; nextNode = nextNode.Parent)
{
m_solution.AddFirst(nextNode);
}
m_solution.AddFirst(m_startNode);
}
public override bool Evaluate(NNode currentNode)
{
if (m_goalNode == currentNode)
{
return(true);
}
return(false);
}
/// <summary>
/// 将一个数据重新添加进队列
/// </summary>
/// <param name="item">The node to return to the available Pool.</param>
/// <exception cref="ArgumentException">
/// The node being returned is invalid.
/// </exception>
/// <exception cref="InvalidOperationException">
/// The node being returned was not active.
/// This probably means the node was previously returned.
/// </exception>
/// <remarks>
/// This method is an O(1) operation.
/// </remarks>
public void Return(NNode item)
{
if (item.NodeIndex < 0 || item.NodeIndex > pool.Length)
{
throw new ArgumentException("Invalid item node!");
}
if (!active [item.NodeIndex])
{
throw new InvalidOperationException("Attempt to return an inactive node.");
}
active [item.NodeIndex] = false;
available.Enqueue(item.NodeIndex);
}
/// <summary>
/// 检测是否寻路失败
/// </summary>
/// <returns><c>true</c>, if failed was planed, <c>false</c> otherwise.</returns>
/// <param name="currentNode">Current node.</param>
protected bool PlanFailed(NNode currentNode)
{
if (currentNode == m_startNode)
{
return(false);
}
if (m_openNodes.Count == m_maxNumberOfNodes)
{
UnityEngine.Debug.LogWarning("Pathplan failed because it reached the max node count. Try increasing " +
"the Max Number Of Nodes Per Planner variable on the PathManager, through " +
"the Inspector window.");
return(true);
}
return(false);
}
public NPool(int Capacity)
{
if (Capacity <= 0)
{
throw new ArgumentOutOfRangeException(
"Pool must contain at least one item.");
}
pool = new NNode[Capacity];
active = new bool[Capacity];
available = new Queue <int> (Capacity);
for (int i = 0; i < Capacity; i++)
{
pool[i] = new NNode();
pool[i].NodeIndex = i;
pool[i].Item = new T();
active[i] = false;
available.Enqueue(i);
}
}
protected void CloseNode(NNode node)
{
node.State = NNode.eState.kClosed;
}
protected void OpenNode(NNode node)
{
System.Diagnostics.Debug.Assert(node.State != NNode.eState.kOpen);
node.State = NNode.eState.kOpen;
m_openNodes.Add(node);
}
/// <summary>
/// 判断寻路是否成功
/// </summary>
/// <returns><c>true</c>, if succeeded was planed, <c>false</c> otherwise.</returns>
/// <param name="currentNode">Current node.</param>
protected bool PlanSucceeded(NNode currentNode)
{
return(m_successCondition.Evaluate(currentNode));
}
/// <summary>
/// Update the current path plan by running a single cycle of the A* search. A "single A* cycle"
/// expands a single node, and all of its neighbors. To run a full A* search, just run this function
/// repeatedly until the function returns kSuccessfullySolvedPath, or kFailedToSolvePath.
/// Note that the openNodes variable is a binary heap data structure.
///
/// Assumptions: The start node has already been added to the openNodes, and the start node is the only node currently
/// stored inside openNodes.
/// </summary>
/// <returns>
/// Return the status of the path being solved. The path has either been solved, we failed to solve the path, or
/// we are still in progress of solving the path.
/// </returns>
protected ePlanStatus RunSingleAStarCycle()
{
// Note: This failure condition must be tested BEFORE we remove an item from the open heap.
if (m_openNodes.Count == 0)
{
return(ePlanStatus.kPlanFailed);
}
// The current least costing pathnode is considered the "current node", which gets removed from the open list and added to the closed list.
NNode currentNode = m_openNodes.Remove();
CloseNode(currentNode);
if (PlanSucceeded(currentNode))
{
ConstructSolution();
return(ePlanStatus.kPlanSucceeded);
}
else if (PlanFailed(currentNode))
{
return(ePlanStatus.kPlanFailed);
}
int[] neighbors = null;
int numNeighbors = World.GetNeighbors(currentNode.Index, ref neighbors);
for (int i = 0; i < numNeighbors; i++)
{
float actualCostFromCurrentNodeToNeighbor, testCost;
int neighborIndex = neighbors[i];
if (neighborIndex == NNode.kInvalidIndex)
{
// This neighbor is off the map.
continue;
}
NPool <NNode> .NNode neighbor = GetNode(neighborIndex);
if (m_expandedNodes.Count == m_maxNumberOfNodes)
{
UnityEngine.Debug.LogWarning("Pathplan failed because it reached the max node count. Try increasing " +
"the Max Number Of Nodes Per Planner variable on the PathManager, through " +
"the Inspector window.");
return(ePlanStatus.kPlanFailed);
}
switch (neighbor.Item.State)
{
case NNode.eState.kBlocked:
case NNode.eState.kClosed:
// Case 1: Ignore
continue;
case NNode.eState.kUnvisited:
// Case 2: Add to open list
RecordParentNodeAndPathCosts(neighbor.Item, currentNode);
OpenNode(neighbor.Item);
break;
case NNode.eState.kOpen:
// Case 3: Update scores
actualCostFromCurrentNodeToNeighbor = World.GetGCost(currentNode.Index, neighbor.Item.Index);
testCost = currentNode.G + actualCostFromCurrentNodeToNeighbor;
if (testCost < neighbor.Item.G)
{
RecordParentNodeAndPathCosts(neighbor.Item, currentNode);
// Maintain the heap property.
m_openNodes.Remove(neighbor.Item);
m_openNodes.Add(neighbor.Item);
}
break;
default:
System.Diagnostics.Debug.Assert(false, "PathNode is in an invalid state when running a single cycle of A*");
break;
}
;
}
return(ePlanStatus.kPlanning);
}
public void Awake(NNode goalNode)
{
m_goalNode = goalNode;
}
/// <summary> ///确定计划已经成功。如果该计划成功，返回真，否则为假。 /// </summary> /// <param name="currentNode"> ///The node currently being evaluated by the planner. /// </param> /// <returns> /// A <see cref="System.Boolean"/> /// </returns> public abstract bool Evaluate(NNode currentNode);
