public virtual float GetCost(MPPathNode to)
{
float cost = Math.Abs(HeightGrade - to.HeightGrade);
cost += Math.Abs(X - to.X) + Math.Abs(Z - to.Z) + Math.Max(0, Subdivision - to.Subdivision);
return(cost);
}
public void GetPathNeighbours(MPPathNode current, int maxX, int maxZ, MPNeigbours neighbours)
{
ulong[] curSlices = HeightSlicePool.GetSlices(current.SliceHeader);
if (curSlices == null)
{
return;
}
//get current ceiling, where I can't go across
ushort maxH = ushort.MaxValue;
if (current.HIdx < curSlices.Length - 1)
{
ulong higherSlice = curSlices[current.HIdx + 1];
ushort higherf = SliceAccessor.flag(higherSlice);
ushort higherheight = SliceAccessor.heightGrade(higherSlice);
if ((higherf & SliceAccessor.SliceCeiling) > 0)
{
maxH = higherheight;
}
}
for (int u = current.BoundaryXMin; u <= current.BoundaryXMax; ++u)
{
if (u < 0 || u >= maxX)
{
continue;
}
if (current.BoundaryZMin >= 0 && current.BoundaryZMin < maxZ)
{
GetPathNeighbour(current.HeightGrade, maxH, u, current.BoundaryZMin, neighbours);
}
if (current.BoundaryZMax >= 0 && current.BoundaryZMax < maxZ)
{
GetPathNeighbour(current.HeightGrade, maxH, u, current.BoundaryZMax, neighbours);
}
}
for (int v = current.BoundaryZMin; v <= current.BoundaryZMax; ++v)
{
if (v < 0 || v >= maxZ)
{
continue;
}
if (current.BoundaryXMin >= 0 && current.BoundaryXMin < maxX)
{
GetPathNeighbour(current.HeightGrade, maxH, current.BoundaryXMin, v, neighbours);
}
if (current.BoundaryXMax >= 0 && current.BoundaryXMax < maxX)
{
GetPathNeighbour(current.HeightGrade, maxH, current.BoundaryXMax, v, neighbours);
}
}
}
public static MPFrontierNode Pop(MPPathNode p)
{
MPFrontierNode node = null;
if (mqPool.Count > 0)
{
node = mqPool.Dequeue();
}
if (node == null)
{
node = new MPFrontierNode();
}
node.Node = p;
return(node);
}
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();
}
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);
}