public float DstimateDistance(I_PathNode a, I_PathNode b)
{
DiamondGrid gridA = a as DiamondGrid;
DiamondGrid gridB = b as DiamondGrid;
return(Math.Abs(gridA.WorldX - gridB.WorldX) + Math.Abs(gridA.WorldZ - gridB.WorldZ));
}
//1.732101616628176
public float AdjacentDistance(I_PathNode a, I_PathNode b)
{
DiamondGrid gridA = a as DiamondGrid;
DiamondGrid gridB = b as DiamondGrid;
return(gridA.Distance(gridB));
}
void DoLoop()
{
SortedDictionary <float, List <Node> > .Enumerator it = m_OpenListIndexByF.GetEnumerator();
it.MoveNext();
Node currNode = it.Current.Value[0];
m_CloseList.Add(currNode.node, currNode);
RemoveFromOpenList(currNode);
I_PathNode[] adjacentNodes = currNode.node.Adjacent;
int adjacentNodeCount = adjacentNodes.Length;
for (int i = 0; i < adjacentNodeCount; i++)
{
I_PathNode aNode = adjacentNodes[i];
if (
m_CloseList.ContainsKey(aNode) || //已经在关闭列表中
aNode.IsObstacle //路点是一个障碍物
)
{
continue;
}
if (m_OpenListIndexByNode.ContainsKey(aNode))//已经在开启列表中
{
Node nodeObj = m_OpenListIndexByNode[aNode];
Node backParent = nodeObj.Parent;
nodeObj.Parent = currNode;
float newG = CountG(nodeObj);
if (newG >= nodeObj.G) //新的移动消耗大于旧的
{
nodeObj.Parent = backParent; //还原旧的移动策略
}
else
{
//从原有F索引中删除
_RemoveFromOpenListIndexByF(nodeObj);
//应用新的G值
nodeObj.G = newG;
//重新添加到F索引
_AddToOpenListIndexByF(nodeObj);
}
}
else//不在开启队列
{
AddToOpenList(aNode, currNode);
}
}
}
void AddToOpenList(I_PathNode a, Node parent)
{
Node aa = new Node();
aa.node = a;
aa.Parent = parent;
aa.H = m_Map.DstimateDistance(a, m_End);
aa.G = CountG(aa);
if (aa.G > m_limitG)
{
return; //移动消耗超出限制
}
_AddToOpenListIndexByF(aa);
m_OpenListIndexByNode.Add(aa.node, aa);
}
/// <summary>
/// 寻路
/// </summary>
/// <param name="map"></param>
/// <param name="start"></param>
/// <param name="end"></param>
/// <param name="limitG">限定寻路最大移动消耗</param>
/// <param name="limitOutCount">限定寻路结果节点输出数量</param>
/// <returns></returns>
public I_PathNode[] FindWay(I_Map map, I_PathNode start, I_PathNode end, float limitG, int limitOutCount)
{
Clear();
m_Map = map;
m_Start = start; //起点
m_End = end; //终点
m_limitG = limitG;
m_limitOutCount = limitOutCount;
AddToOpenList(start, null);//将起点加入开启列表
do
{
DoLoop();
}while(!CheckResult());
return(m_Result);
}
void FillResult(Node node)
{
if (node == null || node.Parent == null)
{
return;
}
m_FillResultCount++;
FillResult(node.Parent);
if (m_FillResultIndex >= m_FillResultCount)
{
return;
}
if (m_FillResultIndex == 0)
{
m_FillResultCount = Math.Min(m_FillResultCount, m_limitOutCount);
m_Result = new I_PathNode[m_FillResultCount];
}
m_Result[m_FillResultIndex++] = node.node;
}