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