private List <Vector2> ShowPath(AstarSearchNode root)
{
List <Vector2> list = new List <Vector2>();
AstarSearchNode p = root;
while (p != null)
{
list.Add(p.pnt.Pos);
p = p.pre;
}
list.Reverse();
return(list);
}
public List <Vector2> StartSearch(Vector2 startPos, Vector2 endPos)
{
pathToShow = null;
Dictionary <PolygonPoint, List <PolygonPoint> > routeMap = new Dictionary <PolygonPoint, List <PolygonPoint> >();
if (polygons == null)
{
return(new List <Vector2>());
}
for (int i = 0; i < dis.GetLength(0); i++)
{
for (int j = 0; j < dis.GetLength(1); j++)
{
if (dis[i, j] < float.MaxValue)
{
if (!routeMap.ContainsKey(PolyPoints[i]))
{
routeMap.Add(PolyPoints[i], new List <PolygonPoint>());
}
routeMap[PolyPoints[i]].Add(PolyPoints[j]);
}
}
}
PolygonPoint startPoint = new PolygonPoint(startPos);
PolygonPoint endPoint = new PolygonPoint(null, PolyPoints.Count, endPos);
if (canPass(startPos, endPos))
{
//Debug.Log("直线可达");
return(new List <Vector2>()
{
startPos, endPos
});
}
for (int i = 0; i < polygons.Count; i++)
{
//计算新加入两个点以后的扩展图
if (isPointOutPoly(startPos, polygons[i]) == -1)
{
Debug.Log("初始卡住啦");
return(new List <Vector2>());
}
for (int j = 0; j < polygons[i].Num; j++)
{
if (canPass(startPos, polygons[i].Points[j].Pos))
{
if (!routeMap.ContainsKey(startPoint))
{
routeMap.Add(startPoint, new List <PolygonPoint>());
}
routeMap[startPoint].Add(polygons[i].Points[j]);
//add routes
}
if (canPass(endPos, polygons[i].Points[j].Pos))
{
if (!routeMap.ContainsKey(polygons[i].Points[j]))
{
routeMap.Add(polygons[i].Points[j], new List <PolygonPoint>());
}
routeMap[polygons[i].Points[j]].Add(endPoint);
//add routes
}
}
}
float[] disss = new float[PolyPoints.Count + 1];
for (int i = 0; i < disss.Length; i++)
{
disss[i] = float.MaxValue;
}
float originH = (endPoint.Pos - startPoint.Pos).magnitude;
AstarSearchNode firstNode = new AstarSearchNode(startPoint, 0, originH, originH);
AstarSearchNode minHNode = firstNode;
PriorityQueue <AstarSearchNode> pq = new PriorityQueue <AstarSearchNode>(new NodeComparater());
pq.Push(firstNode);
while (pq.Count > 0)
{
AstarSearchNode pHead = pq.Pop();
if (pHead.pnt == endPoint)
{
Debug.Log("you way");
pathToShow = ShowPath(pHead);
//found
return(new List <Vector2>(pathToShow));
}
if (disss[pHead.pnt.PntIdx] < float.MaxValue)
{
//表示来过
continue;
}
if (routeMap.ContainsKey(pHead.pnt))
{
for (int i = 0; i < routeMap[pHead.pnt].Count; i++)
{
PolygonPoint next = routeMap[pHead.pnt][i];
float newG = (next.Pos - pHead.pnt.Pos).magnitude + pHead.g;
float newH = (next.Pos - endPoint.Pos).magnitude;
float newF = newG + newH;
AstarSearchNode newNode = new AstarSearchNode(next, newG, newH, newF);
newNode.pre = pHead;
if (newF < disss[next.PntIdx])
{
pq.Push(newNode);
disss[next.PntIdx] = newF;
}
if (newH < minHNode.h)
{
minHNode = newNode;
}
}
}
}
//Debug.Log("no way");
//如果没找到
//返回minHNode
pathToShow = ShowPath(minHNode);
return(new List <Vector2>(pathToShow));
}
