using System.Collections; using System.Collections.Generic; using System.Text; using System.Threading; #if UNITY_EDITOR using UnityEditor; #endif using UnityEngine; namespace Coolape { /// /// CLAS tar path search. 我写的A星寻路,只支持2D /// public class CLAStarPathSearch : MonoBehaviour { public static CLAStarPathSearch current; public int numRows = 10; public int numCols = 10; public float cellSize = 1; //寻路是4方向还是8方向 public NumNeighbours numNeighbours = NumNeighbours.Eight; //扫描类型 public ScanType scanType = ScanType.ObstructNode; //障碍 public LayerMask obstructMask; //可以通行的layer public LayerMask passableMask; //检测障碍时用 public float rayDis4Scan = 1; //检测障碍时用 public RayDirection rayDirection = RayDirection.Up; //自动扫描一次障碍,init后自己用调用 public bool isAutoScan = true; //缓存路径 public bool needCachePaths = false; //过滤掉多余的节(当为true时,障碍物的collider尽量和障碍物保持一至大小,因为是通过射线来检测过滤多余的节点) public bool isFilterPathByRay = false; //柔化路径 public bool isSoftenPath = true; public CLAIPathUtl.SoftenPathType softenPathType = CLAIPathUtl.SoftenPathType.Line; public int softenFactor = 3; //网格基础数据 public GridBase grid = new GridBase(); //节点map public Dictionary nodesMap = new Dictionary(); public bool showGrid = true; public bool showObstruct = true; public bool isIninted = false; [HideInInspector] public Vector3 originPos = Vector3.zero; public ArrayList OnGridStateChgCallbacks = new ArrayList(); //当ray检测后,再检测一次Sphere以保当节点在障碍内部时也可能检测成功 float radius4CheckSphere = 1; //异步的 bool isSearching = false; ListPool listPool = new ListPool(); Queue searchQueue = new Queue(); Queue finishSearchQueue = new Queue(); Dictionary nodesDisMap = new Dictionary(); Dictionary> pathsCache = new Dictionary>(); public CLAStarPathSearch() { current = this; } WaitCallback _threadSearch; WaitCallback threadSearch { get { if (_threadSearch == null) { _threadSearch = new WaitCallback(doSearchPathAsyn); } return _threadSearch; } } // Use this for initialization public void Start() { if (isAutoScan) { init(); } } public void init() { init(transform.position); } /// /// Init this instance.初始化网格 /// 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(); } } /// /// Scan this instance.扫描网格哪些是障碍格 /// public void scan() { pathsCache.Clear(); if (!isIninted) { init(); } for (int i = 0; i < grid.NumberOfCells; i++) { scanOne(i); } onGridStateChg(); } void scanOne(int index) { if (!nodesMap.ContainsKey(index)) { return; } Vector3 position = nodesMap[index].position; if (scanType == ScanType.ObstructNode) { nodesMap[index].isObstruct = raycastCheckCell(position, obstructMask); } else { bool ispass = raycastCheckCell(position, passableMask); bool ishit = raycastCheckCell(position, obstructMask); nodesMap[index].isObstruct = ishit || !ispass; } } bool raycastCheckCell(Vector3 cellPos, LayerMask mask) { bool ishit = false; if (rayDirection == RayDirection.Both) { ishit = Physics.Raycast(cellPos, Vector3.up, rayDis4Scan, mask) || Physics.Raycast(cellPos, -Vector3.up, rayDis4Scan, mask); } else if (rayDirection == RayDirection.Up) { ishit = Physics.Raycast(cellPos, Vector3.up, rayDis4Scan, mask); } else { ishit = Physics.Raycast(cellPos, -Vector3.up, rayDis4Scan, mask); } if (!ishit) { ishit = Physics.CheckSphere(cellPos, radius4CheckSphere, mask); } return ishit; } /// /// Refreshs the range.刷新坐标center,半径为r的网格的障碍状态 /// /// Center. /// The red component.半径格子数 public void scanRange(Vector3 center, int r) { int centerIndex = grid.GetCellIndex(center); scanRange(centerIndex, r); } public void scanRange(int centerIndex, int r) { List cells = grid.getCells(centerIndex, r * 2); for (int i = 0; i < cells.Count; i++) { scanOne(cells[i]); } onGridStateChg(); } /// /// Adds the grid state callback. 添加当网格有变化时的回调 /// /// Callback. public void addGridStateChgCallback(object callback) { if (!OnGridStateChgCallbacks.Contains(callback)) { OnGridStateChgCallbacks.Add(callback); } } /// /// Removes the grid state callback.移除当网格有变化时的回调 /// /// Callback. public void removeGridStateChgCallback(object callback) { OnGridStateChgCallbacks.Remove(callback); } void onGridStateChg() { for (int i = 0; i < OnGridStateChgCallbacks.Count; i++) { Utl.doCallback(OnGridStateChgCallbacks[i]); } } /// /// Revises from node.修正寻路开始节点 /// /// The from node. /// Org from node. 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; } /// /// Searchs the path. 异步的寻路 /// /// true, if path was searched,可以到达 false otherwise.不可到过 /// From.出发点坐标 /// To.目标点坐标 /// finish Search Callback public void searchPathAsyn(Vector3 from, Vector3 to, object finishSearchCallback) { //bool canReach = false; //List vectorList = new List(); //if (getCachePath(from, to, ref vectorList, ref canReach)) //{ // Utl.doCallback(finishSearchCallback, canReach, vectorList); // return; //} ArrayList list = listPool.borrow(); list.Add(from); list.Add(to); list.Add(finishSearchCallback); searchQueue.Enqueue(list); if (!isSearching) { ThreadEx.exec2(threadSearch); } } void doSearchPathAsyn(object obj) { if (searchQueue.Count == 0) { isSearching = false; return; } isSearching = true; ArrayList list = searchQueue.Dequeue(); Vector3 from = (Vector3)(list[0]); Vector3 to = (Vector3)(list[1]); object callback = list[2]; int fromIndex = grid.GetCellIndex(from); int toIndex = grid.GetCellIndex(to); List outPath = null; bool isCachePath = false; bool canReach = searchPath(from, to, ref outPath, ref isCachePath, true); list.Clear(); list.Add(callback); list.Add(canReach); list.Add(outPath); list.Add(isCachePath); list.Add(fromIndex); list.Add(toIndex); finishSearchQueue.Enqueue(list); doSearchPathAsyn(null); } //缓存数据,方便可以快速找到数据 void cachePaths(int fromIndex, int toIndex, List vectorList) { int rang = 6; List list1 = null; List list2 = null; list1 = grid.getCells(fromIndex, rang); list2 = grid.getCells(toIndex, rang); StringBuilder sb = new StringBuilder(); string key = ""; for (int i = 0; i < list1.Count; i++) { for (int j = 0; j < list2.Count; j++) { if (list1[i] < 0 || list2[j] < 0) continue; sb.Clear(); key = sb.Append(list1[i]).Append("_").Append(list2[j]).ToString(); pathsCache[key] = vectorList; sb.Clear(); key = sb.Append(list2[j]).Append("_").Append(list1[i]).ToString(); pathsCache[key] = vectorList; } } sb.Clear(); } public bool getCachePath(Vector3 from, Vector3 to, ref List vectorList, ref bool canReach) { int fromIndex = grid.GetCellIndex(from); int toIndex = grid.GetCellIndex(to); string key = fromIndex + "_" + toIndex; List tmpPath = null; if (pathsCache.TryGetValue(key, out tmpPath)) { if (vectorList == null) { vectorList = new List(); } else { vectorList.Clear(); } vectorList.Add(from); //把路径的第一个点换成新的起始点 for (int i = 1; i < tmpPath.Count; i++) { vectorList.Add(tmpPath[i]); } int index = grid.GetCellIndex(vectorList[vectorList.Count - 1]); if (index == toIndex) { canReach = true; } else { canReach = false; } return true; } return false; } /// /// Searchs the path.寻路 /// /// true, if path was searched,可以到达 false otherwise.不可到过 /// From.出发点坐标 /// To.目标点坐标 /// Vector list.路径点坐标列表 public bool searchPath(Vector3 from, Vector3 to, ref List vectorList) { bool isCachePath = false; return searchPath(from, to, ref vectorList, ref isCachePath); } public bool searchPath(Vector3 from, Vector3 to, ref List vectorList, ref bool isCachePath, bool notPocSoftenPath = false) { if (!isIninted) { init(); } isCachePath = false; int fromIndex = grid.GetCellIndex(from); int toIndex = grid.GetCellIndex(to); if (fromIndex < 0 || toIndex < 0) { Debug.LogWarning("Can not reach"); return false; } CLAStarNode fromNode = nodesMap[fromIndex]; if (fromNode.isObstruct) { fromNode = reviseFromNode(from, fromNode); if (fromNode == null) { Debug.LogWarning("无法到达"); //无法到达 return false; } } if (vectorList == null) { vectorList = new List(); } else { vectorList.Clear(); } if (fromIndex == toIndex) { //就在目标点,直接判断为到达 vectorList.Add(from); vectorList.Add(to); return true; } bool canReach = false; if (getCachePath(from, to, ref vectorList, ref canReach)) { isCachePath = true; return canReach; } // 本次寻路的唯一key,(并发同时处理多个寻路时会用到) string key = fromIndex + "_" + toIndex; CLAStarNode toNode = nodesMap[toIndex]; List openList = new List(); Dictionary closedList = new Dictionary(); // F值缓存 Dictionary fValueMap = new Dictionary(); //先把开始点加入到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; 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); } if (vectorList.Count > 0) { vectorList.Insert(0, from); canReach = false; if (!notPocSoftenPath) { softenPath(ref vectorList); } } return canReach; } public void softenPath(ref List vectorList) { if (isFilterPathByRay) { filterPath(ref vectorList); } if (isSoftenPath) { CLAIPathUtl.softenPath(ref vectorList, softenPathType, softenFactor, cellSize); } } public bool isObstructNode(Vector3 pos) { int index = grid.GetCellIndex(pos); return isObstructNode(index); } public bool isObstructNode(int index) { if (grid.IsInBounds(index)) { CLAStarNode node = nodesMap[index]; return node.isObstruct; } else { return false; } } /// /// Filters the path.过滤掉多余的节(障碍物的collider尽量和障碍物保持一至大小,因为是通过射线来检测过滤多余的节点) /// /// List. public void filterPath(ref List 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); } } } /// /// Calculations the open list.计算可行的点 /// /// From. /// To. /// Open list. void calculationOpenList(string key, CLAStarNode from, CLAStarNode to, ref Dictionary fValueMap, ref List openList, Dictionary 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); } } } /// /// Adds the open node.新加入node,list的最后一位是f值最小的 /// /// Node. /// From. /// To. /// F value map. /// Open list. void addOpenNode(string key, CLAStarNode node, CLAStarNode from, CLAStarNode to, ref Dictionary fValueMap, ref List 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); } /// /// Ises the new open node. 节点是否需要新加入open /// /// true, if new open node was ised, false otherwise. /// Node. /// Open list. /// Closed list. bool isNewOpenNode(CLAStarNode node, List openList, Dictionary closedList) { if (node == null || node.isObstruct || openList.Contains(node) || closedList.ContainsKey(node.index) ) { return false; } return true; } /// /// Distance the specified node1 and node2.两个节点之间的距离 /// /// The distance. /// Node1. /// Node2. 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; } public virtual void Update() { if (finishSearchQueue.Count > 0) { ArrayList list = finishSearchQueue.Dequeue(); object callback = list[0]; bool canReach = (bool)(list[1]); List outPath = list[2] as List; bool isCachePath = (bool)(list[3]); int fromIndex = (int)(list[4]); int toIndex = (int)(list[5]); if (!isCachePath && outPath != null && outPath.Count > 1) { softenPath(ref outPath); if (needCachePaths) { cachePaths(fromIndex, toIndex, outPath); } } Utl.doCallback(callback, canReach, outPath); listPool.release(list); } } //============================================================= //============================================================= //============================================================= #if UNITY_EDITOR Camera sceneCamera; //List _pathList; //public void showPath(List path) { // _pathList = path; //} void OnDrawGizmos() { if (showGrid) { GridBase.DebugDraw(originPos, 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; } } } */ //=========================================== //=========================================== } #endif //============================================== //============================================== public enum RayDirection { Up, /**< Casts the ray from the bottom upwards */ Down, /**< Casts the ray from the top downwards */ Both /**< Casts two rays in either direction */ } public enum NumNeighbours { Four, Eight } public enum ScanType { ObstructNode, PassableNode } public class ListPool { Queue queue = new Queue(); public ArrayList borrow() { if (queue.Count == 0) { newList(); } return queue.Dequeue() as ArrayList; } public void release(ArrayList list) { list.Clear(); queue.Enqueue(list); } void newList() { queue.Enqueue(new ArrayList()); } } } }