bool WalkHoneycomb(int2 vend, Vector3 endPos, List <SearchNode> search, BT.BinaryTree unsearch) { int count = 0; while (true) { SearchNode node = (SearchNode)unsearch.Pop(); if (node == null) { return(false); } //Debug.Log(node.v.x + " " + node.v.y); search.Add(node); if (node.v.Equal(vend)) { return(true); } int current = navData[Int2_Index(node.v)] & 0x3f; int2[] varray = node.v.Neighbour(); int newchild = 0; for (int i = 0; i < varray.Length; i++) { if (!IsIndexValid(varray[i])) { continue; } if (!IsIndexWalkable(varray[i])) { continue; } int by = navData[Int2_Index(varray[i])] & 0x3f; float fHeightOffset = Mathf.Abs(by - current) * LayerHeight; if (fHeightOffset > 0.5f) { continue; } SearchNode n = NewSearchNode(varray[i], endPos); n.SetParent(node, fScale); count++; //Debug.Log(count); newchild++; unsearch.Push(n.weight(), n); MarkSearched(n); } //Debug.Log("Add Node " + newchild); } return(false); }
bool AStar(Vector3 begin, Vector3 end, out List <Vector3> path) { path = null; if (navData == null) { return(false); } int2 vbegin = new int2(); int2 vend = new int2(); if (!GetIndex(begin, ref vbegin) || !GetIndex(end, ref vend)) { return(false); } if (!IsIndexWalkable(vbegin) || !IsIndexWalkable(vend)) { return(false); } //List<SearchNode> lstUnSearch = new List<SearchNode>(); BT.BinaryTree lstUnSearch = new BT.BinaryTree(); SearchNode newNode = NewSearchNode(vbegin, end); lstUnSearch.Push(newNode.weight(), newNode); MarkSearched(newNode); if (!WalkHoneycomb(vend, end, lstSearch, lstUnSearch)) { return(false); } SearchNode node = lstSearch[lstSearch.Count - 1]; path = new List <Vector3>(); while (true) { path.Insert(0, node.p); if (node.parent == null) { break; } else { node = node.parent; } } foreach (SearchNode n in lstSearch) { UnMarkSearched(n); } lstSearch.Clear(); SearchNode no = (SearchNode)lstUnSearch.Pop(); while (true) { if (no == null) { break; } UnMarkSearched(no); no = (SearchNode)lstUnSearch.Pop(); } lstUnSearch.Clear(); return(true); }