852 lines
29 KiB
C#
852 lines
29 KiB
C#
using System.Collections;
|
||
using System.Collections.Generic;
|
||
using System.Text;
|
||
using System.Threading;
|
||
#if UNITY_EDITOR
|
||
using UnityEditor;
|
||
#endif
|
||
using UnityEngine;
|
||
|
||
namespace Coolape
|
||
{
|
||
|
||
/// <summary>
|
||
/// CLAS tar path search. 我写的A星寻路,只支持2D
|
||
/// </summary>
|
||
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<int, CLAStarNode> nodesMap = new Dictionary<int, CLAStarNode>();
|
||
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<ArrayList> searchQueue = new Queue<ArrayList>();
|
||
Queue<ArrayList> finishSearchQueue = new Queue<ArrayList>();
|
||
Dictionary<string, float> nodesDisMap = new Dictionary<string, float>();
|
||
Dictionary<string, List<Vector3>> pathsCache = new Dictionary<string, List<Vector3>>();
|
||
|
||
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);
|
||
}
|
||
/// <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>
|
||
/// Scan this instance.扫描网格哪些是障碍格
|
||
/// </summary>
|
||
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;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Refreshs the range.刷新坐标center，半径为r的网格的障碍状态
|
||
/// </summary>
|
||
/// <param name="center">Center.</param>
|
||
/// <param name="r">The red component.半径格子数</param>
|
||
public void scanRange(Vector3 center, int r)
|
||
{
|
||
int centerIndex = grid.GetCellIndex(center);
|
||
scanRange(centerIndex, r);
|
||
}
|
||
|
||
public void scanRange(int centerIndex, int r)
|
||
{
|
||
List<int> cells = grid.getCells(centerIndex, r * 2);
|
||
for (int i = 0; i < cells.Count; i++)
|
||
{
|
||
scanOne(cells[i]);
|
||
}
|
||
onGridStateChg();
|
||
}
|
||
|
||
/// <summary>
|
||
/// Adds the grid state callback. 添加当网格有变化时的回调
|
||
/// </summary>
|
||
/// <param name="callback">Callback.</param>
|
||
public void addGridStateChgCallback(object callback)
|
||
{
|
||
if (!OnGridStateChgCallbacks.Contains(callback))
|
||
{
|
||
OnGridStateChgCallbacks.Add(callback);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Removes the grid state callback.移除当网格有变化时的回调
|
||
/// </summary>
|
||
/// <param name="callback">Callback.</param>
|
||
public void removeGridStateChgCallback(object callback)
|
||
{
|
||
OnGridStateChgCallbacks.Remove(callback);
|
||
}
|
||
|
||
void onGridStateChg()
|
||
{
|
||
for (int i = 0; i < OnGridStateChgCallbacks.Count; i++)
|
||
{
|
||
Utl.doCallback(OnGridStateChgCallbacks[i]);
|
||
}
|
||
}
|
||
|
||
/// <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>
|
||
/// 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="finishSearchCallback">finish Search Callback</param>
|
||
public void searchPathAsyn(Vector3 from, Vector3 to, object finishSearchCallback)
|
||
{
|
||
|
||
//bool canReach = false;
|
||
//List<Vector3> vectorList = new List<Vector3>();
|
||
//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<Vector3> 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<Vector3> vectorList)
|
||
{
|
||
int rang = 6;
|
||
List<int> list1 = null;
|
||
List<int> 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<Vector3> vectorList, ref bool canReach)
|
||
{
|
||
int fromIndex = grid.GetCellIndex(from);
|
||
int toIndex = grid.GetCellIndex(to);
|
||
string key = fromIndex + "_" + toIndex;
|
||
List<Vector3> tmpPath = null;
|
||
if (pathsCache.TryGetValue(key, out tmpPath))
|
||
{
|
||
if (vectorList == null)
|
||
{
|
||
vectorList = new List<Vector3>();
|
||
}
|
||
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;
|
||
}
|
||
/// <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)
|
||
{
|
||
bool isCachePath = false;
|
||
return searchPath(from, to, ref vectorList, ref isCachePath);
|
||
}
|
||
public bool searchPath(Vector3 from, Vector3 to, ref List<Vector3> 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<Vector3>();
|
||
}
|
||
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<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;
|
||
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<Vector3> 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;
|
||
}
|
||
}
|
||
|
||
/// <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);
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <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>
|
||
/// 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>
|
||
/// 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;
|
||
}
|
||
|
||
/// <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;
|
||
}
|
||
|
||
public virtual void Update()
|
||
{
|
||
if (finishSearchQueue.Count > 0)
|
||
{
|
||
ArrayList list = finishSearchQueue.Dequeue();
|
||
object callback = list[0];
|
||
bool canReach = (bool)(list[1]);
|
||
List<Vector3> outPath = list[2] as List<Vector3>;
|
||
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<Vector3> _pathList;
|
||
//public void showPath(List<Vector3> 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());
|
||
}
|
||
}
|
||
}
|
||
}
|