public static uint allocLink(dtMeshTile tile)
{
if (tile.linksFreeList == Detour.DT_NULL_LINK)
{
return(DT_NULL_LINK);
}
uint link = tile.linksFreeList;
tile.linksFreeList = tile.links[link].next;
return(link);
}
/// Builds internal polygons links for a tile.
public void connectIntLinks(dtMeshTile tile)
{
if (tile == null)
return;
dtPolyRef polyRefBase = getPolyRefBase(tile);
for (int i = 0; i < tile.header.polyCount; ++i)
{
dtPoly poly = tile.polys[i];
poly.firstLink = DT_NULL_LINK;
if (poly.getType() == (byte) dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
continue;
// Build edge links backwards so that the links will be
// in the linked list from lowest index to highest.
for (int j = poly.vertCount-1; j >= 0; --j)
{
// Skip hard and non-internal edges.
if (poly.neis[j] == 0 || (poly.neis[j] & DT_EXT_LINK) != 0)
continue;
uint idx = allocLink(tile);
if (idx != DT_NULL_LINK)
{
dtLink link = tile.links[idx];
link.polyRef = polyRefBase | (dtPolyRef)(poly.neis[j]-1);
link.edge = (byte)j;
link.side = 0xff;
link.bmin = link.bmax = 0;
// Add to linked list.
link.next = poly.firstLink;
poly.firstLink = idx;
}
}
}
}
/// Builds external polygon links for a tile.
public void connectExtOffMeshLinks(dtMeshTile tile, dtMeshTile target, int side)
{
if (tile == null)
return;
// Connect off-mesh links.
// We are interested on links which land from target tile to this tile.
byte oppositeSide = (side == -1) ? (byte)0xff : (byte)dtOppositeTile(side);
for (int i = 0; i < target.header.offMeshConCount; ++i)
{
dtOffMeshConnection targetCon = target.offMeshCons[i];
if (targetCon.side != oppositeSide)
continue;
dtPoly targetPoly = target.polys[targetCon.poly];
// Skip off-mesh connections which start location could not be connected at all.
if (targetPoly.firstLink == DT_NULL_LINK)
continue;
float[] ext = new float[] { targetCon.rad, target.header.walkableClimb, targetCon.rad };
// Find polygon to connect to.
//const float* p = &targetCon.pos[3];
int pIndex = 3;
float[] nearestPt = new float[3];
dtPolyRef polyRef = findNearestPolyInTile(tile, targetCon.pos, pIndex, ext, nearestPt);
if (polyRef == 0)
continue;
// findNearestPoly may return too optimistic results, further check to make sure.
if (dtSqr(nearestPt[0]-targetCon.pos[pIndex])+dtSqr(nearestPt[2]-targetCon.pos[pIndex + 2]) > dtSqr(targetCon.rad))
continue;
// Make sure the location is on current mesh.
//float* v = &target.verts[targetPoly.verts[1]*3];
int vIndex = targetPoly.verts[1]*3;
dtVcopy(target.verts, vIndex, nearestPt, 0);
// Link off-mesh connection to target poly.
uint idx = allocLink(target);
if (idx != DT_NULL_LINK)
{
dtLink link = target.links[idx];
link.polyRef = polyRef;
link.edge = (byte)1;
link.side = oppositeSide;
link.bmin = link.bmax = 0;
// Add to linked list.
link.next = targetPoly.firstLink;
targetPoly.firstLink = idx;
}
// Link target poly to off-mesh connection.
if ((targetCon.flags & DT_OFFMESH_CON_BIDIR )!= 0)
{
uint tidx = allocLink(tile);
if (tidx != DT_NULL_LINK)
{
ushort landPolyIdx = (ushort)decodePolyIdPoly(polyRef);
dtPoly landPoly = tile.polys[landPolyIdx];
dtLink link = tile.links[tidx];
link.polyRef = getPolyRefBase(target) | (dtPolyRef)(targetCon.poly);
link.edge = 0xff;
link.side = (byte)(side == -1 ? 0xff : side);
link.bmin = link.bmax = 0;
// Add to linked list.
link.next = landPoly.firstLink;
landPoly.firstLink = tidx;
}
}
}
}
/// Builds external polygon links for a tile.
public void connectExtLinks(dtMeshTile tile, dtMeshTile target, int side)
{
if (tile == null)
return;
// Connect border links.
for (int i = 0; i < tile.header.polyCount; ++i)
{
dtPoly poly = tile.polys[i];
// Create new links.
// ushort m = DT_EXT_LINK | (ushort)side;
int nv = (int)poly.vertCount;
for (int j = 0; j < nv; ++j)
{
// Skip non-portal edges.
if ((poly.neis[j] & DT_EXT_LINK) == 0)
continue;
int dir = (int)(poly.neis[j] & 0xff);
if (side != -1 && dir != side)
continue;
// Create new links
//const float* va = &tile.verts[poly.verts[j]*3];
//const float* vb = &tile.verts[poly.verts[(j+1) % nv]*3];
int vaStart = poly.verts[j]*3;
int vbStart = poly.verts[(j+1) % nv]*3;
dtPolyRef[] nei = new dtPolyRef[4];
float[] neia = new float[4*2];
int nnei = findConnectingPolys(tile.verts,vaStart,tile.verts,vbStart, target, dtOppositeTile(dir), nei,neia,4);
for (int k = 0; k < nnei; ++k)
{
uint idx = allocLink(tile);
if (idx != DT_NULL_LINK)
{
dtLink link = tile.links[idx];
link.polyRef = nei[k];
link.edge = (byte)j;
link.side = (byte)dir;
link.next = poly.firstLink;
poly.firstLink = idx;
// Compress portal limits to a byte value.
if (dir == 0 || dir == 4)
{
float tmin = (neia[k*2+0]-tile.verts[vaStart + 2]) / (tile.verts[vbStart + 2]-tile.verts[vaStart + 2]);
float tmax = (neia[k*2+1]-tile.verts[vaStart + 2]) / (tile.verts[vbStart +2]-tile.verts[vaStart + 2]);
if (tmin > tmax)
dtSwap(ref tmin,ref tmax);
link.bmin = (byte)(dtClamp(tmin, 0.0f, 1.0f)*255.0f);
link.bmax = (byte)(dtClamp(tmax, 0.0f, 1.0f)*255.0f);
}
else if (dir == 2 || dir == 6)
{
float tmin = (neia[k*2+0]-tile.verts[vaStart + 0]) / (tile.verts[vbStart +0]-tile.verts[vaStart + 0]);
float tmax = (neia[k*2+1]-tile.verts[vaStart + 0]) / (tile.verts[vbStart +0]-tile.verts[vaStart + 0]);
if (tmin > tmax)
dtSwap(ref tmin,ref tmax);
link.bmin = (byte)(dtClamp(tmin, 0.0f, 1.0f)*255.0f);
link.bmax = (byte)(dtClamp(tmax, 0.0f, 1.0f)*255.0f);
}
}
}
}
}
}
/// @par
///
/// Example use case:
/// @code
///
/// const dtPolyRef base = navmesh.getPolyRefBase(tile);
/// for (int i = 0; i < tile.header.polyCount; ++i)
/// {
/// const dtPoly* p = &tile.polys[i];
/// const dtPolyRef ref = base | (dtPolyRef)i;
///
/// // Use the reference to access the polygon data.
/// }
/// @endcode
/// Gets the polygon reference for the tile's base polygon.
/// @param[in] tile The tile.
/// @return The polygon reference for the base polygon in the specified tile.
public dtPolyRef getPolyRefBase(dtMeshTile tile)
{
if (tile == null) return 0;
uint it = (uint) Array.IndexOf(m_tiles, tile);
return encodePolyId(tile.salt, it, 0);
}
dtStatus getEdgeMidPoint(dtPolyRef from, dtPoly fromPoly, dtMeshTile fromTile,
dtPolyRef to, dtPoly toPoly, dtMeshTile toTile,
float[] mid)
{
float[] left = new float[3];//, right[3];
float[] right = new float[3];
if (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right)))
return DT_FAILURE | DT_INVALID_PARAM;
mid[0] = (left[0]+right[0])*0.5f;
mid[1] = (left[1]+right[1])*0.5f;
mid[2] = (left[2]+right[2])*0.5f;
return DT_SUCCESS;
}
/// Finds polygons that overlap the search box.
/// @param[in] center The center of the search box. [(x, y, z)]
/// @param[in] extents The search distance along each axis. [(x, y, z)]
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] polys The reference ids of the polygons that overlap the query box.
/// @param[out] polyCount The number of polygons in the search result.
/// @param[in] maxPolys The maximum number of polygons the search result can hold.
/// @returns The status flags for the query.
/// @par
///
/// If no polygons are found, the function will return #DT_SUCCESS with a
/// @p polyCount of zero.
///
/// If @p polys is too small to hold the entire result set, then the array will
/// be filled to capacity. The method of choosing which polygons from the
/// full set are included in the partial result set is undefined.
///
public dtStatus queryPolygons(float[] center, float[] extents,
dtQueryFilter filter,
dtPolyRef[] polys, ref int polyCount, int maxPolys)
{
Debug.Assert(m_nav != null);
float[] bmin = new float[3];//, bmax[3];
float[] bmax = new float[3];
dtVsub(bmin, center, extents);
dtVadd(bmax, center, extents);
// Find tiles the query touches.
int minx = 0, miny = 0, maxx = 0 , maxy = 0;
m_nav.calcTileLoc(bmin, ref minx, ref miny);
m_nav.calcTileLoc(bmax, ref maxx, ref maxy);
int MAX_NEIS = 32;
dtMeshTile[] neis = new dtMeshTile[MAX_NEIS];
int n = 0;
for (int y = miny; y <= maxy; ++y)
{
for (int x = minx; x <= maxx; ++x)
{
int nneis = m_nav.getTilesAt(x,y,neis,MAX_NEIS);
for (int j = 0; j < nneis; ++j)
{
n += queryPolygonsInTile(neis[j], bmin, bmax, filter, polys, n, maxPolys-n);
if (n >= maxPolys)
{
polyCount = n;
return DT_SUCCESS | DT_BUFFER_TOO_SMALL;
}
}
}
}
polyCount = n;
return DT_SUCCESS;
}
public int queryPolygonsInTile(dtMeshTile tile, float[] qmin, float[] qmax,
dtPolyRef[] polys, int maxPolys)
{
if (tile.bvTree != null)
{
// tile.bvTree[0];
//dtBVNode end = tile.bvTree[tile.header.bvNodeCount];
int endNodeIndex = tile.header.bvNodeCount;
float[] tbmin = tile.header.bmin;
float[] tbmax = tile.header.bmax;
float qfac = tile.header.bvQuantFactor;
// Calculate quantized box
ushort[] bmin = new ushort[3];//, bmax[3];
ushort[] bmax = new ushort[3];
// dtClamp query box to world box.
float minx = dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0];
float miny = dtClamp(qmin[1], tbmin[1], tbmax[1]) - tbmin[1];
float minz = dtClamp(qmin[2], tbmin[2], tbmax[2]) - tbmin[2];
float maxx = dtClamp(qmax[0], tbmin[0], tbmax[0]) - tbmin[0];
float maxy = dtClamp(qmax[1], tbmin[1], tbmax[1]) - tbmin[1];
float maxz = dtClamp(qmax[2], tbmin[2], tbmax[2]) - tbmin[2];
// Quantize
bmin[0] = (ushort)((ushort)(qfac * minx) & 0xfffe);
bmin[1] = (ushort)((ushort)(qfac * miny) & 0xfffe);
bmin[2] = (ushort)((ushort)(qfac * minz) & 0xfffe);
bmax[0] = (ushort)((ushort)(qfac * maxx + 1) | 1);
bmax[1] = (ushort)((ushort)(qfac * maxy + 1) | 1);
bmax[2] = (ushort)((ushort)(qfac * maxz + 1) | 1);
// Traverse tree
dtPolyRef polyRefBase = getPolyRefBase(tile);
int n = 0;
int curNode = 0;
dtBVNode node = null;
while (curNode < endNodeIndex)
{
node = tile.bvTree[curNode];
bool overlap = dtOverlapQuantBounds(bmin, bmax, node.bmin, node.bmax);
bool isLeafNode = node.i >= 0;
if (isLeafNode && overlap)
{
if (n < maxPolys){
polys[n++] = polyRefBase | (dtPolyRef)node.i;
}
}
if (overlap || isLeafNode){
//node++;
++curNode;
}
else
{
int escapeIndex = - node.i;
curNode += escapeIndex;
}
}
return n;
}
else
{
float[] bmin = new float[3];//, bmax[3];
float[] bmax = new float[3];
int n = 0;
dtPolyRef polyRefBase = getPolyRefBase(tile);
for (int i = 0; i < tile.header.polyCount; ++i)
{
dtPoly p = tile.polys[i];
// Do not return off-mesh connection polygons.
if (p.getType() == (byte) dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
continue;
// Calc polygon bounds.
//float[] v = tile.verts[p.verts[0]*3];
int vIndex = p.verts[0]*3;
dtVcopy(bmin,0 ,tile.verts, vIndex);
dtVcopy(bmax,0 ,tile.verts, vIndex);
for (int j = 1; j < p.vertCount; ++j)
{
//v = &tile.verts[p.verts[j]*3];
vIndex = p.verts[j]*3;
dtVmin(bmin, 0 ,tile.verts, vIndex);
dtVmax(bmax, 0 ,tile.verts, vIndex);
}
if (dtOverlapBounds(qmin,qmax, bmin,bmax))
{
if (n < maxPolys)
polys[n++] = polyRefBase | (dtPolyRef)i;
}
}
return n;
}
}
/// @par
///
/// This function will not fail if the tiles array is too small to hold the
/// entire result set. It will simply fill the array to capacity.
/// Gets all tiles at the specified grid location. (All layers.)
/// @param[in] x The tile's x-location. (x, y)
/// @param[in] y The tile's y-location. (x, y)
/// @param[out] tiles A pointer to an array of tiles that will hold the result.
/// @param[in] maxTiles The maximum tiles the tiles parameter can hold.
/// @return The number of tiles returned in the tiles array.
public int getTilesAt(int x, int y, dtMeshTile[] tiles, int maxTiles)
{
int n = 0;
// Find tile based on hash.
int h = computeTileHash(x,y,m_tileLutMask);
dtMeshTile tile = m_posLookup[h];
while (tile != null)
{
if (tile.header != null &&
tile.header.x == x &&
tile.header.y == y)
{
if (n < maxTiles)
tiles[n++] = tile;
}
tile = tile.next;
}
return n;
}
/// @{
/// @name Initialization and Tile Management
/// Initializes the navigation mesh for tiled use.
/// @param[in] params Initialization parameters.
/// @return The status flags for the operation.
public dtStatus init(dtNavMeshParams navMeshParams)
{
//memcpy(&m_params, params, sizeof(dtNavMeshParams));
m_params = navMeshParams.Clone();
dtVcopy(m_orig, navMeshParams.orig);
m_tileWidth = navMeshParams.tileWidth;
m_tileHeight = navMeshParams.tileHeight;
// Init tiles
m_maxTiles = navMeshParams.maxTiles;
m_tileLutSize = (int)dtNextPow2((uint)(navMeshParams.maxTiles/4));
if (m_tileLutSize == 0)
m_tileLutSize = 1;
m_tileLutMask = m_tileLutSize-1;
//m_tiles = (dtMeshTile*)dtAlloc(sizeof(dtMeshTile)*m_maxTiles, DT_ALLOC_PERM);
m_tiles = new dtMeshTile[m_maxTiles];
dtcsArrayItemsCreate(m_tiles);
if (m_tiles == null)
return (DT_FAILURE | DT_OUT_OF_MEMORY);
//m_posLookup = (dtMeshTile**)dtAlloc(sizeof(dtMeshTile*)*m_tileLutSize, DT_ALLOC_PERM);
m_posLookup = new dtMeshTile[m_tileLutSize];
dtcsArrayItemsCreate(m_posLookup);
if (m_posLookup == null)
return DT_FAILURE | DT_OUT_OF_MEMORY;
//memset(m_tiles, 0, sizeof(dtMeshTile)*m_maxTiles);
//memset(m_posLookup, 0, sizeof(dtMeshTile*)*m_tileLutSize);
m_nextFree = null;
for (int i = m_maxTiles-1; i >= 0; --i)
{
m_tiles[i].salt = 1;
m_tiles[i].next = m_nextFree;
m_nextFree = m_tiles[i];
}
// Init ID generator values.
#if DT_POLYREF64
#else
m_tileBits = (dtStatus)dtIlog2(dtNextPow2((uint)navMeshParams.maxTiles));
m_polyBits = (dtStatus)dtIlog2(dtNextPow2((uint)navMeshParams.maxPolys));
// Only allow 31 salt bits, since the salt mask is calculated using 32bit uint and it will overflow.
m_saltBits = Math.Min((uint)31, 32 - m_tileBits - m_polyBits);
if (m_saltBits < 10)
return DT_FAILURE | DT_INVALID_PARAM;
#endif
return DT_SUCCESS;
}
/// Gets the tile reference for the specified tile.
/// @param[in] tile The tile.
/// @return The tile reference of the tile.
public dtTileRef getTileRef(dtMeshTile tile)
{
if (tile == null) return 0;
uint it = (uint) Array.IndexOf(m_tiles, tile); //(uint)(tile - m_tiles);
return encodePolyId(tile.salt, it, 0);
}
public void getTileAndPolyByRefUnsafe(dtPolyRef polyRef, ref dtMeshTile tile, ref dtPoly poly, ref uint ip)
{
uint salt = 0, it = 0;
decodePolyId(polyRef,ref salt,ref it,ref ip);
tile = m_tiles[it];
poly = m_tiles[it].polys[ip];
}
//C# port : also return ip because the code used to do pointer arithmetics on the
// array's addresses, which is a no in C# because managed array may not be contiguous in memory
public dtStatus getTileAndPolyByRef(dtPolyRef polyRef, ref dtMeshTile tile, ref dtPoly poly, ref uint ip)
{
if (polyRef == 0)
return DT_FAILURE;
uint salt = 0, it = 0;
decodePolyId(polyRef, ref salt, ref it, ref ip);
if (it >= (uint)m_maxTiles)
return DT_FAILURE | DT_INVALID_PARAM;
if (m_tiles[it].salt != salt || m_tiles[it].header == null)
return DT_FAILURE | DT_INVALID_PARAM;
if (ip >= (uint)m_tiles[it].header.polyCount)
return DT_FAILURE | DT_INVALID_PARAM;
tile = m_tiles[it];
poly = m_tiles[it].polys[ip];
return DT_SUCCESS;
}
//////////////////////////////////////////////////////////////////////////////////////////
/// Returns all polygons in neighbour tile based on portal defined by the segment.
public int findConnectingPolys(float[] va, int vaStart, float[] vb, int vbStart,
dtMeshTile tile, int side,
dtPolyRef[] con, float[] conarea, int maxcon)
{
if (tile == null)
return 0;
float[] amin = new float[2];
float[] amax = new float[2];
calcSlabEndPoints(va, vaStart,vb, vbStart, amin,amax, side);
float apos = getSlabCoord(va, vaStart, side);
// Remove links pointing to 'side' and compact the links array.
float[] bmin = new float[2];
float[] bmax = new float[2];
ushort m =(ushort)( DT_EXT_LINK | (ushort)side );
int n = 0;
dtPolyRef polyRefBase = getPolyRefBase(tile);
for (int i = 0; i < tile.header.polyCount; ++i)
{
dtPoly poly = tile.polys[i];
int nv = (int)poly.vertCount;
for (int j = 0; j < nv; ++j)
{
// Skip edges which do not point to the right side.
if (poly.neis[j] != m)
continue;
//const float* vc = &tile.verts[poly.verts[j]*3];
//const float* vd = &tile.verts[poly.verts[(j+1) % nv]*3];
int vcStart = poly.verts[j]*3;
int vdStart = poly.verts[(j+1) % nv]*3;
float bpos = getSlabCoord(tile.verts, vcStart, side);
// Segments are not close enough.
if (Math.Abs(apos-bpos) > 0.01f)
continue;
// Check if the segments touch.
calcSlabEndPoints(tile.verts,vcStart,tile.verts,vdStart, bmin,bmax, side);
if (!overlapSlabs(amin,amax, bmin,bmax, 0.01f, tile.header.walkableClimb))
continue;
// Add return value.
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] = polyRefBase | (dtPolyRef)i;
n++;
}
break;
}
}
return n;
}
/// @par
///
/// This function returns the data for the tile so that, if desired,
/// it can be added back to the navigation mesh at a later point.
///
/// @see #addTile
/// Removes the specified tile from the navigation mesh.
/// @param[in] ref The reference of the tile to remove.
/// @param[out] data Data associated with deleted tile.
/// @param[out] dataSize Size of the data associated with deleted tile.
/// @return The status flags for the operation.
public dtStatus removeTile(dtTileRef tileRef, out dtRawTileData rawTileData)
{
rawTileData = null;
if (tileRef == 0)
return DT_FAILURE | DT_INVALID_PARAM;
uint tileIndex = decodePolyIdTile((dtPolyRef)tileRef);
uint tileSalt = decodePolyIdSalt((dtPolyRef)tileRef);
if ((int)tileIndex >= m_maxTiles)
return DT_FAILURE | DT_INVALID_PARAM;
dtMeshTile tile = m_tiles[tileIndex];
if (tile.salt != tileSalt)
return DT_FAILURE | DT_INVALID_PARAM;
// Remove tile from hash lookup.
int h = computeTileHash(tile.header.x,tile.header.y,m_tileLutMask);
dtMeshTile prev = null;
dtMeshTile cur = m_posLookup[h];
while (cur != null)
{
if (cur == tile)
{
if (prev != null)
prev.next = cur.next;
else
m_posLookup[h] = cur.next;
break;
}
prev = cur;
cur = cur.next;
}
// Remove connections to neighbour tiles.
// Create connections with neighbour tiles.
const int MAX_NEIS = 32;
dtMeshTile[] neis = new dtMeshTile[MAX_NEIS];
int nneis;
// Connect with layers in current tile.
nneis = getTilesAt(tile.header.x, tile.header.y, neis, MAX_NEIS);
for (int j = 0; j < nneis; ++j)
{
if (neis[j] == tile) continue;
unconnectExtLinks(neis[j], tile);
}
// Connect with neighbour tiles.
for (int i = 0; i < 8; ++i)
{
nneis = getNeighbourTilesAt(tile.header.x, tile.header.y, i, neis, MAX_NEIS);
for (int j = 0; j < nneis; ++j)
unconnectExtLinks(neis[j], tile);
}
// Reset tile.
if ((tile.flags & (int)dtTileFlags.DT_TILE_FREE_DATA) != 0)
{
// Owns data
//dtFree(tile.data);
tile.data = null;
//tile.dataSize = 0;
//if (data) *data = 0;
//if (dataSize) *dataSize = 0;
rawTileData = null;
}
else
{
//if (data) *data = tile.data;
//if (dataSize) *dataSize = tile.dataSize;
rawTileData = tile.data;
}
tile.header = null;
tile.flags = 0;
tile.linksFreeList = 0;
tile.polys = null;
tile.verts = null;
tile.links = null;
tile.detailMeshes = null;
tile.detailVerts = null;
tile.detailTris = null;
tile.bvTree = null;
tile.offMeshCons = null;
// Update salt, salt should never be zero.
#if DT_POLYREF64
tile.salt = (tile.salt+1) & ((1<<DT_SALT_BITS)-1);
#else
tile.salt = (dtTileRef)( (tile.salt+1) & ((1<<(int)m_saltBits)-1) );
#endif
if (tile.salt == 0)
tile.salt++;
// Add to free list.
tile.next = m_nextFree;
m_nextFree = tile;
return DT_SUCCESS;
}
public dtPolyRef findNearestPolyInTile(dtMeshTile tile,
float[] center, int centerStart,
float[] extents,
float[] nearestPt)
{
float[] bmin = new float[3];//, bmax[3];
float[] bmax = new float[3];
dtVsub(bmin, 0, center, centerStart, extents, 0);
dtVadd(bmax, 0, center, centerStart, extents, 0);
// Get nearby polygons from proximity grid.
dtPolyRef[] polys = new dtPolyRef[128];
int polyCount = queryPolygonsInTile(tile, bmin, bmax, polys, 128);
// Find nearest polygon amongst the nearby polygons.
dtPolyRef nearest = 0;
float nearestDistanceSqr = float.MaxValue;
for (int i = 0; i < polyCount; ++i)
{
dtPolyRef polyRef = polys[i];
float[] closestPtPoly = new float[3];
float[] diff = new float[3];
bool posOverPoly = false;
float d = 0;
closestPointOnPoly(polyRef, center, centerStart, closestPtPoly, ref posOverPoly);
// If a point is directly over a polygon and closer than
// climb height, favor that instead of straight line nearest point.
dtVsub(diff, 0, center, centerStart, closestPtPoly, 0);
if (posOverPoly)
{
d = Math.Abs(diff[1]) - tile.header.walkableClimb;
d = d > 0 ? d*d : 0;
}
else
{
d = dtVlenSqr(diff);
}
if (d < nearestDistanceSqr)
{
dtVcopy(nearestPt, closestPtPoly);
nearestDistanceSqr = d;
nearest = polyRef;
}
}
return nearest;
}
/// Removes external links at specified side.
public void unconnectExtLinks(dtMeshTile tile, dtMeshTile target)
{
if (tile == null || target == null)
return;
uint targetNum = decodePolyIdTile(getTileRef(target));
for (int i = 0; i < tile.header.polyCount; ++i)
{
dtPoly poly = tile.polys[i];
uint j = poly.firstLink;
uint pj = DT_NULL_LINK;
while (j != DT_NULL_LINK)
{
if (tile.links[j].side != 0xff &&
decodePolyIdTile(tile.links[j].polyRef) == targetNum)
{
// Revove link.
uint nj = tile.links[j].next;
if (pj == DT_NULL_LINK)
poly.firstLink = nj;
else
tile.links[pj].next = nj;
freeLink(tile, j);
j = nj;
}
else
{
// Advance
pj = j;
j = tile.links[j].next;
}
}
}
}
public static void freeLink(dtMeshTile tile, uint link)
{
tile.links[link].next = tile.linksFreeList;
tile.linksFreeList = link;
}
/// @par
///
/// The add operation will fail if the data is in the wrong format, the allocated tile
/// space is full, or there is a tile already at the specified reference.
///
/// The lastRef parameter is used to restore a tile with the same tile
/// reference it had previously used. In this case the #dtPolyRef's for the
/// tile will be restored to the same values they were before the tile was
/// removed.
///
/// @see dtCreateNavMeshData, #removeTile
/// Adds a tile to the navigation mesh.
/// @param[in] data Data for the new tile mesh. (See: #dtCreateNavMeshData)
/// @param[in] dataSize Data size of the new tile mesh.
/// @param[in] flags Tile flags. (See: #dtTileFlags)
/// @param[in] lastRef The desired reference for the tile. (When reloading a tile.) [opt] [Default: 0]
/// @param[out] result The tile reference. (If the tile was succesfully added.) [opt]
/// @return The status flags for the operation.
public dtStatus addTile(dtRawTileData rawTileData, int flags, dtTileRef lastRef, ref dtTileRef result)
{
//C#: Using an intermediate class dtRawTileData because Cpp uses a binary buffer.
// Make sure the data is in right format.
dtMeshHeader header = rawTileData.header;
if (header.magic != DT_NAVMESH_MAGIC)
return DT_FAILURE | DT_WRONG_MAGIC;
if (header.version != DT_NAVMESH_VERSION)
return DT_FAILURE | DT_WRONG_VERSION;
// Make sure the location is free.
if (getTileAt(header.x, header.y, header.layer) != null)
return DT_FAILURE;
// Allocate a tile.
dtMeshTile tile = null;
if (lastRef == 0)
{
if (m_nextFree != null)
{
tile = m_nextFree;
m_nextFree = tile.next;
tile.next = null;
}
}
else
{
// Try to relocate the tile to specific index with same salt.
int tileIndex = (int)decodePolyIdTile((dtPolyRef)lastRef);
if (tileIndex >= m_maxTiles)
return DT_FAILURE | DT_OUT_OF_MEMORY;
// Try to find the specific tile id from the free list.
dtMeshTile target = m_tiles[tileIndex];
dtMeshTile prev = null;
tile = m_nextFree;
while (tile != null && tile != target)
{
prev = tile;
tile = tile.next;
}
// Could not find the correct location.
if (tile != target)
return DT_FAILURE | DT_OUT_OF_MEMORY;
// Remove from freelist
if (prev == null)
m_nextFree = tile.next;
else
prev.next = tile.next;
// Restore salt.
tile.salt = decodePolyIdSalt((dtPolyRef)lastRef);
}
// Make sure we could allocate a tile.
if (tile == null)
return DT_FAILURE | DT_OUT_OF_MEMORY;
// Insert tile into the position lut.
int h = computeTileHash(header.x, header.y, m_tileLutMask);
tile.next = m_posLookup[h];
m_posLookup[h] = tile;
// Patch header pointers.
//int vertsCount = 3*header.vertCount;
//int polysSize = header.polyCount;
//int linksSize = header.maxLinkCount;
//int detailMeshesSize = header.detailMeshCount;
//int detailVertsSize = 3*header.detailVertCount;
//int detailTrisSize = 4*header.detailTriCount;
//int bvtreeSize = header.bvNodeCount;
//int offMeshLinksSize = header.offMeshConCount;
//byte* d = data + headerSize;
tile.verts = rawTileData.verts;
tile.polys = rawTileData.polys;
tile.links = rawTileData.links;
tile.detailMeshes = rawTileData.detailMeshes;
tile.detailVerts = rawTileData.detailVerts;
tile.detailTris = rawTileData.detailTris;
tile.bvTree = rawTileData.bvTree;
tile.offMeshCons = rawTileData.offMeshCons;
// If there are no items in the bvtree, reset the tree pointer.
//c#: unnecessary, Cpp is afraid to point to whatever data ends up here
//if (bvtreeSize == 0)
// tile.bvTree = null;
// Build links freelist
tile.linksFreeList = 0;
tile.links[header.maxLinkCount-1].next = DT_NULL_LINK;
for (int i = 0; i < header.maxLinkCount-1; ++i){
tile.links[i].next = (dtTileRef) (i+1);
}
// Init tile.
tile.header = header;
tile.data = rawTileData;
//tile.dataSize = dataSize;
tile.flags = flags;
connectIntLinks(tile);
baseOffMeshLinks(tile);
// Create connections with neighbour tiles.
const int MAX_NEIS = 32;
dtMeshTile[] neis = new dtMeshTile[MAX_NEIS];
int nneis;
// Connect with layers in current tile.
nneis = getTilesAt(header.x, header.y, neis, MAX_NEIS);
for (int j = 0; j < nneis; ++j)
{
if (neis[j] != tile)
{
connectExtLinks(tile, neis[j], -1);
connectExtLinks(neis[j], tile, -1);
}
connectExtOffMeshLinks(tile, neis[j], -1);
connectExtOffMeshLinks(neis[j], tile, -1);
}
// Connect with neighbour tiles.
for (int i = 0; i < 8; ++i)
{
nneis = getNeighbourTilesAt(header.x, header.y, i, neis, MAX_NEIS);
for (int j = 0; j < nneis; ++j)
{
connectExtLinks(tile, neis[j], i);
connectExtLinks(neis[j], tile, dtOppositeTile(i));
connectExtOffMeshLinks(tile, neis[j], i);
connectExtOffMeshLinks(neis[j], tile, dtOppositeTile(i));
}
}
result = getTileRef(tile);
return DT_SUCCESS;
}
// Returns portal points between two polygons.
dtStatus getPortalPoints(dtPolyRef from, dtPoly fromPoly, dtMeshTile fromTile,
dtPolyRef to, dtPoly toPoly, dtMeshTile toTile,
float[] left, float[] right)
{
// Find the link that points to the 'to' polygon.
dtLink link = null;
for (uint i = fromPoly.firstLink; i != DT_NULL_LINK; i = fromTile.links[i].next)
{
if (fromTile.links[i].polyRef == to)
{
link = fromTile.links[i];
break;
}
}
if (link == null)
return DT_FAILURE | DT_INVALID_PARAM;
// Handle off-mesh connections.
if (fromPoly.getType() == (byte) dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
{
// Find link that points to first vertex.
for (uint i = fromPoly.firstLink; i != DT_NULL_LINK; i = fromTile.links[i].next)
{
if (fromTile.links[i].polyRef == to)
{
int v = fromTile.links[i].edge;
dtVcopy(left, 0, fromTile.verts,fromPoly.verts[v]*3);
dtVcopy(right, 0, fromTile.verts,fromPoly.verts[v]*3);
return DT_SUCCESS;
}
}
return DT_FAILURE | DT_INVALID_PARAM;
}
if (toPoly.getType() == (byte) dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
{
for (uint i = toPoly.firstLink; i != DT_NULL_LINK; i = toTile.links[i].next)
{
if (toTile.links[i].polyRef == from)
{
int v = toTile.links[i].edge;
dtVcopy(left, 0, toTile.verts, toPoly.verts[v]*3);
dtVcopy(right, 0, toTile.verts, toPoly.verts[v]*3);
return DT_SUCCESS;
}
}
return DT_FAILURE | DT_INVALID_PARAM;
}
// Find portal vertices.
int v0 = fromPoly.verts[link.edge];
int v1 = fromPoly.verts[(link.edge+1) % (int)fromPoly.vertCount];
dtVcopy(left, 0, fromTile.verts,v0*3);
dtVcopy(right, 0, fromTile.verts,v1*3);
// If the link is at tile boundary, dtClamp the vertices to
// the link width.
if (link.side != 0xff)
{
// Unpack portal limits.
if (link.bmin != 0 || link.bmax != 255)
{
float s = 1.0f/255.0f;
float tmin = link.bmin*s;
float tmax = link.bmax*s;
dtVlerp(left, 0, fromTile.verts, v0*3, fromTile.verts, v1*3, tmin);
dtVlerp(right, 0, fromTile.verts, v0*3, fromTile.verts, v1*3, tmax);
}
}
return DT_SUCCESS;
}
/// Builds internal polygons links for a tile.
public void baseOffMeshLinks(dtMeshTile tile)
{
if (tile == null)
return;
dtPolyRef polyRefBase = getPolyRefBase(tile);
// Base off-mesh connection start points.
for (int i = 0; i < tile.header.offMeshConCount; ++i)
{
dtOffMeshConnection con = tile.offMeshCons[i];
dtPoly poly = tile.polys[con.poly];
float[] ext = new float[] { con.rad, tile.header.walkableClimb, con.rad };
// Find polygon to connect to.
//const float* p = &con.pos[0]; // First vertex
float[] nearestPt = new float[3];
dtPolyRef polyRef = findNearestPolyInTile(tile, con.pos, 0, ext, nearestPt);
if (polyRef == 0)
continue;
// findNearestPoly may return too optimistic results, further check to make sure.
if (dtSqr(nearestPt[0]-con.pos[0])+dtSqr(nearestPt[2]-con.pos[2]) > dtSqr(con.rad))
continue;
// Make sure the location is on current mesh.
//float* v = &tile.verts[poly.verts[0]*3];
int vIndex = poly.verts[0]*3;
dtVcopy(tile.verts, vIndex, nearestPt, 0);
// Link off-mesh connection to target poly.
uint idx = allocLink(tile);
if (idx != DT_NULL_LINK)
{
dtLink link = tile.links[idx];
link.polyRef = polyRef;
link.edge = (byte)0;
link.side = 0xff;
link.bmin = link.bmax = 0;
// Add to linked list.
link.next = poly.firstLink;
poly.firstLink = idx;
}
// Start end-point is always connect back to off-mesh connection.
uint tidx = allocLink(tile);
if (tidx != DT_NULL_LINK)
{
ushort landPolyIdx = (ushort)decodePolyIdPoly(polyRef);
dtPoly landPoly = tile.polys[landPolyIdx];
dtLink link = tile.links[tidx];
link.polyRef = polyRefBase | (dtPolyRef)(con.poly);
link.edge = 0xff;
link.side = 0xff;
link.bmin = link.bmax = 0;
// Add to linked list.
link.next = landPoly.firstLink;
landPoly.firstLink = tidx;
}
}
}
/// Queries polygons within a tile.
public int queryPolygonsInTile(dtMeshTile tile, float[] qmin, float[] qmax,
dtQueryFilter filter,
dtPolyRef[] polys, int polyStart, int maxPolys)
{
Debug.Assert(m_nav != null);
if (tile.bvTree != null)
{
dtBVNode node = tile.bvTree[0];
//dtBVNode* end = &tile.bvTree[tile.header.bvNodeCount];
int endIndex = tile.header.bvNodeCount;
float[] tbmin = tile.header.bmin;
float[] tbmax = tile.header.bmax;
float qfac = tile.header.bvQuantFactor;
// Calculate quantized box
ushort[] bmin = new ushort[3];//, bmax[3];
ushort[] bmax = new ushort[3];
// dtClamp query box to world box.
float minx = dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0];
float miny = dtClamp(qmin[1], tbmin[1], tbmax[1]) - tbmin[1];
float minz = dtClamp(qmin[2], tbmin[2], tbmax[2]) - tbmin[2];
float maxx = dtClamp(qmax[0], tbmin[0], tbmax[0]) - tbmin[0];
float maxy = dtClamp(qmax[1], tbmin[1], tbmax[1]) - tbmin[1];
float maxz = dtClamp(qmax[2], tbmin[2], tbmax[2]) - tbmin[2];
// Quantize
bmin[0] = (ushort)((int)(qfac * minx) & 0xfffe);
bmin[1] = (ushort)((int)(qfac * miny) & 0xfffe);
bmin[2] = (ushort)((int)(qfac * minz) & 0xfffe);
bmax[0] = (ushort)((int)(qfac * maxx + 1) | 1);
bmax[1] = (ushort)((int)(qfac * maxy + 1) | 1);
bmax[2] = (ushort)((int)(qfac * maxz + 1) | 1);
// Traverse tree
dtPolyRef polyRefBase = m_nav.getPolyRefBase(tile);
int n = 0;
int nodeIndex = 0;
while (nodeIndex < endIndex)
{
node = tile.bvTree[nodeIndex];
bool overlap = dtOverlapQuantBounds(bmin, bmax, node.bmin, node.bmax);
bool isLeafNode = node.i >= 0;
if (isLeafNode && overlap)
{
dtPolyRef polyRef = polyRefBase | (dtPolyRef)node.i;
if (filter.passFilter(polyRef, tile, tile.polys[node.i]))
{
if (n < maxPolys)
polys[polyStart + n++] = polyRef;
}
}
if (overlap || isLeafNode){
nodeIndex++;
}
else
{
int escapeIndex = -node.i;
nodeIndex += escapeIndex;
}
}
return n;
}
else
{
float[] bmin = new float[3];//, bmax[3];
float[] bmax = new float[3];
int n = 0;
dtPolyRef polyRefBase = m_nav.getPolyRefBase(tile);
for (int i = 0; i < tile.header.polyCount; ++i)
{
dtPoly p = tile.polys[i];
// Do not return off-mesh connection polygons.
if (p.getType() == (byte)dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
continue;
// Must pass filter
dtPolyRef polyRef = polyRefBase | (dtPolyRef)i;
if (!filter.passFilter(polyRef, tile, p))
continue;
// Calc polygon bounds.
//const float* v = &tile.verts[p.verts[0]*3];
int vStart = p.verts[0]*3;
dtVcopy(bmin,0, tile.verts,vStart);
dtVcopy(bmax,0, tile.verts,vStart);
for (int j = 1; j < p.vertCount; ++j)
{
//v = &tile.verts[p.verts[j]*3];
vStart = p.verts[j]*3;
dtVmin(bmin,0, tile.verts,vStart);
dtVmax(bmax,0, tile.verts,vStart);
}
if (dtOverlapBounds(qmin,qmax, bmin,bmax))
{
if (n < maxPolys)
polys[polyStart + n++] = polyRef;
}
}
return n;
}
}
public int getNeighbourTilesAt(int x, int y, int side, dtMeshTile[] tiles, int maxTiles)
{
int nx = x, ny = y;
switch (side)
{
case 0: nx++; break;
case 1: nx++; ny++; break;
case 2: ny++; break;
case 3: nx--; ny++; break;
case 4: nx--; break;
case 5: nx--; ny--; break;
case 6: ny--; break;
case 7: nx++; ny--; break;
};
return getTilesAt(nx, ny, tiles, maxTiles);
}
