// 重绘网格结构，只计算与目标的改变，节省资源
private static void _refresh_grid_partly(Nav2DNode _target)
{
foreach (Nav2DNode _node in mNode_Graph.Values)
{
_node.Calculate_LinkedNodes_Partly(_target);
}
}
// 重新计算各个顶点权值
private static void _refresh_cost(Nav2DNode _targetNode)
{
foreach (Nav2DNode _node in mNode_Graph.Values)
{
_node.G_Cost = 0;
_node.H_Cost = _calculate_cost(_node, _targetNode);
}
}
// 计算网格信息
private static void _calculate_grid()
{
if (mNode_Graph == null)
{
mNode_Graph = new Dictionary <int, Nav2DNode>();
}
else
{
mNode_Graph.Clear();
}
Nav2DNode.Reset_NodeUniqueID_StartFromZero();
Nav2DNonWalkable[] _areas = GameObject.FindObjectsOfType <Nav2DNonWalkable>();
foreach (Nav2DNonWalkable _area in _areas)
{
_area.UpdateVertexBuffer();
Vector3[] _vertexs = _area.GetVertexBuffer().ToArray();
for (int i = 0; i < _vertexs.Length; i++)
{
Nav2DNode _node = new Nav2DNode(_vertexs[i]);
mNode_Graph.Add(_node.NodeID, _node);
if (!_is_vertext_valid(_vertexs[i]))
{
_node.ReachAble = false;
}
}
// 分别将多边形左右节点加入可视列表
for (int i = Nav2DNode.Node_UniqueID - _vertexs.Length; i < Nav2DNode.Node_UniqueID; i++)
{
if (i - 1 >= 0)
{
mNode_Graph[i].AdjNodeLeft = mNode_Graph[i - 1];
}
else
{
mNode_Graph[i].AdjNodeLeft = mNode_Graph[Nav2DNode.Node_UniqueID - 1];
}
if (i + 1 < Nav2DNode.Node_UniqueID)
{
mNode_Graph[i].AdjNodeRight = mNode_Graph[i + 1];
}
else
{
mNode_Graph[i].AdjNodeRight = mNode_Graph[Nav2DNode.Node_UniqueID - _vertexs.Length];
}
}
}
}
// 得到路径
private static List <Nav2DNode> _retrieve_path(Nav2DNode _node)
{
List <Nav2DNode> _resultSet = new List <Nav2DNode>();
Nav2DNode _currentNode = _node;
while (_currentNode.PrevNode != null)
{
_resultSet.Add(_currentNode);
_currentNode = _currentNode.PrevNode;
}
// 结果倒叙
_resultSet.InvertedOrder();
// 返回结果列表
return(_resultSet);
}
// 得到下一个顶点
private static Nav2DNode _min_fcostnode(List <Nav2DNode> _list)
{
if (_list == null && _list.Count == 0)
{
return(null);
}
Nav2DNode _result = _list[0];
for (int i = 1; i < _list.Count; i++)
{
if (_list[i].F_Cost < _result.F_Cost)
{
_result = _list[i];
}
}
return(_result);
}
// 增加开始和结束两个节点进图
private static List <Nav2DNode> _add_StartEnd_graph(Nav2DNode _start, Nav2DNode _end)
{
// start
// 如果起始节点在不可行走区域
if (Physics2D.OverlapPoint(_start.NodePosition, 1 << Nav2DConst.Non_Walkable_LayerID))
{
List <Nav2DNode> _path = new List <Nav2DNode>();
_path.Add(_min_disnode(_start));
return(_path);
}
_start.Calculate_LinkedNodes_All(GetGraph());
mNode_Graph.Add(_start.NodeID, _start);
// end
_end.Calculate_LinkedNodes_All(GetGraph());
mNode_Graph.Add(_end.NodeID, _end);
return(null);
}
private static Nav2DNode _min_disnode(Nav2DNode _cur)
{
int _curID = 1;
int _minID = 0;
float _minDis = 0;
_minDis = Vector3.Distance(mNode_Graph[0].NodePosition, _cur.NodePosition);
while (_curID < mNode_Graph.Count)
{
float _temp = Vector3.Distance(mNode_Graph[_curID].NodePosition, _cur.NodePosition);
if (_temp < _minDis)
{
_minDis = _temp;
_minID = _curID;
}
_curID++;
}
return(mNode_Graph[_minID]);
}
/// <summary>
/// 只计算这个点和目标顶点的连接情况
/// </summary>
/// <param name="_targetNode"></param>
public void Calculate_LinkedNodes_Partly(Nav2DNode _targetNode)
{
if (this.VisibleNodesCount_ExceptTarget < this.VisibleNodes.Count)
{
this.VisibleNodes.RemoveAt(this.VisibleNodes.Count - 1);
}
Vector3 _dir = this.NodePosition - _targetNode.NodePosition;
if (_targetNode == this)
{
return;
}
if (!Physics2D.Raycast(_targetNode.NodePosition + _dir.normalized * 0.1f, _dir, _dir.magnitude - 0.2f, 1 << Nav2DConst.Non_Walkable_LayerID))
{
// 如果与目标点相连
if (!this.VisibleNodes.Contains(_targetNode))
{
this.VisibleNodes.Add(_targetNode);
}
}
}
// 计算路径
public static List <Nav2DNode> Calculate_Navigation(Vector3 _startPos, Vector3 _endPos)
{
// 初始化开表 和 闭表
mOpenSet.Clear();
mCloseSet.Clear();
// 创建起始节点
Nav2DNode _start = new Nav2DNode(_startPos);
Nav2DNode _end = new Nav2DNode(_endPos);
// 添加起始节点进网格,检查当前的位置是否在可行走的区域内，如果不在，则返回最近的可行走点作为路径返回
List <Nav2DNode> _walkableCheck = _add_StartEnd_graph(_start, _end);
if (_walkableCheck != null)
{
return(_walkableCheck);
}
// 更新网格信息
// 动态重绘，这个选项将重绘所有网格信息，消耗较大
// 静态重绘，这个选项只会绘制有变化的网格信息，消耗较小
if (DynamicGraph)
{
_refresh_grid_all();
}
else
{
_refresh_grid_partly(_end);
}
// 重新计算各个顶点的权值，权值分为三个分别是 估计值 实际值 最终评估值
// 估计值：只与当前顶点到目标点的直接距离
// 实际值：起始顶点通过寻路图到当前节点的路程
// 评估值：估计值 加上 实际值
_refresh_cost(_end);
// 将起始顶点加入开表中
mOpenSet.Add(_start);
// 如果开表中还有顶点
while (mOpenSet.Count > 0)
{
// 从开表中选择评估值最小的节点
Nav2DNode _current = _min_fcostnode(mOpenSet);
//Debug.Log("当前路径所在的节点为" + _current.NodeID + "------------------------" );
// 如果没有最小值或者开表为空了，则空，表示寻路完成并且没有找到可行路径
if (_current == null)
{
//Debug.Log("没有有效的路径");
return(null);
}
// ..
// ..
// ..
// 如果当前顶点已连接顶点已连接终点，则返回路径
if (_current.VisibleNodes.Contains(_end))
{
//Debug.Log("-------------------------找到路径，最终节点为" + _current.NodeID);
// 将最终顶点的前置节点设置为当前顶点
_end.PrevNode = _current;
// 返回路径，并从图中清除此次寻路的起始顶点
mNode_Graph.Remove(_start.NodeID);
mNode_Graph.Remove(_end.NodeID);
return(_retrieve_path(_end));
}
// 当前顶点考察完毕，把当前顶点加入闭表中
mCloseSet.Add(_current);
// 把当前顶点从开表中移除
mOpenSet.Remove(_current);
// 现在开始访问当前顶点的所有相连的顶点
for (int i = 0; i < _current.VisibleNodes.Count; i++)
{
// 如果这个顶点为空，则直接访问下一个顶点
// 如果这个顶点在闭表中，直接访问下一个顶点
if (_current.VisibleNodes[i] == null || mCloseSet.Contains(_current.VisibleNodes[i]))
{
//Debug.Log("查看子节点" + _current.VisibleNodes[i].NodeID + ">>>>>在闭表中或者这个节点为空");
continue;
}
// 如果这个顶点不合法或者不可到达
if (!_current.VisibleNodes[i].ReachAble)
{
continue;
}
// 通过当前节点路径到这个顶点的实际值
float _toChildGCost = _current.G_Cost + _calculate_cost(_current, _current.VisibleNodes[i]);
// 如果这个顶点在不在开表中
if (!mOpenSet.Contains(_current.VisibleNodes[i]))
{
// 设置这个顶点的实际值
_current.VisibleNodes[i].G_Cost = _toChildGCost;
// 设置这个顶点的前置节点为当前节点
_current.VisibleNodes[i].PrevNode = _current;
//Debug.Log("查看子节点" + _current.VisibleNodes[i].NodeID + ">>>>>不在开表中，加入开表>>>" + "设置Prev为" + _current.NodeID);
// 把这个节点添加进开表中
mOpenSet.Add(_current.VisibleNodes[i]);
}
else
{
// 如果这个顶点在开表中，并且其实际值大于当前节点路径到这个顶点（没有当前路径优）
if (_current.VisibleNodes[i].G_Cost > _toChildGCost)
{
// 更新其路径，并把这个顶点的前置节点设置为当前节点
_current.VisibleNodes[i].G_Cost = _toChildGCost;
_current.VisibleNodes[i].PrevNode = _current;
//Debug.Log("查看子节点" + _current.VisibleNodes[i].NodeID + ">>>>>在开表中且优于当前路径");
}
}
}
}
//Debug.Log("没有找到路径");
// 查看完所有顶点，并没有找到路径
// 按道理来说程序是不会运行到这里的
// ..
mNode_Graph.Remove(_start.NodeID);
mNode_Graph.Remove(_end.NodeID);
return(null);
}
// 计算两点之间的消耗
private static float _calculate_cost(Nav2DNode _from, Nav2DNode _to)
{
return(Vector3.Distance(_from.NodePosition, _to.NodePosition));
}