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); }