public hkRootLevelContainer BuildNavmesh(BuildParams p, List <Vector3> verts, List <int> indices)
{
var root = new hkRootLevelContainer();
NavMeshNative.SetNavmeshBuildParams(p.Cellsize, p.Cellheight, p.SlopeAngle, p.AgentHeight, p.AgentClimb, p.AgentRadius, p.MinRegionArea);
var buildSuccess = NavMeshNative.BuildNavmeshForMesh(verts.ToArray(), verts.Count, indices.ToArray(), indices.Count);
if (!buildSuccess)
{
return(null);
}
var vcount = NavMeshNative.GetMeshVertCount();
var icount = NavMeshNative.GetMeshTriCount();
if (vcount == 0 || icount == 0)
{
return(null);
}
ushort[] bverts = new ushort[vcount * 3];
ushort[] bindices = new ushort[icount * 3 * 2];
Vector3[] vbverts = new Vector3[vcount];
NavMeshNative.GetMeshVerts(bverts);
NavMeshNative.GetMeshTris(bindices);
Vector3[] bounds = new Vector3[2];
NavMeshNative.GetBoundingBox(bounds);
var nmesh = new hkaiNavMesh();
nmesh.m_aabb = new hkAabb();
nmesh.m_aabb.m_min = new Vector4(bounds[0].X, bounds[0].Y, bounds[0].Z, 1.0f);
nmesh.m_aabb.m_max = new Vector4(bounds[1].X, bounds[1].Y, bounds[1].Z, 1.0f);
nmesh.m_edgeData = new List <int>();
nmesh.m_edgeDataStriding = 1;
nmesh.m_edges = new List <hkaiNavMeshEdge>();
nmesh.m_erosionRadius = 0.0f;
nmesh.m_faceData = new List <int>();
nmesh.m_faceDataStriding = 1;
nmesh.m_faces = new List <hkaiNavMeshFace>();
nmesh.m_flags = 0;
nmesh.m_vertices = new List <Vector4>();
for (int i = 0; i < bverts.Length / 3; i++)
{
var vx = bverts[i * 3];
var vy = bverts[i * 3 + 1];
var vz = bverts[i * 3 + 2];
var vert = new Vector3(bounds[0].X + (float)vx * p.Cellsize,
bounds[0].Y + (float)vy * p.Cellheight,
bounds[0].Z + (float)vz * p.Cellsize);
nmesh.m_vertices.Add(new Vector4(vert.X, vert.Y, vert.Z, 1.0f));
vbverts[i] = vert;
}
for (int t = 0; t < bindices.Length / 2; t += 3)
{
var f = new hkaiNavMeshFace();
f.m_clusterIndex = 0;
f.m_numEdges = 3;
f.m_startEdgeIndex = nmesh.m_edges.Count;
f.m_startUserEdgeIndex = -1;
f.m_padding = 0xCDCD;
nmesh.m_faces.Add(f);
nmesh.m_faceData.Add(0);
for (int i = 0; i < 3; i++)
{
var e = new hkaiNavMeshEdge();
e.m_a = bindices[t * 2 + i];
e.m_b = bindices[t * 2 + ((i + 1) % 3)];
e.m_flags = 4;
// Record adjacency
if (bindices[t * 2 + 3 + i] == 0xFFFF)
{
// No adjacency
e.m_oppositeEdge = 0xFFFFFFFF;
e.m_oppositeFace = 0xFFFFFFFF;
}
else
{
e.m_oppositeFace = bindices[t * 2 + 3 + i];
// Find the edge that has this face as an adjancency
for (int j = 0; j < 3; j++)
{
var edge = bindices[t * 2 + 3 + i] * 6 + 3 + j;
if (bindices[edge] == (t / 3))
{
e.m_oppositeEdge = (uint)bindices[t * 2 + 3 + i] * 3 + (uint)j;
}
}
}
nmesh.m_edges.Add(e);
nmesh.m_edgeData.Add(0);
}
}
root.m_namedVariants = new List <hkRootLevelContainerNamedVariant>();
var variant = new hkRootLevelContainerNamedVariant();
variant.m_className = "hkaiNavMesh";
variant.m_name = "hkaiNavMesh";
variant.m_variant = nmesh;
root.m_namedVariants.Add(variant);
// Next step: build a bvh
var shortIndices = new ushort[bindices.Length / 2];
for (int i = 0; i < bindices.Length / 2; i += 3)
{
shortIndices[i] = bindices[i * 2];
shortIndices[i + 1] = bindices[i * 2 + 1];
shortIndices[i + 2] = bindices[i * 2 + 2];
}
bool didbuild = BVHNative.BuildBVHForMesh(vbverts, vbverts.Length, shortIndices, shortIndices.Length);
if (!didbuild)
{
return(null);
}
var nodecount = BVHNative.GetBVHSize();
var nsize = BVHNative.GetNodeSize();
var nodes = new NativeBVHNode[nodecount];
BVHNative.GetBVHNodes(nodes);
// Rebuild in friendlier tree form
List <BVNode> bnodes = new List <BVNode>((int)nodecount);
foreach (var n in nodes)
{
var bnode = new BVNode();
bnode.Min = new Vector3(n.minX, n.minY, n.minZ);
bnode.Max = new Vector3(n.maxX, n.maxY, n.maxZ);
bnode.IsLeaf = n.isLeaf == 1;
bnode.PrimitiveCount = n.primitiveCount;
bnode.Primitive = n.firstChildOrPrimitive;
bnodes.Add(bnode);
}
for (int i = 0; i < nodes.Length; i++)
{
if (nodes[i].isLeaf == 0)
{
bnodes[i].Left = bnodes[(int)nodes[i].firstChildOrPrimitive];
bnodes[i].Right = bnodes[(int)nodes[i].firstChildOrPrimitive + 1];
}
}
var bvhvariant = new hkRootLevelContainerNamedVariant();
bvhvariant.m_className = "hkcdStaticAabbTree";
bvhvariant.m_name = "hkcdStaticAabbTree";
var tree = new hkcdStaticAabbTree();
bvhvariant.m_variant = tree;
root.m_namedVariants.Add(bvhvariant);
tree.m_treePtr = new hkcdStaticTreeDefaultTreeStorage6();
tree.m_treePtr.m_nodes = bnodes[0].BuildAxis6Tree();
var min = bnodes[0].Min;
var max = bnodes[0].Max;
tree.m_treePtr.m_domain = new hkAabb();
tree.m_treePtr.m_domain.m_min = new Vector4(min.X, min.Y, min.Z, 1.0f);
tree.m_treePtr.m_domain.m_max = new Vector4(max.X, max.Y, max.Z, 1.0f);
// Build a dummy directed graph
var gvariant = new hkRootLevelContainerNamedVariant();
gvariant.m_className = "hkaiDirectedGraphExplicitCost";
gvariant.m_name = "hkaiDirectedGraphExplicitCost";
var graph = new hkaiDirectedGraphExplicitCost();
gvariant.m_variant = graph;
root.m_namedVariants.Add(gvariant);
graph.m_nodes = new List <hkaiDirectedGraphExplicitCostNode>();
var node = new hkaiDirectedGraphExplicitCostNode();
node.m_numEdges = 0;
node.m_startEdgeIndex = 0;
graph.m_nodes.Add(node);
graph.m_positions = new List <Vector4>();
var c = (max - min) / 2;
graph.m_positions.Add(new Vector4(c.X, c.Y, c.Z, 1.0f));
return(root);
}
unsafe private void ProcessMesh(hkaiNavMesh mesh)
{
var verts = mesh.m_vertices;
int indexCount = 0;
foreach (var f in mesh.m_faces)
{
// Simple formula for indices count for a triangulation of a poly
indexCount += (f.m_numEdges - 2) * 3;
}
var MeshIndices = new int[indexCount * 3];
var MeshVertices = new NavmeshLayout[indexCount * 3];
PickingVertices = new Vector3[indexCount * 3];
PickingIndices = new int[indexCount * 3];
var factory = Scene.Renderer.Factory;
int idx = 0;
int maxcluster = 0;
for (int id = 0; id < mesh.m_faces.Count; id++)
{
if (mesh.m_faces[id].m_clusterIndex > maxcluster)
{
maxcluster = mesh.m_faces[id].m_clusterIndex;
}
var sedge = mesh.m_faces[id].m_startEdgeIndex;
var ecount = mesh.m_faces[id].m_numEdges;
// Use simple algorithm for convex polygon trianglization
for (int t = 0; t < ecount - 2; t++)
{
if (ecount > 3)
{
//ecount = ecount;
}
var end = (t + 2 >= ecount) ? sedge : sedge + t + 2;
var vert1 = mesh.m_vertices[mesh.m_edges[sedge].m_a];
var vert2 = mesh.m_vertices[mesh.m_edges[sedge + t + 1].m_a];
var vert3 = mesh.m_vertices[mesh.m_edges[end].m_a];
MeshVertices[idx] = new NavmeshLayout();
MeshVertices[idx + 1] = new NavmeshLayout();
MeshVertices[idx + 2] = new NavmeshLayout();
MeshVertices[idx].Position = new Vector3(vert1.X, vert1.Y, vert1.Z);
MeshVertices[idx + 1].Position = new Vector3(vert2.X, vert2.Y, vert2.Z);
MeshVertices[idx + 2].Position = new Vector3(vert3.X, vert3.Y, vert3.Z);
PickingVertices[idx] = new Vector3(vert1.X, vert1.Y, vert1.Z);
PickingVertices[idx + 1] = new Vector3(vert2.X, vert2.Y, vert2.Z);
PickingVertices[idx + 2] = new Vector3(vert3.X, vert3.Y, vert3.Z);
var n = Vector3.Normalize(Vector3.Cross(MeshVertices[idx + 2].Position - MeshVertices[idx].Position, MeshVertices[idx + 1].Position - MeshVertices[idx].Position));
MeshVertices[idx].Normal[0] = (sbyte)(n.X * 127.0f);
MeshVertices[idx].Normal[1] = (sbyte)(n.Y * 127.0f);
MeshVertices[idx].Normal[2] = (sbyte)(n.Z * 127.0f);
MeshVertices[idx + 1].Normal[0] = (sbyte)(n.X * 127.0f);
MeshVertices[idx + 1].Normal[1] = (sbyte)(n.Y * 127.0f);
MeshVertices[idx + 1].Normal[2] = (sbyte)(n.Z * 127.0f);
MeshVertices[idx + 2].Normal[0] = (sbyte)(n.X * 127.0f);
MeshVertices[idx + 2].Normal[1] = (sbyte)(n.Y * 127.0f);
MeshVertices[idx + 2].Normal[2] = (sbyte)(n.Z * 127.0f);
MeshVertices[idx].Color[0] = (byte)(157);
MeshVertices[idx].Color[1] = (byte)(53);
MeshVertices[idx].Color[2] = (byte)(255);
MeshVertices[idx].Color[3] = (byte)(255);
MeshVertices[idx + 1].Color[0] = (byte)(157);
MeshVertices[idx + 1].Color[1] = (byte)(53);
MeshVertices[idx + 1].Color[2] = (byte)(255);
MeshVertices[idx + 1].Color[3] = (byte)(255);
MeshVertices[idx + 2].Color[0] = (byte)(157);
MeshVertices[idx + 2].Color[1] = (byte)(53);
MeshVertices[idx + 2].Color[2] = (byte)(255);
MeshVertices[idx + 2].Color[3] = (byte)(255);
MeshVertices[idx].Barycentric[0] = (byte)(0);
MeshVertices[idx].Barycentric[1] = (byte)(0);
MeshVertices[idx + 1].Barycentric[0] = (byte)(1);
MeshVertices[idx + 1].Barycentric[1] = (byte)(0);
MeshVertices[idx + 2].Barycentric[0] = (byte)(0);
MeshVertices[idx + 2].Barycentric[1] = (byte)(1);
MeshIndices[idx] = idx;
MeshIndices[idx + 1] = idx + 1;
MeshIndices[idx + 2] = idx + 2;
PickingIndices[idx] = idx;
PickingIndices[idx + 1] = idx + 1;
PickingIndices[idx + 2] = idx + 2;
idx += 3;
}
}
VertexCount = MeshVertices.Length;
IndexCount = MeshIndices.Length;
uint buffersize = (uint)IndexCount * 4u;
if (VertexCount > 0)
{
fixed(void *ptr = PickingVertices)
{
Bounds = BoundingBox.CreateFromPoints((Vector3 *)ptr, PickingVertices.Count(), 12, Quaternion.Identity, Vector3.Zero, Vector3.One);
}
}
else
{
Bounds = new BoundingBox();
}
uint vbuffersize = (uint)MeshVertices.Length * NavmeshLayout.SizeInBytes;
GeomBuffer = Scene.Renderer.GeometryBufferAllocator.Allocate(vbuffersize, buffersize, (int)NavmeshLayout.SizeInBytes, 4, (h) =>
{
h.FillIBuffer(MeshIndices, () =>
{
MeshIndices = null;
});
h.FillVBuffer(MeshVertices, () =>
{
MeshVertices = null;
});
});
}
