static int PrepareCornerVerts(PNavEdgeLoop edgeLoop, PNavIsland island, Fix64Vec2[] verts, int[] indexes)
{
List <PNavNode> corners = new List <PNavNode>();
foreach (PNavNode node in edgeLoop.nodes)
{
if (node.isCorner)
{
corners.Add(node);
}
}
int cornersCount = corners.Count;
for (int i = 0; i < cornersCount; i++)
{
PNavNode node = corners[i];
Fix64Vec3 center = node.Center;
verts[i] = new Fix64Vec2(center.x, center.z);
indexes[i] = island.nodes.IndexOf(node);
}
bool isClockwise = IsClockwise(verts, cornersCount);
if (!isClockwise)
{
InverseVerticesAndIndexes(verts, indexes, cornersCount);
}
return(cornersCount);
}
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);
}
static bool isWalkableNode(PNavPoint self, PNavPoint max, PNavPoint delta, PNavColumn[,] columns)
{
PNavPoint pOut;
bool valid = PNavMeshHelper.GetPoint(self, max, delta, out pOut);
if (valid)
{
PNavNode n = columns[pOut.x, pOut.z].SurfaceNode();
valid = (n != null && n.walkable);
}
return(valid);
}
static bool isInnerNode(PNavPoint point, PNavPoint max, PNavPoint delta, int nodes, PNavColumn[,] columns)
{
PNavPoint pOut;
PNavPoint newDelta = new PNavPoint(delta.x * nodes, delta.z * nodes);
bool valid = PNavMeshHelper.GetPoint(point, max, newDelta, out pOut);
if (valid)
{
PNavNode n = columns[pOut.x, pOut.z].SurfaceNode();
valid = (n != null && n.IsInner);
}
return(valid);
}
static bool isEdgeNode(PNavPoint point, PNavPoint max, PNavPoint delta, PNavColumn[,] columns)
{
PNavPoint pOut;
bool valid = PNavMeshHelper.GetPoint(point, max, delta, out pOut);
if (valid)
{
PNavNode n = columns[pOut.x, pOut.z].SurfaceNode();
if (n == null)
{
return(false);
}
return(n.IsLeftEdge || n.IsRghtEdge || n.IsBackEdge || n.IsFrontEdge);
}
return(false);
}
public static void Process(PNavMesh pNavMesh)
{
using (new SProfiler($"Build polygon graph"))
{
foreach (PNavIsland island in pNavMesh.islands)
{
Dictionary <long, PNavEdge> edgeMap = new Dictionary <long, PNavEdge>();
PNavPolygonGraph graph = new PNavPolygonGraph();
int indiceRead = 0;
for (int p = 0; p < island.polygonCount; p++)
{
int ic = island.indiceCountsOfPolygons[p];
Fix64Vec2[] verts = new Fix64Vec2[ic];
int[] indices = new int[ic];
for (int indiceOfPolygon = 0; indiceOfPolygon < ic; indiceOfPolygon++)
{
int nodeIndex = island.indices[indiceRead];
PNavNode vertNode = island.nodes[nodeIndex];
verts[indiceOfPolygon] = new Fix64Vec2(vertNode.Center.x, vertNode.Center.z);
indices[indiceOfPolygon] = nodeIndex;
indiceRead++;
}
PNavPolygon polygon = new PNavPolygon(verts, ic);
//polygon index should match its index in graph.polygons list
polygon.index = p;
graph.AddPolygon(polygon);
BuildEdges(graph, indices, ic, polygon, edgeMap);
}
island.graph = graph;
}
}
}
public static void DrawPolygon(PNavMesh pNavMesh)
{
foreach (PNavIsland island in pNavMesh.islands)
{
Fix64Vec3[] verts = new Fix64Vec3[1024];
int indiceRead = 0;
for (int p = 0; p < island.polygonCount; p++)
{
int ic = island.indiceCountsOfPolygons[p];
for (int indiceOfPolygon = 0; indiceOfPolygon < ic; indiceOfPolygon++)
{
int nodeIndex = island.indices[indiceRead];
PNavNode vertNode = island.nodes[nodeIndex];
verts[indiceOfPolygon] = vertNode.Center;
indiceRead++;
}
DrawPolygon(verts, ic, Color.cyan);
}
}
}
public static void Process(PNavMesh pNavMesh)
{
foreach (PNavIsland island in pNavMesh.islands)
{
foreach (PNavEdgeLoop edgeLoop in island.edgeLoops)
{
PNavNode previousNode = edgeLoop.nodes[edgeLoop.nodes.Count - 1];
Vector2 direction = Vector2.zero;
foreach (PNavNode node in edgeLoop.nodes)
{
node.isCorner = false;
Vector2 newDirection = new Vector2(node.point.x - previousNode.point.x, node.point.z - previousNode.point.z).normalized;
float angle = Vector2.Angle(newDirection, direction);
previousNode.angle = angle;
if (angle >= 45)
{
previousNode.isCorner = true;
}
direction = newDirection;
previousNode = node;
}
PNavNode node1 = edgeLoop.nodes[0];
Vector2 newDirection1 = new Vector2(node1.point.x - previousNode.point.x, node1.point.z - previousNode.point.z).normalized;
float angle1 = Vector2.Angle(newDirection1, direction);
previousNode.angle = angle1;
if (angle1 >= 45)
{
previousNode.isCorner = true;
}
}
}
}
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 CheckIfNodeIsValidEdge(PNavNode node, PNavColumn[,] columns, PNavPoint pointMax)
{
node.isEdge = false;
node.edgeFlag = 0;
bool allSurroundingAreInner = true;
for (int i = 0; i < 8; i++)
{
walkables[i] = false;
PNavPoint pOut;
bool valid = PNavMeshHelper.GetPoint(node.point, pointMax, points2[i], out pOut);
if (valid)
{
PNavNode n = columns[pOut.x, pOut.z].SurfaceNode();
if (n != null)
{
if (n.walkable)
{
walkables[i] = true;
node.edgeFlag = (byte)(node.edgeFlag | 1 << i);
}
if (!n.IsInner)
{
allSurroundingAreInner = false;
}
}
}
}
if (!allSurroundingAreInner)
{
node.isEdge = true;
}
if (walkables[0] != walkables[4]) //left and right
{
node.isEdge = true;
// if top and bot are not walkable, not valid
if (!walkables[2] && !walkables[6])
{
node.isEdge = false;
}
}
if (walkables[2] != walkables[6]) // top and bot
{
node.isEdge = true;
// if left and right are not walkable, not valid
if (!walkables[0] && !walkables[4])
{
node.isEdge = false;
}
}
// if all diagonal nodes are not walkable, not valid
if (!walkables[1] && !walkables[3] && !walkables[5] && !walkables[7])
{
node.isEdge = false;
}
return(node.isEdge);
}
static bool DetectEdgeLoop(PNavNode node, PNavEdgeLoop edgeLoop, int edgeLoopIndex, PNavColumn[,] columns, PNavPoint pointMax)
{
bool foundLoop = false;
if (node == null || !node.walkable || node.edgeLoopIndex >= 0)
{
return(false);
}
bool isEdge = CheckIfNodeIsValidEdge(node, columns, pointMax);
if (!isEdge)
{
return(false);
}
Stack <PNavNode> stack = new Stack <PNavNode>();
stack.Push(node);
while (stack.Count > 0)
{
PNavNode n = stack.Pop();
//possible to be added more than once
if (n.edgeLoopIndex >= 0)
{
continue;
}
edgeLoop.nodes.Add(n);
n.edgeLoopIndex = edgeLoopIndex;
foundLoop = true;
for (int i = 0; i < 8; i++)
{
PNavPoint pOut;
bool valid = PNavMeshHelper.GetPoint(n.point, pointMax, points[i], out pOut);
if (valid)
{
PNavNode nPush = columns[pOut.x, pOut.z].SurfaceNode();
if (nPush != null)
{
if (!nPush.walkable || nPush.edgeLoopIndex >= 0)
{
continue;
}
bool isEdge1 = CheckIfNodeIsValidEdge(nPush, columns, pointMax);
if (!isEdge1)
{
continue;
}
stack.Push(nPush);
}
}
}
}
return(foundLoop);
}
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 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;
}
}
}
}
public static void ApplyEdgeGap(PNavColumn[,] columns, PNavPoint pointMax, PNavIsland island, int edgeGap)
{
foreach (PNavNode node in island.nodes)
{
if (node.IsInner)
{
//===remove narrow path
// check top and bot
bool top2Valid = isInnerNode(node.point, pointMax, TOP, 2, columns);
bool top1Valid = isInnerNode(node.point, pointMax, TOP, 1, columns);
bool bot1Valid = isInnerNode(node.point, pointMax, BOTTOM, 1, columns);
bool bot2Valid = isInnerNode(node.point, pointMax, BOTTOM, 2, columns);
if (top2Valid && top1Valid || top1Valid && bot1Valid || bot1Valid && bot2Valid)
{
// valid
}
else
{
node.walkable = false;
}
// check left and right
bool left2Valid = isInnerNode(node.point, pointMax, LEFT, 2, columns);
bool left1Valid = isInnerNode(node.point, pointMax, LEFT, 1, columns);
bool right1Valid = isInnerNode(node.point, pointMax, RIGHT, 1, columns);
bool right2Valid = isInnerNode(node.point, pointMax, RIGHT, 2, columns);
if (left2Valid && left1Valid || left1Valid && right1Valid || right1Valid && right2Valid)
{
// valid
}
else
{
node.walkable = false;
}
// diagonals
//bool topLeft2Valid = isInnerNode(node.point, pointMax, TOPLEFT, 2, columns);
//bool topLeft1Valid = isInnerNode(node.point, pointMax, TOPLEFT, 1, columns);
//bool botRight1Valid = isInnerNode(node.point, pointMax, BOTTOMRIGHT, 1, columns);
//bool botRight2Valid = isInnerNode(node.point, pointMax, BOTTOMRIGHT, 2, columns);
//if (topLeft2Valid && topLeft1Valid || topLeft1Valid && botRight1Valid || botRight1Valid && botRight2Valid)
//{
// // valid
//}
//else
//{
// node.walkable = false;
//}
//bool topRight2Valid = isInnerNode(node.point, pointMax, TOPRIGHT, 2, columns);
//bool topRight1Valid = isInnerNode(node.point, pointMax, TOPRIGHT, 1, columns);
//bool botLeft1Valid = isInnerNode(node.point, pointMax, BOTTOMLEFT, 1, columns);
//bool botLeft2Valid = isInnerNode(node.point, pointMax, BOTTOMLEFT, 2, columns);
//if (topRight2Valid && topRight1Valid || topRight1Valid && botLeft1Valid || botLeft1Valid && botLeft2Valid)
//{
// // valid
//}
//else
//{
// node.walkable = false;
//}
continue;
}
else
{
node.walkable = false;
// remove edgeGap
//left
for (int i = 0; i < edgeGap; i++)
{
//left
{
PNavPoint pOut;
bool valid = GetLeftPoint(node.point, pointMax, i, out pOut);
if (valid)
{
PNavNode n = columns[pOut.x, pOut.z].SurfaceNode();
if (n != null)
{
n.walkable = false;
}
}
}
//right
{
PNavPoint pOut;
bool valid = GetRightPoint(node.point, pointMax, i, out pOut);
if (valid)
{
PNavNode n = columns[pOut.x, pOut.z].SurfaceNode();
if (n != null)
{
n.walkable = false;
}
}
}
//top
{
PNavPoint pOut;
bool valid = GetTopPoint(node.point, pointMax, i, out pOut);
if (valid)
{
PNavNode n = columns[pOut.x, pOut.z].SurfaceNode();
if (n != null)
{
n.walkable = false;
}
}
}
//bottom
{
PNavPoint pOut;
bool valid = GetBottomPoint(node.point, pointMax, i, out pOut);
if (valid)
{
PNavNode n = columns[pOut.x, pOut.z].SurfaceNode();
if (n != null)
{
n.walkable = false;
}
}
}
}
}
}
}
