public static void DrawPolygonGraph(PNavMesh pNavMesh)
{
Color c = Handles.color;
foreach (PNavIsland island in pNavMesh.islands)
{
int index = 0;
foreach (PNavPolygon polygon in island.graph.polygons)
{
Handles.color = Color.green;
Handles.SphereHandleCap(0, (Vector3)polygon.Centroid3D, Quaternion.identity, 0.1f, EventType.Repaint);
Handles.Label((Vector3)polygon.Centroid3D + Vector3.forward * 0.5f, index.ToString());
foreach (PNavEdge edge in polygon.edges)
{
if (edge.hasOther)
{
PNavPolygon self = island.graph.polygons[edge.selfPolygonIndex];
PNavPolygon other = island.graph.polygons[edge.otherPolygonIndex];
Handles.DrawLine((Vector3)self.Centroid3D, (Vector3)other.Centroid3D);
}
}
index++;
}
}
Handles.color = c;
}
public static void DrawEdgeGap(PNavMesh pNavMesh)
{
foreach (PNavIsland island in pNavMesh.islands)
{
foreach (PNavNode node in island.nodes)
{
Color c = Handles.color;
if (node.IsInner)
{
if (node.walkable)
{
}
else
{
Handles.color = Color.yellow;
Handles.SphereHandleCap(0, (Vector3)node.Center, Quaternion.identity, 0.1f, EventType.Repaint);
}
continue;
}
Handles.color = c;
}
}
}
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");
}
}
public static void DrawWalkable(PNavMesh pNavMesh)
{
foreach (PNavIsland island in pNavMesh.islands)
{
foreach (PNavNode node in island.nodes)
{
Color c = Handles.color;
if (node.IsInner)
{
if (node.walkable)
{
if (node.isCorner)
{
Handles.color = Color.cyan;
Handles.SphereHandleCap(0, (Vector3)node.Center, Quaternion.identity, 0.1f, EventType.Repaint);
}
else if (node.isEdge)
{
Handles.color = Color.magenta;
Handles.SphereHandleCap(0, (Vector3)node.Center, Quaternion.identity, 0.1f, EventType.Repaint);
}
else
{
Handles.color = Color.green;
Handles.SphereHandleCap(0, (Vector3)node.Center, Quaternion.identity, 0.1f, EventType.Repaint);
}
}
}
Handles.color = c;
}
}
}
static void CreateNewNavMesh()
{
PNavMesh newNavMesh = ScriptableObject.CreateInstance <PNavMesh>();
AssetDatabase.CreateAsset(newNavMesh, "Assets/NewParallelNavMesh.asset");
AssetDatabase.SaveAssets();
EditorUtility.FocusProjectWindow();
Selection.activeObject = newNavMesh;
}
public static void Process(PNavMesh pNavMesh)
{
using (new SProfiler("Finding Edge Loops"))
{
int maxX = pNavMesh.columns.GetLength(0);
int maxZ = pNavMesh.columns.GetLength(1);
PNavPoint pointMax = new PNavPoint(maxX, maxZ);
FindEdgeLoops(pNavMesh.columns, pointMax, pNavMesh);
}
}
public static void Process(PNavMesh pNavMesh)
{
using (new SProfiler($"Remove Narrow Path"))
{
foreach (PNavIsland island in pNavMesh.islands)
{
foreach (PNavNode node in island.nodes)
{
}
}
}
}
void OnEnable()
{
if (EditorPrefs.HasKey("ObjectPath"))
{
string objectPath = EditorPrefs.GetString("ObjectPath");
pNavMesh = AssetDatabase.LoadAssetAtPath(objectPath, typeof(PNavMesh)) as PNavMesh;
if (pNavMesh)
{
so = new SerializedObject(pNavMesh);
}
}
SceneView.duringSceneGui += this.OnSceneGUI;
}
void ShowSelectWndow()
{
string[] filters = { "Parallel NavMesh", "asset" };
string absPath = EditorUtility.OpenFilePanelWithFilters("Select Parallel NavMesh ScriptableObject", "Assets", filters);
//string absPath = EditorUtility.OpenFilePanel("Select Parallel NavMesh ScriptableObject", "", "");
if (absPath.StartsWith(Application.dataPath))
{
string relPath = absPath.Substring(Application.dataPath.Length - "Assets".Length);
pNavMesh = AssetDatabase.LoadAssetAtPath(relPath, typeof(PNavMesh)) as PNavMesh;
if (pNavMesh)
{
EditorPrefs.SetString("ObjectPath", relPath);
so = new SerializedObject(pNavMesh);
}
}
}
public static void DrawEdge(PNavMesh pNavMesh)
{
float arrowSize = (float)pNavMesh.gridSize.x / 2;
foreach (PNavIsland island in pNavMesh.islands)
{
foreach (PNavNode node in island.nodes)
{
Color c = Handles.color;
if (node.IsInner)
{
}
else
{
Handles.color = Color.red;
Handles.SphereHandleCap(0, (Vector3)node.Center, Quaternion.identity, 0.1f, EventType.Repaint);
if (node.IsLeftEdge)
{
Handles.DrawLine((Vector3)node.LeftCenter - arrowSize * Vector3.forward, (Vector3)node.LeftCenter + arrowSize * Vector3.forward);
}
if (node.IsRghtEdge)
{
Handles.DrawLine((Vector3)node.RightCenter - arrowSize * Vector3.forward, (Vector3)node.RightCenter + arrowSize * Vector3.forward);
}
if (node.IsFrontEdge)
{
Handles.DrawLine((Vector3)node.FrontCenter - arrowSize * Vector3.right, (Vector3)node.FrontCenter + arrowSize * Vector3.right);
}
if (node.IsBackEdge)
{
Handles.DrawLine((Vector3)node.BackCenter - arrowSize * Vector3.right, (Vector3)node.BackCenter + arrowSize * Vector3.right);
}
}
Handles.color = c;
}
}
}
public static void CheckVolume(PNavMesh pNavMesh, int xStart, int xEnd, int zStart, int zEnd, int yStart, int yEnd, int scale, Action <int, int, int, int, int, int> callback)
{
PShapeOverlapResult3D result = new PShapeOverlapResult3D();
Fix64Vec3 size = pNavMesh.gridSize * (Fix64)scale;
Fix64Vec3 toCenter = Fix64.half * size;
int scaledXEnd = xEnd / scale;
int scaledYEnd = yEnd / scale;
int scaledZEnd = zEnd / scale;
for (int x = xStart; x < scaledXEnd; x++)
{
for (int z = zStart; z < scaledZEnd; z++)
{
for (int y = yStart; y < scaledZEnd; y++)
{
int scaledX = x * scale;
int scaledY = y * scale;
int scaledZ = z * scale;
Fix64Vec3 l;
Fix64Vec3 u;
pNavMesh.GetAABB(scaledX, scaledY, scaledZ, out l, out u);
Fix64Vec3 center = l + toCenter;
Fix64Quat rot = Fix64Quat.identity;
Parallel3D.OverlapCube(
center, rot,
size.x, size.y, size.z,
-1,
result);
if (result.count > 0)
{
callback(scaledX, scaledX + scale, scaledZ, scaledZ + scale, scaledY, scaledY + scale);
}
}
}
}
}
static void FindEdgeLoops(PNavColumn[,] columns, PNavPoint pointMax, PNavMesh pNavMesh)
{
List <PNavIsland> islandsToRemove = new List <PNavIsland>();
foreach (PNavIsland island in pNavMesh.islands)
{
int edgeLoopIndex = 0;
int maxNodeCount = 0;
PNavEdgeLoop edgeLoop = new PNavEdgeLoop();
foreach (PNavNode node in island.nodes)
{
if (node.walkable)
{
bool foundLoop = DetectEdgeLoop(node, edgeLoop, edgeLoopIndex, columns, pointMax);
if (foundLoop)
{
int nodeCount = edgeLoop.nodes.Count;
if (nodeCount > maxNodeCount)
{
maxNodeCount = nodeCount;
island.boundaryEdgeLoopIndex = edgeLoopIndex;
}
edgeLoopIndex++;
island.edgeLoops.Add(edgeLoop);
edgeLoop = new PNavEdgeLoop();
}
}
}
if (island.boundaryEdgeLoopIndex < 0)
{
islandsToRemove.Add(island);
}
}
foreach (PNavIsland island in islandsToRemove)
{
pNavMesh.islands.Remove(island);
}
}
public static void DrawEdgeLoop(PNavMesh pNavMesh)
{
Color c = Handles.color;
foreach (PNavIsland island in pNavMesh.islands)
{
int loopIndex = 0;
foreach (PNavEdgeLoop loop in island.edgeLoops)
{
loopIndex++;
Handles.color = colors[loopIndex];
foreach (PNavNode node in loop.nodes)
{
Handles.SphereHandleCap(0, (Vector3)node.Center, Quaternion.identity, 0.1f, EventType.Repaint);
}
}
}
Handles.color = c;
}
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 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 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 NavMeshGenerator(PNavMesh pNavMesh)
{
_pNavMesh = pNavMesh;
}
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 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 Process(PNavMesh pNavMesh)
{
const int INDICE_COUNT = 1024;
using (new SProfiler("Triangulation"))
{
int[] indices = new int[INDICE_COUNT];
int[] indiceCounts = new int[INDICE_COUNT];
Fix64Vec2[] verts = new Fix64Vec2[INDICE_COUNT];
int[] indexes = new int[INDICE_COUNT];
int index = 0;
foreach (PNavIsland island in pNavMesh.islands)
{
//build island with island boundary
if (island.boundaryEdgeLoopIndex < 0)
{
continue;
}
Debug.Log($"Generating Polygon graph for island={index}");
index++;
PNavEdgeLoop boundary = island.edgeLoops[island.boundaryEdgeLoopIndex];
int boundaryCornersCount = PrepareCornerVerts(boundary, island, verts, indexes);
PolyIsland polyIsland = Parallel3D.CreatePolyIsland(verts, indexes, boundaryCornersCount);
//add other edgeloops as holes
int edgeLoopIndex = -1;
foreach (PNavEdgeLoop edgeLoop in island.edgeLoops)
{
edgeLoopIndex++;
if (edgeLoopIndex == island.boundaryEdgeLoopIndex)
{
continue;
}
int holeCornersCount = PrepareCornerVerts(edgeLoop, island, verts, indexes);
Parallel3D.AddHolePolyIsland(verts, indexes, holeCornersCount, polyIsland);
}
int polygonCount = 0;
int totalIndicsCount = 0;
bool ok = Parallel3D.TriangulatePolyIsland(indices, indiceCounts, ref polygonCount, ref totalIndicsCount, 2, polyIsland);
int[] indicesCopy = new int[totalIndicsCount];
Array.Copy(indices, 0, indicesCopy, 0, totalIndicsCount);
int[] indiceCountsCopy = new int[polygonCount];
Array.Copy(indiceCounts, 0, indiceCountsCopy, 0, polygonCount);
island.indices = indicesCopy;
island.indiceCountsOfPolygons = indiceCountsCopy;
island.indicsCount = totalIndicsCount;
island.polygonCount = polygonCount;
Parallel3D.DestroyPolyIsland(polyIsland);
}
}
}
