public static bool AddRight(PNavColumn[,] columns, PNavPoint point, out PNavPoint pOut, PNavPoint pointMax, int verticalDrop, int edgeGap, int islandIndex, PNavIsland island, Queue <PNavPoint> queue)
{
PNavColumn column = columns[point.x, point.z];
PNavNode node = column.SurfaceNode();
bool valid = GetRightPoint(point, pointMax, 1, out pOut);
bool connected = false;
bool alreadyAdded = false;
if (valid)
{
PNavColumn c = columns[pOut.x, pOut.z];
PNavNode sn = c.SurfaceNode();
//already checked
if (sn != null && sn.islandIndex >= 0)
{
alreadyAdded = true;
}
connected = column.IsConnected(c, verticalDrop);
if (connected && !alreadyAdded)
{
sn.islandIndex = islandIndex;
island.nodes.Add(sn);
//add top to queue
PNavPoint pTopOut;
bool addedTop = AddTop(columns, pOut, out pTopOut, pointMax, verticalDrop, edgeGap, islandIndex, island);
if (addedTop)
{
queue.Enqueue(pTopOut);
}
//add bottom to queue
PNavPoint pBottomOut;
bool addedBottom = AddBottom(columns, pOut, out pBottomOut, pointMax, verticalDrop, edgeGap, islandIndex, island);
if (addedBottom)
{
queue.Enqueue(pBottomOut);
}
}
}
if (!connected)
{
node.type = node.type | (int)ParallelNavIslandNodeType.RightEdge;
}
return(connected && !alreadyAdded);
}
public bool IsConnected(PNavColumn c, int verticalDrop)
{
if (type == ParallelNavColumnType.Walkable && c.type == ParallelNavColumnType.Walkable)
{
int surfaceNodeIndex = c.surfaceNodeIndexes[0];
if (Mathf.Abs(surfaceNodeIndexes[0] - surfaceNodeIndex) <= verticalDrop)
{
return(true);
}
}
return(false);
}
public static void DrawAABB(PNavMesh pNavMesh)
{
if (pNavMesh.columns != null)
{
for (int x = 0; x < pNavMesh.columns.GetLength(0); x++)
{
//if (x * pNavMesh.gridSize.x < 95)
{
//continue;
}
for (int z = 0; z < pNavMesh.columns.GetLength(1); z++)
{
PNavColumn column = pNavMesh.columns[x, z];
for (int y = 0; y < column.nodes.Length; y++)
{
int objectCount = column.nodes[y].objectCount;
Fix64Vec3 l = column.nodes[y].lower;
Fix64Vec3 u = column.nodes[y].upper;
//if (l.x * pNavMesh.gridSize.x < 30)
{
//continue;
}
if (objectCount > 0)
{
SceneDebugDraw.DrawAABB(l, u, Color.green);
}
else
{
//DrawAABB(l, u, Color.green);
}
}
}
}
}
}
public static bool AddBottom(PNavColumn[,] columns, PNavPoint point, out PNavPoint pOut, PNavPoint pointMax, int verticalDrop, int edgeGap, int islandIndex, PNavIsland island)
{
PNavColumn column = columns[point.x, point.z];
PNavNode node = column.SurfaceNode();
bool valid = GetBottomPoint(point, pointMax, edgeGap, out pOut);
bool connected = false;
bool alreadyAdded = false;
if (valid)
{
PNavColumn c = columns[pOut.x, pOut.z];
PNavNode sn = c.SurfaceNode();
//already checked
if (sn != null && sn.islandIndex >= 0)
{
alreadyAdded = true;
}
connected = column.IsConnected(c, verticalDrop);
if (connected && !alreadyAdded)
{
sn.islandIndex = islandIndex;
island.nodes.Add(sn);
}
}
if (!connected)
{
node.type = node.type | (int)ParallelNavIslandNodeType.BottomEdge;
}
return(connected && !alreadyAdded);
}
static bool DetectIsland(PNavColumn[,] columns, PNavPoint point, PNavPoint pointMax, int edgeGap, int verticalDrop, int islandIndex, PNavIsland island)
{
PNavColumn column = columns[point.x, point.z];
if (column.type != ParallelNavColumnType.Walkable)
{
return(false);
}
PNavNode surfaceNode = column.SurfaceNode();
if (surfaceNode == null)
{
return(false);
}
//already checked
if (surfaceNode.islandIndex >= 0)
{
return(false);
}
surfaceNode.islandIndex = islandIndex;
island.nodes.Add(surfaceNode);
Queue <PNavPoint> queue = new Queue <PNavPoint>();
queue.Enqueue(point);
while (queue.Count > 0)
{
PNavPoint p = queue.Dequeue();
PNavColumn c = columns[p.x, p.z];
//check top edge
PNavPoint pTopOut;
bool addedTop = PNavMeshHelper.AddTop(columns, p, out pTopOut, pointMax, verticalDrop, edgeGap, islandIndex, island);
if (addedTop)
{
queue.Enqueue(pTopOut);
}
//check bottom edge
PNavPoint pBottomOut;
bool addedBottom = PNavMeshHelper.AddBottom(columns, p, out pBottomOut, pointMax, verticalDrop, edgeGap, islandIndex, island);
if (addedBottom)
{
queue.Enqueue(pBottomOut);
}
//search left
PNavPoint left = p;
while (true)
{
PNavPoint pLeftOut;
bool added = PNavMeshHelper.AddLeft(columns, left, out pLeftOut, pointMax, verticalDrop, edgeGap, islandIndex, island, queue);
if (!added)
{
break;
}
else
{
left = pLeftOut;
}
}
//search right
PNavPoint right = p;
while (true)
{
PNavPoint pRightOut;
bool added = PNavMeshHelper.AddRight(columns, right, out pRightOut, pointMax, verticalDrop, edgeGap, islandIndex, island, queue);
if (!added)
{
break;
}
else
{
right = pRightOut;
}
}
}
return(true);
}
public static void Process(PNavMesh pNavMesh)
{
ParallelRigidbody3D[] pRigidbody3Ds = GameObject.FindObjectsOfType <ParallelRigidbody3D>();
using (new SProfiler($"Insert static bodies"))
{
//insert static bodies
foreach (ParallelRigidbody3D rigidbody3D in pRigidbody3Ds)
{
if (rigidbody3D.bodyType != BodyType.Static)
{
continue;
}
rigidbody3D.AddToWorldForPathFinding();
}
}
// make grid
pNavMesh.xCount = (int)((pNavMesh.worldSize.x) / pNavMesh.gridSize.x);
pNavMesh.yCount = (int)((pNavMesh.worldSize.y) / pNavMesh.gridSize.y);
pNavMesh.zCount = (int)((pNavMesh.worldSize.z) / pNavMesh.gridSize.z);
PNavColumn[,] columns = new PNavColumn[pNavMesh.xCount, pNavMesh.zCount];
for (int x = 0; x < columns.GetLength(0); x++)
{
for (int z = 0; z < columns.GetLength(1); z++)
{
PNavColumn column = new PNavColumn();
column.nodes = new PNavNode[pNavMesh.yCount];
column.type = ParallelNavColumnType.Empty;
column.surfaceNodeIndexes = new int[1];
column.surfaceNodeIndexes[0] = -1;
columns[x, z] = column;
}
}
pNavMesh.columns = columns;
using (new SProfiler($"CubeCast"))
{
int xStart1 = 0;
int xEnd1 = columns.GetLength(0);
int zStart1 = 0;
int zEnd1 = columns.GetLength(1);
int yStart1 = 0;
int yEnd1 = pNavMesh.yCount;
CubeCastInRange(pNavMesh, xStart1, xEnd1, zStart1, zEnd1, yStart1, yEnd1);
/*
* int scale2 = 5;
*
* CheckVolume(xStart1, xEnd1, zStart1, zEnd1, yStart1, yEnd1, scale1, (xS, xE, zS, zE, yS, yE) =>
* {
* CubeCastInRange(xS, xE, zS, zE, yS, yE);
* CheckVolume(xS, xE, zS, zE, yS, yE, scale2, (xS2, xE2, zS2, zE2, yS2, yE2) =>
* {
* CubeCastInRange(xS2, xE2, zS2, zE2, yS2, yE2);
* });
*
* });
*/
}
Parallel3D.CleanUp();
}
public static void CubeCastInRange(PNavMesh pNavMesh, int xStart, int xEnd, int zStart, int zEnd, int yStart, int yEnd)
{
PShapeOverlapResult3D result = new PShapeOverlapResult3D();
if (xEnd > pNavMesh.xCount)
{
xEnd = pNavMesh.xCount;
}
if (zEnd > pNavMesh.zCount)
{
zEnd = pNavMesh.zCount;
}
if (yEnd > pNavMesh.yCount)
{
yEnd = pNavMesh.yCount;
}
Debug.Log("CubeCastInRange:" + " xStart=" + xStart + " xEnd=" + xEnd + " zStart=" + zStart + " zEnd=" + zEnd + " yStart=" + yStart + " yEnd=" + yEnd);
for (int x = xStart; x < xEnd; x++)
{
for (int z = zStart; z < zEnd; z++)
{
PNavColumn column = pNavMesh.columns[x, z];
int surfaceIndex = -1;
for (int y = yStart; y < yEnd; y++)
{
Fix64Vec3 l;
Fix64Vec3 u;
pNavMesh.GetAABB(x, y, z, out l, out u);
Fix64Vec3 center = Fix64.half * (l + u);
Fix64Quat rot = Fix64Quat.identity;
Parallel3D.OverlapCube(
center, rot,
pNavMesh.gridSize.x, pNavMesh.gridSize.y, pNavMesh.gridSize.z,
-1,
result
);
PNavNode node = new PNavNode();
node.lower = l;
node.upper = u;
node.objectCount = result.count;
node.islandIndex = -1;
node.point = new PNavPoint(x, z);
if (result.count > 0 && y > surfaceIndex)
{
surfaceIndex = y;
}
column.nodes[y] = node;
}
column.surfaceNodeIndexes[0] = surfaceIndex;
if (surfaceIndex >= 0)
{
column.type = ParallelNavColumnType.Walkable;
}
else
{
column.type = ParallelNavColumnType.Empty;
}
}
}
}
