/// <summary>
/// Revises from node.修正寻路开始节点
/// </summary>
/// <returns>The from node.</returns>
/// <param name="orgFromNode">Org from node.</param>
CLAStarNode reviseFromNode(Vector3 fromPos, CLAStarNode orgFromNode)
{
if (!orgFromNode.isObstruct)
{
return(orgFromNode);
}
int count = orgFromNode.aroundList.Count;
CLAStarNode node = null;
CLAStarNode fromNode = null;
float dis = -1;
float tmpDis = 0;
for (int i = 0; i < count; i++)
{
node = orgFromNode.aroundList[i];
if (node != null && !node.isObstruct)
{
tmpDis = Vector3.Distance(node.position, fromPos);
if (dis < 0 || tmpDis < dis)
{
dis = tmpDis;
fromNode = orgFromNode.aroundList[i];
}
}
}
return(fromNode);
}
/// <summary>
/// Filters the path.过滤掉多余的节(障碍物的collider尽量和障碍物保持一至大小，因为是通过射线来检测过滤多余的节点)
/// </summary>
/// <param name="list">List.</param>
public void filterPath(ref List <Vector3> list)
{
if (list == null || list.Count < 4)
{
return;
}
Vector3 from = list[0];
int i = 2;
int fromIndex = grid.GetCellIndex(from);
CLAStarNode fromNode = nodesMap[fromIndex];
if (fromNode.isObstruct)
{
//因为list[0]有可能正好在障碍物的里面，这时射线是检测不到的
from = list[1];
i = 3;
}
float dis = 0;
while (i < list.Count)
{
dis = Vector3.Distance(from, list[i]);
if (Physics.Raycast(from, list[i] - from, dis, obstructMask))
{
from = list[i - 1];
i++;
}
else
{
list.RemoveAt(i - 1);
}
}
}
public CLAStarNode getParentNode(string key)
{
CLAStarNode parent = null;
if (parentNodesMap.TryGetValue(key, out parent))
{
return(parent);
}
return(null);
}
/// <summary>
/// Ises the new open node. 节点是否需要新加入open
/// </summary>
/// <returns><c>true</c>, if new open node was ised, <c>false</c> otherwise.</returns>
/// <param name="node">Node.</param>
/// <param name="openList">Open list.</param>
/// <param name="closedList">Closed list.</param>
bool isNewOpenNode(CLAStarNode node, List <CLAStarNode> openList, Dictionary <int, bool> closedList)
{
if (node == null ||
node.isObstruct ||
openList.Contains(node) ||
closedList.ContainsKey(node.index)
)
{
return(false);
}
return(true);
}
public bool isObstructNode(int index)
{
if (grid.IsInBounds(index))
{
CLAStarNode node = nodesMap[index];
return(node.isObstruct);
}
else
{
return(false);
}
}
//List<Vector3> _pathList;
//public void showPath(List<Vector3> path) {
// _pathList = path;
//}
void OnDrawGizmos()
{
if (showGrid)
{
GridBase.DebugDraw(transform.position, numRows, numCols, cellSize, Color.white);
}
if (showObstruct)
{
Vector3 pos;
for (int i = 0; i < grid.NumberOfCells; i++)
{
CLAStarNode node = nodesMap[i];
if (node.isObstruct)
{
//显示障碍格子
pos = node.position;
Gizmos.color = Color.red;
Gizmos.DrawCube(pos, Vector3.one * cellSize);
Gizmos.color = Color.white;
}
}
}
//===========================================
//显示cell的周围格子============================
/*
* Vector3 mousePosition = Event.current.mousePosition;
* sceneCamera = SceneView.lastActiveSceneView.camera;
* mousePosition.y = sceneCamera.pixelHeight - mousePosition.y;
* mousePosition = sceneCamera.ScreenToWorldPoint(mousePosition);
* //mousePosition.y = -mousePosition.y;
* mousePosition.y = 0;
* int index = grid.GetCellIndex(mousePosition);
* if (index >= 0)
* {
* CLAStarNode node = nodesMap[index];
* if (node != null)
* {
* for (int j = 0; j < node.aroundList.Count; j++)
* {
* pos = node.aroundList[j].position;
* Gizmos.color = Color.green;
* Gizmos.DrawCube(pos, Vector3.one * cellSize);
* Gizmos.color = Color.white;
* }
* }
* }
*/
//===========================================
//===========================================
}
public void init(
CLAStarNode left,
CLAStarNode right,
CLAStarNode up,
CLAStarNode down,
CLAStarNode leftUp,
CLAStarNode leftDown,
CLAStarNode rightUp,
CLAStarNode rightDown)
{
aroundList.Clear();
this.left = left;
if (left != null)
{
aroundList.Add(left);
}
this.right = right;
if (right != null)
{
aroundList.Add(right);
}
this.up = up;
if (up != null)
{
aroundList.Add(up);
}
this.down = down;
if (down != null)
{
aroundList.Add(down);
}
this.leftUp = leftUp;
if (leftUp != null)
{
aroundList.Add(leftUp);
}
this.leftDown = leftDown;
if (leftDown != null)
{
aroundList.Add(leftDown);
}
this.rightUp = rightUp;
if (rightUp != null)
{
aroundList.Add(rightUp);
}
this.rightDown = rightDown;
if (rightDown != null)
{
aroundList.Add(rightDown);
}
}
/// <summary>
/// Init this instance.初始化网格
/// </summary>
public void init(Vector3 origin)
{
isSearching = false;
originPos = origin;
radius4CheckSphere = cellSize / 4;
grid.init(origin, numRows, numCols, cellSize);
nodesMap.Clear();
nodesDisMap.Clear();
pathsCache.Clear();
for (int i = 0; i < grid.NumberOfCells; i++)
{
nodesMap[i] = new CLAStarNode(i, grid.GetCellCenter(i));
}
//设置每个节点的左右上下一周的节点
for (int i = 0; i < grid.NumberOfCells; i++)
{
CLAStarNode left = null;
nodesMap.TryGetValue(grid.LeftIndex(i), out left);
CLAStarNode right = null;
nodesMap.TryGetValue(grid.RightIndex(i), out right);
CLAStarNode up = null;
nodesMap.TryGetValue(grid.UpIndex(i), out up);
CLAStarNode down = null;
nodesMap.TryGetValue(grid.DownIndex(i), out down);
CLAStarNode leftUp = null;
nodesMap.TryGetValue(grid.LeftUpIndex(i), out leftUp);
CLAStarNode leftDown = null;
nodesMap.TryGetValue(grid.LeftDownIndex(i), out leftDown);
CLAStarNode rightUp = null;
nodesMap.TryGetValue(grid.RightUpIndex(i), out rightUp);
CLAStarNode rightDown = null;
nodesMap.TryGetValue(grid.RightDownIndex(i), out rightDown);
if (nodesMap[i] != null)
{
nodesMap[i].init(left, right, up, down, leftUp, leftDown, rightUp, rightDown);
}
}
isIninted = true;
if (isAutoScan)
{
scan();
}
}
/// <summary>
/// Adds the open node.新加入node，list的最后一位是f值最小的
/// </summary>
/// <param name="node">Node.</param>
/// <param name="from">From.</param>
/// <param name="to">To.</param>
/// <param name="fValueMap">F value map.</param>
/// <param name="openList">Open list.</param>
void addOpenNode(string key, CLAStarNode node, CLAStarNode from, CLAStarNode to, ref Dictionary <int, float> fValueMap, ref List <CLAStarNode> openList)
{
float fval = distance(node, from) + distance(node, to);
fValueMap[node.index] = fval;
int i = openList.Count - 1;
for (; i >= 0; i--)
{
float fval2 = fValueMap[openList[i].index];
if (fval <= fval2)
{
break;
}
}
//列表最后是F值最小的
openList.Insert(i + 1, node);
//设置该点的父节点，以便路径回溯时用
node.setParentNode(from, key);
}
/// <summary>
/// Distance the specified node1 and node2.两个节点之间的距离
/// </summary>
/// <returns>The distance.</returns>
/// <param name="node1">Node1.</param>
/// <param name="node2">Node2.</param>
public float distance(CLAStarNode node1, CLAStarNode node2)
{
StringBuilder sb = new StringBuilder();
string key = sb.Append(node1.index).Append("_").Append(node2.index).ToString();
sb.Clear();
string key2 = sb.Append(node2.index).Append("_").Append(node1.index).ToString();
sb.Clear();
float dis = 0;
if (nodesDisMap.TryGetValue(key, out dis))
{
return(dis);
}
dis = Vector3.Distance(node1.position, node2.position);
nodesDisMap[key] = dis;
nodesDisMap[key2] = dis;
return(dis);
}
/// <summary>
/// Init this instance.初始化网格
/// </summary>
public void init()
{
radius4CheckSphere = cellSize / 4;
grid.Awake(transform.position, numRows, numCols, cellSize, false);
for (int i = 0; i < grid.NumberOfCells; i++)
{
nodesMap[i] = new CLAStarNode(i, grid.GetCellCenter(i));
}
//设置每个节点的左右上下一周的节点
for (int i = 0; i < grid.NumberOfCells; i++)
{
CLAStarNode left = null;
nodesMap.TryGetValue(grid.LeftIndex(i), out left);
CLAStarNode right = null;
nodesMap.TryGetValue(grid.RightIndex(i), out right);
CLAStarNode up = null;
nodesMap.TryGetValue(grid.UpIndex(i), out up);
CLAStarNode down = null;
nodesMap.TryGetValue(grid.DownIndex(i), out down);
CLAStarNode leftUp = null;
nodesMap.TryGetValue(grid.LeftUpIndex(i), out leftUp);
CLAStarNode leftDown = null;
nodesMap.TryGetValue(grid.LeftDownIndex(i), out leftDown);
CLAStarNode rightUp = null;
nodesMap.TryGetValue(grid.RightUpIndex(i), out rightUp);
CLAStarNode rightDown = null;
nodesMap.TryGetValue(grid.RightDownIndex(i), out rightDown);
if (nodesMap[i] != null)
{
nodesMap[i].init(left, right, up, down, leftUp, leftDown, rightUp, rightDown);
}
}
isIninted = true;
if (isAutoScan)
{
scan();
}
}
/// <summary>
/// Calculations the open list.计算可行的点
/// </summary>
/// <param name="from">From.</param>
/// <param name="to">To.</param>
/// <param name="openList">Open list.</param>
void calculationOpenList(string key, CLAStarNode from, CLAStarNode to, ref Dictionary <int, float> fValueMap, ref List <CLAStarNode> openList, Dictionary <int, bool> closedList)
{
if (isNewOpenNode(from.up, openList, closedList))
{
addOpenNode(key, from.up, from, to, ref fValueMap, ref openList);
}
if (isNewOpenNode(from.down, openList, closedList))
{
addOpenNode(key, from.down, from, to, ref fValueMap, ref openList);
}
if (isNewOpenNode(from.left, openList, closedList))
{
addOpenNode(key, from.left, from, to, ref fValueMap, ref openList);
}
if (isNewOpenNode(from.right, openList, closedList))
{
addOpenNode(key, from.right, from, to, ref fValueMap, ref openList);
}
if (numNeighbours == NumNeighbours.Eight)
{
if (isNewOpenNode(from.leftUp, openList, closedList))
{
addOpenNode(key, from.leftUp, from, to, ref fValueMap, ref openList);
}
if (isNewOpenNode(from.leftDown, openList, closedList))
{
addOpenNode(key, from.leftDown, from, to, ref fValueMap, ref openList);
}
if (isNewOpenNode(from.rightUp, openList, closedList))
{
addOpenNode(key, from.rightUp, from, to, ref fValueMap, ref openList);
}
if (isNewOpenNode(from.rightDown, openList, closedList))
{
addOpenNode(key, from.rightDown, from, to, ref fValueMap, ref openList);
}
}
}
/// <summary>
/// Distance the specified node1 and node2.两个节点之间的距离
/// </summary>
/// <returns>The distance.</returns>
/// <param name="node1">Node1.</param>
/// <param name="node2">Node2.</param>
public float distance(CLAStarNode node1, CLAStarNode node2)
{
return(Vector3.Distance(node1.position, node2.position));
}
/// <summary>
/// Searchs the path.寻路
/// </summary>
/// <returns><c>true</c>, if path was searched,可以到达 <c>false</c> otherwise.不可到过</returns>
/// <param name="from">From.出发点坐标</param>
/// <param name="to">To.目标点坐标</param>
/// <param name="vectorList">Vector list.路径点坐标列表</param>
public bool searchPath(Vector3 from, Vector3 to, ref List <Vector3> vectorList)
{
if (!isIninted)
{
init();
}
if (vectorList == null)
{
vectorList = new List <Vector3>();
}
else
{
vectorList.Clear();
}
int fromIndex = grid.GetCellIndex(from);
int toIndex = grid.GetCellIndex(to);
if (fromIndex < 0 || toIndex < 0)
{
Debug.LogWarning("Can not reach");
return(false);
}
if (fromIndex == toIndex)
{
//就在目标点，直接判断为到达
vectorList.Add(from);
vectorList.Add(to);
return(true);
}
CLAStarNode fromNode = nodesMap[fromIndex];
if (fromNode.isObstruct)
{
fromNode = reviseFromNode(from, fromNode);
if (fromNode == null)
{
Debug.LogWarning("无法到达");
//无法到达
return(false);
}
}
CLAStarNode toNode = nodesMap[toIndex];
// 本次寻路的唯一key，（并发同时处理多个寻路时会用到）
string key = fromNode.index + "_" + toNode.index;
List <CLAStarNode> openList = new List <CLAStarNode>();
Dictionary <int, bool> closedList = new Dictionary <int, bool>();
// F值缓存
Dictionary <int, float> fValueMap = new Dictionary <int, float>();
//先把开始点加入到closedList
closedList[fromIndex] = true;
//计算一次open点列表
calculationOpenList(key, fromNode, toNode, ref fValueMap, ref openList, closedList);
//离目标点最近的节点
CLAStarNode nodeNearest = fromNode;
CLAStarNode node = null;
float dis4Target = -1;
float tmpdis4Target = 0;
int count = openList.Count;
bool canReach = false;
while (count > 0)
{
node = openList[count - 1];
openList.RemoveAt(count - 1); //从openlist中移除
closedList[node.index] = true; //设为closed
if (node.index == toNode.index)
{
//reached
nodeNearest = node;
canReach = true;
break;
}
// 设置离目标点最新的点
tmpdis4Target = distance(node, toNode);
if (dis4Target < 0 || tmpdis4Target < dis4Target)
{
dis4Target = tmpdis4Target;
nodeNearest = node;
}
//重新处理新的open点
calculationOpenList(key, node, toNode, ref fValueMap, ref openList, closedList);
count = openList.Count;
}
//回溯路径＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝
CLAStarNode parentNode = null;
if (canReach)
{
vectorList.Insert(0, to);
parentNode = nodeNearest.getParentNode(key);
}
else
{
parentNode = nodeNearest;
}
while (parentNode != null && parentNode != fromNode)
{
vectorList.Insert(0, parentNode.position);
parentNode = parentNode.getParentNode(key);
}
vectorList.Insert(0, from);
if (isFilterPathByRay)
{
filterPath(ref vectorList);
}
if (isSoftenPath)
{
CLAIPathUtl.softenPath(ref vectorList, softenPathType, softenFactor, cellSize);
}
return(canReach);
}
public void setParentNode(CLAStarNode preNode, string key)
{
parentNodesMap[key] = preNode;
}