public void EndFindPath()
{
pathResult.Clear();
debugnodes.Clear();
isDebug = false;
foreach (var node in mPathNodeCache.Values)
{
MPPathNodePool.Push(node);
}
mPathNodeCache.Clear();
}
public void FindPath(Vector3 start, Vector3 dest, Stack <MPPathResult> result, Queue <MPDebugPlane> qDebug = null)
{
int srcx = 0;
int srcz = 0;
ushort srcH = (ushort)(Mathf.Clamp(start.y - VolumeFloor, 0, VolumeCeiling) / VolumeHSliceT);
TransformPos2UV(start, ref srcx, ref srcz);
ushort fixH = srcH;
MPPathNode srcNode = mData.GetStandablePathNode(srcx, srcz, srcH);
if (srcNode == null)
{
return;
}
int destx = 0;
int destz = 0;
ushort destH = (ushort)(Mathf.Clamp(dest.y - VolumeFloor, 0, VolumeCeiling) / VolumeHSliceT);
TransformPos2UV(dest, ref destx, ref destz);
MPPathNode destNode = mData.GetStandablePathNode(destx, destz, destH);
if (destNode == null)
{
return;
}
//
result.Push(MPPathResultPool.Pop(dest, fixH, 0));
if (srcNode.UID == destNode.UID)
{
MPPathNodePool.Push(srcNode);
MPPathNodePool.Push(destNode);
return;
}
//
Vector3 volumnMin = new Vector3(VolumeCenterX - 0.5f * mVolumeSizeX, VolumeFloor,
VolumeCenterZ - 0.5f * mVolumeSizeZ);
Vector2 minCellSize = new Vector2(mDetailGridX, mDetailGridZ);
if (qDebug != null)
{
isDebug = true;
}
if (FindPath(srcNode, destNode))
{
while (pathResult.Count > 0)
{
MPPathNode node = pathResult.Dequeue();
//use request node as final node
if (node.UID == destNode.UID)
{
continue;
}
if (node.UID == srcNode.UID)
{
result.Push(MPPathResultPool.Pop(start, node.HeightGrade, node.Flag));
continue;
}
Vector3 nodeCenter = volumnMin + node.X * Vector3.right + node.Z * Vector3.forward +
node.HeightGrade * VolumeHSliceT * Vector3.up;
result.Push(MPPathResultPool.Pop(nodeCenter, node.HeightGrade, node.Flag));
}
}
else
{
MPLog.Log("no path result.");
result.Pop();
}
//debug
while (isDebug && debugnodes.Count > 0)
{
MPPathNode node = debugnodes.Dequeue();
MPDebugPlane plane = new MPDebugPlane();
plane.center = volumnMin + node.X * Vector3.right + node.Z * Vector3.forward +
(node.HeightGrade + 2) * VolumeHSliceT * Vector3.up;
plane.size = (1 << node.Subdivision) * minCellSize;
qDebug.Enqueue(plane);
}
EndFindPath();
}