public bool isLastContain(List <AStarNodeItem> pList, AStarNodeItem pNode) { foreach (var v in pList) { if (v.x == pNode.x && v.y == pNode.y) { return(true); } } return(false); }
// A* ALG. /* * @ param src: start point * @ param des: end point * @param pAStarComputer: find path method * @return : a list point as Stack * */ public Stack <Vector3> AStarFindPath(Vector3 src, Vector3 des, AStarComputer pAStarComputer) { PriorityQueue <AStarNodeItem> openList = new PriorityQueue <AStarNodeItem> (10, new AStarNodeItemCamparer()); List <AStarNodeItem> closedList = new List <AStarNodeItem> (); Stack <Vector3> pathLast = new Stack <Vector3> (); Vector2 start = getTilefromPosition(new Vector2(src.x, src.z)); Vector2 end = getTilefromPosition(new Vector2(des.x, des.z)); int count = 0; if (!(start.x == end.x && start.y == end.y)) { AStarNodeItem AStarforStart = new AStarNodeItem(src, (int)start.x, (int)start.y); AStarforStart.gCost = 0; AStarforStart.hCost = pAStarComputer(src, des); openList.Push(AStarforStart); } while (openList.Count != 0) { if (count >= 1000) { break; } AStarNodeItem tAStarNodeItem = openList.Pop(); if (tAStarNodeItem.x == end.x && tAStarNodeItem.y == end.y) { while (tAStarNodeItem.parent != null) { int index = mDictionary ["Collision"]; pathLast.Push(tAStarNodeItem.pos); tAStarNodeItem = tAStarNodeItem.parent; } return(pathLast); } closedList.Add(tAStarNodeItem); for (int i = 0; i < NineGrid.Length; i++) { if (tAStarNodeItem.x + NineGrid [i].x < 0 || tAStarNodeItem.x + NineGrid [i].x >= wtileCount || tAStarNodeItem.y + NineGrid [i].y >= htileCount || tAStarNodeItem.y + NineGrid [i].y < 0) { continue; } Vector2 tv = new Vector2(tAStarNodeItem.x + NineGrid[i].x, tAStarNodeItem.y + NineGrid[i].y); Vector3 tp = GetPositionByTile(tv); AStarNodeItem ttAStarNodeItem = new AStarNodeItem(tp, (int)tv.x, (int)tv.y); if (isLastContain(closedList, ttAStarNodeItem) || isWall(new Vector2(tAStarNodeItem.x + NineGrid [i].x, tAStarNodeItem.y + NineGrid [i].y))) { continue; } ttAStarNodeItem.parent = tAStarNodeItem; ttAStarNodeItem.gCost = (int)(NineGrid [i].magnitude * 10); ttAStarNodeItem.hCost = pAStarComputer(tp, des); openList.Push(ttAStarNodeItem); } count++; } //Debug.Log ("NEW"); return(new Stack <Vector3>()); }