public void Clear()
{
MPFrontierNode curNode = mHead;
while (curNode != null)
{
MPFrontierNode removal = curNode;
curNode = curNode.Next;
MPFrontierPool.Push(removal);
}
mHead = null;
}
protected virtual bool FindPath(MPPathNode srcNode, MPPathNode destNode)
{
camefrom.Clear();
cost_sofar.Clear();
//has to create a copy, for mem pool clean
mFrontier.Enqueue(MPFrontierPool.Pop(srcNode));
cost_sofar.Add(srcNode.UID, 0);
mPathNodeCache.Add(srcNode.UID, srcNode);
mPathNodeCache.Add(destNode.UID, destNode);
mNeighbourCache.PathNodeCache = mPathNodeCache;
//
while (!mFrontier.IsEmpty)
{
MPFrontierNode frontier = mFrontier.Dequeue();
MPPathNode current = frontier.Node;
MPFrontierPool.Push(frontier);
//check if I've got it
if (current.UID == destNode.UID)
{
break;
}
mNeighbourCache.Reset();
mData.GetPathNeighbours(current, mMaxDeatailX, mMaxDeatailZ, mNeighbourCache);
for (int i = 0; i < mNeighbourCache.Length; ++i)
{
MPPathNode n = mNeighbourCache.Data[i];
//add subdivision level to make path perfer bigger subdivision
float cost = cost_sofar[current.UID] + current.GetCost(n);
if (!cost_sofar.ContainsKey(n.UID) || cost < cost_sofar[n.UID])
{
cost_sofar[n.UID] = cost;
//add distance to goal for heuristic
MPFrontierNode next = MPFrontierPool.Pop(n);
next.Priority = cost + destNode.GetCost(next.Node);
mFrontier.Enqueue(next);
if (isDebug)
{
debugnodes.Enqueue(n);
}
if (camefrom.ContainsKey(n.UID))
{
camefrom[n.UID] = current.UID;
}
else
{
camefrom.Add(n.UID, current.UID);
}
}
}
}
mFrontier.Clear();
if (camefrom.ContainsKey(destNode.UID))
{
//output path
uint currentUID = destNode.UID;
pathResult.Enqueue(mPathNodeCache[destNode.UID]);
while (currentUID != srcNode.UID)
{
if (camefrom.ContainsKey(currentUID))
{
uint from = camefrom[currentUID];
pathResult.Enqueue(mPathNodeCache[from]);
currentUID = from;
}
else
{
break;
}
}
return(true);
}
return(false);
}