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;
}
}
}
static void BuildEdges(PNavPolygonGraph graph, int[] indices, int count, PNavPolygon polygon, Dictionary <long, PNavEdge> edgeMap)
{
int previous = 0;
Fix64Vec2 previousVector2 = Fix64Vec2.zero;
for (int i = 0; i < count; i++)
{
int current = indices[i];
Fix64Vec2 currentVector2 = polygon.verts[i];
if (i != 0)
{
long key = KeyForIndices(previous, current);
if (edgeMap.ContainsKey(key))
{
PNavEdge edge = edgeMap[key];
edge.otherPolygonIndex = polygon.index;
edge.hasOther = true;
PNavPolygon self = graph.polygons[edge.selfPolygonIndex];
edge.distance = Fix64Vec2.Distance(self.centroid, polygon.centroid);
PNavEdge newEdge = new PNavEdge(edge);
newEdge.SwapDirection();
polygon.edges.Add(newEdge);
}
else
{
PNavEdge edge = new PNavEdge();
edge.selfPolygonIndex = polygon.index;
edge.width = Fix64Vec2.Distance(previousVector2, currentVector2);
edge.pointA = previousVector2;
edge.pointB = currentVector2;
edgeMap[key] = edge;
polygon.edges.Add(edge);
}
}
previous = current;
previousVector2 = currentVector2;
}
int lastCurrent = indices[0];
long lastKey = KeyForIndices(previous, lastCurrent);
if (edgeMap.ContainsKey(lastKey))
{
PNavEdge edge = edgeMap[lastKey];
edge.otherPolygonIndex = polygon.index;
edge.hasOther = true;
PNavPolygon self = graph.polygons[edge.selfPolygonIndex];
edge.distance = Fix64Vec2.Distance(self.centroid, polygon.centroid);
PNavEdge newEdge = new PNavEdge(edge);
newEdge.SwapDirection();
polygon.edges.Add(newEdge);
}
else
{
PNavEdge edge = new PNavEdge();
edge.selfPolygonIndex = polygon.index;
edge.width = Fix64Vec2.Distance(previousVector2, polygon.verts[0]);
edge.pointA = previousVector2;
edge.pointB = polygon.verts[0];
edgeMap[lastKey] = edge;
polygon.edges.Add(edge);
}
}