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);
}
