public void SetPolyIndex(ref PolyId polyBase, int newPoly, out PolyId result)
{
newPoly &= polyMask;
//first clear poly then OR with new poly
result = new PolyId((polyBase.Id & ~polyMask) | newPoly);
}
public override void Serialize(string path, TiledNavMesh mesh)
{
JObject root = new JObject();
root.Add("meta", JToken.FromObject(new
{
version = new
{
snj = FormatVersion,
sharpnav = Assembly.GetExecutingAssembly().GetCustomAttribute <AssemblyInformationalVersionAttribute>().InformationalVersion
}
}));
root.Add("origin", JToken.FromObject(mesh.Origin, serializer));
root.Add("tileWidth", JToken.FromObject(mesh.TileWidth, serializer));
root.Add("tileHeight", JToken.FromObject(mesh.TileHeight, serializer));
root.Add("maxTiles", JToken.FromObject(mesh.MaxTiles, serializer));
root.Add("maxPolys", JToken.FromObject(mesh.MaxPolys, serializer));
var tilesArray = new JArray();
var tiles = (List <MeshTile>)GetPrivateField(mesh, typeof(TiledNavMesh), "tileList");
var tileRefs = (Dictionary <MeshTile, PolyId>)GetPrivateField(mesh, typeof(TiledNavMesh), "tileRefs");
foreach (MeshTile tile in tiles)
{
PolyId id = mesh.GetTileRef(tile);
tilesArray.Add(SerializeMeshTile(tile, id));
}
root.Add("tiles", tilesArray);
File.WriteAllText(path, root.ToString());
}
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>
/// Resets the path to the first polygon.
/// </summary>
/// <param name="reference">The starting polygon reference</param>
/// <param name="pos">Starting position</param>
public void Reset(PolyId reference, Vector3 pos)
{
this.pos = pos;
this.target = pos;
this.path[0] = reference;
this.pathCount = 1;
}
/// <summary>
/// Check if polygon reference is valid.
/// </summary>
/// <param name="reference">Polygon reference</param>
/// <returns>True if valid</returns>
public bool IsValidPolyRef(PolyId reference)
{
if (reference == PolyId.Null)
{
return(false);
}
int salt, polyIndex, tileIndex;
reference.Decode(polyBits, tileBits, saltBits, out polyIndex, out tileIndex, out salt);
if (tileIndex >= maxTiles)
{
return(false);
}
if (tiles[tileIndex].Salt != salt || tiles[tileIndex].Header == null)
{
return(false);
}
if (polyIndex >= tiles[tileIndex].Header.PolyCount)
{
return(false);
}
return(true);
}
/// <summary>
/// Use an efficient local visibility search to try to optimize the corridor between the current position and the next.
/// </summary>
/// <param name="next">The next postion</param>
/// <param name="pathOptimizationRange">The range</param>
/// <param name="navquery">The NavMeshQuery</param>
public void OptimizePathVisibility(Vector3 next, float pathOptimizationRange, NavMeshQuery navquery)
{
//clamp the ray to max distance
Vector3 goal = next;
float dist = Vector3Extensions.Distance2D(pos, goal);
//if too close to the goal, do not try to optimize
if (dist < 0.01f)
{
return;
}
dist = Math.Min(dist + 0.01f, pathOptimizationRange);
//adjust ray length
Vector3 delta = goal - pos;
goal = pos + delta * (pathOptimizationRange / dist);
const int MaxRes = 32;
PolyId[] res = new PolyId[MaxRes];
float t = 0;
Vector3 norm = new Vector3();
int nres = 0;
navquery.Raycast(new NavPoint(path[0], pos), goal, ref t, ref norm, res, ref nres, MaxRes);
if (nres > 1 && t > 0.99f)
{
pathCount = MergeCorridorStartShortcut(path, pathCount, maxPath, res, nres);
}
}
/// <summary>
/// Check if polygon reference is valid.
/// </summary>
/// <param name="reference">Polygon reference</param>
/// <returns>True if valid</returns>
public bool IsValidPolyRef(PolyId reference)
{
if (reference == PolyId.Null)
{
return(false);
}
int salt, polyIndex, tileIndex;
idManager.Decode(ref reference, out polyIndex, out tileIndex, out salt);
if (tileIndex >= maxTiles)
{
return(false);
}
if (tileList[tileIndex].Salt != salt)
{
return(false);
}
if (polyIndex >= tileList[tileIndex].PolyCount)
{
return(false);
}
return(true);
}
/// <summary>
/// Move along the NavMeshQuery and update the position
/// </summary>
/// <param name="npos">Current position</param>
/// <param name="navquery">The NavMeshQuery</param>
/// <returns>True if position changed, false if not</returns>
public bool MovePosition(Vector3 npos, NavMeshQuery navquery)
{
//move along navmesh and update new position
Vector3 result = new Vector3();
const int MaxVisited = 16;
PolyId[] visited = new PolyId[MaxVisited];
List <PolyId> listVisited = new List <PolyId>(MaxVisited);
bool status = navquery.MoveAlongSurface(new NavPoint(path[0], pos), npos, ref result, listVisited);
visited = listVisited.ToArray(); //HACK why?
if (status == true)
{
pathCount = MergeCorridorStartMoved(path, pathCount, maxPath, visited, listVisited.Count);
//adjust the position to stay on top of the navmesh
float h = pos.Y;
navquery.GetPolyHeight(path[0], result, ref h);
result.Y = h;
pos = result;
return(true);
}
return(false);
}
public void Reset(PolyId reference, Vector3 nearest)
{
this.corridor.Reset(reference, nearest);
this.boundary.Reset();
this.partial = false;
this.topologyOptTime = 0;
this.TargetReplanTime = 0;
this.numNeis = 0;
this.DesiredVel = new Vector3(0.0f, 0.0f, 0.0f);
this.NVel = new Vector3(0.0f, 0.0f, 0.0f);
this.Vel = new Vector3(0.0f, 0.0f, 0.0f);
this.currentPos = nearest;
this.DesiredSpeed = 0;
if (reference != PolyId.Null)
{
this.state = AgentState.Walking;
}
else
{
this.state = AgentState.Invalid;
}
this.TargetState = TargetState.None;
}
public void SetPolyIndex(ref PolyId polyBase, int newPoly, out PolyId result)
{
newPoly &= polyMask;
//first clear poly then OR with new poly
result = new PolyId((polyBase.Id & ~polyMask) | newPoly);
}
/// <summary>
/// Try to find the node. If it doesn't exist, create a new node.
/// </summary>
/// <param name="id">Node's id</param>
/// <returns>The node</returns>
public Node GetNode(PolyId id)
{
Node node;
if (nodeDict.TryGetValue(id, out node))
{
return(node);
}
if (nodes.Count >= maxNodes)
{
return(null);
}
Node newNode = new Node();
newNode.ParentIdx = 0;
newNode.cost = 0;
newNode.total = 0;
newNode.Id = id;
newNode.Flags = 0;
nodes.Add(newNode);
nodeDict.Add(id, newNode);
return(newNode);
}
public void AddTileAt(MeshTile tile, PolyId id)
{
//TODO more error checking, what if tile already exists?
Vector2i loc = tile.Location;
List <MeshTile> locList;
if (!tileSet.TryGetValue(loc, out locList))
{
locList = new List <MeshTile>();
locList.Add(tile);
tileSet.Add(loc, locList);
}
else
{
locList.Add(tile);
}
tileRefs.Add(tile, id);
int index = idManager.DecodeTileIndex(ref id);
//HACK this is pretty bad but only way to insert at index
//TODO tileIndex should have a level of indirection from the list?
while (index >= tileList.Count)
{
tileList.Add(null);
}
tileList[index] = tile;
}
/// <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 PolyId 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 <PolyId> polys = new List <PolyId>(128);
int polyCount = QueryPolygonsInTile(tile, bounds, polys);
//Find nearest polygon amongst the nearby polygons
PolyId nearest = PolyId.Null;
float nearestDistanceSqr = float.MaxValue;
//Iterate throuh all the polygons
for (int i = 0; i < polyCount; i++)
{
PolyId reference = polys[i];
Vector3 closestPtPoly = new Vector3();
tile.ClosestPointOnPoly(reference.DecodePolyIndex(polyBits), center, ref closestPtPoly);
float d = (center - closestPtPoly).LengthSquared();
if (d < nearestDistanceSqr)
{
nearestPt = closestPtPoly;
nearestDistanceSqr = d;
nearest = reference;
}
}
return(nearest);
}
private JObject SerializeMeshTile(MeshTile tile, PolyId id)
{
var result = new JObject();
result.Add("polyId", JToken.FromObject(id, serializer));
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);
}
/// <summary>
/// Use a local area path search to try to reoptimize this corridor
/// </summary>
/// <param name="navquery">The NavMeshQuery</param>
/// <returns>True if optimized, false if not</returns>
public bool OptimizePathTopology(NavMeshQuery navquery)
{
if (pathCount < 3)
{
return(false);
}
const int MaxIter = 32;
const int MaxRes = 32;
PolyId[] res = new PolyId[MaxRes];
int numRes = 0;
int tempInt = 0;
navquery.InitSlicedFindPath(new NavPoint(path[0], pos), new NavPoint(path[pathCount - 1], target));
navquery.UpdateSlicedFindPath(MaxIter, ref tempInt);
bool status = navquery.FinalizedSlicedPathPartial(path, pathCount, res, ref numRes, MaxRes);
if (status == true && numRes > 0)
{
pathCount = MergeCorridorStartShortcut(path, pathCount, maxPath, res, numRes);
return(true);
}
return(false);
}
public MeshTile this[PolyId id]
{
get
{
int index = idManager.DecodeTileIndex(ref id);
return(this[index]);
}
}
/// <summary>
/// Decode a standard polygon reference.
/// </summary>
/// <param name="polyBits">The number of bits to use for the polygon value.</param>
/// <param name="tileBits">The number of bits to use for the tile value.</param>
/// <param name="saltBits">The number of bits to use for the salt.</param>
/// <param name="polyIndex">Resulting poly index.</param>
/// <param name="tileIndex">Resulting tile index.</param>
/// <param name="salt">Resulting salt value.</param>
public void Decode(ref PolyId id, out int polyIndex, out int tileIndex, out int salt)
{
int bits = id.Id;
salt = (bits >> saltOffset) & saltMask;
tileIndex = (bits >> tileOffset) & tileMask;
polyIndex = bits & polyMask;
}
/// <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>
/// Derive a standard polygon reference, which compresses salt, tile index, and poly index together.
/// </summary>
/// <param name="polyBits">The number of bits to use for the polygon value.</param>
/// <param name="tileBits">The number of bits to use for the tile value.</param>
/// <param name="salt">Salt value</param>
/// <param name="tileIndex">Tile index</param>
/// <param name="polyIndex">Poly index</param>
/// <returns>Polygon reference</returns>
public void Encode(int salt, int tileIndex, int polyIndex, out PolyId result)
{
polyIndex &= polyMask;
tileIndex &= tileMask;
salt &= saltMask;
result = new PolyId((salt << saltOffset) | (tileIndex << tileOffset) | polyIndex);
}
/// <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>
/// Decode a standard polygon reference.
/// </summary>
/// <param name="polyBits">The number of bits to use for the polygon value.</param>
/// <param name="tileBits">The number of bits to use for the tile value.</param>
/// <param name="saltBits">The number of bits to use for the salt.</param>
/// <param name="polyIndex">Resulting poly index.</param>
/// <param name="tileIndex">Resulting tile index.</param>
/// <param name="salt">Resulting salt value.</param>
public void Decode(ref PolyId id, out int polyIndex, out int tileIndex, out int salt)
{
int bits = id.Id;
salt = (bits >> saltOffset) & saltMask;
tileIndex = (bits >> tileOffset) & tileMask;
polyIndex = bits & polyMask;
}
/// <summary>
/// Derive a standard polygon reference, which compresses salt, tile index, and poly index together.
/// </summary>
/// <param name="polyBits">The number of bits to use for the polygon value.</param>
/// <param name="tileBits">The number of bits to use for the tile value.</param>
/// <param name="salt">Salt value</param>
/// <param name="tileIndex">Tile index</param>
/// <param name="polyIndex">Poly index</param>
/// <returns>Polygon reference</returns>
public void Encode(int salt, int tileIndex, int polyIndex, out PolyId result)
{
polyIndex &= polyMask;
tileIndex &= tileMask;
salt &= saltMask;
result = new PolyId((salt << saltOffset) | (tileIndex << tileOffset) | polyIndex);
}
/// <summary>
/// Reset the move target of an agent
/// </summary>
public void ResetMoveTarget()
{
//initialize request
this.TargetRef = PolyId.Null;
this.targetPos = new Vector3(0.0f, 0.0f, 0.0f);
this.TargetPathQueryIndex = PathQueue.Invalid;
this.TargetReplan = false;
this.targetState = TargetState.None;
}
/// <summary>
/// Request a new move velocity
/// </summary>
/// <param name="vel">The agent's velocity</param>
public void RequestMoveVelocity(Vector3 vel)
{
//initialize request
this.TargetRef = PolyId.Null;
this.targetPos = vel;
this.TargetPathQueryIndex = PathQueue.Invalid;
this.TargetReplan = false;
this.targetState = TargetState.Velocity;
}
/// <summary>
/// Try to find a node.
/// </summary>
/// <param name="id">Node's id</param>
/// <returns>The node, if found. Null, if otherwise.</returns>
public Node FindNode(PolyId id)
{
Node node;
if (nodeDict.TryGetValue(id, out node))
{
return node;
}
return null;
}
/// <summary>
/// Try to find a node.
/// </summary>
/// <param name="id">Node's id</param>
/// <returns>The node, if found. Null, if otherwise.</returns>
public Node FindNode(PolyId id)
{
Node node;
if (nodeDict.TryGetValue(id, out node))
{
return(node);
}
return(null);
}
/// <summary>
/// Retrieve the endpoints of the offmesh connection at the specified polygon
/// </summary>
/// <param name="prevRef">The previous polygon reference</param>
/// <param name="polyRef">The current polygon reference</param>
/// <param name="startPos">The starting position</param>
/// <param name="endPos">The ending position</param>
/// <returns>True if endpoints found, false if not</returns>
public bool GetOffMeshConnectionPolyEndPoints(PolyId prevRef, PolyId polyRef, ref Vector3 startPos, ref Vector3 endPos)
{
int salt = 0, indexTile = 0, indexPoly = 0;
if (polyRef == PolyId.Null)
{
return(false);
}
//get current polygon
idManager.Decode(ref polyRef, out indexPoly, out indexTile, out salt);
if (indexTile >= maxTiles)
{
return(false);
}
if (tileList[indexTile].Salt != salt)
{
return(false);
}
MeshTile tile = tileList[indexTile];
if (indexPoly >= tile.PolyCount)
{
return(false);
}
Poly poly = tile.Polys[indexPoly];
if (poly.PolyType != PolygonType.OffMeshConnection)
{
return(false);
}
int idx0 = 0, idx1 = 1;
//find the link that points to the first vertex
foreach (Link link in poly.Links)
{
if (link.Edge == 0)
{
if (link.Reference != prevRef)
{
idx0 = 1;
idx1 = 0;
}
break;
}
}
startPos = tile.Verts[poly.Verts[idx0]];
endPos = tile.Verts[poly.Verts[idx1]];
return(true);
}
/// <summary>
/// Retrieve the endpoints of the offmesh connection at the specified polygon
/// </summary>
/// <param name="prevRef">The previous polygon reference</param>
/// <param name="polyRef">The current polygon reference</param>
/// <param name="startPos">The starting position</param>
/// <param name="endPos">The ending position</param>
/// <returns>True if endpoints found, false if not</returns>
public bool GetOffMeshConnectionPolyEndPoints(PolyId prevRef, PolyId polyRef, ref Vector3 startPos, ref Vector3 endPos)
{
int salt = 0, indexTile = 0, indexPoly = 0;
if (polyRef == PolyId.Null)
{
return(false);
}
//get current polygon
polyRef.Decode(polyBits, tileBits, saltBits, out indexPoly, out indexTile, out salt);
if (indexTile >= maxTiles)
{
return(false);
}
if (tiles[indexTile].Salt != salt || tiles[indexTile].Header == null)
{
return(false);
}
MeshTile tile = tiles[indexTile];
if (indexPoly >= tile.Header.PolyCount)
{
return(false);
}
Poly poly = tile.Polys[indexPoly];
if (poly.PolyType != PolygonType.OffMeshConnection)
{
return(false);
}
int idx0 = 0, idx1 = 1;
//find the link that points to the first vertex
for (int i = poly.FirstLink; i != Link.Null; i = tile.Links[i].Next)
{
if (tile.Links[i].Edge == 0)
{
if (tile.Links[i].Reference != prevRef)
{
idx0 = 1;
idx1 = 0;
}
break;
}
}
startPos = tile.Verts[poly.Verts[idx0]];
endPos = tile.Verts[poly.Verts[idx1]];
return(true);
}
/// <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;
}
PolyId 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
PolyId id;
PolyId.SetPolyIndex(ref polyBase, tile.Polys[i].Neis[j] - 1, out id);
tile.Links[idx].Reference = id;
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;
}
}
}
}
/// <summary>
/// Change the move target
/// </summary>
/// <param name="reference">The polygon reference</param>
/// <param name="pos">The target's coordinates</param>
public void RequestMoveTargetReplan(PolyId reference, Vector3 pos)
{
//initialize request
this.TargetRef = reference;
this.targetPos = pos;
this.TargetPathQueryIndex = PathQueue.Invalid;
this.TargetReplan = true;
if (this.TargetRef != PolyId.Null)
{
this.TargetState = TargetState.Requesting;
}
else
{
this.TargetState = TargetState.Failed;
}
}
private bool GetSteerTarget(NavMeshQuery navMeshQuery, SVector3 startPos, SVector3 endPos, float minTargetDist, PolyId[] path, int pathSize,
ref SVector3 steerPos, ref int steerPosFlag, ref PolyId steerPosRef)
{
int MAX_STEER_POINTS = 3;
SVector3[] steerPath = new SVector3[MAX_STEER_POINTS];
int[] steerPathFlags = new int[MAX_STEER_POINTS];
PolyId[] steerPathPolys = new PolyId[MAX_STEER_POINTS];
int nsteerPath = 0;
navMeshQuery.FindStraightPath(startPos, endPos, path, pathSize,
steerPath, steerPathFlags, steerPathPolys, ref nsteerPath, MAX_STEER_POINTS, 0);
if (nsteerPath == 0)
{
return(false);
}
//find vertex far enough to steer to
int ns = 0;
while (ns < nsteerPath)
{
if ((steerPathFlags[ns] & PathfindingCommon.STRAIGHTPATH_OFFMESH_CONNECTION) != 0 ||
!InRange(steerPath[ns], startPos, minTargetDist, 1000.0f))
{
break;
}
ns++;
}
//failed to find good point to steer to
if (ns >= nsteerPath)
{
return(false);
}
steerPos = steerPath[ns];
steerPos.Y = startPos.Y;
steerPosFlag = steerPathFlags[ns];
steerPosRef = steerPathPolys[ns];
return(true);
}
/// <summary>
/// Adjust the beginning of the path
/// </summary>
/// <param name="safeRef">The starting polygon reference</param>
/// <param name="safePos">The starting position</param>
/// <returns>True if path start changed, false if not</returns>
public bool FixPathStart(PolyId safeRef, Vector3 safePos)
{
pos = safePos;
if (pathCount < 3 && pathCount > 0)
{
path[2] = path[pathCount - 1];
path[0] = safeRef;
path[1] = PolyId.Null;
pathCount = 3;
}
else
{
path[0] = safeRef;
path[1] = PolyId.Null;
}
return(true);
}
private int polyBits; //number of poly bits in ID
/// <summary>
/// Initializes a new instance of the <see cref="TiledNavMesh"/> class.
/// </summary>
/// <param name="data">The Navigation Mesh data</param>
public TiledNavMesh(NavMeshBuilder data)
{
this.origin = data.Header.Bounds.Min;
this.tileWidth = data.Header.Bounds.Max.X - data.Header.Bounds.Min.X;
this.tileHeight = data.Header.Bounds.Max.Z - data.Header.Bounds.Min.Z;
this.maxTiles = 1;
this.maxPolys = data.Header.PolyCount;
if (!InitTileNavMesh())
{
return;
}
//HACK is tileRef actually being used for anything?
PolyId tileRef = PolyId.Null;
AddTile(data, PolyId.Null, out tileRef);
}
/// <summary>
/// Get the base reference for the polygons in a tile.
/// </summary>
/// <param name="tile">Current Tile</param>
/// <returns>Base poly reference</returns>
public PolyId GetPolyRefBase(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>
/// Try to find the node. If it doesn't exist, create a new node.
/// </summary>
/// <param name="id">Node's id</param>
/// <returns>The node</returns>
public Node GetNode(PolyId id)
{
Node node;
if (nodeDict.TryGetValue(id, out node))
{
return node;
}
if (nodes.Count >= maxNodes)
return null;
Node newNode = new Node();
newNode.ParentIdx = 0;
newNode.cost = 0;
newNode.total = 0;
newNode.Id = id;
newNode.Flags = 0;
nodes.Add(newNode);
nodeDict.Add(id, newNode);
return newNode;
}
public bool IsValidPolyRef(PolyId reference)
{
MeshTile tile;
Poly poly;
bool status = nav.TryGetTileAndPolyByRef(reference, out tile, out poly);
if (status == false)
return false;
return true;
}
/// <summary>
/// Insert a segment into the array
/// </summary>
/// <param name="ints">The array of segments</param>
/// <param name="nints">The number of segments</param>
/// <param name="maxInts">The maximium number of segments allowed</param>
/// <param name="tmin">Parameter t minimum</param>
/// <param name="tmax">Parameter t maximum</param>
/// <param name="reference">Polygon reference</param>
public void InsertInterval(SegInterval[] ints, ref int nints, int maxInts, int tmin, int tmax, PolyId reference)
{
if (nints + 1 > maxInts)
return;
//find insertion point
int idx = 0;
while (idx < nints)
{
if (tmax <= ints[idx].TMin)
break;
idx++;
}
//move current results
if (nints - idx > 0)
{
for (int i = 0; i < nints - idx; i++)
ints[idx + 1 + i] = ints[idx + i];
}
//store
ints[idx].Reference = reference;
ints[idx].TMin = tmin;
ints[idx].TMax = tmax;
nints++;
}
/// <summary>
/// Add a vertex to the straight path.
/// </summary>
/// <param name="pos"></param>
/// <param name="flags"></param>
/// <param name="reference"></param>
/// <param name="straightPath">An array of points on the straight path</param>
/// <param name="straightPathFlags">An array of flags</param>
/// <param name="straightPathRefs">An array of polygon references</param>
/// <param name="straightPathCount">The number of points on the path</param>
/// <param name="maxStraightPath">The maximum length allowed for the straight path</param>
/// <returns>True, if end of path hasn't been reached yet and path isn't full. False, if otherwise.</returns>
public bool AppendVertex(Vector3 pos, int flags, PolyId reference, Vector3[] straightPath, int[] straightPathFlags, PolyId[] straightPathRefs, ref int straightPathCount, int maxStraightPath)
{
if (straightPathCount > 0 && straightPath[straightPathCount - 1] == pos)
{
//the vertices are equal
//update flags and polys
if (straightPathFlags.Length != 0)
straightPathFlags[straightPathCount - 1] = flags;
if (straightPathRefs.Length != 0)
straightPathRefs[straightPathCount - 1] = reference;
}
else
{
//append new vertex
straightPath[straightPathCount] = pos;
if (straightPathFlags.Length != 0)
straightPathFlags[straightPathCount] = flags;
if (straightPathRefs.Length != 0)
straightPathRefs[straightPathCount] = reference;
straightPathCount++;
if (flags == PathfindingCommon.STRAIGHTPATH_END || straightPathCount >= maxStraightPath)
{
return false;
}
}
return true;
}
/// <summary>
/// Adjust the beginning of the path
/// </summary>
/// <param name="safeRef">The starting polygon reference</param>
/// <param name="safePos">The starting position</param>
/// <returns>True if path start changed, false if not</returns>
public bool FixPathStart(PolyId safeRef, Vector3 safePos)
{
pos = safePos;
if (pathCount < 3 && pathCount > 0)
{
path[2] = path[pathCount - 1];
path[0] = safeRef;
path[1] = PolyId.Null;
pathCount = 3;
}
else
{
path[0] = safeRef;
path[1] = PolyId.Null;
}
return true;
}
/// <summary>
/// Resets the path to the first polygon.
/// </summary>
/// <param name="reference">The starting polygon reference</param>
/// <param name="pos">Starting position</param>
public void Reset(PolyId reference, Vector3 pos)
{
this.pos = pos;
this.target = pos;
this.path[0] = reference;
this.pathCount = 1;
}
/// <summary>
/// Save the sliced path
/// </summary>
/// <param name="path">The path in terms of polygon references</param>
/// <param name="pathCount">The path length</param>
/// <param name="maxPath">The maximum path length allowed</param>
/// <returns>True if the path is saved, false if not</returns>
public bool FinalizeSlicedFindPath(PolyId[] path, ref int pathCount, int maxPath)
{
pathCount = 0;
if (query.Status == false)
{
query = new QueryData();
return false;
}
int n = 0;
if (query.Start.Polygon == query.End.Polygon)
{
//special case: the search starts and ends at the same poly
path[n++] = query.Start.Polygon;
}
else
{
//reverse the path
Node prev = null;
Node node = query.LastBestNode;
do
{
Node next = nodePool.GetNodeAtIdx(node.ParentIdx);
node.ParentIdx = nodePool.GetNodeIdx(prev);
prev = node;
node = next;
}
while (node != null);
//store path
node = prev;
do
{
path[n++] = node.Id;
if (n >= maxPath)
break;
node = nodePool.GetNodeAtIdx(node.ParentIdx);
}
while (node != null);
}
//reset query
query = new QueryData();
//remember to update the path length
pathCount = n;
return true;
}
/// <summary>
/// Given a point on a polygon, find the closest point which lies on the polygon boundary.
/// </summary>
/// <param name="reference">Polygon reference</param>
/// <param name="pos">Current position</param>
/// <param name="closest">Resulting closest point</param>
/// <returns>True, if the closest point is found. False, if otherwise.</returns>
public bool ClosestPointOnPolyBoundary(PolyId reference, Vector3 pos, ref Vector3 closest)
{
MeshTile tile;
Poly poly;
if (nav.TryGetTileAndPolyByRef(reference, out tile, out poly) == false)
return false;
tile.ClosestPointOnPolyBoundary(poly, pos, out closest);
return true;
}
/// <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>
/// Add vertices and portals to a regular path computed from the method FindPath().
/// </summary>
/// <param name="startPos">Starting position</param>
/// <param name="endPos">Ending position</param>
/// <param name="path">Path of polygon references</param>
/// <param name="pathSize">Length of path</param>
/// <param name="straightPath">An array of points on the straight path</param>
/// <param name="straightPathFlags">An array of flags</param>
/// <param name="straightPathRefs">An array of polygon references</param>
/// <param name="straightPathCount">The number of points on the path</param>
/// <param name="maxStraightPath">The maximum length allowed for the straight path</param>
/// <param name="options">Options flag</param>
/// <returns>True, if path found. False, if otherwise.</returns>
public bool FindStraightPath(Vector3 startPos, Vector3 endPos, PolyId[] path, int pathSize, Vector3[] straightPath, int[] straightPathFlags, PolyId[] straightPathRefs, ref int straightPathCount, int maxStraightPath, PathBuildFlags options)
{
straightPathCount = 0;
if (path.Length == 0)
return false;
bool stat = false;
Vector3 closestStartPos = new Vector3();
ClosestPointOnPolyBoundary(path[0], startPos, ref closestStartPos);
Vector3 closestEndPos = new Vector3();
ClosestPointOnPolyBoundary(path[pathSize - 1], endPos, ref closestEndPos);
stat = AppendVertex(closestStartPos, PathfindingCommon.STRAIGHTPATH_START, path[0], straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath);
if (!stat)
return true;
if (pathSize > 1)
{
Vector3 portalApex = closestStartPos;
Vector3 portalLeft = portalApex;
Vector3 portalRight = portalApex;
int apexIndex = 0;
int leftIndex = 0;
int rightIndex = 0;
PolygonType leftPolyType = 0;
PolygonType rightPolyType = 0;
PolyId leftPolyRef = path[0];
PolyId rightPolyRef = path[0];
for (int i = 0; i < pathSize; i++)
{
Vector3 left = new Vector3();
Vector3 right = new Vector3();
PolygonType fromType = 0, toType = 0;
if (i + 1 < pathSize)
{
//next portal
if (GetPortalPoints(path[i], path[i + 1], ref left, ref right, ref fromType, ref toType) == false)
{
//failed to get portal points means path[i + 1] is an invalid polygon
//clamp end point to path[i] and return path so far
if (ClosestPointOnPolyBoundary(path[i], endPos, ref closestEndPos) == false)
{
//first polygon is invalid
return false;
}
if ((options & (PathBuildFlags.AreaCrossingVertices | PathBuildFlags.AllCrossingVertices)) != 0)
{
//append portals
stat = AppendPortals(apexIndex, i, closestEndPos, path, straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath, options);
}
stat = AppendVertex(closestEndPos, 0, path[i], straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath);
return true;
}
//if starting really close to the portal, advance
if (i == 0)
{
float t;
if (Distance.PointToSegment2DSquared(ref portalApex, ref left, ref right, out t) < 0.001 * 0.001)
continue;
}
}
else
{
//end of the path
left = closestEndPos;
right = closestEndPos;
fromType = toType = PolygonType.Ground;
}
//right vertex
float triArea2D;
Triangle3.Area2D(ref portalApex, ref portalRight, ref right, out triArea2D);
if (triArea2D <= 0.0)
{
Triangle3.Area2D(ref portalApex, ref portalLeft, ref right, out triArea2D);
if (portalApex == portalRight || triArea2D > 0.0)
{
portalRight = right;
rightPolyRef = (i + 1 < pathSize) ? path[i + 1] : PolyId.Null;
rightPolyType = toType;
rightIndex = i;
}
else
{
//append portals along current straight path segment
if ((options & (PathBuildFlags.AreaCrossingVertices | PathBuildFlags.AllCrossingVertices)) != 0)
{
stat = AppendPortals(apexIndex, leftIndex, portalLeft, path, straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath, options);
if (stat != true)
return true;
}
portalApex = portalLeft;
apexIndex = leftIndex;
int flags = 0;
if (leftPolyRef == PolyId.Null)
flags = PathfindingCommon.STRAIGHTPATH_END;
else if (leftPolyType == PolygonType.OffMeshConnection)
flags = PathfindingCommon.STRAIGHTPATH_OFFMESH_CONNECTION;
PolyId reference = leftPolyRef;
//append or update vertex
stat = AppendVertex(portalApex, flags, reference, straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath);
if (stat != true)
return true;
portalLeft = portalApex;
portalRight = portalApex;
leftIndex = apexIndex;
rightIndex = apexIndex;
//restart
i = apexIndex;
continue;
}
}
//left vertex
Triangle3.Area2D(ref portalApex, ref portalLeft, ref left, out triArea2D);
if (triArea2D >= 0.0)
{
Triangle3.Area2D(ref portalApex, ref portalRight, ref left, out triArea2D);
if (portalApex == portalLeft || triArea2D < 0.0f)
{
portalLeft = left;
leftPolyRef = (i + 1 < pathSize) ? path[i + 1] : PolyId.Null;
leftPolyType = toType;
leftIndex = i;
}
else
{
if ((options & (PathBuildFlags.AreaCrossingVertices | PathBuildFlags.AllCrossingVertices)) != 0)
{
stat = AppendPortals(apexIndex, rightIndex, portalRight, path, straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath, options);
if (stat != true)
return true;
}
portalApex = portalRight;
apexIndex = rightIndex;
int flags = 0;
if (rightPolyRef == PolyId.Null)
flags = PathfindingCommon.STRAIGHTPATH_END;
else if (rightPolyType == PolygonType.OffMeshConnection)
flags = PathfindingCommon.STRAIGHTPATH_OFFMESH_CONNECTION;
PolyId reference = rightPolyRef;
//append or update vertex
stat = AppendVertex(portalApex, flags, reference, straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath);
if (stat != true)
return true;
portalLeft = portalApex;
portalRight = portalApex;
leftIndex = apexIndex;
rightIndex = apexIndex;
//restart
i = apexIndex;
continue;
}
}
}
//append portals along the current straight line segment
if ((options & (PathBuildFlags.AreaCrossingVertices | PathBuildFlags.AllCrossingVertices)) != 0)
{
stat = AppendPortals(apexIndex, pathSize - 1, closestEndPos, path, straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath, options);
if (stat != true)
return true;
}
}
stat = AppendVertex(closestEndPos, PathfindingCommon.STRAIGHTPATH_END, PolyId.Null, straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath);
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>
/// <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>
/// 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>
/// Update the vertices on the straight path
/// </summary>
/// <param name="startIdx">Original path's starting index</param>
/// <param name="endIdx">Original path's end index</param>
/// <param name="endPos">The end position</param>
/// <param name="path">The original path of polygon references</param>
/// <param name="straightPath">An array of points on the straight path</param>
/// <param name="straightPathFlags">An array of flags</param>
/// <param name="straightPathRefs">An array of polygon references</param>
/// <param name="straightPathCount">The number of points on the path</param>
/// <param name="maxStraightPath">The maximum length allowed for the straight path</param>
/// <param name="options">Options flag</param>
/// <returns></returns>
public bool AppendPortals(int startIdx, int endIdx, Vector3 endPos, PolyId[] path, Vector3[] straightPath, int[] straightPathFlags, PolyId[] straightPathRefs, ref int straightPathCount, int maxStraightPath, PathBuildFlags options)
{
Vector3 startPos = straightPath[straightPathCount - 1];
//append or update last vertex
bool stat = false;
for (int i = startIdx; i < endIdx; i++)
{
//calculate portal
PolyId from = path[i];
MeshTile fromTile;
Poly fromPoly;
if (nav.TryGetTileAndPolyByRef(from, out fromTile, out fromPoly) == false)
return false;
PolyId to = path[i + 1];
MeshTile toTile;
Poly toPoly;
if (nav.TryGetTileAndPolyByRef(to, out toTile, out toPoly) == false)
return false;
Vector3 left = new Vector3();
Vector3 right = new Vector3();
if (GetPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, ref left, ref right) == false)
break;
if ((options & PathBuildFlags.AreaCrossingVertices) != 0)
{
//skip intersection if only area crossings are requested
if (fromPoly.Area == toPoly.Area)
continue;
}
//append intersection
float s, t;
if (Intersection.SegmentSegment2D(ref startPos, ref endPos, ref left, ref right, out s, out t))
{
Vector3 pt = Vector3.Lerp(left, right, t);
stat = AppendVertex(pt, 0, path[i + 1], straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath);
if (stat != true)
return true;
}
}
return true;
}
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;
}
public bool MoveOverOffmeshConnection(PolyId offMeshConRef, PolyId[] refs, ref Vector3 startPos, ref Vector3 endPos, NavMeshQuery navquery)
{
//advance the path up to and over the off-mesh connection
PolyId prevRef = PolyId.Null, polyRef = path[0];
int npos = 0;
while (npos < pathCount && polyRef != offMeshConRef)
{
prevRef = polyRef;
polyRef = path[npos];
npos++;
}
if (npos == pathCount)
{
//could not find offMeshConRef
return false;
}
//prune path
for (int i = npos; i < pathCount; i++)
path[i - npos] = path[i];
pathCount -= npos;
refs[0] = prevRef;
refs[1] = polyRef;
TiledNavMesh nav = navquery.NavMesh;
if (nav.GetOffMeshConnectionPolyEndPoints(refs[0], refs[1], ref startPos, ref endPos) == true)
{
pos = endPos;
return true;
}
return false;
}
public bool Raycast(NavPoint startPoint, Vector3 endPos, ref float t, ref Vector3 hitNormal, PolyId[] path, ref int pathCount, int maxPath)
{
t = 0;
pathCount = 0;
//validate input
if (startPoint.Polygon == PolyId.Null || !nav.IsValidPolyRef(startPoint.Polygon))
return false;
PolyId curRef = startPoint.Polygon;
Vector3[] verts = new Vector3[PathfindingCommon.VERTS_PER_POLYGON];
int n = 0;
hitNormal = new Vector3(0, 0, 0);
while (curRef != PolyId.Null)
{
//cast ray against current polygon
MeshTile tile;
Poly poly;
nav.TryGetTileAndPolyByRefUnsafe(curRef, out tile, out poly);
//collect vertices
int nv = 0;
for (int i = 0; i < poly.VertCount; i++)
{
verts[nv] = tile.Verts[poly.Verts[i]];
nv++;
}
float tmin, tmax;
int segMin, segMax;
if (!Intersection.SegmentPoly2D(startPoint.Position, endPos, verts, nv, out tmin, out tmax, out segMin, out segMax))
{
//could not hit the polygon, keep the old t and report hit
pathCount = n;
return true;
}
//keep track of furthest t so far
if (tmax > t)
t = tmax;
//store visited polygons
if (n < maxPath)
path[n++] = curRef;
//ray end is completely inside the polygon
if (segMax == -1)
{
t = float.MaxValue;
pathCount = n;
return true;
}
//follow neighbours
PolyId nextRef = PolyId.Null;
for (int i = poly.FirstLink; i != Link.Null; i = tile.Links[i].Next)
{
Link link = tile.Links[i];
//find link which contains the edge
if (link.Edge != segMax)
continue;
//get pointer to the next polygon
MeshTile nextTile;
Poly nextPoly;
nav.TryGetTileAndPolyByRefUnsafe(link.Reference, out nextTile, out nextPoly);
//skip off-mesh connection
if (nextPoly.PolyType == PolygonType.OffMeshConnection)
continue;
//if the link is internal, just return the ref
if (link.Side == BoundarySide.Internal)
{
nextRef = link.Reference;
break;
}
//if the link is at the tile boundary
//check if the link spans the whole edge and accept
if (link.BMin == 0 && link.BMax == 255)
{
nextRef = link.Reference;
break;
}
//check for partial edge links
int v0 = poly.Verts[link.Edge];
int v1 = poly.Verts[(link.Edge + 1) % poly.VertCount];
Vector3 left = tile.Verts[v0];
Vector3 right = tile.Verts[v1];
//check that the intersection lies inside the link portal
if (link.Side == BoundarySide.PlusX || link.Side == BoundarySide.MinusX)
{
//calculate link size
float s = 1.0f / 255.0f;
float lmin = left.Z + (right.Z - left.Z) * (link.BMin * s);
float lmax = left.Z + (right.Z - left.Z) * (link.BMax * s);
if (lmin > lmax)
{
//swap
float temp = lmin;
lmin = lmax;
lmax = temp;
}
//find z intersection
float z = startPoint.Position.Z + (endPos.Z - startPoint.Position.Z) * tmax;
if (z >= lmin && z <= lmax)
{
nextRef = link.Reference;
break;
}
}
else if (link.Side == BoundarySide.PlusZ || link.Side == BoundarySide.MinusZ)
{
//calculate link size
float s = 1.0f / 255.0f;
float lmin = left.X + (right.X - left.X) * (link.BMin * s);
float lmax = left.X + (right.X - left.X) * (link.BMax * s);
if (lmin > lmax)
{
//swap
float temp = lmin;
lmin = lmax;
lmax = temp;
}
//find x intersection
float x = startPoint.Position.X + (endPos.X - startPoint.Position.X) * tmax;
if (x >= lmin && x <= lmax)
{
nextRef = link.Reference;
break;
}
}
}
if (nextRef == PolyId.Null)
{
//no neighbour, we hit a wall
//calculate hit normal
int a = segMax;
int b = (segMax + 1) < nv ? segMax + 1 : 0;
Vector3 va = verts[a];
Vector3 vb = verts[b];
float dx = vb.X - va.X;
float dz = vb.Z - va.Z;
hitNormal.X = dz;
hitNormal.Y = 0;
hitNormal.Z = -dx;
hitNormal.Normalize();
pathCount = n;
return true;
}
//no hit, advance to neighbour polygon
curRef = nextRef;
}
pathCount = n;
return true;
}
/// <summary>
/// Given a point on the polygon, find the closest point
/// </summary>
/// <param name="reference">Polygon reference</param>
/// <param name="pos">Current position</param>
/// <param name="closest">Resulting closest position</param>
/// <param name="posOverPoly">Determines whether the position can be found on the polygon</param>
/// <returns>True, if the closest point is found. False, if otherwise.</returns>
public bool ClosestPointOnPoly(PolyId reference, Vector3 pos, out Vector3 closest, out bool posOverPoly)
{
posOverPoly = false;
closest = Vector3.Zero;
MeshTile tile;
Poly poly;
if (!nav.TryGetTileAndPolyByRef(reference, out tile, out poly))
return false;
if (tile == null)
return false;
if (poly.PolyType == PolygonType.OffMeshConnection)
{
Vector3 v0 = tile.Verts[poly.Verts[0]];
Vector3 v1 = tile.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);
return true;
}
int indexPoly = 0;
for (int i = 0; i < tile.Polys.Length; i++)
{
if (tile.Polys[i] == poly)
{
indexPoly = i;
break;
}
}
PolyMeshDetail.MeshData pd = tile.DetailMeshes[indexPoly];
//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] = tile.Verts[poly.Verts[i]];
closest = pos;
if (!Distance.PointToPolygonEdgeSquared(pos, verts, numPolyVerts, edgeDistance, edgeT))
{
//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]);
}
else
{
posOverPoly = false;
}
//find height at the location
for (int j = 0; j < tile.DetailMeshes[indexPoly].TriangleCount; j++)
{
PolyMeshDetail.TriangleData t = tile.DetailTris[pd.TriangleIndex + j];
Vector3 va, vb, vc;
if (t.VertexHash0 < poly.VertCount)
va = tile.Verts[poly.Verts[t.VertexHash0]];
else
va = tile.DetailVerts[pd.VertexIndex + (t.VertexHash0 - poly.VertCount)];
if (t.VertexHash1 < poly.VertCount)
vb = tile.Verts[poly.Verts[t.VertexHash1]];
else
vb = tile.DetailVerts[pd.VertexIndex + (t.VertexHash1 - poly.VertCount)];
if (t.VertexHash2 < poly.VertCount)
vc = tile.Verts[poly.Verts[t.VertexHash2]];
else
vc = tile.DetailVerts[pd.VertexIndex + (t.VertexHash2 - poly.VertCount)];
float h;
if (Distance.PointToTriangle(pos, va, vb, vc, out h))
{
closest.Y = h;
break;
}
}
return true;
}
private int FixupCorridor(PolyId[] path, int npath, int maxPath, List<PolyId> visited)
{
int furthestPath = -1;
int furthestVisited = -1;
//find furhtest common polygon
for (int i = npath - 1; i >= 0; i--)
{
bool found = false;
for (int j = visited.Count - 1; j >= 0; j--)
{
if (path[i] == visited[j])
{
furthestPath = i;
furthestVisited = j;
found = true;
}
}
if (found)
break;
}
//if no intersection found, return current path
if (furthestPath == -1 || furthestVisited == -1)
return npath;
//concatenate paths
//adjust beginning of the buffer to include the visited
int req = visited.Count - furthestVisited;
int orig = Math.Min(furthestPath + 1, npath);
int size = Math.Max(0, npath - orig);
if (req + size > maxPath)
size = maxPath - req;
for (int i = 0; i < size; i++)
path[req + i] = path[orig + i];
//store visited
for (int i = 0; i < req; i++)
path[i] = visited[(visited.Count - 1) - i];
return req + size;
}
/// <summary>
/// Save a partial path
/// </summary>
/// <param name="existing">Existing path</param>
/// <param name="existingSize">Existing path's length</param>
/// <param name="path">New path</param>
/// <param name="pathCount">New path's length</param>
/// <param name="maxPath">Maximum path length allowed</param>
/// <returns>True if path saved, false if not</returns>
public bool FinalizedSlicedPathPartial(PolyId[] existing, int existingSize, PolyId[] path, ref int pathCount, int maxPath)
{
pathCount = 0;
if (existingSize == 0)
{
return false;
}
if (query.Status == false)
{
query = new QueryData();
return false;
}
int n = 0;
if (query.Start.Polygon == query.End.Polygon)
{
//special case: the search starts and ends at the same poly
path[n++] = query.Start.Polygon;
}
else
{
//find furthest existing node that was visited
Node prev = null;
Node node = null;
for (int i = existingSize - 1; i >= 0; i--)
{
node = nodePool.FindNode(existing[i]);
if (node != null)
break;
}
if (node == null)
{
node = query.LastBestNode;
}
//reverse the path
do
{
Node next = nodePool.GetNodeAtIdx(node.ParentIdx);
node.ParentIdx = nodePool.GetNodeIdx(prev);
prev = node;
node = next;
}
while (node != null);
//store path
node = prev;
do
{
path[n++] = node.Id;
if (n >= maxPath)
{
break;
}
node = nodePool.GetNodeAtIdx(node.ParentIdx);
}
while (node != null);
}
//reset query
query = new QueryData();
//remember to update the path length
pathCount = n;
return true;
}
/// <summary>
/// Examine polygons in the NavMeshQuery and add polygon edges
/// </summary>
/// <param name="reference">The starting polygon reference</param>
/// <param name="pos">Current position</param>
/// <param name="collisionQueryRange">Range to query</param>
/// <param name="navquery">The NavMeshQuery</param>
public void Update(PolyId reference, Vector3 pos, float collisionQueryRange, NavMeshQuery navquery)
{
const int MAX_SEGS_PER_POLY = PathfindingCommon.VERTS_PER_POLYGON;
if (reference == PolyId.Null)
{
this.center = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
this.segCount = 0;
this.numPolys = 0;
return;
}
this.center = pos;
//first query non-overlapping polygons
PolyId[] tempArray = new PolyId[polys.Length];
navquery.FindLocalNeighbourhood(new NavPoint(reference, pos), collisionQueryRange, polys, tempArray, ref numPolys, MaxLocalPolys);
//secondly, store all polygon edges
this.segCount = 0;
Segment[] segs = new Segment[MAX_SEGS_PER_POLY];
int numSegs = 0;
for (int j = 0; j < numPolys; j++)
{
tempArray = new PolyId[segs.Length];
navquery.GetPolyWallSegments(polys[j], segs, tempArray, ref numSegs, MAX_SEGS_PER_POLY);
for (int k = 0; k < numSegs; k++)
{
//skip too distant segments
float tseg;
float distSqr = Distance.PointToSegment2DSquared(ref pos, ref segs[k].Start, ref segs[k].End, out tseg);
if (distSqr > collisionQueryRange * collisionQueryRange)
continue;
AddSegment(distSqr, segs[k]);
}
}
}
/// <summary>
/// Store polygons that are within a certain range from the current polygon
/// </summary>
/// <param name="centerPoint">Starting position</param>
/// <param name="radius">Range to search within</param>
/// <param name="resultRef">All the polygons within range</param>
/// <param name="resultParent">Polygon's parents</param>
/// <param name="resultCount">Number of polygons stored</param>
/// <param name="maxResult">Maximum number of polygons allowed</param>
/// <returns>True, unless input is invalid</returns>
public bool FindLocalNeighbourhood(NavPoint centerPoint, float radius, PolyId[] resultRef, PolyId[] resultParent, ref int resultCount, int maxResult)
{
resultCount = 0;
//validate input
if (centerPoint.Polygon == PolyId.Null || !nav.IsValidPolyRef(centerPoint.Polygon))
return false;
int MAX_STACK = 48;
Node[] stack = new Node[MAX_STACK];
int nstack = 0;
tinyNodePool.Clear();
Node startNode = tinyNodePool.GetNode(centerPoint.Polygon);
startNode.ParentIdx = 0;
startNode.Id = centerPoint.Polygon;
startNode.Flags = NodeFlags.Closed;
stack[nstack++] = startNode;
float radiusSqr = radius * radius;
Vector3[] pa = new Vector3[PathfindingCommon.VERTS_PER_POLYGON];
Vector3[] pb = new Vector3[PathfindingCommon.VERTS_PER_POLYGON];
int n = 0;
if (n < maxResult)
{
resultRef[n] = startNode.Id;
resultParent[n] = PolyId.Null;
++n;
}
while (nstack > 0)
{
//pop front
Node curNode = stack[0];
for (int i = 0; i < nstack - 1; i++)
stack[i] = stack[i + 1];
nstack--;
//get poly and tile
PolyId curRef = curNode.Id;
MeshTile curTile;
Poly curPoly;
nav.TryGetTileAndPolyByRefUnsafe(curRef, out curTile, out curPoly);
for (int i = curPoly.FirstLink; i != Link.Null; i = curTile.Links[i].Next)
{
Link link = curTile.Links[i];
PolyId neighbourRef = link.Reference;
//skip invalid neighbours
if (neighbourRef == PolyId.Null)
continue;
//skip if cannot allocate more nodes
Node neighbourNode = tinyNodePool.GetNode(neighbourRef);
if (neighbourNode == null)
continue;
//skip visited
if ((neighbourNode.Flags & NodeFlags.Closed) != 0)
continue;
//expand to neighbour
MeshTile neighbourTile;
Poly neighbourPoly;
nav.TryGetTileAndPolyByRefUnsafe(neighbourRef, out neighbourTile, out neighbourPoly);
//skip off-mesh connections
if (neighbourPoly.PolyType == PolygonType.OffMeshConnection)
continue;
//find edge and calculate distance to edge
Vector3 va = new Vector3();
Vector3 vb = new Vector3();
if (!GetPortalPoints(curRef, curPoly, curTile, neighbourRef, neighbourPoly, neighbourTile, ref va, ref vb))
continue;
//if the circle is not touching the next polygon, skip it
float tseg;
float distSqr = Distance.PointToSegment2DSquared(ref centerPoint.Position, ref va, ref vb, out tseg);
if (distSqr > radiusSqr)
continue;
//mark node visited
neighbourNode.Flags |= NodeFlags.Closed;
neighbourNode.ParentIdx = tinyNodePool.GetNodeIdx(curNode);
//check that the polygon doesn't collide with existing polygons
//collect vertices of the neighbour poly
int npa = neighbourPoly.VertCount;
for (int k = 0; k < npa; k++)
pa[k] = neighbourTile.Verts[neighbourPoly.Verts[k]];
bool overlap = false;
for (int j = 0; j < n; j++)
{
PolyId pastRef = resultRef[j];
//connected polys do not overlap
bool connected = false;
for (int k = curPoly.FirstLink; k != Link.Null; k = curTile.Links[k].Next)
{
if (curTile.Links[k].Reference == pastRef)
{
connected = true;
break;
}
}
if (connected)
continue;
//potentially overlapping
MeshTile pastTile;
Poly pastPoly;
nav.TryGetTileAndPolyByRefUnsafe(pastRef, out pastTile, out pastPoly);
//get vertices and test overlap
int npb = pastPoly.VertCount;
for (int k = 0; k < npb; k++)
pb[k] = pastTile.Verts[pastPoly.Verts[k]];
if (Intersection.PolyPoly2D(pa, npa, pb, npb))
{
overlap = true;
break;
}
}
if (overlap)
continue;
//store poly
if (n < maxResult)
{
resultRef[n] = neighbourRef;
resultParent[n] = curRef;
++n;
}
if (nstack < MAX_STACK)
{
stack[nstack++] = neighbourNode;
}
}
}
resultCount = n;
return true;
}
/// <summary>
/// Find points on the left and right side.
/// </summary>
/// <param name="from">"From" polygon reference</param>
/// <param name="to">"To" polygon reference</param>
/// <param name="left">Point on the left side</param>
/// <param name="right">Point on the right side</param>
/// <param name="fromType">Polygon type of "From" polygon</param>
/// <param name="toType">Polygon type of "To" polygon</param>
/// <returns>True, if points found. False, if otherwise.</returns>
public bool GetPortalPoints(PolyId from, PolyId to, ref Vector3 left, ref Vector3 right, ref PolygonType fromType, ref PolygonType toType)
{
MeshTile fromTile;
Poly fromPoly;
if (nav.TryGetTileAndPolyByRef(from, out fromTile, out fromPoly) == false)
return false;
fromType = fromPoly.PolyType;
MeshTile toTile;
Poly toPoly;
if (nav.TryGetTileAndPolyByRef(to, out toTile, out toPoly) == false)
return false;
toType = toPoly.PolyType;
return GetPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, ref left, ref right);
}
/// <summary>
/// Return false if the provided position is outside the xz-bounds.
/// </summary>
/// <param name="reference">Polygon reference</param>
/// <param name="pos">Current position</param>
/// <param name="height">Resulting polygon height</param>
/// <returns>True, if height found. False, if otherwise.</returns>
public bool GetPolyHeight(PolyId reference, Vector3 pos, ref float height)
{
if (nav == null)
return false;
MeshTile tile;
Poly poly;
if (!nav.TryGetTileAndPolyByRef(reference, out tile, out poly))
return false;
//off-mesh connections don't have detail polygons
if (poly.PolyType == PolygonType.OffMeshConnection)
{
Vector3 closest;
tile.ClosestPointOnPolyOffMeshConnection(poly, pos, out closest);
height = closest.Y;
return true;
}
else
{
int indexPoly = 0;
for (int i = 0; i < tile.Polys.Length; i++)
{
if (tile.Polys[i] == poly)
{
indexPoly = i;
break;
}
}
float h = 0;
if (tile.ClosestHeight(indexPoly, pos, out h))
{
height = h;
return true;
}
}
return false;
}
/// <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;
}
private bool GetSteerTarget(NavMeshQuery navMeshQuery, SVector3 startPos, SVector3 endPos, float minTargetDist, PolyId[] path, int pathSize,
ref SVector3 steerPos, ref int steerPosFlag, ref PolyId steerPosRef)
{
int MAX_STEER_POINTS = 3;
SVector3[] steerPath = new SVector3[MAX_STEER_POINTS];
int[] steerPathFlags = new int[MAX_STEER_POINTS];
PolyId[] steerPathPolys = new PolyId[MAX_STEER_POINTS];
int nsteerPath = 0;
navMeshQuery.FindStraightPath(startPos, endPos, path, pathSize,
steerPath, steerPathFlags, steerPathPolys, ref nsteerPath, MAX_STEER_POINTS, 0);
if (nsteerPath == 0)
return false;
//find vertex far enough to steer to
int ns = 0;
while (ns < nsteerPath)
{
if ((steerPathFlags[ns] & PathfindingCommon.STRAIGHTPATH_OFFMESH_CONNECTION) != 0 ||
!InRange(steerPath[ns], startPos, minTargetDist, 1000.0f))
break;
ns++;
}
//failed to find good point to steer to
if (ns >= nsteerPath)
return false;
steerPos = steerPath[ns];
steerPos.Y = startPos.Y;
steerPosFlag = steerPathFlags[ns];
steerPosRef = steerPathPolys[ns];
return true;
}
/// <summary>
/// Collect all the edges from a polygon.
/// </summary>
/// <param name="reference">The polygon reference</param>
/// <param name="segmentVerts">Segment vertices</param>
/// <param name="segmentRefs">The polygon reference containing the segment</param>
/// <param name="segmentCount">The number of segments stored</param>
/// <param name="maxSegments">The maximum number of segments allowed</param>
/// <returns>True, unless the polygon reference is invalid</returns>
public bool GetPolyWallSegments(PolyId reference, Crowds.LocalBoundary.Segment[] segmentVerts, PolyId[] segmentRefs, ref int segmentCount, int maxSegments)
{
segmentCount = 0;
MeshTile tile;
Poly poly;
if (nav.TryGetTileAndPolyByRef(reference, out tile, out poly) == false)
return false;
int n = 0;
int MAX_INTERVAL = 16;
SegInterval[] ints = new SegInterval[MAX_INTERVAL];
int nints;
bool storePortals = segmentRefs.Length != 0;
for (int i = 0, j = poly.VertCount - 1; i < poly.VertCount; j = i++)
{
//skip non-solid edges
nints = 0;
if ((poly.Neis[j] & Link.External) != 0)
{
//tile border
for (int k = poly.FirstLink; k != Link.Null; k = tile.Links[k].Next)
{
Link link = tile.Links[k];
if (link.Edge == j)
{
if (link.Reference != PolyId.Null)
{
MeshTile neiTile;
Poly neiPoly;
nav.TryGetTileAndPolyByRefUnsafe(link.Reference, out neiTile, out neiPoly);
InsertInterval(ints, ref nints, MAX_INTERVAL, link.BMin, link.BMax, link.Reference);
}
}
}
}
else
{
//internal edge
PolyId neiRef = PolyId.Null;
if (poly.Neis[j] != 0)
{
int idx = poly.Neis[j] - 1;
PolyId id = nav.GetPolyRefBase(tile);
PolyId.SetPolyIndex(ref id, idx, out neiRef);
}
//if the edge leads to another polygon and portals are not stored, skip
if (neiRef != PolyId.Null && !storePortals)
continue;
if (n < maxSegments)
{
Vector3 vj = tile.Verts[poly.Verts[j]];
Vector3 vi = tile.Verts[poly.Verts[i]];
segmentVerts[n].Start = vj;
segmentVerts[n].End = vi;
segmentRefs[n] = neiRef;
n++; //could be n += 2, since segments have 2 vertices
}
continue;
}
//add sentinels
InsertInterval(ints, ref nints, MAX_INTERVAL, -1, 0, PolyId.Null);
InsertInterval(ints, ref nints, MAX_INTERVAL, 255, 256, PolyId.Null);
//store segments
Vector3 vj2 = tile.Verts[poly.Verts[j]];
Vector3 vi2 = tile.Verts[poly.Verts[i]];
for (int k = 1; k < nints; k++)
{
//portal segment
if (storePortals && ints[k].Reference != PolyId.Null)
{
float tmin = ints[k].TMin / 255.0f;
float tmax = ints[k].TMax / 255.0f;
if (n < maxSegments)
{
Vector3.Lerp(ref vj2, ref vi2, tmin, out segmentVerts[n].Start);
Vector3.Lerp(ref vj2, ref vi2, tmax, out segmentVerts[n].End);
segmentRefs[n] = ints[k].Reference;
n++;
}
}
//wall segment
int imin = ints[k - 1].TMax;
int imax = ints[k].TMin;
if (imin != imax)
{
float tmin = imin / 255.0f;
float tmax = imax / 255.0f;
if (n < maxSegments)
{
Vector3.Lerp(ref vj2, ref vi2, tmin, out segmentVerts[n].Start);
Vector3.Lerp(ref vj2, ref vi2, tmax, out segmentVerts[n].End);
segmentRefs[n] = PolyId.Null;
n++;
}
}
}
}
segmentCount = n;
return true;
}
