public Status GetPolyArea(long refId, ref short resultArea)
{
long salt, it, ip;
if (refId == 0)
{
return(Status.Failure);
}
DecodePolyId(refId, out salt, out it, out ip);
if (it >= _maxTiles)
{
return(Status.Failure | Status.InvalidParam);
}
if (_tiles[it].Salt != salt || _tiles[it].Header == null)
{
return(Status.Failure | Status.InvalidParam);
}
MeshTile tile = _tiles[it];
if (ip >= _tiles[it].Header.PolyCount)
{
return(Status.Failure | Status.InvalidParam);
}
Poly poly = tile.Polys[ip];
resultArea = poly.Area;
return(Status.Success);
}
public Status RestoreTileState(MeshTile tile, TileState tileState)
{
if (tileState.Magic != Helper.NavMeshMagic)
{
return(Status.Failure | Status.WrongMagic);
}
if (tileState.Version != Helper.NavMeshVersion)
{
return(Status.Failure | Status.WrongVersion);
}
if (tileState.Ref != GetTileRef(tile))
{
return(Status.Failure | Status.InvalidParam);
}
for (int i = 0; i < tile.Header.PolyCount; i++)
{
Poly p = tile.Polys[i];
PolyState s = tileState.PolyStates[i];
p.Flags = s.Flags;
p.Area = s.Area;
}
return(Status.Success);
}
public Status SetPolyFlags(long refId, int flags)
{
long salt, it, ip;
if (refId == 0)
{
return(Status.Failure);
}
DecodePolyId(refId, out salt, out it, out ip);
if (it >= _maxTiles)
{
return(Status.Failure | Status.InvalidParam);
}
if (_tiles[it].Salt != salt || _tiles[it].Header == null)
{
return(Status.Failure | Status.InvalidParam);
}
MeshTile tile = _tiles[it];
if (ip >= _tiles[it].Header.PolyCount)
{
return(Status.Failure | Status.InvalidParam);
}
Poly poly = tile.Polys[ip];
poly.Flags = flags;
return(Status.Success);
}
public float GetCost(float pax, float pay, float paz, float pbx, float pby, float pbz,
long prevRef, MeshTile prevTile, Poly prevPoly,
long curRef, MeshTile curTile, Poly curPoly,
long nextRef, MeshTile nextTile, Poly nextPoly)
{
return(Helper.VDist(pax, pay, paz, pbx, pby, pbz) * _areaCost[curPoly.Area]);
}
public Status GetTileAndPolyByRef(long refid, ref MeshTile tile, ref Poly poly)
{
if (refid == 0)
{
return(Status.Failure);
}
long salt, it, ip;
DecodePolyId(refid, out salt, out it, out ip);
if (it >= _maxTiles)
{
return(Status.Failure | Status.InvalidParam);
}
if (_tiles[it].Salt != salt || _tiles[it].Header == null)
{
return(Status.Failure | Status.InvalidParam);
}
if (ip >= _tiles[it].Header.PolyCount)
{
return(Status.Failure | Status.InvalidParam);
}
tile = _tiles[it];
poly = _tiles[it].Polys[ip];
return(Status.Success);
}
public void GetTileAndPolyByRefUnsafe(long refId, out MeshTile tile, out Poly poly)
{
long salt, it, ip;
DecodePolyId(refId, out salt, out it, out ip);
tile = _tiles[it];
poly = _tiles[it].Polys[ip];
}
private void BaseOffMeshLinks(MeshTile tile)
{
if (tile == null)
{
return;
}
long baseId = GetPolyRefBase(tile);
for (int i = 0; i < tile.Header.OffMeshConCount; i++)
{
OffMeshConnection con = tile.OffMeshCons[i];
Poly poly = tile.Polys[con.Poly];
float[] ext = { con.Rad, tile.Header.WalkableClimb, con.Rad };
int p = 0;
float[] nearestPt = new float[3];
long refId = FindNearestPolyInTile(tile, con.Pos[p + 0], con.Pos[p + 1], con.Pos[p + 2], ext[0], ext[1],
ext[2], ref nearestPt);
if (refId <= 0)
{
continue;
}
if (((nearestPt[0] - con.Pos[p + 0]) * (nearestPt[0] - con.Pos[p + 0])) +
((nearestPt[2] - con.Pos[p + 2]) * (nearestPt[2] - con.Pos[p + 2])) > (con.Rad * con.Rad))
{
continue;
}
int v = poly.Verts[0] * 3;
Array.Copy(nearestPt, 0, tile.Verts, v, 3);
long idx = AllocLink(tile);
if (idx != NullLink)
{
Link link = tile.Links[idx];
link.Ref = refId;
link.Edge = 0;
link.Side = 0xff;
link.BMin = link.BMax = 0;
link.Next = poly.FirstLink;
poly.FirstLink = idx;
}
long tidx = AllocLink(tile);
if (tidx != NullLink)
{
int landPolyIdx = (int)DecodePolyIdPoly(refId);
Poly landPoly = tile.Polys[landPolyIdx];
Link link = tile.Links[tidx];
link.Ref = baseId | con.Poly;
link.Edge = 0xff;
link.Side = 0xff;
link.BMin = link.BMax = 0;
link.Next = landPoly.FirstLink;
landPoly.FirstLink = tidx;
}
}
}
private int FindConnectingPolys(float vax, float vay, float vaz, float vbx, float vby, float vbz, MeshTile tile,
int side, ref long[] con, ref float[] conarea, int maxcon)
{
if (tile == null)
{
return(0);
}
float[] amin = new float[2], amax = new float[2];
Helper.CalcSlabEndPoints(vax, vay, vaz, vbx, vby, vbz, ref amin, ref amax, side);
float apos = Helper.GetSlabCoord(vax, vay, vaz, side);
float[] bmin = new float[2], bmax = new float[2];
int m = NavMeshBuilder.ExtLink | side;
int n = 0;
long baseId = GetPolyRefBase(tile);
for (int i = 0; i < tile.Header.PolyCount; i++)
{
Poly poly = tile.Polys[i];
int nv = poly.VertCount;
for (int j = 0; j < nv; j++)
{
if (poly.Neis[j] != m)
{
continue;
}
int vc = poly.Verts[j] * 3;
int vd = poly.Verts[(j + 1) % nv] * 3;
float bpos = Helper.GetSlabCoord(tile.Verts[vc + 0], tile.Verts[vc + 1], tile.Verts[vc + 2], side);
if (Math.Abs(apos - bpos) > 0.01f)
{
continue;
}
Helper.CalcSlabEndPoints(tile.Verts[vc + 0], tile.Verts[vc + 1], tile.Verts[vc + 2], tile.Verts[vd + 0], tile.Verts[vd + 1], tile.Verts[vd + 2], ref bmin, ref bmax, side);
if (!Helper.OverlapSlabs(amin, amax, bmin, bmax, 0.01f, tile.Header.WalkableClimb))
{
continue;
}
if (n < maxcon)
{
conarea[n * 2 + 0] = Math.Max(amin[0], bmin[0]);
conarea[n * 2 + 1] = Math.Min(amax[0], bmax[0]);
con[n] = baseId | i;
n++;
}
break;
}
}
return(n);
}
public Status GetOffMeshConnectionPolyEndPoints(long prevRef, long polyRef, ref float[] startPos, ref float[] endPos)
{
long salt, it, ip;
if (polyRef == 0)
{
return(Status.Failure);
}
DecodePolyId(polyRef, out salt, out it, out ip);
if (it >= _maxTiles)
{
return(Status.Failure | Status.InvalidParam);
}
if (_tiles[it].Salt != salt || _tiles[it].Header == null)
{
return(Status.Failure | Status.InvalidParam);
}
MeshTile tile = _tiles[it];
if (ip >= _tiles[it].Header.PolyCount)
{
return(Status.Failure | Status.InvalidParam);
}
Poly poly = tile.Polys[ip];
if (poly.Type != NavMeshBuilder.PolyTypeOffMeshConnection)
{
return(Status.Failure);
}
int idx0 = 0, idx1 = 1;
for (long i = poly.FirstLink; i != NullLink; i = tile.Links[i].Next)
{
if (tile.Links[i].Edge == 0)
{
if (tile.Links[i].Ref != prevRef)
{
idx0 = 1;
idx1 = 0;
}
break;
}
}
Array.Copy(tile.Verts, poly.Verts[idx0] * 3, startPos, 0, 3);
Array.Copy(tile.Verts, poly.Verts[idx1] * 3, endPos, 0, 3);
return(Status.Success);
}
public Status StoreTileState(MeshTile tile, out TileState tileState)
{
tileState = new TileState();
tileState.Magic = Helper.NavMeshMagic;
tileState.Version = Helper.NavMeshVersion;
tileState.Ref = GetTileRef(tile);
tileState.PolyStates = new PolyState[tile.Header.PolyCount];
for (int i = 0; i < tile.Header.PolyCount; i++)
{
Poly p = tile.Polys[i];
tileState.PolyStates[i] = new PolyState();
tileState.PolyStates[i].Flags = p.Flags;
tileState.PolyStates[i].Area = p.Area;
}
return(Status.Success);
}
private void ConnectIntLinks(MeshTile tile)
{
if (tile == null)
{
return;
}
long baseId = GetPolyRefBase(tile);
for (int i = 0; i < tile.Header.PolyCount; i++)
{
Poly poly = tile.Polys[i];
poly.FirstLink = NullLink;
if (poly.Type == NavMeshBuilder.PolyTypeOffMeshConnection)
{
continue;
}
for (int j = poly.VertCount - 1; j >= 0; j--)
{
if (poly.Neis[j] == 0 || (poly.Neis[j] & NavMeshBuilder.ExtLink) != 0)
{
continue;
}
long idx = AllocLink(tile);
if (idx != NullLink)
{
Link link = tile.Links[idx];
link.Ref = baseId | (poly.Neis[j] - 1);
link.Edge = (short)j;
link.Side = 0xff;
link.BMin = link.BMax = 0;
link.Next = poly.FirstLink;
poly.FirstLink = idx;
}
}
}
}
private void UnconnectExtLinks(ref MeshTile tile, ref MeshTile target)
{
if (tile == null || target == null)
{
return;
}
long targetNum = DecodePolyIdTile(GetTileRef(target));
for (int i = 0; i < tile.Header.PolyCount; i++)
{
Poly poly = tile.Polys[i];
long j = poly.FirstLink;
long pj = NullLink;
while (j != NullLink)
{
if (tile.Links[j].Side != 0xff && DecodePolyIdTile(tile.Links[j].Ref) == targetNum)
{
long nj = tile.Links[j].Next;
if (pj == NullLink)
{
poly.FirstLink = nj;
}
else
{
tile.Links[pj].Next = nj;
}
FreeLink(ref tile, j);
j = nj;
}
else
{
pj = j;
j = tile.Links[j].Next;
}
}
}
}
public OffMeshConnection GetOffMeshConnectionByRef(long refId)
{
long salt, it, ip;
if (refId == 0)
{
return(null);
}
DecodePolyId(refId, out salt, out it, out ip);
if (it >= _maxTiles)
{
return(null);
}
if (_tiles[it].Salt != salt || _tiles[it].Header == null)
{
return(null);
}
MeshTile tile = _tiles[it];
if (ip >= _tiles[it].Header.PolyCount)
{
return(null);
}
Poly poly = tile.Polys[ip];
if (poly.Type != NavMeshBuilder.PolyTypeOffMeshConnection)
{
return(null);
}
long idx = ip - tile.Header.OffMeshBase;
return(tile.OffMeshCons[idx]);
}
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++;
}
}
}
public bool PassFilter(long refId, MeshTile tile, Poly poly)
{
return((poly.Flags & IncludeFlags) != 0 && (poly.Flags & ExcludeFlags) == 0);
}
private void ConnectExtOffMeshLinks(ref MeshTile tile, ref MeshTile target, int side)
{
if (tile == null)
{
return;
}
short oppositeSide = (side == -1) ? (short)0xff : (short)Helper.OppositeTile(side);
for (int i = 0; i < target.Header.OffMeshConCount; i++)
{
OffMeshConnection targetCon = target.OffMeshCons[i];
if (targetCon.Side != oppositeSide)
{
continue;
}
Poly targetPoly = target.Polys[targetCon.Poly];
if (targetPoly.FirstLink == NullLink)
{
continue;
}
float[] ext = { targetCon.Rad, target.Header.WalkableClimb, targetCon.Rad };
int p = 3;
float[] nearestPt = new float[3];
long refId = FindNearestPolyInTile(tile, targetCon.Pos[p + 0], targetCon.Pos[p + 1], targetCon.Pos[p + 2],
ext[0], ext[1], ext[2], ref nearestPt);
if (refId <= 0)
{
continue;
}
if (((nearestPt[0] - targetCon.Pos[p + 0]) * (nearestPt[0] - targetCon.Pos[p + 0])) + ((nearestPt[2] - targetCon.Pos[p + 2]) * (nearestPt[2] - targetCon.Pos[p + 2])) > (targetCon.Rad * targetCon.Rad))
{
continue;
}
int v = targetPoly.Verts[1] * 3;
Array.Copy(nearestPt, 0, target.Verts, v, 3);
long idx = AllocLink(target);
if (idx != NullLink)
{
Link link = target.Links[idx];
link.Ref = refId;
link.Edge = 1;
link.Side = oppositeSide;
link.BMin = link.BMax = 0;
link.Next = targetPoly.FirstLink;
targetPoly.FirstLink = idx;
}
if ((targetCon.Flags & NavMeshBuilder.OffMeshConBiDir) != 0)
{
long tidx = AllocLink(tile);
if (tidx != NullLink)
{
int landPolyIdx = (int)DecodePolyIdPoly(refId);
Poly landPoly = tile.Polys[landPolyIdx];
Link link = tile.Links[tidx];
link.Ref = GetPolyRefBase(target) | (targetCon.Poly);
link.Edge = 0xff;
link.Side = side == -1 ? (short)0xff : (short)side;
link.BMin = link.BMax = 0;
link.Next = landPoly.FirstLink;
landPoly.FirstLink = tidx;
}
}
}
}
private int QueryPolygonsInTile(MeshTile tile, float qminx, float qminy, float qminz, float qmaxx, float qmaxy,
float qmaxz, ref long[] polys, int maxPolys)
{
if (tile.BVTree != null)
{
int node = 0;
int end = tile.Header.BVNodeCount;
float[] tbmin = tile.Header.BMin;
float[] tbmax = tile.Header.BMax;
float qfac = tile.Header.BVQuantFactor;
int[] bmin = new int[3], bmax = new int[3];
float minx = Math.Min(tbmax[0], Math.Max(qminx, tbmin[0])) - tbmin[0];
float miny = Math.Min(tbmax[1], Math.Max(qminy, tbmin[1])) - tbmin[1];
float minz = Math.Min(tbmax[2], Math.Max(qminz, tbmin[2])) - tbmin[2];
float maxx = Math.Min(tbmax[0], Math.Max(qmaxx, tbmin[0])) - tbmin[0];
float maxy = Math.Min(tbmax[1], Math.Max(qmaxy, tbmin[1])) - tbmin[1];
float maxz = Math.Min(tbmax[2], Math.Max(qmaxz, tbmin[2])) - tbmin[2];
bmin[0] = (int)(qfac * minx) & 0xfffe;
bmin[1] = (int)(qfac * miny) & 0xfffe;
bmin[2] = (int)(qfac * minz) & 0xfffe;
bmax[0] = (int)(qfac * maxx + 1) | 1;
bmax[1] = (int)(qfac * maxy + 1) | 1;
bmax[2] = (int)(qfac * maxz + 1) | 1;
long baseId = GetPolyRefBase(tile);
int n = 0;
while (node < end)
{
bool overlap = Helper.OverlapQuantBounds(bmin, bmax, tile.BVTree[node].BMin, tile.BVTree[node].BMax);
bool isLeafNode = tile.BVTree[node].I >= 0;
if (isLeafNode && overlap)
{
if (n < maxPolys)
{
polys[n++] = baseId | tile.BVTree[node].I;
}
}
if (overlap || isLeafNode)
{
node++;
}
else
{
int escapeIndex = -tile.BVTree[node].I;
node += escapeIndex;
}
}
return(n);
}
else
{
float[] bmin = new float[3], bmax = new float[3];
int n = 0;
long baseId = GetPolyRefBase(tile);
for (int i = 0; i < tile.Header.PolyCount; i++)
{
Poly p = tile.Polys[i];
if (p.Type == NavMeshBuilder.PolyTypeOffMeshConnection)
{
continue;
}
int v = p.Verts[0 * 3];
Array.Copy(tile.Verts, v, bmin, 0, 3);
Array.Copy(tile.Verts, v, bmax, 0, 3);
for (int j = 1; j < p.VertCount; j++)
{
v = p.Verts[j] * 3;
Helper.VMin(ref bmin, tile.Verts[v + 0], tile.Verts[v + 1], tile.Verts[v + 2]);
Helper.VMax(ref bmax, tile.Verts[v + 0], tile.Verts[v + 1], tile.Verts[v + 2]);
}
if (Helper.OverlapBounds(qminx, qminy, qminz, qmaxx, qmaxy, qmaxz, bmin[0], bmin[1], bmin[2],
bmax[0], bmax[1], bmax[2]))
{
if (n < maxPolys)
{
polys[n++] = baseId | i;
}
}
}
return(n);
}
}
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;
}
}
}
private void ConnectExtLinks(ref MeshTile tile, ref MeshTile target, int side)
{
if (tile == null)
{
return;
}
for (int i = 0; i < tile.Header.PolyCount; i++)
{
Poly poly = tile.Polys[i];
int nv = poly.VertCount;
for (int j = 0; j < nv; j++)
{
if ((poly.Neis[j] & NavMeshBuilder.ExtLink) == 0)
{
continue;
}
int dir = (int)(poly.Neis[j] & 0xff);
if (side != -1 && dir != side)
{
continue;
}
int va = poly.Verts[j] * 3;
int vb = poly.Verts[(j + 1) % nv] * 3;
long[] nei = new long[4];
float[] neia = new float[4 * 2];
int nnei = FindConnectingPolys(tile.Verts[va + 0], tile.Verts[va + 1], tile.Verts[va + 2],
tile.Verts[vb + 0], tile.Verts[vb + 1], tile.Verts[vb + 2], target,
Helper.OppositeTile(dir), ref nei, ref neia, 4);
for (int k = 0; k < nnei; k++)
{
long idx = AllocLink(tile);
if (idx != NullLink)
{
Link link = tile.Links[idx];
link.Ref = nei[k];
link.Edge = (short)j;
link.Side = (short)dir;
link.Next = poly.FirstLink;
poly.FirstLink = idx;
if (dir == 0 || dir == 4)
{
float tmin = (neia[k * 2 + 0] - tile.Verts[va + 2]) /
(tile.Verts[vb + 2] - tile.Verts[va + 2]);
float tmax = (neia[k * 2 + 1] - tile.Verts[va + 2]) /
(tile.Verts[vb + 2] - tile.Verts[va + 2]);
if (tmin > tmax)
{
float temp = tmin;
tmin = tmax;
tmax = temp;
}
link.BMin = (short)(Math.Min(1.0f, Math.Max(tmin, 0.0f)) * 255.0f);
link.BMax = (short)(Math.Min(1.0f, Math.Max(tmax, 0.0f)) * 255.0f);
}
else if (dir == 2 || dir == 6)
{
float tmin = (neia[k * 2 + 0] - tile.Verts[va + 0]) /
(tile.Verts[vb + 0] - tile.Verts[va + 0]);
float tmax = (neia[k * 2 + 1] - tile.Verts[va + 0]) /
(tile.Verts[vb + 0] - tile.Verts[va + 0]);
if (tmin > tmax)
{
float temp = tmin;
tmin = tmax;
tmax = temp;
}
link.BMin = (short)(Math.Min(1.0f, Math.Max(tmin, 0.0f)) * 255.0f);
link.BMax = (short)(Math.Min(1.0f, Math.Max(tmax, 0.0f)) * 255.0f);
}
}
}
}
}
}
