private void ClosestPointOnPolyInTile(MeshTile tile, long ip, float posx, float posy, float posz, ref float[] closestPt)
{
Poly poly = tile.Polys[ip];
if (poly.Type == NavMeshBuilder.PolyTypeOffMeshConnection)
{
int v0 = poly.Verts[0] * 3;
int v1 = poly.Verts[1] * 3;
float d0 = Helper.VDist(posx, posy, posz, tile.Verts[v0 + 0], tile.Verts[v0 + 1], tile.Verts[v0 + 2]);
float d1 = Helper.VDist(posx, posy, posz, tile.Verts[v1 + 0], tile.Verts[v1 + 1], tile.Verts[v1 + 2]);
float u = d0 / (d0 + d1);
Helper.VLerp(ref closestPt, tile.Verts[v0 + 0], tile.Verts[v0 + 1], tile.Verts[v0 + 2], tile.Verts[v1 + 0], tile.Verts[v1 + 1], tile.Verts[v1 + 2], u);
return;
}
PolyDetail pd = tile.DetailMeshes[ip];
float[] verts = new float[NavMeshBuilder.VertsPerPoly * 3];
float[] edged = new float[NavMeshBuilder.VertsPerPoly];
float[] edget = new float[NavMeshBuilder.VertsPerPoly];
int nv = poly.VertCount;
for (int i = 0; i < nv; i++)
{
Array.Copy(tile.Verts, poly.Verts[i] * 3, verts, i * 3, 3);
}
closestPt[0] = posx;
closestPt[1] = posy;
closestPt[2] = posz;
if (!Helper.DistancePtPolyEdgesSqr(posx, posy, posz, verts, nv, ref edged, ref edget))
{
float dmin = float.MaxValue;
int imin = -1;
for (int i = 0; i < nv; i++)
{
if (edged[i] < dmin)
{
dmin = edged[i];
imin = i;
}
}
int va = imin * 3;
int vb = ((imin + 1) % nv) * 3;
Helper.VLerp(ref closestPt, verts[va + 0], verts[va + 1], verts[va + 2], verts[vb + 0], verts[vb + 1], verts[vb + 2], edget[imin]);
}
for (int j = 0; j < pd.TriCount; j++)
{
int t = (int)(pd.TriBase + j) * 4;
float[] v = new float[9];
for (int k = 0; k < 3; k++)
{
if (tile.DetailTris[t + k] < poly.VertCount)
{
Array.Copy(tile.Verts, poly.Verts[tile.DetailTris[t + k]] * 3, v, k * 3, 3);
//v[k] = tile.Verts[poly.Verts[tile.DetailTris[t + k]]*3];
}
else
{
Array.Copy(tile.DetailVerts, (pd.VertBase + (tile.DetailTris[t + k] - poly.VertCount)) * 3, v, k * 3, 3);
//v[k] = tile.DetailVerts[(pd.VertBase + (tile.DetailTris[t + k] - poly.VertCount))*3];
}
}
float h = 0;
if (Helper.ClosestHeightPointTriangle(posx, posy, posz, v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8], ref h))
{
closestPt[1] = h;
break;
}
}
}
public NavMeshBuilder(NavMeshCreateParams param)
{
if (param.Nvp > VertsPerPoly)
{
throw new ArgumentException("Too many Verts per Poly for NavMeshBuilder");
}
if (param.VertCount >= 0xffff)
{
throw new ArgumentException("Too many total verticies for NavMeshBuilder");
}
if (param.VertCount == 0 || param.Verts == null)
{
throw new ArgumentException("No vertices, cannot generate nav mesh");
}
if (param.PolyCount == 0 || param.Polys == null)
{
throw new ArgumentException("No Polygons, cannot generate nav mesh");
}
int nvp = param.Nvp;
short[] offMeshConClass = new short[0];
int storedOffMeshConCount = 0;
int offMeshConLinkCount = 0;
if (param.OffMeshConCount > 0)
{
offMeshConClass = new short[param.OffMeshConCount * 2];
float hmin = float.MaxValue;
float hmax = float.MinValue;
if (param.DetailVerts != null && param.DetailVertsCount > 0)
{
for (int i = 0; i < param.DetailVertsCount; i++)
{
int h = i * 3 + 1;
hmin = Math.Min(hmin, param.DetailVerts[h]);
hmax = Math.Max(hmax, param.DetailVerts[h]);
}
}
else
{
for (int i = 0; i < param.VertCount; i++)
{
int iv = i * 3;
float h = param.BMin[1] + param.Verts[iv + 1] * param.Ch;
hmin = Math.Min(hmin, h);
hmax = Math.Max(hmax, h);
}
}
hmin -= param.WalkableClimb;
hmax += param.WalkableClimb;
float[] bmin = new float[3], bmax = new float[3];
Array.Copy(param.BMin, bmin, 3);
Array.Copy(param.BMax, bmax, 3);
bmin[1] = hmin;
bmax[1] = hmax;
for (int i = 0; i < param.OffMeshConCount; i++)
{
int p0 = (i * 2 + 0) * 3;
int p1 = (i * 2 + 1) * 3;
offMeshConClass[i * 2 + 0] = ClassifyOffMeshPoint(param.OffMeshConVerts[p0 + 0],
param.OffMeshConVerts[p0 + 1],
param.OffMeshConVerts[p0 + 2], bmin, bmax);
offMeshConClass[i * 2 + 1] = ClassifyOffMeshPoint(param.OffMeshConVerts[p1 + 0],
param.OffMeshConVerts[p1 + 1],
param.OffMeshConVerts[p1 + 2], bmin, bmax);
if (offMeshConClass[i * 2 + 0] == 0xff)
{
if (param.OffMeshConVerts[p0 + 1] < bmin[1] || param.OffMeshConVerts[p0 + 1] > bmax[1])
{
offMeshConClass[i * 2 + 0] = 0;
}
}
if (offMeshConClass[i * 2 + 0] == 0xff)
{
offMeshConLinkCount++;
}
if (offMeshConClass[i * 2 + 1] == 0xff)
{
offMeshConLinkCount++;
}
if (offMeshConClass[i * 2 + 0] == 0xff)
{
storedOffMeshConCount++;
}
}
}
int totPolyCount = param.PolyCount + storedOffMeshConCount;
int totVertCount = param.VertCount + storedOffMeshConCount * 2;
int edgeCount = 0;
int portalCount = 0;
for (int i = 0; i < param.PolyCount; i++)
{
int p = i * 2 * nvp;
for (int j = 0; j < nvp; j++)
{
if (param.Polys[p + j] == PolyMesh.MeshNullIdx)
{
break;
}
edgeCount++;
if ((param.Polys[p + nvp + j] & 0x8000) != 0)
{
int dir = param.Polys[p + nvp + j] & 0xf;
if (dir != 0xf)
{
portalCount++;
}
}
}
}
int maxLinkCount = edgeCount + portalCount * 2 + offMeshConLinkCount * 2;
int uniqueDetailVertCount = 0;
int detailTryCount = 0;
if (param.DetailMeshes != null)
{
detailTryCount = param.DetailTriCount;
for (int i = 0; i < param.PolyCount; i++)
{
int p = i * nvp * 2;
int ndv = (int)param.DetailMeshes[i * 4 + 1];
int nv = 0;
for (int j = 0; j < nvp; j++)
{
if (param.Polys[p + j] == PolyMesh.MeshNullIdx)
{
break;
}
nv++;
}
ndv -= nv;
uniqueDetailVertCount += ndv;
}
}
else
{
uniqueDetailVertCount = 0;
detailTryCount = 0;
for (int i = 0; i < param.PolyCount; i++)
{
int p = i * nvp * 2;
int nv = 0;
for (int j = 0; j < nvp; j++)
{
if (param.Polys[p + j] == PolyMesh.MeshNullIdx)
{
break;
}
nv++;
}
detailTryCount += nv - 2;
}
}
// Initialize the header and all nav data.
Header = new MeshHeader
{
Magic = Helper.NavMeshMagic,
Version = Helper.NavMeshVersion,
X = param.TileX,
Y = param.TileY,
Layer = param.TileLayer,
UserId = param.UserId,
PolyCount = totPolyCount,
VertCount = totVertCount,
MaxLinkCount = maxLinkCount,
DetailMeshCount = param.PolyCount,
DetailVertCount = uniqueDetailVertCount,
DetailTriCount = detailTryCount,
BVQuantFactor = 1.0f / param.Cs,
OffMeshBase = param.PolyCount,
WalkableHeight = param.WalkableHeight,
WalkableRadius = param.WalkableRadius,
WalkableClimb = param.WalkableClimb,
OffMeshConCount = storedOffMeshConCount,
BVNodeCount = param.BuildBvTree ? param.PolyCount * 2 : 0,
BMin = new float[3],
BMax = new float[3]
};
Array.Copy(param.BMin, Header.BMin, 3);
Array.Copy(param.BMax, Header.BMax, 3);
NavVerts = new float[totVertCount * 3];
NavPolys = new Poly[totPolyCount];
for (int i = 0; i < totPolyCount; i++)
{
NavPolys[i] = new Poly();
}
NavLinks = new Link[maxLinkCount];
for (int i = 0; i < maxLinkCount; i++)
{
NavLinks[i] = new Link();
}
NavDMeshes = new PolyDetail[param.PolyCount];
for (int i = 0; i < param.PolyCount; i++)
{
NavDMeshes[i] = new PolyDetail();
}
NavDVerts = new float[3 * uniqueDetailVertCount];
NavDTris = new short[4 * detailTryCount];
NavBvTree = param.BuildBvTree ? new BVNode[param.PolyCount * 2] : new BVNode[0];
if (param.BuildBvTree)
{
for (int i = 0; i < param.PolyCount * 2; i++)
{
NavBvTree[i] = new BVNode();
}
}
OffMeshCons = new OffMeshConnection[storedOffMeshConCount];
for (int i = 0; i < storedOffMeshConCount; i++)
{
OffMeshCons[i] = new OffMeshConnection();
}
int offMeshVertsBase = param.VertCount;
int offMeshPolyBase = param.PolyCount;
// store vertices
// Mesh
for (int i = 0; i < param.VertCount; i++)
{
int iv = i * 3;
int v = i * 3;
NavVerts[v + 0] = param.BMin[0] + param.Verts[iv + 0] * param.Cs;
NavVerts[v + 1] = param.BMin[1] + param.Verts[iv + 1] * param.Ch;
NavVerts[v + 2] = param.BMin[2] + param.Verts[iv + 2] * param.Cs;
}
// off-link
int n = 0;
for (int i = 0; i < param.OffMeshConCount; i++)
{
if (offMeshConClass[i * 2 + 0] == 0xff)
{
int linkv = i * 2 * 3;
int v = (offMeshVertsBase + n * 2) * 3;
Array.Copy(param.OffMeshConVerts, linkv, NavVerts, v, 3);
Array.Copy(param.OffMeshConVerts, linkv + 3, NavVerts, v + 3, 3);
n++;
}
}
// store polygons
// mesh
int src = 0;
for (int i = 0; i < param.PolyCount; i++)
{
Poly p = NavPolys[i];
p.VertCount = 0;
p.Flags = param.PolyFlags[i];
p.Area = param.PolyAreas[i];
p.Type = PolyTypeGround;
for (int j = 0; j < nvp; j++)
{
if (param.Polys[src + j] == PolyMesh.MeshNullIdx)
{
break;
}
p.Verts[j] = param.Polys[src + j];
if ((param.Polys[src + nvp + j] & 0x8000) != 0)
{
int dir = param.Polys[src + nvp + j] & 0xf;
if (dir == 0xf)
{
p.Neis[j] = 0;
}
else if (dir == 0)
{
p.Neis[j] = ExtLink | 4;
}
else if (dir == 1)
{
p.Neis[j] = ExtLink | 2;
}
else if (dir == 2)
{
p.Neis[j] = ExtLink | 0;
}
else if (dir == 3)
{
p.Neis[j] = ExtLink | 6;
}
}
else
{
p.Neis[j] = param.Polys[src + nvp + j] + 1;
}
p.VertCount++;
}
src += nvp * 2;
}
// off mesh
n = 0;
for (int i = 0; i < param.OffMeshConCount; i++)
{
if (offMeshConClass[i * 2 + 0] == 0xff)
{
Poly p = NavPolys[offMeshPolyBase + n];
p.VertCount = 2;
p.Verts[0] = (offMeshVertsBase + n * 2 + 0);
p.Verts[1] = (offMeshVertsBase + n * 2 + 1);
p.Flags = param.OffMeshConFlags[i];
p.Area = (short)param.OffMeshConAreas[i];
p.Type = PolyTypeOffMeshConnection;
n++;
}
}
// Store detail meshes and verts
if (param.DetailMeshes != null)
{
int vbase = 0;
for (int i = 0; i < param.PolyCount; i++)
{
PolyDetail dtl = NavDMeshes[i];
int vb = (int)param.DetailMeshes[i * 4 + 0];
int ndv = (int)param.DetailMeshes[i * 4 + 1];
int nv = NavPolys[i].VertCount;
dtl.VertBase = vbase;
dtl.VertCount = (short)(ndv - nv);
dtl.TriBase = param.DetailMeshes[i * 4 + 2];
dtl.TriCount = (short)param.DetailMeshes[i * 4 + 3];
if (ndv - nv > 0)
{
Array.Copy(param.DetailVerts, (vb + nv) * 3, NavDVerts, vbase * 3, (ndv - nv) * 3);
vbase += (short)(ndv - nv);
}
}
Array.Copy(param.DetailTris, NavDTris, param.DetailTriCount * 4);
}
else
{
// Create dummy detail mesh
int tbase = 0;
for (int i = 0; i < param.PolyCount; i++)
{
PolyDetail dtl = NavDMeshes[i];
int nv = NavPolys[i].VertCount;
dtl.VertBase = 0;
dtl.VertCount = 0;
dtl.TriBase = tbase;
dtl.TriCount = (short)(nv - 2);
for (int j = 2; j < nv; j++)
{
int t = tbase * 4;
NavDTris[t + 0] = 0;
NavDTris[t + 1] = (short)(j - 1);
NavDTris[t + 2] = (short)j;
NavDTris[t + 3] = (1 << 2);
if (j == 2)
{
NavDTris[t + 3] |= (1 << 0);
}
if (j == nv - 1)
{
NavDTris[t + 3] |= (1 << 4);
}
tbase++;
}
}
}
// Store and create BVTree
if (param.BuildBvTree)
{
CreateBVTree(param.Verts, param.VertCount, param.Polys, param.PolyCount, nvp, param.Cs, param.Ch, param.PolyCount * 2);
}
// store off-mesh connections
n = 0;
for (int i = 0; i < param.OffMeshConCount; i++)
{
if (offMeshConClass[i * 2 + 0] == 0xff)
{
OffMeshConnection con = OffMeshCons[n];
con.Poly = offMeshPolyBase + n;
int endPts = i * 2 * 3;
Array.Copy(param.OffMeshConVerts, endPts, con.Pos, 0, 3);
Array.Copy(param.OffMeshConVerts, endPts + 3, con.Pos, 3, 3);
con.Rad = param.OffMeshConRad[i];
con.Flags = param.OffMeshConDir[i] > 0 ? OffMeshConBiDir : (short)0;
con.Side = offMeshConClass[i * 2 + 1];
if (param.OffMeshConUserId != null)
{
con.UserId = param.OffMeshConUserId[i];
}
n++;
}
}
}