/// <summary>
/// Allocate links for each of the tile's polygons' vertices
/// </summary>
public void ConnectIntLinks()
{
//Iterate through all the polygons
for (int i = 0; i < PolyCount; i++)
{
Poly p = Polys[i];
//Avoid Off-Mesh Connection polygons
if (p.PolyType == PolygonType.OffMeshConnection)
{
continue;
}
//Build edge links
for (int j = p.VertCount - 1; j >= 0; j--)
{
//Skip hard and non-internal edges
if (p.Neis[j] == 0 || Link.IsExternal(p.Neis[j]))
{
continue;
}
PolyId id;
idManager.SetPolyIndex(ref baseRef, p.Neis[j] - 1, out id);
//Initialize a new link
Link link = new Link();
link.Reference = id;
link.Edge = j;
link.Side = BoundarySide.Internal;
link.BMin = link.BMax = 0;
p.Links.Add(link);
}
}
}
/// <summary>
/// Find the distance from a point to a triangle.
/// </summary>
/// <param name="indexPoly">Current polygon's index</param>
/// <param name="pos">Current position</param>
/// <param name="h">Resulting height</param>
/// <returns>True, if a height is found. False, if otherwise.</returns>
public bool ClosestHeight(int indexPoly, Vector3 pos, out float h)
{
Poly poly = Polys[indexPoly];
PolyMeshDetail.MeshData pd = DetailMeshes[indexPoly];
//find height at the location
for (int j = 0; j < DetailMeshes[indexPoly].TriangleCount; j++)
{
PolyMeshDetail.TriangleData t = DetailTris[pd.TriangleIndex + j];
Vector3[] v = new Vector3[3];
for (int k = 0; k < 3; k++)
{
if (t[k] < poly.VertCount)
{
v[k] = Verts[poly.Verts[t[k]]];
}
else
{
v[k] = DetailVerts[pd.VertexIndex + (t[k] - poly.VertCount)];
}
}
if (Distance.PointToTriangle(pos, v[0], v[1], v[2], out h))
{
return(true);
}
}
h = float.MaxValue;
return(false);
}
/// <summary>
/// Find the closest point on an offmesh connection, which is in between the two points.
/// </summary>
/// <param name="poly">Current polygon</param>
/// <param name="pos">Current position</param>
/// <param name="closest">Resulting point that is closest.</param>
public void ClosestPointOnPolyOffMeshConnection(Poly poly, Vector3 pos, out Vector3 closest)
{
Vector3 v0 = Verts[poly.Verts[0]];
Vector3 v1 = Verts[poly.Verts[1]];
float d0 = (pos - v0).Length();
float d1 = (pos - v1).Length();
float u = d0 / (d0 + d1);
closest = Vector3.Lerp(v0, v1, u);
}
/// <summary>
/// Begin creating off-mesh links between the tile polygons.
/// </summary>
public void BaseOffMeshLinks()
{
//Base off-mesh connection start points
for (int i = 0; i < OffMeshConnectionCount; i++)
{
int con = i;
OffMeshConnection omc = OffMeshConnections[con];
Vector3 extents = new Vector3(omc.Radius, WalkableClimb, omc.Radius);
//Find polygon to connect to
Vector3 p = omc.Pos0;
Vector3 nearestPt = new Vector3();
PolyId reference = FindNearestPoly(p, extents, ref nearestPt);
if (reference == PolyId.Null)
{
continue;
}
//Do extra checks
if ((nearestPt.X - p.X) * (nearestPt.X - p.X) + (nearestPt.Z - p.Z) * (nearestPt.Z - p.Z) >
OffMeshConnections[con].Radius * OffMeshConnections[con].Radius)
{
continue;
}
Poly poly = this.Polys[omc.Poly];
//Make sure location is on current mesh
Verts[poly.Verts[0]] = nearestPt;
Link link = new Link();
link.Reference = reference;
link.Edge = 0;
link.Side = BoundarySide.Internal;
poly.Links.Add(link);
//Start end-point always conects back to off-mesh connection
int landPolyIdx = idManager.DecodePolyIndex(ref reference);
PolyId id;
idManager.SetPolyIndex(ref baseRef, OffMeshConnections[con].Poly, out id);
Link link2 = new Link();
link2.Reference = id;
link2.Edge = 0xff;
link2.Side = BoundarySide.Internal;
Polys[landPolyIdx].Links.Add(link2);
}
}
/// <summary>
/// Given a point, find the closest point on that poly.
/// </summary>
/// <param name="poly">The current polygon.</param>
/// <param name="pos">The current position</param>
/// <param name="closest">Reference to the closest point</param>
public void ClosestPointOnPoly(Poly poly, Vector3 pos, ref Vector3 closest)
{
int indexPoly = 0;
for (int i = 0; i < Polys.Length; i++)
{
if (Polys[i] == poly)
{
indexPoly = i;
break;
}
}
ClosestPointOnPoly(indexPoly, pos, ref closest);
}
/// <summary>
/// Given a point, find the closest point on that poly.
/// </summary>
/// <param name="indexPoly">The current poly's index</param>
/// <param name="pos">The current position</param>
/// <param name="closest">Reference to the closest point</param>
public void ClosestPointOnPoly(int indexPoly, Vector3 pos, ref Vector3 closest)
{
Poly poly = Polys[indexPoly];
//Off-mesh connections don't have detail polygons
if (Polys[indexPoly].PolyType == PolygonType.OffMeshConnection)
{
ClosestPointOnPolyOffMeshConnection(poly, pos, out closest);
return;
}
ClosestPointOnPolyBoundary(poly, pos, out closest);
float h;
if (ClosestHeight(indexPoly, pos, out h))
{
closest.Y = h;
}
}
/// <summary>
/// Given a point, find the closest point on that poly.
/// </summary>
/// <param name="poly">The current polygon.</param>
/// <param name="pos">The current position</param>
/// <param name="closest">Reference to the closest point</param>
public void ClosestPointOnPolyBoundary(Poly poly, Vector3 pos, out Vector3 closest)
{
//Clamp point to be inside the polygon
Vector3[] verts = new Vector3[PathfindingCommon.VERTS_PER_POLYGON];
float[] edgeDistance = new float[PathfindingCommon.VERTS_PER_POLYGON];
float[] edgeT = new float[PathfindingCommon.VERTS_PER_POLYGON];
int numPolyVerts = poly.VertCount;
for (int i = 0; i < numPolyVerts; i++)
{
verts[i] = Verts[poly.Verts[i]];
}
bool inside = Distance.PointToPolygonEdgeSquared(pos, verts, numPolyVerts, edgeDistance, edgeT);
if (inside)
{
//Point is inside the polygon
closest = pos;
}
else
{
//Point is outside the polygon
//Clamp to nearest edge
float minDistance = float.MaxValue;
int minIndex = -1;
for (int i = 0; i < numPolyVerts; i++)
{
if (edgeDistance[i] < minDistance)
{
minDistance = edgeDistance[i];
minIndex = i;
}
}
Vector3 va = verts[minIndex];
Vector3 vb = verts[(minIndex + 1) % numPolyVerts];
closest = Vector3.Lerp(va, vb, edgeT[minIndex]);
}
}
/// <summary>
/// Find the closest point on an offmesh connection, which is in between the two points.
/// </summary>
/// <param name="poly">Current polygon</param>
/// <param name="pos">Current position</param>
/// <param name="closest">Resulting point that is closest.</param>
public void ClosestPointOnPolyOffMeshConnection(Poly poly, Vector3 pos, out Vector3 closest)
{
Vector3 v0 = Verts[poly.Verts[0]];
Vector3 v1 = Verts[poly.Verts[1]];
float d0 = (pos - v0).Length();
float d1 = (pos - v1).Length();
float u = d0 / (d0 + d1);
closest = Vector3.Lerp(v0, v1, u);
}
/// <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>
/// 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>
/// Connect Off-Mesh links between polygons from two different tiles.
/// </summary>
/// <param name="target">Target Tile</param>
/// <param name="side">Polygon edge</param>
public void ConnectExtOffMeshLinks(MeshTile target, BoundarySide side)
{
//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.OffMeshConnectionCount; 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 (targetPoly.Links.Count == 0)
{
continue;
}
Vector3 extents = new Vector3(targetCon.Radius, target.WalkableClimb, targetCon.Radius);
//Find polygon to connect to
Vector3 p = targetCon.Pos1;
Vector3 nearestPt = new Vector3();
PolyId reference = FindNearestPoly(p, extents, ref nearestPt);
if (reference == PolyId.Null)
{
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
Link link = new Link();
link.Reference = reference;
link.Edge = i;
link.Side = oppositeSide;
target.Polys[i].Links.Add(link);
//link target poly to off-mesh connection
if ((targetCon.Flags & OffMeshConnectionFlags.Bidirectional) != 0)
{
int landPolyIdx = idManager.DecodePolyIndex(ref reference);
PolyId id;
id = target.baseRef;
idManager.SetPolyIndex(ref id, targetCon.Poly, out id);
Link bidiLink = new Link();
bidiLink.Reference = id;
bidiLink.Edge = 0xff;
bidiLink.Side = side;
Polys[landPolyIdx].Links.Add(bidiLink);
}
}
}
/// <summary>
/// The cost between two points may vary depending on the type of polygon.
/// </summary>
/// <param name="pa">Point A</param>
/// <param name="pb">Point B</param>
/// <param name="curPoly">Current polygon</param>
/// <returns>Cost</returns>
public float GetCost(Vector3 pa, Vector3 pb, Poly curPoly)
{
return (pa - pb).Length() * areaCost[(int)curPoly.Area.Id];
}
/// <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>
/// 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>
/// 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>
/// Given a point, find the closest point on that poly.
/// </summary>
/// <param name="poly">The current polygon.</param>
/// <param name="pos">The current position</param>
/// <param name="closest">Reference to the closest point</param>
public void ClosestPointOnPolyBoundary(Poly poly, Vector3 pos, out Vector3 closest)
{
//Clamp point to be inside the polygon
Vector3[] verts = new Vector3[PathfindingCommon.VERTS_PER_POLYGON];
float[] edgeDistance = new float[PathfindingCommon.VERTS_PER_POLYGON];
float[] edgeT = new float[PathfindingCommon.VERTS_PER_POLYGON];
int numPolyVerts = poly.VertCount;
for (int i = 0; i < numPolyVerts; i++)
verts[i] = Verts[poly.Verts[i]];
bool inside = Distance.PointToPolygonEdgeSquared(pos, verts, numPolyVerts, edgeDistance, edgeT);
if (inside)
{
//Point is inside the polygon
closest = pos;
}
else
{
//Point is outside the polygon
//Clamp to nearest edge
float minDistance = float.MaxValue;
int minIndex = -1;
for (int i = 0; i < numPolyVerts; i++)
{
if (edgeDistance[i] < minDistance)
{
minDistance = edgeDistance[i];
minIndex = i;
}
}
Vector3 va = verts[minIndex];
Vector3 vb = verts[(minIndex + 1) % numPolyVerts];
closest = Vector3.Lerp(va, vb, edgeT[minIndex]);
}
}
/// <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>
/// Initializes a new instance of the <see cref="NavMeshBuilder" /> class.
/// Add all the PolyMesh and PolyMeshDetail attributes to the Navigation Mesh.
/// Then, add Off-Mesh connection support.
/// </summary>
/// <param name="polyMesh">The PolyMesh</param>
/// <param name="polyMeshDetail">The PolyMeshDetail</param>
/// <param name="offMeshCons">Offmesh connection data</param>
/// <param name="settings">The settings used to build.</param>
public NavMeshBuilder(PolyMesh polyMesh, PolyMeshDetail polyMeshDetail, OffMeshConnection[] offMeshCons, NavMeshGenerationSettings settings)
{
if (settings.VertsPerPoly > PathfindingCommon.VERTS_PER_POLYGON)
throw new InvalidOperationException("The number of vertices per polygon is above SharpNav's limit");
if (polyMesh.VertCount == 0)
throw new InvalidOperationException("The provided PolyMesh has no vertices.");
if (polyMesh.PolyCount == 0)
throw new InvalidOperationException("The provided PolyMesh has not polys.");
int nvp = settings.VertsPerPoly;
//classify off-mesh connection points
BoundarySide[] offMeshSides = new BoundarySide[offMeshCons.Length * 2];
int storedOffMeshConCount = 0;
int offMeshConLinkCount = 0;
if (offMeshCons.Length > 0)
{
//find height bounds
float hmin = float.MaxValue;
float hmax = -float.MaxValue;
if (polyMeshDetail != null)
{
for (int i = 0; i < polyMeshDetail.VertCount; i++)
{
float h = polyMeshDetail.Verts[i].Y;
hmin = Math.Min(hmin, h);
hmax = Math.Max(hmax, h);
}
}
else
{
for (int i = 0; i < polyMesh.VertCount; i++)
{
PolyVertex iv = polyMesh.Verts[i];
float h = polyMesh.Bounds.Min.Y + iv.Y * settings.CellHeight;
hmin = Math.Min(hmin, h);
hmax = Math.Max(hmax, h);
}
}
hmin -= settings.MaxClimb;
hmax += settings.MaxClimb;
BBox3 bounds = polyMesh.Bounds;
bounds.Min.Y = hmin;
bounds.Max.Y = hmax;
for (int i = 0; i < offMeshCons.Length; i++)
{
Vector3 p0 = offMeshCons[i].Pos0;
Vector3 p1 = offMeshCons[i].Pos1;
offMeshSides[i * 2 + 0] = BoundarySideExtensions.FromPoint(p0, bounds);
offMeshSides[i * 2 + 1] = BoundarySideExtensions.FromPoint(p1, bounds);
//off-mesh start position isn't touching mesh
if (offMeshSides[i * 2 + 0] == BoundarySide.Internal)
{
if (p0.Y < bounds.Min.Y || p0.Y > bounds.Max.Y)
offMeshSides[i * 2 + 0] = 0;
}
//count number of links to allocate
if (offMeshSides[i * 2 + 0] == BoundarySide.Internal)
offMeshConLinkCount++;
if (offMeshSides[i * 2 + 1] == BoundarySide.Internal)
offMeshConLinkCount++;
if (offMeshSides[i * 2 + 0] == BoundarySide.Internal)
storedOffMeshConCount++;
}
}
//off-mesh connections stored as polygons, adjust values
int totPolyCount = polyMesh.PolyCount + storedOffMeshConCount;
int totVertCount = polyMesh.VertCount + storedOffMeshConCount * 2;
//find portal edges
int edgeCount = 0;
int portalCount = 0;
for (int i = 0; i < polyMesh.PolyCount; i++)
{
PolyMesh.Polygon p = polyMesh.Polys[i];
for (int j = 0; j < nvp; j++)
{
if (p.Vertices[j] == PolyMesh.NullId)
break;
edgeCount++;
if (PolyMesh.IsBoundaryEdge(p.NeighborEdges[j]))
{
int dir = p.NeighborEdges[j] % 16;
if (dir != 15)
portalCount++;
}
}
}
int maxLinkCount = edgeCount + portalCount * 2 + offMeshConLinkCount * 2;
//find unique detail vertices
int uniqueDetailVertCount = 0;
int detailTriCount = 0;
if (polyMeshDetail != null)
{
detailTriCount = polyMeshDetail.TrisCount;
for (int i = 0; i < polyMesh.PolyCount; i++)
{
int numDetailVerts = polyMeshDetail.Meshes[i].VertexCount;
int numPolyVerts = polyMesh.Polys[i].VertexCount;
uniqueDetailVertCount += numDetailVerts - numPolyVerts;
}
}
else
{
uniqueDetailVertCount = 0;
detailTriCount = 0;
for (int i = 0; i < polyMesh.PolyCount; i++)
{
int numPolyVerts = polyMesh.Polys[i].VertexCount;
uniqueDetailVertCount += numPolyVerts - 2;
}
}
//allocate data
header = new PathfindingCommon.NavMeshInfo();
navVerts = new Vector3[totVertCount];
navPolys = new Poly[totPolyCount];
navDMeshes = new PolyMeshDetail.MeshData[polyMesh.PolyCount];
navDVerts = new Vector3[uniqueDetailVertCount];
navDTris = new PolyMeshDetail.TriangleData[detailTriCount];
offMeshConnections = new OffMeshConnection[storedOffMeshConCount];
//store header
//HACK TiledNavMesh should figure out the X/Y/layer instead of the user maybe?
header.X = 0;
header.Y = 0;
header.Layer = 0;
header.PolyCount = totPolyCount;
header.VertCount = totVertCount;
header.MaxLinkCount = maxLinkCount;
header.Bounds = polyMesh.Bounds;
header.DetailMeshCount = polyMesh.PolyCount;
header.DetailVertCount = uniqueDetailVertCount;
header.DetailTriCount = detailTriCount;
header.OffMeshBase = polyMesh.PolyCount;
header.WalkableHeight = settings.AgentHeight;
header.WalkableRadius = settings.AgentRadius;
header.WalkableClimb = settings.MaxClimb;
header.OffMeshConCount = storedOffMeshConCount;
header.BvNodeCount = settings.BuildBoundingVolumeTree ? polyMesh.PolyCount * 2 : 0;
header.BvQuantFactor = 1f / settings.CellSize;
int offMeshVertsBase = polyMesh.VertCount;
int offMeshPolyBase = polyMesh.PolyCount;
//store vertices
for (int i = 0; i < polyMesh.VertCount; i++)
{
PolyVertex iv = polyMesh.Verts[i];
navVerts[i].X = polyMesh.Bounds.Min.X + iv.X * settings.CellSize;
navVerts[i].Y = polyMesh.Bounds.Min.Y + iv.Y * settings.CellHeight;
navVerts[i].Z = polyMesh.Bounds.Min.Z + iv.Z * settings.CellSize;
}
//off-mesh link vertices
int n = 0;
for (int i = 0; i < offMeshCons.Length; i++)
{
//only store connections which start from this tile
if (offMeshSides[i * 2 + 0] == BoundarySide.Internal)
{
navVerts[offMeshVertsBase + (n * 2 + 0)] = offMeshCons[i].Pos0;
navVerts[offMeshVertsBase + (n * 2 + 1)] = offMeshCons[i].Pos1;
n++;
}
}
//store polygons
for (int i = 0; i < polyMesh.PolyCount; i++)
{
navPolys[i] = new Poly();
navPolys[i].VertCount = 0;
navPolys[i].Tag = polyMesh.Polys[i].Tag;
navPolys[i].Area = polyMesh.Polys[i].Area;
navPolys[i].PolyType = PolygonType.Ground;
navPolys[i].Verts = new int[nvp];
navPolys[i].Neis = new int[nvp];
for (int j = 0; j < nvp; j++)
{
if (polyMesh.Polys[i].Vertices[j] == PolyMesh.NullId)
break;
navPolys[i].Verts[j] = polyMesh.Polys[i].Vertices[j];
if (PolyMesh.IsBoundaryEdge(polyMesh.Polys[i].NeighborEdges[j]))
{
//border or portal edge
int dir = polyMesh.Polys[i].NeighborEdges[j] % 16;
if (dir == 0xf) //border
navPolys[i].Neis[j] = 0;
else if (dir == 0) //portal x-
navPolys[i].Neis[j] = Link.External | 4;
else if (dir == 1) //portal z+
navPolys[i].Neis[j] = Link.External | 2;
else if (dir == 2) //portal x+
navPolys[i].Neis[j] = Link.External | 0;
else if (dir == 3) //portal z-
navPolys[i].Neis[j] = Link.External | 6;
}
else
{
//normal connection
navPolys[i].Neis[j] = polyMesh.Polys[i].NeighborEdges[j] + 1;
}
navPolys[i].VertCount++;
}
}
//off-mesh connection vertices
n = 0;
for (int i = 0; i < offMeshCons.Length; i++)
{
//only store connections which start from this tile
if (offMeshSides[i * 2 + 0] == BoundarySide.Internal)
{
navPolys[offMeshPolyBase + n].VertCount = 2;
navPolys[offMeshPolyBase + n].Verts = new int[nvp];
navPolys[offMeshPolyBase + n].Verts[0] = offMeshVertsBase + (n * 2 + 0);
navPolys[offMeshPolyBase + n].Verts[1] = offMeshVertsBase + (n * 2 + 1);
navPolys[offMeshPolyBase + n].Tag = offMeshCons[i].Flags;
navPolys[offMeshPolyBase + n].Area = polyMesh.Polys[offMeshCons[i].Poly].Area; //HACK is this correct?
navPolys[offMeshPolyBase + n].PolyType = PolygonType.OffMeshConnection;
n++;
}
}
//store detail meshes and vertices
if (polyMeshDetail != null)
{
int vbase = 0;
List<Vector3> storedDetailVerts = new List<Vector3>();
for (int i = 0; i < polyMesh.PolyCount; i++)
{
int vb = polyMeshDetail.Meshes[i].VertexIndex;
int numDetailVerts = polyMeshDetail.Meshes[i].VertexCount;
int numPolyVerts = navPolys[i].VertCount;
navDMeshes[i].VertexIndex = vbase;
navDMeshes[i].VertexCount = numDetailVerts - numPolyVerts;
navDMeshes[i].TriangleIndex = polyMeshDetail.Meshes[i].TriangleIndex;
navDMeshes[i].TriangleCount = polyMeshDetail.Meshes[i].TriangleCount;
//Copy detail vertices
//first 'nv' verts are equal to nav poly verts
//the rest are detail verts
for (int j = 0; j < navDMeshes[i].VertexCount; j++)
{
storedDetailVerts.Add(polyMeshDetail.Verts[vb + numPolyVerts + j]);
}
vbase += numDetailVerts - numPolyVerts;
}
navDVerts = storedDetailVerts.ToArray();
//store triangles
for (int j = 0; j < polyMeshDetail.TrisCount; j++)
navDTris[j] = polyMeshDetail.Tris[j];
}
else
{
//create dummy detail mesh by triangulating polys
int tbase = 0;
for (int i = 0; i < polyMesh.PolyCount; i++)
{
int numPolyVerts = navPolys[i].VertCount;
navDMeshes[i].VertexIndex = 0;
navDMeshes[i].VertexCount = 0;
navDMeshes[i].TriangleIndex = tbase;
navDMeshes[i].TriangleCount = numPolyVerts - 2;
//triangulate polygon
for (int j = 2; j < numPolyVerts; j++)
{
navDTris[tbase].VertexHash0 = 0;
navDTris[tbase].VertexHash1 = j - 1;
navDTris[tbase].VertexHash2 = j;
//bit for each edge that belongs to the poly boundary
navDTris[tbase].Flags = 1 << 2;
if (j == 2)
navDTris[tbase].Flags |= 1 << 0;
if (j == numPolyVerts - 1)
navDTris[tbase].Flags |= 1 << 4;
tbase++;
}
}
}
//store and create BV tree
if (settings.BuildBoundingVolumeTree)
{
//build tree
navBvTree = new BVTree(polyMesh.Verts, polyMesh.Polys, nvp, settings.CellSize, settings.CellHeight);
}
//store off-mesh connections
n = 0;
for (int i = 0; i < offMeshConnections.Length; i++)
{
//only store connections which start from this tile
if (offMeshSides[i * 2 + 0] == BoundarySide.Internal)
{
offMeshConnections[n].Poly = offMeshPolyBase + n;
//copy connection end points
offMeshConnections[n].Pos0 = offMeshCons[i].Pos0;
offMeshConnections[n].Pos1 = offMeshCons[i].Pos1;
offMeshConnections[n].Radius = offMeshCons[i].Radius;
offMeshConnections[n].Flags = offMeshCons[i].Flags;
offMeshConnections[n].Side = offMeshSides[i * 2 + 1];
offMeshConnections[n].Tag = offMeshCons[i].Tag;
n++;
}
}
}
/// <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>
/// 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>
/// Given a point, find the closest point on that poly.
/// </summary>
/// <param name="poly">The current polygon.</param>
/// <param name="pos">The current position</param>
/// <param name="closest">Reference to the closest point</param>
public void ClosestPointOnPoly(Poly poly, Vector3 pos, ref Vector3 closest)
{
int indexPoly = 0;
for (int i = 0; i < Polys.Length; i++)
{
if (Polys[i] == poly)
{
indexPoly = i;
break;
}
}
ClosestPointOnPoly(indexPoly, pos, ref closest);
}
