/// <summary>
/// Only use this function if it is known that the provided polygon reference is valid.
/// </summary>
/// <param name="reference">Polygon reference</param>
/// <param name="tile">Resulting tile</param>
/// <param name="poly">Resulting poly</param>
public void TryGetTileAndPolyByRefUnsafe(PolyId reference, out MeshTile tile, out Poly poly)
{
int salt, polyIndex, tileIndex;
idManager.Decode(ref reference, out polyIndex, out tileIndex, out salt);
tile = tileList[tileIndex];
poly = tileList[tileIndex].Polys[polyIndex];
}
/// <summary>
/// Finds a random point on a polygon.
/// </summary>
/// <param name="tile">The current mesh tile</param>
/// <param name="poly">The current polygon</param>
/// <param name="polyRef">Polygon reference</param>
/// <param name="randomPt">Resulting random point</param>
public void FindRandomPointOnPoly(MeshTile tile, Poly poly, PolyId polyRef, out Vector3 randomPt)
{
Vector3[] verts = new Vector3[PathfindingCommon.VERTS_PER_POLYGON];
float[] areas = new float[PathfindingCommon.VERTS_PER_POLYGON];
for (int j = 0; j < poly.VertCount; j++)
verts[j] = tile.Verts[poly.Verts[j]];
float s = (float)rand.NextDouble();
float t = (float)rand.NextDouble();
PathfindingCommon.RandomPointInConvexPoly(verts, poly.VertCount, areas, s, t, out randomPt);
//TODO bad state again.
float h = 0.0f;
if (!GetPolyHeight(polyRef, randomPt, ref h))
throw new InvalidOperationException("Outside bounds?");
randomPt.Y = h;
}
/// <summary>
/// Find points on the left and right side.
/// </summary>
/// <param name="from">"From" polygon reference</param>
/// <param name="fromPoly">"From" polygon data</param>
/// <param name="fromTile">"From" mesh tile</param>
/// <param name="to">"To" polygon reference</param>
/// <param name="toPoly">"To" polygon data</param>
/// <param name="toTile">"To" mesh tile</param>
/// <param name="left">Resulting point on the left side</param>
/// <param name="right">Resulting point on the right side</param>
/// <returns>True, if points found. False, if otherwise.</returns>
public bool GetPortalPoints(PolyId from, Poly fromPoly, MeshTile fromTile, PolyId to, Poly toPoly, MeshTile toTile, ref Vector3 left, ref Vector3 right)
{
//find the link that points to the 'to' polygon
Link link = null;
for (int i = fromPoly.FirstLink; i != Link.Null; i = fromTile.Links[i].Next)
{
if (fromTile.Links[i].Reference == to)
{
link = fromTile.Links[i];
break;
}
}
if (link == null)
return false;
//handle off-mesh connections
if (fromPoly.PolyType == PolygonType.OffMeshConnection)
{
//find link that points to first vertex
for (int i = fromPoly.FirstLink; i != Link.Null; i = fromTile.Links[i].Next)
{
if (fromTile.Links[i].Reference == to)
{
int v = fromTile.Links[i].Edge;
left = fromTile.Verts[fromPoly.Verts[v]];
right = fromTile.Verts[fromPoly.Verts[v]];
return true;
}
}
return false;
}
if (toPoly.PolyType == PolygonType.OffMeshConnection)
{
//find link that points to first vertex
for (int i = toPoly.FirstLink; i != Link.Null; i = toTile.Links[i].Next)
{
if (toTile.Links[i].Reference == from)
{
int v = toTile.Links[i].Edge;
left = toTile.Verts[toPoly.Verts[v]];
right = toTile.Verts[toPoly.Verts[v]];
return true;
}
}
return false;
}
//find portal vertices
int v0 = fromPoly.Verts[link.Edge];
int v1 = fromPoly.Verts[(link.Edge + 1) % fromPoly.VertCount];
left = fromTile.Verts[v0];
right = fromTile.Verts[v1];
//if the link is at the tile boundary, clamp the vertices to tile width
if (link.Side != BoundarySide.Internal)
{
//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;
left = Vector3.Lerp(fromTile.Verts[v0], fromTile.Verts[v1], tmin);
right = Vector3.Lerp(fromTile.Verts[v0], fromTile.Verts[v1], tmax);
}
}
return true;
}
/// <summary>
/// Find the closest polygon possible in the tile under certain constraints.
/// </summary>
/// <param name="tile">Current tile</param>
/// <param name="center">Center starting point</param>
/// <param name="extents">Range of search</param>
/// <param name="nearestPt">Resulting nearest point</param>
/// <returns>Polygon Reference which contains nearest point</returns>
public int FindNearestPolyInTile(MeshTile tile, Vector3 center, Vector3 extents, ref Vector3 nearestPt)
{
BBox3 bounds;
bounds.Min = center - extents;
bounds.Max = center + extents;
//Get nearby polygons from proximity grid
List<int> polys = new List<int>(128);
int polyCount = QueryPolygonsInTile(tile, bounds, polys);
//Find nearest polygon amongst the nearby polygons
int nearest = 0;
float nearestDistanceSqr = float.MaxValue;
//Iterate throuh all the polygons
for (int i = 0; i < polyCount; i++)
{
int reference = polys[i];
Vector3 closestPtPoly = new Vector3();
tile.ClosestPointOnPoly(DecodePolyIdPoly(reference), center, ref closestPtPoly);
float d = (center - closestPtPoly).LengthSquared();
if (d < nearestDistanceSqr)
{
nearestPt = closestPtPoly;
nearestDistanceSqr = d;
nearest = reference;
}
}
return nearest;
}
/// <summary>
/// Get the tile reference
/// </summary>
/// <param name="tile">Tile to look for</param>
/// <returns>Tile reference</returns>
public int GetTileRef(MeshTile tile)
{
if (tile == null)
return 0;
int it = 0;
for (int i = 0; i < tiles.Length; i++)
{
if (tiles[i] == tile)
{
it = i;
break;
}
}
return EncodePolyId(tile.Salt, it, 0);
}
/// <summary>
/// Connect polygons from two different tiles.
/// </summary>
/// <param name="tile">Current Tile</param>
/// <param name="target">Target Tile</param>
/// <param name="side">Polygon edge</param>
public void ConnectExtLinks(ref MeshTile tile, ref MeshTile target, BoundarySide side)
{
if (tile == null)
return;
//Connect border links
for (int i = 0; i < tile.Header.PolyCount; i++)
{
int numPolyVerts = tile.Polys[i].VertCount;
for (int j = 0; j < numPolyVerts; j++)
{
//Skip non-portal edges
if ((tile.Polys[i].Neis[j] & Link.External) == 0)
continue;
BoundarySide dir = (BoundarySide)(tile.Polys[i].Neis[j] & 0xff);
if (side != BoundarySide.Internal && dir != side)
continue;
//Create new links
Vector3 va = tile.Verts[tile.Polys[i].Verts[j]];
Vector3 vb = tile.Verts[tile.Polys[i].Verts[(j + 1) % numPolyVerts]];
List<int> nei = new List<int>(4);
List<float> neia = new List<float>(4 * 2);
FindConnectingPolys(va, vb, target, dir.GetOpposite(), nei, neia);
//Iterate through neighbors
for (int k = 0; k < nei.Count; k++)
{
//Allocate a new link if possible
int idx = AllocLink(tile);
if (IsLinkAllocated(idx))
{
tile.Links[idx].Reference = nei[k];
tile.Links[idx].Edge = j;
tile.Links[idx].Side = dir;
tile.Links[idx].Next = tile.Polys[i].FirstLink;
tile.Polys[i].FirstLink = idx;
//Compress portal limits to a value
if (dir == BoundarySide.PlusX || dir == BoundarySide.MinusX)
{
float tmin = (neia[k * 2 + 0] - va.Z) / (vb.Z - va.Z);
float tmax = (neia[k * 2 + 1] - va.Z) / (vb.Z - va.Z);
if (tmin > tmax)
{
float temp = tmin;
tmin = tmax;
tmax = temp;
}
tile.Links[idx].BMin = (int)(MathHelper.Clamp(tmin, 0.0f, 1.0f) * 255.0f);
tile.Links[idx].BMax = (int)(MathHelper.Clamp(tmax, 0.0f, 1.0f) * 255.0f);
}
else if (dir == BoundarySide.PlusZ || dir == BoundarySide.MinusZ)
{
float tmin = (neia[k * 2 + 0] - va.X) / (vb.X - va.X);
float tmax = (neia[k * 2 + 1] - va.X) / (vb.X - va.X);
if (tmin > tmax)
{
float temp = tmin;
tmin = tmax;
tmax = temp;
}
tile.Links[idx].BMin = (int)(MathHelper.Clamp(tmin, 0.0f, 1.0f) * 255.0f);
tile.Links[idx].BMax = (int)(MathHelper.Clamp(tmax, 0.0f, 1.0f) * 255.0f);
}
}
}
}
}
}
/// <summary>
/// Allocate links for each of the tile's polygons' vertices
/// </summary>
/// <param name="tile">A tile contains a set of polygons, which are linked to each other</param>
public void ConnectIntLinks(ref MeshTile tile)
{
if (tile == null)
return;
int polyBase = GetPolyRefBase(tile);
//Iterate through all the polygons
for (int i = 0; i < tile.Header.PolyCount; i++)
{
//The polygon links will end in a null link
tile.Polys[i].FirstLink = Link.Null;
//Avoid Off-Mesh Connection polygons
if (tile.Polys[i].PolyType == PolygonType.OffMeshConnection)
continue;
//Build edge links
for (int j = tile.Polys[i].VertCount - 1; j >= 0; j--)
{
//Skip hard and non-internal edges
if (tile.Polys[i].Neis[j] == 0 || IsExternalLink(tile.Polys[i].Neis[j]))
continue;
//Allocate a new link if possible
int idx = AllocLink(tile);
//Allocation of link should be successful
if (IsLinkAllocated(idx))
{
//Initialize a new link
tile.Links[idx].Reference = GetReference(polyBase, tile.Polys[i].Neis[j] - 1);
tile.Links[idx].Edge = j;
tile.Links[idx].Side = BoundarySide.Internal;
tile.Links[idx].BMin = tile.Links[idx].BMax = 0;
//Add to polygon's links list
tile.Links[idx].Next = tile.Polys[i].FirstLink;
tile.Polys[i].FirstLink = idx;
}
}
}
}
private JArray SerializeMeshTilesArray(MeshTile[] tiles)
{
var tilesArray = new JArray();
foreach (var tile in tiles)
{
tilesArray.Add(SerializeMeshTile(tile));
}
return tilesArray;
}
private JObject SerializeMeshTile(MeshTile tile)
{
var result = new JObject();
result.Add("location", JToken.FromObject(tile.Location, serializer));
result.Add("layer", JToken.FromObject(tile.Layer, serializer));
result.Add("salt", JToken.FromObject(tile.Salt, serializer));
result.Add("bounds", JToken.FromObject(tile.Bounds, serializer));
result.Add("polys", JToken.FromObject(tile.Polys, serializer));
result.Add("verts", JToken.FromObject(tile.Verts, serializer));
result.Add("detailMeshes", JToken.FromObject(tile.DetailMeshes, serializer));
result.Add("detailVerts", JToken.FromObject(tile.DetailVerts, serializer));
result.Add("detailTris", JToken.FromObject(tile.DetailTris, serializer));
result.Add("offMeshConnections", JToken.FromObject(tile.OffMeshConnections, serializer));
var treeNodes = (BVTree.Node[])GetPrivateField(tile.BVTree, "nodes");
JObject treeObject = new JObject();
treeObject.Add("nodes", JToken.FromObject(treeNodes, serializer));
result.Add("bvTree", treeObject);
result.Add("bvQuantFactor", JToken.FromObject(tile.BvQuantFactor, serializer));
result.Add("bvNodeCount", JToken.FromObject(tile.BvNodeCount, serializer));
result.Add("walkableClimb", JToken.FromObject(tile.WalkableClimb, serializer));
return result;
}
private MeshTile DeserializeMeshTile(JToken token)
{
JObject jObject = (JObject) token;
MeshTile result = new MeshTile();
result.Salt = jObject.GetValue("Salt").Value<int>();
result.LinksFreeList = jObject.GetValue("LinksFreeList").Value<int>();
result.Header = jObject.GetValue("Header").ToObject<PathfindingCommon.NavMeshInfo>();
result.Polys = jObject.GetValue("Polys").ToObject<Poly[]>();
result.Verts = jObject.GetValue("Verts").ToObject<Vector3[]>();
result.Links = jObject.GetValue("Links").ToObject<Link[]>();
result.DetailMeshes = jObject.GetValue("DetailMeshes").ToObject<PolyMeshDetail.MeshData[]>();
result.DetailVerts = jObject.GetValue("DetailVerts").ToObject<Vector3[]>();
result.DetailTris = jObject.GetValue("DetailTris").ToObject<PolyMeshDetail.TriangleData[]>();
result.OffMeshConnections = jObject.GetValue("OffMeshConnections").ToObject<OffMeshConnection[]>();
var tree = (BVTree) FormatterServices.GetUninitializedObject(typeof(BVTree));
var treeObject = (JObject) jObject.GetValue("BVTree");
var nodes = treeObject.GetValue("nodes").ToObject<BVTree.Node[]>();
SetPrivateField(tree, "nodes", nodes);
result.BVTree = tree;
return result;
}
private JObject SerializeMeshTile(MeshTile tile)
{
var result = new JObject();
result.Add("Salt", tile.Salt);
result.Add("LinksFreeList", tile.LinksFreeList);
result.Add("Header", JToken.FromObject(tile.Header, serializer));
result.Add("Polys", JToken.FromObject(tile.Polys, serializer));
result.Add("Verts", JToken.FromObject(tile.Verts, serializer));
result.Add("Links", JToken.FromObject(tile.Links, serializer));
result.Add("DetailMeshes", JToken.FromObject(tile.DetailMeshes, serializer));
result.Add("DetailVerts", JToken.FromObject(tile.DetailVerts, serializer));
result.Add("DetailTris", JToken.FromObject(tile.DetailTris, serializer));
result.Add("OffMeshConnections", JToken.FromObject(tile.OffMeshConnections, serializer));
var treeNodes = (BVTree.Node[])GetPrivateField(tile.BVTree, "nodes");
JObject treeObject = new JObject();
treeObject.Add("nodes", JToken.FromObject(treeNodes, serializer));
result.Add("BVTree", treeObject);
return result;
}
/// <summary>
/// Get the tile reference
/// </summary>
/// <param name="tile">Tile to look for</param>
/// <returns>Tile reference</returns>
public PolyId GetTileRef(MeshTile tile)
{
if (tile == null)
return PolyId.Null;
int it = 0;
for (int i = 0; i < tiles.Length; i++)
{
if (tiles[i] == tile)
{
it = i;
break;
}
}
return PolyId.Encode(polyBits, tileBits, tile.Salt, it, 0);
}
/// <summary>
/// Only use this function if it is known that the provided polygon reference is valid.
/// </summary>
/// <param name="reference">Polygon reference</param>
/// <param name="tile">Resulting tile</param>
/// <param name="poly">Resulting poly</param>
public void TryGetTileAndPolyByRefUnsafe(PolyId reference, out MeshTile tile, out Poly poly)
{
int salt, polyIndex, tileIndex;
reference.Decode(polyBits, tileBits, saltBits, out polyIndex, out tileIndex, out salt);
tile = tiles[tileIndex];
poly = tiles[tileIndex].Polys[polyIndex];
}
/// <summary>
/// Retrieve the tile and poly based off of a polygon reference
/// </summary>
/// <param name="reference">Polygon reference</param>
/// <param name="tile">Resulting tile</param>
/// <param name="poly">Resulting poly</param>
/// <returns>True if tile and poly successfully retrieved</returns>
public bool TryGetTileAndPolyByRef(PolyId reference, out MeshTile tile, out Poly poly)
{
tile = null;
poly = null;
if (reference == PolyId.Null)
return false;
//Get tile and poly indices
int salt, polyIndex, tileIndex;
reference.Decode(polyBits, tileBits, saltBits, out polyIndex, out tileIndex, out salt);
//Make sure indices are valid
if (tileIndex >= maxTiles)
return false;
if (tiles[tileIndex].Salt != salt || tiles[tileIndex].Header == null)
return false;
if (polyIndex >= tiles[tileIndex].Header.PolyCount)
return false;
//Retrieve tile and poly
tile = tiles[tileIndex];
poly = tiles[tileIndex].Polys[polyIndex];
return true;
}
/// <summary>
/// Allocate a new link if possible.
/// </summary>
/// <param name="tile">Current tile</param>
/// <returns>New link number</returns>
public int AllocLink(MeshTile tile)
{
if (!IsLinkAllocated(tile.LinksFreeList))
return Link.Null;
int link = tile.LinksFreeList;
tile.LinksFreeList = tile.Links[link].Next;
return link;
}
private MeshTile DeserializeMeshTile(JToken token, PolyIdManager manager, PolyId refId)
{
Vector2i location = token["location"].ToObject<Vector2i>(serializer);
int layer = token["layer"].ToObject<int>(serializer);
MeshTile result = new MeshTile(location, layer, manager, refId);
result.Salt = token["salt"].ToObject<int>(serializer);
result.Bounds = token["bounds"].ToObject<BBox3>(serializer);
result.Polys = token["polys"].ToObject<Poly[]>(serializer);
result.PolyCount = result.Polys.Length;
result.Verts = token["verts"].ToObject<Vector3[]>(serializer);
result.DetailMeshes = token["detailMeshes"].ToObject<PolyMeshDetail.MeshData[]>(serializer);
result.DetailVerts = token["detailVerts"].ToObject<Vector3[]>(serializer);
result.DetailTris = token["detailTris"].ToObject<PolyMeshDetail.TriangleData[]>(serializer);
result.OffMeshConnections = token["offMeshConnections"].ToObject<OffMeshConnection[]>(serializer);
result.OffMeshConnectionCount = result.OffMeshConnections.Length;
result.BvNodeCount = token["bvNodeCount"].ToObject<int>(serializer);
result.BvQuantFactor = token["bvQuantFactor"].ToObject<float>(serializer);
result.WalkableClimb = token["walkableClimb"].ToObject<float>(serializer);
var tree = (BVTree) FormatterServices.GetUninitializedObject(typeof(BVTree));
var treeObject = (JObject) token["bvTree"];
var nodes = treeObject.GetValue("nodes").ToObject<BVTree.Node[]>();
SetPrivateField(tree, "nodes", nodes);
result.BVTree = tree;
return result;
}
/// <summary>
/// Begin creating off-mesh links between the tile polygons.
/// </summary>
/// <param name="tile">Current Tile</param>
public void BaseOffMeshLinks(ref MeshTile tile)
{
if (tile == null)
return;
int polyBase = GetPolyRefBase(tile);
//Base off-mesh connection start points
for (int i = 0; i < tile.Header.OffMeshConCount; i++)
{
int con = i;
int poly = tile.OffMeshConnections[con].Poly;
Vector3 extents = new Vector3(tile.OffMeshConnections[con].Radius, tile.Header.WalkableClimb, tile.OffMeshConnections[con].Radius);
//Find polygon to connect to
Vector3 p = tile.OffMeshConnections[con].Pos0;
Vector3 nearestPt = new Vector3();
int reference = FindNearestPolyInTile(tile, p, extents, ref nearestPt);
if (reference == 0)
continue;
//Do extra checks
if ((nearestPt.X - p.X) * (nearestPt.X - p.X) + (nearestPt.Z - p.Z) * (nearestPt.Z - p.Z) >
tile.OffMeshConnections[con].Radius * tile.OffMeshConnections[con].Radius)
continue;
//Make sure location is on current mesh
tile.Verts[tile.Polys[poly].Verts[0]] = nearestPt;
//Link off-mesh connection to target poly
int idx = AllocLink(tile);
if (IsLinkAllocated(idx))
{
//Initialize a new link
tile.Links[idx].Reference = reference;
tile.Links[idx].Edge = 0;
tile.Links[idx].Side = BoundarySide.Internal;
tile.Links[idx].BMin = tile.Links[idx].BMax = 0;
//Add to polygon's links list
tile.Links[idx].Next = tile.Polys[poly].FirstLink;
tile.Polys[poly].FirstLink = idx;
}
//Start end-point always conects back to off-mesh connection
int tidx = AllocLink(tile);
if (IsLinkAllocated(tidx))
{
//Initialize a new link
int landPolyIdx = DecodePolyIdPoly(reference);
tile.Links[idx].Reference = GetReference(polyBase, tile.OffMeshConnections[con].Poly);
tile.Links[idx].Edge = 0xff;
tile.Links[idx].Side = BoundarySide.Internal;
tile.Links[idx].BMin = tile.Links[idx].BMax = 0;
//Add to polygon's links list
tile.Links[idx].Next = tile.Polys[landPolyIdx].FirstLink;
tile.Polys[landPolyIdx].FirstLink = tidx;
}
}
}
/// <summary>
/// Find and add a tile if it is found
/// </summary>
/// <param name="x">The x-coordinate</param>
/// <param name="y">The y-coordinate</param>
/// <param name="tiles">Tile array</param>
/// <returns>Number of tiles satisfying condition</returns>
public int GetTilesAt(int x, int y, MeshTile[] tiles)
{
int n = 0;
//Find tile based on hash
int h = ComputeTileHash(x, y, tileLookupTableMask);
MeshTile tile = posLookup[h];
while (tile != null)
{
//Tile found.
//Add to tile array
if (tile.Header != null && tile.Header.X == x && tile.Header.Y == y)
{
if (n < tiles.Length)
tiles[n++] = tile;
}
//Keep searching
tile = tile.Next;
}
return n;
}
/// <summary>
/// Connect Off-Mesh links between polygons from two different tiles.
/// </summary>
/// <param name="tile">Current Tile</param>
/// <param name="target">Target Tile</param>
/// <param name="side">Polygon edge</param>
public void ConnectExtOffMeshLinks(ref MeshTile tile, ref MeshTile target, BoundarySide side)
{
if (tile == null)
return;
//Connect off-mesh links, specifically links which land from target tile to this tile
BoundarySide oppositeSide = side.GetOpposite();
//Iterate through all the off-mesh connections of target tile
for (int i = 0; i < target.Header.OffMeshConCount; i++)
{
OffMeshConnection targetCon = target.OffMeshConnections[i];
if (targetCon.Side != oppositeSide)
continue;
Poly targetPoly = target.Polys[targetCon.Poly];
//Skip off-mesh connections which start location could not be connected at all
if (!IsLinkAllocated(targetPoly.FirstLink))
continue;
Vector3 extents = new Vector3(targetCon.Radius, target.Header.WalkableClimb, targetCon.Radius);
//Find polygon to connect to
Vector3 p = targetCon.Pos1;
Vector3 nearestPt = new Vector3();
int reference = FindNearestPolyInTile(tile, p, extents, ref nearestPt);
if (reference == 0)
continue;
//Further checks
if ((nearestPt.X - p.X) * (nearestPt.X - p.X) + (nearestPt.Z - p.Z) * (nearestPt.Z - p.Z) >
(targetCon.Radius * targetCon.Radius))
continue;
//Make sure the location is on the current mesh
target.Verts[targetPoly.Verts[1]] = nearestPt;
//Link off-mesh connection to target poly
int idx = AllocLink(target);
if (IsLinkAllocated(idx))
{
target.Links[idx].Reference = reference;
target.Links[idx].Edge = i;
target.Links[idx].Side = oppositeSide;
target.Links[idx].BMin = target.Links[idx].BMax = 0;
//add to linked list
target.Links[idx].Next = target.Polys[i].FirstLink;
target.Polys[i].FirstLink = idx;
}
//link target poly to off-mesh connection
if ((targetCon.Flags & OffMeshConnectionFlags.Bidirectional) != 0)
{
int tidx = AllocLink(tile);
if (IsLinkAllocated(tidx))
{
int landPolyIdx = DecodePolyIdPoly(reference);
tile.Links[tidx].Reference = GetReference(GetPolyRefBase(target), targetCon.Poly);
tile.Links[tidx].Edge = 0xff;
tile.Links[tidx].Side = side;
tile.Links[tidx].BMin = tile.Links[tidx].BMax = 0;
//add to linked list
tile.Links[tidx].Next = tile.Polys[landPolyIdx].FirstLink;
tile.Polys[landPolyIdx].FirstLink = tidx;
}
}
}
}
/// <summary>
/// Initialize the Tiled Navigation Mesh variables and arrays.
/// </summary>
/// <param name="parameters">Tiled Navigation Mesh attributes</param>
/// <returns>True if initialization is successful</returns>
public bool InitTileNavMesh(TiledNavMeshParams parameters)
{
this.parameters = parameters;
origin = parameters.Origin;
tileWidth = parameters.TileWidth;
tileHeight = parameters.TileHeight;
//init tiles
maxTiles = parameters.MaxTiles;
tileLookupTableSize = MathHelper.NextPowerOfTwo(parameters.MaxTiles / 4);
if (tileLookupTableSize == 0)
tileLookupTableSize = 1;
tileLookupTableMask = tileLookupTableSize - 1;
tiles = new MeshTile[maxTiles];
posLookup = new MeshTile[tileLookupTableSize];
for (int i = 0; i < tiles.Length; i++)
tiles[i] = new MeshTile();
for (int i = 0; i < posLookup.Length; i++)
posLookup[i] = null;
//create a linked list of tiles
nextFree = null;
for (int i = maxTiles - 1; i >= 0; i--)
{
tiles[i].Salt = 1;
tiles[i].Next = nextFree;
nextFree = tiles[i];
}
//init ID generator values
tileBits = MathHelper.Log2(MathHelper.NextPowerOfTwo(parameters.MaxTiles));
polyBits = MathHelper.Log2(MathHelper.NextPowerOfTwo(parameters.MaxPolys));
//only allow 31 salt bits, since salt mask is calculated using 32-bit int and it will overflow
saltBits = Math.Min(31, 32 - tileBits - polyBits);
if (saltBits < 10)
return false;
return true;
}
/// <summary>
/// Search for neighbor polygons in the tile.
/// </summary>
/// <param name="va">Vertex A</param>
/// <param name="vb">Vertex B</param>
/// <param name="tile">Current tile</param>
/// <param name="side">Polygon edge</param>
/// <param name="con">Resulting Connection polygon</param>
/// <param name="conarea">Resulting Connection area</param>
public void FindConnectingPolys(Vector3 va, Vector3 vb, MeshTile tile, BoundarySide side, List<int> con, List<float> conarea)
{
if (tile == null)
return;
Vector2 amin = Vector2.Zero;
Vector2 amax = Vector2.Zero;
CalcSlabEndPoints(va, vb, amin, amax, side);
float apos = GetSlabCoord(va, side);
//Remove links pointing to 'side' and compact the links array
Vector2 bmin = Vector2.Zero;
Vector2 bmax = Vector2.Zero;
int polyBase = GetPolyRefBase(tile);
//Iterate through all the tile's polygons
for (int i = 0; i < tile.Header.PolyCount; i++)
{
int numPolyVerts = tile.Polys[i].VertCount;
//Iterate through all the vertices
for (int j = 0; j < numPolyVerts; j++)
{
//Skip edges which do not point to the right side
if (tile.Polys[i].Neis[j] != (Link.External | (int)side))
continue;
//Grab two adjacent vertices
Vector3 vc = tile.Verts[tile.Polys[i].Verts[j]];
Vector3 vd = tile.Verts[tile.Polys[i].Verts[(j + 1) % numPolyVerts]];
float bpos = GetSlabCoord(vc, side);
//Segments are not close enough
if (Math.Abs(apos - bpos) > 0.01f)
continue;
//Check if the segments touch
CalcSlabEndPoints(vc, vd, bmin, bmax, side);
//Skip if slabs don't overlap
if (!OverlapSlabs(amin, amax, bmin, bmax, 0.01f, tile.Header.WalkableClimb))
continue;
//Add return value
if (con.Count < con.Capacity)
{
conarea.Add(Math.Max(amin.X, bmin.X));
conarea.Add(Math.Min(amax.X, bmax.X));
con.Add(GetReference(polyBase, i));
}
break;
}
}
}
/// <summary>
/// Find all the polygons within a certain bounding box.
/// </summary>
/// <param name="tile">Current tile</param>
/// <param name="qbounds">The bounds</param>
/// <param name="polys">List of polygons</param>
/// <returns>Number of polygons found</returns>
public int QueryPolygonsInTile(MeshTile tile, BBox3 qbounds, List<int> polys)
{
if (tile.BVTree.Count != 0)
{
int node = 0;
int end = tile.Header.BvNodeCount;
Vector3 tbmin = tile.Header.Bounds.Min;
Vector3 tbmax = tile.Header.Bounds.Max;
//Clamp query box to world box
Vector3 qbmin = qbounds.Min;
Vector3 qbmax = qbounds.Max;
PolyBounds b;
float bminx = MathHelper.Clamp(qbmin.X, tbmin.X, tbmax.X) - tbmin.X;
float bminy = MathHelper.Clamp(qbmin.Y, tbmin.Y, tbmax.Y) - tbmin.Y;
float bminz = MathHelper.Clamp(qbmin.Z, tbmin.Z, tbmax.Z) - tbmin.Z;
float bmaxx = MathHelper.Clamp(qbmax.X, tbmin.X, tbmax.X) - tbmin.X;
float bmaxy = MathHelper.Clamp(qbmax.Y, tbmin.Y, tbmax.Y) - tbmin.Y;
float bmaxz = MathHelper.Clamp(qbmax.Z, tbmin.Z, tbmax.Z) - tbmin.Z;
const int MinMask = unchecked((int)0xfffffffe);
b.Min.X = (int)(bminx * tile.Header.BvQuantFactor) & MinMask;
b.Min.Y = (int)(bminy * tile.Header.BvQuantFactor) & MinMask;
b.Min.Z = (int)(bminz * tile.Header.BvQuantFactor) & MinMask;
b.Max.X = (int)(bmaxx * tile.Header.BvQuantFactor + 1) | 1;
b.Max.Y = (int)(bmaxy * tile.Header.BvQuantFactor + 1) | 1;
b.Max.Z = (int)(bmaxz * tile.Header.BvQuantFactor + 1) | 1;
//traverse tree
int polyBase = GetPolyRefBase(tile);
while (node < end)
{
BVTree.Node bvNode = tile.BVTree[node];
bool overlap = PolyBounds.Overlapping(ref b, ref bvNode.Bounds);
bool isLeafNode = bvNode.Index >= 0;
if (isLeafNode && overlap)
{
if (polys.Count < polys.Capacity)
polys.Add(GetReference(polyBase, bvNode.Index));
}
if (overlap || isLeafNode)
{
node++;
}
else
{
int escapeIndex = -bvNode.Index;
node += escapeIndex;
}
}
return polys.Count;
}
else
{
BBox3 b;
int polyBase = GetPolyRefBase(tile);
for (int i = 0; i < tile.Header.PolyCount; i++)
{
var poly = tile.Polys[i];
//don't return off-mesh connection polygons
if (poly.PolyType == PolygonType.OffMeshConnection)
continue;
//calculate polygon bounds
b.Max = b.Min = tile.Verts[poly.Verts[0]];
for (int j = 1; j < poly.VertCount; j++)
{
Vector3 v = tile.Verts[poly.Verts[j]];
Vector3Extensions.ComponentMin(ref b.Min, ref v, out b.Min);
Vector3Extensions.ComponentMax(ref b.Max, ref v, out b.Max);
}
if (BBox3.Overlapping(ref qbounds, ref b))
{
if (polys.Count < polys.Capacity)
polys.Add(GetReference(polyBase, i));
}
}
return polys.Count;
}
}
/// <summary>
/// Gets the neighboring tile at that position
/// </summary>
/// <param name="x">The x-coordinate</param>
/// <param name="y">The y-coordinate</param>
/// <param name="side">The side value</param>
/// <param name="tiles">An array of MeshTiles</param>
/// <returns>The number of tiles satisfying the condition</returns>
public int GetNeighbourTilesAt(int x, int y, BoundarySide side, MeshTile[] tiles)
{
int nx = x, ny = y;
switch (side)
{
case BoundarySide.PlusX:
nx++;
break;
case BoundarySide.PlusXPlusZ:
nx++;
ny++;
break;
case BoundarySide.PlusZ:
ny++;
break;
case BoundarySide.MinusXPlusZ:
nx--;
ny++;
break;
case BoundarySide.MinusX:
nx--;
break;
case BoundarySide.MinusXMinusZ:
nx--;
ny--;
break;
case BoundarySide.MinusZ:
ny--;
break;
case BoundarySide.PlusXMinusZ:
nx++;
ny--;
break;
}
return GetTilesAt(nx, ny, tiles);
}
/// <summary>
/// Retrieve the tile and poly based off of a polygon reference
/// </summary>
/// <param name="reference">Polygon reference</param>
/// <param name="tile">Resulting tile</param>
/// <param name="poly">Resulting poly</param>
/// <returns>True if tile and poly successfully retrieved</returns>
public bool TryGetTileAndPolyByRef(int reference, out MeshTile tile, out Poly poly)
{
tile = null;
poly = null;
if (reference == 0)
return false;
//Get tile and poly indices
int salt = 0, indexTile = 0, indexPoly = 0;
DecodePolyId(reference, ref salt, ref indexTile, ref indexPoly);
//Make sure indices are valid
if (indexTile >= maxTiles)
return false;
if (tiles[indexTile].Salt != salt || tiles[indexTile].Header == null)
return false;
if (indexPoly >= tiles[indexTile].Header.PolyCount)
return false;
//Retrieve tile and poly
tile = tiles[indexTile];
poly = tiles[indexTile].Polys[indexPoly];
return true;
}
/// <summary>
/// Finds a random point on a polygon.
/// </summary>
/// <param name="tile">The current mesh tile</param>
/// <param name="poly">The current polygon</param>
/// <param name="polyRef">Polygon reference</param>
/// <returns>Resulting random point</returns>
public Vector3 FindRandomPointOnPoly(MeshTile tile, Poly poly, PolyId polyRef)
{
Vector3 result;
this.FindRandomPointOnPoly(tile, poly, polyRef, out result);
return result;
}
/// <summary>
/// Build a tile and link all the polygons togther, both internally and externally.
/// Make sure to link off-mesh connections as well.
/// </summary>
/// <param name="data">Navigation Mesh data</param>
/// <param name="lastRef">Last polygon reference</param>
/// <param name="result">Last tile reference</param>
public void AddTile(NavMeshBuilder data, int lastRef, ref int result)
{
//make sure data is in right format
PathfindingCommon.NavMeshInfo header = data.Header;
//make sure location is free
if (GetTileAt(header.X, header.Y, header.Layer) != null)
return;
//allocate a tile
MeshTile tile = null;
if (lastRef == 0)
{
if (nextFree != null)
{
tile = nextFree;
nextFree = tile.Next;
tile.Next = null;
}
}
else
{
//try to relocate tile to specific index with the same salt
int tileIndex = DecodePolyIdTile(lastRef);
if (tileIndex >= maxTiles)
return;
//try to find specific tile id from free list
MeshTile target = tiles[tileIndex];
MeshTile prev = null;
tile = nextFree;
while (tile != null && tile != target)
{
prev = tile;
tile = tile.Next;
}
//couldn't find correct location
if (tile != target)
return;
//remove from freelist
if (prev == null)
nextFree = tile.Next;
else
prev.Next = tile.Next;
//restore salt
tile.Salt = DecodePolyIdSalt(lastRef);
}
//make sure we could allocate a tile
if (tile == null)
return;
//insert tile into position LookUp Table (lut)
int h = ComputeTileHash(header.X, header.Y, tileLookupTableMask);
tile.Next = posLookup[h];
posLookup[h] = tile;
if (header.BvNodeCount == 0)
tile.BVTree = null;
//patch header
tile.Verts = data.NavVerts;
tile.Polys = data.NavPolys;
tile.DetailMeshes = data.NavDMeshes;
tile.DetailVerts = data.NavDVerts;
tile.DetailTris = data.NavDTris;
tile.BVTree = data.NavBvTree;
tile.OffMeshConnections = data.OffMeshCons;
//build links freelist
tile.LinksFreeList = 0;
tile.Links = new Link[header.MaxLinkCount];
for (int i = 0; i < header.MaxLinkCount; i++)
tile.Links[i] = new Link();
tile.Links[header.MaxLinkCount - 1].Next = Link.Null;
for (int i = 0; i < header.MaxLinkCount - 1; i++)
tile.Links[i].Next = i + 1;
//init tile
tile.Header = header;
tile.Data = data;
ConnectIntLinks(ref tile);
BaseOffMeshLinks(ref tile);
//create connections with neighbor tiles
MeshTile[] neis = new MeshTile[32];
int nneis;
//connect with layers in current tile
nneis = GetTilesAt(header.X, header.Y, neis);
for (int j = 0; j < nneis; j++)
{
if (neis[j] != tile)
{
ConnectExtLinks(ref tile, ref neis[j], BoundarySide.Internal);
ConnectExtLinks(ref neis[j], ref tile, BoundarySide.Internal);
}
ConnectExtOffMeshLinks(ref tile, ref neis[j], BoundarySide.Internal);
ConnectExtOffMeshLinks(ref neis[j], ref tile, BoundarySide.Internal);
}
//connect with neighbour tiles
for (int i = 0; i < 8; i++)
{
BoundarySide b = (BoundarySide)i;
BoundarySide bo = b.GetOpposite();
nneis = GetNeighbourTilesAt(header.X, header.Y, b, neis);
for (int j = 0; j < nneis; j++)
{
ConnectExtLinks(ref tile, ref neis[j], b);
ConnectExtLinks(ref neis[j], ref tile, bo);
ConnectExtOffMeshLinks(ref tile, ref neis[j], b);
ConnectExtOffMeshLinks(ref neis[j], ref tile, bo);
}
}
result = GetTileRef(tile);
}
/// <summary>
/// Get edge midpoint between two prolygons
/// </summary>
/// <param name="from">"From" polygon reference</param>
/// <param name="fromPoly">"From" polygon data</param>
/// <param name="fromTile">"From" mesh tile</param>
/// <param name="to">"To" polygon reference</param>
/// <param name="toPoly">"To" polygon data</param>
/// <param name="toTile">"To" mesh tile</param>
/// <param name="mid">Edge midpoint</param>
/// <returns>True, if midpoint found. False, if otherwise.</returns>
public bool GetEdgeMidPoint(PolyId from, Poly fromPoly, MeshTile fromTile, PolyId to, Poly toPoly, MeshTile toTile, ref Vector3 mid)
{
Vector3 left = new Vector3();
Vector3 right = new Vector3();
if (!GetPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, ref left, ref right))
return false;
mid = (left + right) * 0.5f;
return true;
}
/// <summary>
/// Only use this function if it is known that the provided polygon reference is valid.
/// </summary>
/// <param name="reference">Polygon reference</param>
/// <param name="tile">Resulting tile</param>
/// <param name="poly">Resulting poly</param>
public void TryGetTileAndPolyByRefUnsafe(int reference, out MeshTile tile, out Poly poly)
{
int salt = 0, indexTile = 0, indexPoly = 0;
DecodePolyId(reference, ref salt, ref indexTile, ref indexPoly);
tile = tiles[indexTile];
poly = tiles[indexTile].Polys[indexPoly];
}
/// <summary>
/// Finds nearby polygons within a certain range.
/// </summary>
/// <param name="center">The starting point</param>
/// <param name="extent">The range to search within</param>
/// <param name="polys">A list of polygons</param>
/// <returns>True, if successful. False, if otherwise.</returns>
public bool QueryPolygons(ref Vector3 center, ref Vector3 extent, List<PolyId> polys)
{
Vector3 bmin = center - extent;
Vector3 bmax = center + extent;
int minx, miny, maxx, maxy;
nav.CalcTileLoc(ref bmin, out minx, out miny);
nav.CalcTileLoc(ref bmax, out maxx, out maxy);
MeshTile[] neis = new MeshTile[32];
BBox3 bounds = new BBox3(bmin, bmax);
int n = 0;
for (int y = miny; y <= maxy; y++)
{
for (int x = minx; x <= maxx; x++)
{
int nneis = nav.GetTilesAt(x, y, neis);
for (int j = 0; j < nneis; j++)
{
n += nav.QueryPolygonsInTile(neis[j], bounds, polys);
if (n >= polys.Capacity)
{
return true;
}
}
}
}
return polys.Count != 0;
}
/// <summary>
/// Get the tile reference
/// </summary>
/// <param name="tile">Tile to look for</param>
/// <returns>Tile reference</returns>
public PolyId GetTileRef(MeshTile tile)
{
if (tile == null)
return PolyId.Null;
PolyId id;
if (!tileRefs.TryGetValue(tile, out id))
id = PolyId.Null;
return id;
}
