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 void Process(PNavMesh pNavMesh)
{
using (new SProfiler("Finding Islands"))
{
// find islands
List <PNavIsland> islands = new List <PNavIsland>();
int maxX = pNavMesh.columns.GetLength(0) - 1;
int maxZ = pNavMesh.columns.GetLength(1) - 1;
PNavPoint pointMax = new PNavPoint(maxX, maxZ);
int islandIndex = 0;
PNavIsland island = new PNavIsland();
for (int x = 0; x < maxX; x++)
{
for (int z = 0; z < maxZ; z++)
{
PNavPoint point = new PNavPoint(x, z);
bool foundIsland = DetectIsland(pNavMesh.columns, point, pointMax, 1, pNavMesh.verticalDrop, islandIndex, island);
if (foundIsland)
{
islandIndex++;
islands.Add(island);
PNavMeshHelper.DetectEdgeCorner(pNavMesh.columns, pointMax, island);
PNavMeshHelper.ApplyEdgeGap(pNavMesh.columns, pointMax, island, pNavMesh.edgeGap);
island = new PNavIsland();
}
}
}
pNavMesh.islands = islands;
Debug.Log($"Found {islands.Count} islands");
}
}
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 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;
}
}
}
}
}
}
}
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);
}
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 static void DetectEdgeCorner(PNavColumn[,] columns, PNavPoint pointMax, PNavIsland island)
{
foreach (PNavNode node in island.nodes)
{
if (node.IsInner)
{
bool topEdge = isEdgeNode(node.point, pointMax, TOP, columns);
bool botEdge = isEdgeNode(node.point, pointMax, BOTTOM, columns);
bool leftEdge = isEdgeNode(node.point, pointMax, LEFT, columns);
bool rightEdge = isEdgeNode(node.point, pointMax, RIGHT, columns);
bool topLeftEdge = isEdgeNode(node.point, pointMax, TOPLEFT, columns);
bool topRightEdge = isEdgeNode(node.point, pointMax, TOPRIGHT, columns);
bool botLeftEdge = isEdgeNode(node.point, pointMax, BOTTOMLEFT, columns);
bool botRightEdge = isEdgeNode(node.point, pointMax, BOTTOMRIGHT, columns);
if (topEdge && leftEdge && !topLeftEdge)
{
node.type = node.type | (int)ParallelNavIslandNodeType.CornerEdge;
continue;
}
if (topEdge && rightEdge && !topRightEdge)
{
node.type = node.type | (int)ParallelNavIslandNodeType.CornerEdge;
continue;
}
if (botEdge && leftEdge && !botLeftEdge)
{
node.type = node.type | (int)ParallelNavIslandNodeType.CornerEdge;
continue;
}
if (botEdge && rightEdge && !botRightEdge)
{
node.type = node.type | (int)ParallelNavIslandNodeType.CornerEdge;
continue;
}
}
}
}
public PNavMeshPath CalculatePath(Fix64Vec3 start, Fix64Vec3 end)
{
Fix64Vec2 startPosition = new Fix64Vec2(start.x, start.z);
Fix64Vec2 endPosition = new Fix64Vec2(end.x, end.z);
PNavPolygon startPolygon = null;
PNavPolygon endPolygon = null;
bool sameIsland = false;
NavMeshAStart astart = null;
PNavMeshPath result = new PNavMeshPath();
result.Destination = end;
result.Destination2D = endPosition;
result.Status = ParallelNavMeshPathStatus.Invalid;
result.navMesh = navMesh;
PNavIsland endIsland = null;
PNavIsland startIsland = null;
foreach (PNavIsland island in navMesh.islands)
{
bool foundStart = false;
bool foundEnd = false;
foreach (PNavPolygon polygon in island.graph.polygons)
{
bool isStart = polygon.TestPoint(startPosition);
bool isEnd = polygon.TestPoint(endPosition);
if (isStart && isEnd)
{
result.Status = ParallelNavMeshPathStatus.Valid;
result.startIndex = -1;
return(result);
}
if (isStart)
{
startPolygon = polygon;
startIsland = island;
foundStart = true;
}
if (isEnd)
{
endPolygon = polygon;
endIsland = island;
foundEnd = true;
}
}
if (foundStart && foundEnd)
{
sameIsland = true;
astart = astartDictionary[island];
result.island = island;
break;
}
if (sameIsland)
{
break;
}
}
if (startPolygon != null && endPolygon == null)
{
//we find the polygon that is closest to the end position in the start position island
sameIsland = true;
astart = astartDictionary[startIsland];
result.island = startIsland;
endPolygon = startIsland.FindNearestPolygong(endPosition);
}
else if (startPolygon == null && endPolygon != null)
{
//we find the polygon that is closest to the start position in the end position island
sameIsland = true;
astart = astartDictionary[endIsland];
result.island = endIsland;
startPolygon = endIsland.FindNearestPolygong(startPosition);
}
else if (startPolygon == null && endPolygon == null)
{
//we find the polygon that is closest to the start position
//we then find the polygon that is closest to the end position in the same island
Fix64 minStart = Fix64.FromDivision(1000, 1);
PNavPolygon minStartPolygon = null;
PNavIsland minStartIsland = null;
foreach (PNavIsland island in navMesh.islands)
{
foreach (PNavPolygon polygon in island.graph.polygons)
{
Fix64 dis = Fix64Vec2.Distance(polygon.centroid, startPosition);
if (dis < minStart)
{
minStart = dis;
minStartPolygon = polygon;
minStartIsland = island;
}
}
}
sameIsland = true;
astart = astartDictionary[minStartIsland];
result.island = minStartIsland;
startPolygon = minStartPolygon;
endPolygon = minStartIsland.FindNearestPolygong(endPosition);
}
if (sameIsland)
{
if (astart != null && startPolygon != null && endPolygon != null)
{
NavMeshAStarNode startNode = astart.FindNode(startPolygon.index);
NavMeshAStarNode endNode = astart.FindNode(endPolygon.index);
NavMeshAStarNode lastNode = null;
using (new SProfiler("Pathfinding"))
{
astart.PrePathFinding();
startNode.startPoint = startPosition;
endNode.isLastNode = true;
endNode.endPoint = endPosition;
lastNode = astart.FindPath(startNode, endNode);
}
result.polygonIndexes = new int[PNavMeshPath.MAX_CORNER_COUNT];
int startIndex = 127;
while (lastNode != null)
{
result.polygonIndexes[startIndex] = lastNode.UserObject.index;
lastNode = (NavMeshAStarNode)lastNode.Parent;
startIndex--;
}
result.startIndex = startIndex + 1;
result.Status = ParallelNavMeshPathStatus.Valid;
}
}
return(result);
}
