public void AddTileAt(NavTile tile, NavPolyId id)
{
//TODO more error checking, what if tile already exists?
Vector2i loc = tile.Location;
List <NavTile> locList;
if (!tileSet.TryGetValue(loc, out locList))
{
locList = new List <NavTile>();
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;
}
public virtual float GetCost(Vector3 a, Vector3 b,
NavPolyId prevRef, NavTile prevTile, NavPoly prevPoly,
NavPolyId curRef, NavTile curTile, NavPoly curPoly,
NavPolyId nextRef, NavTile nextTile, NavPoly nextPoly)
{
return (a - b).Length() * areaCost[(int)curPoly.Area.Id];
}
public bool MoveOverOffmeshConnection(NavPolyId offMeshConRef, NavPolyId[] refs, ref Vector3 startPos, ref Vector3 endPos, NavMeshQuery navquery)
{
//advance the path up to and over the off-mesh connection
NavPolyId prevRef = NavPolyId.Null, polyRef = path[0];
int npos = 0;
while (npos < path.Count && polyRef != offMeshConRef)
{
prevRef = polyRef;
polyRef = path[npos];
npos++;
}
if (npos == path.Count)
{
//could not find offMeshConRef
return(false);
}
//prune path
path.RemoveRange(0, 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);
}
/// <summary>
/// Check if polygon reference is valid.
/// </summary>
/// <param name="reference">Polygon reference</param>
/// <returns>True if valid</returns>
public bool IsValidPolyRef(NavPolyId reference)
{
if (reference == NavPolyId.Null)
{
return(false);
}
int salt, polyIndex, tileIndex;
idManager.Decode(ref reference, out polyIndex, out tileIndex, out salt);
if (tileIndex >= maxTiles)
{
return(false);
}
NavTile foundTile;
if (!tileIndices.TryGetValue(tileIndex, out foundTile))
{
return(false);
}
if (foundTile.Salt != salt)
{
return(false);
}
if (polyIndex >= foundTile.PolyCount)
{
return(false);
}
return(true);
}
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();
foreach (NavTile tile in mesh.Tiles)
{
NavPolyId id = mesh.GetTileRef(tile);
tilesArray.Add(SerializeMeshTile(tile, id));
}
root.Add("tiles", tilesArray);
File.WriteAllText(path, root.ToString());
}
private JObject SerializeMeshTile(NavTile tile, NavPolyId 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));
JObject treeObject = new JObject();
JArray treeNodes = new JArray();
for (int i = 0; i < tile.BVTree.Count; i++)
{
treeNodes.Add(JToken.FromObject(tile.BVTree[i], serializer));
}
treeObject.Add("nodes", treeNodes);
result.Add("bvTree", treeObject);
result.Add("bvQuantFactor", JToken.FromObject(tile.BvQuantFactor, serializer));
result.Add("bvNodeCount", JToken.FromObject(tile.BvNodeCount, serializer));
result.Add("walkableClimb", JToken.FromObject(tile.WalkableClimb, serializer));
return(result);
}
private NavTile DeserializeMeshTile(JToken token, NavPolyIdManager manager, out NavPolyId refId)
{
refId = token["polyId"].ToObject <NavPolyId>(serializer);
Vector2i location = token["location"].ToObject <Vector2i>(serializer);
int layer = token["layer"].ToObject <int>(serializer);
NavTile result = new NavTile(location, layer, manager, refId);
result.Salt = token["salt"].ToObject <int>(serializer);
result.Bounds = token["bounds"].ToObject <BBox3>(serializer);
result.Polys = token["polys"].ToObject <NavPoly[]>(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 treeObject = (JObject)token["bvTree"];
var nodes = treeObject.GetValue("nodes").ToObject <BVTree.Node[]>();
result.BVTree = new BVTree(nodes);
return(result);
}
public void SetPolyIndex(ref NavPolyId polyBase, int newPoly, out NavPolyId result)
{
newPoly &= polyMask;
//first clear poly then OR with new poly
result = new NavPolyId((polyBase.Id & ~polyMask) | newPoly);
}
/// <summary>
/// Check if polygon reference is valid.
/// </summary>
/// <param name="reference">Polygon reference</param>
/// <returns>True if valid</returns>
public bool IsValidPolyRef(NavPolyId reference)
{
if (reference == NavPolyId.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>
/// 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(NavPolyId reference, Vector3 pos)
{
this.pos = pos;
this.target = pos;
path.Clear();
path.Add(reference);
}
public void AddTileAt(NavTile tile, NavPolyId id)
{
if (!idManager.HasSameConfigAs(tile.IdManager))
{
throw new ArgumentException("Added tile has a different id manager. This likely means it came from another nav mesh");
}
//TODO more error checking, what if tile already exists?
Vector2i loc = tile.Location;
List <NavTile> locList;
if (!tileSet.TryGetValue(loc, out locList))
{
locList = new List <NavTile>();
locList.Add(tile);
tileSet.Add(loc, locList);
}
else
{
locList.Add(tile);
}
tileRefs.Add(tile, id);
int index = idManager.DecodeTileIndex(ref id);
tileIndices.Add(index, tile);
++totalSpawnedTiles;
}
public void Reset(NavPolyId 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 != NavPolyId.Null)
{
this.state = AgentState.Walking;
}
else
{
this.state = AgentState.Invalid;
}
this.TargetState = TargetState.None;
}
/// <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 NavNode GetNode(NavPolyId id)
{
NavNode node;
if (nodeDict.TryGetValue(id, out node))
{
return(node);
}
if (nodes.Count >= maxNodes)
{
return(null);
}
NavNode newNode = new NavNode();
newNode.ParentIndex = 0;
newNode.PolyCost = 0;
newNode.TotalCost = 0;
newNode.Id = id;
newNode.Flags = 0;
nodes.Add(newNode);
nodeDict.Add(id, newNode);
return(newNode);
}
/// <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 NavPolyId 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;
}
public NavTile this[NavPolyId id]
{
get
{
int index = idManager.DecodeTileIndex(ref id);
return(this[index]);
}
}
/// <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 NavPolyId result)
{
polyIndex &= polyMask;
tileIndex &= tileMask;
salt &= saltMask;
result = new NavPolyId((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(NavPolyId reference, out NavTile tile, out NavPoly poly)
{
int salt, polyIndex, tileIndex;
idManager.Decode(ref reference, out polyIndex, out tileIndex, out salt);
tile = tileIndices[tileIndex];
poly = tileIndices[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 NavPolyId 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>
/// Request a new move velocity
/// </summary>
/// <param name="vel">The agent's velocity</param>
public void RequestMoveVelocity(Vector3 vel)
{
//initialize request
this.TargetRef = NavPolyId.Null;
this.targetPos = vel;
this.TargetPathQueryIndex = PathQueue.Invalid;
this.TargetReplan = false;
this.targetState = TargetState.Velocity;
}
/// <summary>
/// Reset the move target of an agent
/// </summary>
public void ResetMoveTarget()
{
//initialize request
this.TargetRef = NavPolyId.Null;
this.targetPos = new Vector3(0.0f, 0.0f, 0.0f);
this.TargetPathQueryIndex = PathQueue.Invalid;
this.TargetReplan = false;
this.targetState = TargetState.None;
}
/// <summary>
/// Try to find a node.
/// </summary>
/// <param name="id">Node's id</param>
/// <returns>The node, if found. Null, if otherwise.</returns>
public NavNode FindNode(NavPolyId id)
{
NavNode 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(NavPolyId prevRef, NavPolyId polyRef, ref Vector3 startPos, ref Vector3 endPos)
{
int salt = 0, indexTile = 0, indexPoly = 0;
if (polyRef == NavPolyId.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);
}
NavTile tile = tileList[indexTile];
if (indexPoly >= tile.PolyCount)
{
return(false);
}
NavPoly poly = tile.Polys[indexPoly];
if (poly.PolyType != NavPolyType.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>
/// Try to find a node.
/// </summary>
/// <param name="id">Node's id</param>
/// <returns>The node, if found. Null, if otherwise.</returns>
public NavNode FindNode(NavPolyId id)
{
NavNode node;
if (nodeDict.TryGetValue(id, out node))
{
return(node);
}
return(null);
}
/// <summary>
/// Change the move target
/// </summary>
/// <param name="reference">The polygon reference</param>
/// <param name="pos">The target's coordinates</param>
public void RequestMoveTargetReplan(NavPolyId reference, Vector3 pos)
{
//initialize request
this.TargetRef = reference;
this.targetPos = pos;
this.TargetPathQueryIndex = PathQueue.Invalid;
this.TargetReplan = true;
if (this.TargetRef != NavPolyId.Null)
{
this.TargetState = TargetState.Requesting;
}
else
{
this.TargetState = TargetState.Failed;
}
}
/// <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(NavPolyId reference, out NavTile tile, out NavPoly poly)
{
tile = null;
poly = null;
if (reference == NavPolyId.Null)
{
return(false);
}
//Get tile and poly indices
int salt, polyIndex, tileIndex;
idManager.Decode(ref reference, out polyIndex, out tileIndex, out salt);
//Make sure indices are valid
if (tileIndex >= maxTiles)
{
return(false);
}
NavTile foundTile;
if (!tileIndices.TryGetValue(tileIndex, out foundTile))
{
return(false);
}
if (foundTile.Salt != salt)
{
return(false);
}
if (polyIndex >= foundTile.PolyCount)
{
return(false);
}
//Retrieve tile and poly
tile = tileIndices[tileIndex];
poly = tileIndices[tileIndex].Polys[polyIndex];
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(NavPolyId reference, Vector3 pos, float collisionQueryRange, NavMeshQuery navquery)
{
const int MAX_SEGS_PER_POLY = PathfindingCommon.VERTS_PER_POLYGON;
if (reference == NavPolyId.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
NavPolyId[] tempArray = new NavPolyId[polys.Length];
NavPoint centerPoint = new NavPoint(reference, pos);
navquery.FindLocalNeighborhood(ref centerPoint, 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 NavPolyId[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>
/// 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(NavPolyId safeRef, Vector3 safePos)
{
this.pos = safePos;
if (path.Count < 3 && path.Count > 0)
{
NavPolyId lastPathId = path[path.Count - 1];
path.Clear();
path.Add(safeRef);
path.Add(NavPolyId.Null);
path.Add(lastPathId);
}
else
{
path[0] = safeRef;
path[1] = NavPolyId.Null;
}
return(true);
}
public bool RemoveTile(NavTile tile)
{
List <NavTile> tiles;
if (tileSet.TryGetValue(tile.Location, out tiles))
{
tiles.Remove(tile);
}
else
{
return(false);
}
foreach (NavTile neighbourTile in GetTilesAt(tile.Location))
{
if (tile != neighbourTile)
{
UnconnectLinks(neighbourTile, tile);
}
}
for (int n = 0; n < 8; ++n)
{
foreach (NavTile neighbourTile in GetNeighborTilesAt(tile.Location, (BoundarySide)n))
{
UnconnectLinks(neighbourTile, tile);
}
}
NavPolyId id = tileRefs[tile];
int index = idManager.DecodeTileIndex(ref id);
tileIndices.Remove(index);
tileRefs.Remove(tile);
return(true);
}
/// <summary>
/// Request a new move target
/// </summary>
/// <param name="reference">The polygon reference</param>
/// <param name="pos">The target's coordinates</param>
/// <returns>True if request met, false if not</returns>
public bool RequestMoveTarget(NavPolyId reference, Vector3 pos)
{
if (reference == NavPolyId.Null)
{
return(false);
}
//initialize request
this.TargetRef = reference;
this.targetPos = pos;
this.TargetPathQueryIndex = PathQueue.Invalid;
this.TargetReplan = false;
if (this.TargetRef != NavPolyId.Null)
{
this.targetState = TargetState.Requesting;
}
else
{
this.targetState = TargetState.Failed;
}
return(true);
}
/// <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 NavNode GetNode(NavPolyId id)
{
NavNode node;
if (nodeDict.TryGetValue(id, out node))
{
return node;
}
if (nodes.Count >= maxNodes)
return null;
NavNode newNode = new NavNode();
newNode.ParentIndex = 0;
newNode.PolyCost = 0;
newNode.TotalCost = 0;
newNode.Id = id;
newNode.Flags = 0;
nodes.Add(newNode);
nodeDict.Add(id, newNode);
return newNode;
}
/// <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(NavPolyId prevRef, NavPolyId polyRef, ref Vector3 startPos, ref Vector3 endPos)
{
NavTile tile;
NavPoly poly;
if (TryGetTileAndPolyByRef(polyRef, out tile, out poly))
{
return(false);
}
if (poly.PolyType != NavPolyType.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);
}
private bool GetSteerTarget(NavMeshQuery navMeshQuery, SVector3 startPos, SVector3 endPos, float minTargetDist, SharpNav.Pathfinding.Path path,
ref SVector3 steerPos, ref StraightPathFlags steerPosFlag, ref NavPolyId steerPosRef)
{
StraightPath steerPath = new StraightPath();
navMeshQuery.FindStraightPath(startPos, endPos, path, steerPath, 0);
int nsteerPath = steerPath.Count;
if (nsteerPath == 0)
{
return(false);
}
//find vertex far enough to steer to
int ns = 0;
while (ns < nsteerPath)
{
if ((steerPath[ns].Flags & StraightPathFlags.OffMeshConnection) != 0 ||
!InRange(steerPath[ns].Point.Position, startPos, minTargetDist, 1000.0f))
{
break;
}
ns++;
}
//failed to find good point to steer to
if (ns >= nsteerPath)
{
return(false);
}
steerPos = steerPath[ns].Point.Position;
steerPos.Y = startPos.Y;
steerPosFlag = steerPath[ns].Flags;
if (steerPosFlag == StraightPathFlags.None && ns == (nsteerPath - 1))
{
steerPosFlag = StraightPathFlags.End; // otherwise seeks path infinitely!!!
}
steerPosRef = steerPath[ns].Point.Polygon;
return(true);
}
public void Add(NavPolyId poly)
{
polys.Add(poly);
}
/// <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 FindLocalNeighborhood(ref NavPoint centerPoint, float radius, NavPolyId[] resultRef, NavPolyId[] resultParent, ref int resultCount, int maxResult)
{
resultCount = 0;
//validate input
if (centerPoint.Polygon == NavPolyId.Null || !nav.IsValidPolyRef(centerPoint.Polygon))
return false;
int MAX_STACK = 48;
NavNode[] stack = new NavNode[MAX_STACK];
int nstack = 0;
tinyNodePool.Clear();
NavNode startNode = tinyNodePool.GetNode(centerPoint.Polygon);
startNode.ParentIndex = 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] = NavPolyId.Null;
++n;
}
while (nstack > 0)
{
//pop front
NavNode curNode = stack[0];
for (int i = 0; i < nstack - 1; i++)
stack[i] = stack[i + 1];
nstack--;
//get poly and tile
NavPolyId curRef = curNode.Id;
NavTile curTile;
NavPoly curPoly;
nav.TryGetTileAndPolyByRefUnsafe(curRef, out curTile, out curPoly);
foreach (Link link in curPoly.Links)
{
NavPolyId neighborRef = link.Reference;
//skip invalid neighbors
if (neighborRef == NavPolyId.Null)
continue;
//skip if cannot allocate more nodes
NavNode neighborNode = tinyNodePool.GetNode(neighborRef);
if (neighborNode == null)
continue;
//skip visited
if ((neighborNode.Flags & NodeFlags.Closed) != 0)
continue;
//expand to neighbor
NavTile neighborTile;
NavPoly neighborPoly;
nav.TryGetTileAndPolyByRefUnsafe(neighborRef, out neighborTile, out neighborPoly);
//skip off-mesh connections
if (neighborPoly.PolyType == NavPolyType.OffMeshConnection)
continue;
//find edge and calculate distance to edge
Vector3 va = new Vector3();
Vector3 vb = new Vector3();
if (!GetPortalPoints(curRef, curPoly, curTile, neighborRef, neighborPoly, neighborTile, 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
neighborNode.Flags |= NodeFlags.Closed;
neighborNode.ParentIndex = tinyNodePool.GetNodeIdx(curNode);
//check that the polygon doesn't collide with existing polygons
//collect vertices of the neighbor poly
int npa = neighborPoly.VertCount;
for (int k = 0; k < npa; k++)
pa[k] = neighborTile.Verts[neighborPoly.Verts[k]];
bool overlap = false;
for (int j = 0; j < n; j++)
{
NavPolyId pastRef = resultRef[j];
//connected polys do not overlap
bool connected = false;
foreach (Link link2 in curPoly.Links)
{
if (link2.Reference == pastRef)
{
connected = true;
break;
}
}
if (connected)
continue;
//potentially overlapping
NavTile pastTile;
NavPoly 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] = neighborRef;
resultParent[n] = curRef;
++n;
}
if (nstack < MAX_STACK)
{
stack[nstack++] = neighborNode;
}
}
}
resultCount = n;
return true;
}
public bool Raycast(ref NavPoint startPoint, ref Vector3 endPos, NavPolyId prevRef, RaycastOptions options, out RaycastHit hit, Path hitPath)
{
hit = new RaycastHit();
if (hitPath != null)
hitPath.Clear();
//validate input
if (startPoint.Polygon == NavPolyId.Null || !nav.IsValidPolyRef(startPoint.Polygon))
return false;
if (prevRef != NavPolyId.Null && !nav.IsValidPolyRef(prevRef))
return false;
Vector3[] verts = new Vector3[PathfindingCommon.VERTS_PER_POLYGON];
NavTile prevTile, curTile, nextTile;
NavPoly prevPoly, curPoly, nextPoly;
NavPolyId curRef = startPoint.Polygon;
nav.TryGetTileAndPolyByRefUnsafe(curRef, out curTile, out curPoly);
nextTile = prevTile = curTile;
nextPoly = prevPoly = curPoly;
if (prevRef != NavPolyId.Null)
nav.TryGetTileAndPolyByRefUnsafe(prevRef, out prevTile, out prevPoly);
while (curRef != NavPolyId.Null)
{
//collect vertices
int nv = 0;
for (int i = 0; i < curPoly.VertCount; i++)
{
verts[nv] = curTile.Verts[curPoly.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
return true;
}
hit.EdgeIndex = segMax;
//keep track of furthest t so far
if (tmax > hit.T)
hit.T = tmax;
//store visited polygons
if (hitPath != null)
hitPath.Add(curRef);
//ray end is completely inside the polygon
if (segMax == -1)
{
hit.T = float.MaxValue;
return true;
}
//follow neighbors
NavPolyId nextRef = NavPolyId.Null;
foreach (Link link in curPoly.Links)
{
//find link which contains the edge
if (link.Edge != segMax)
continue;
//get pointer to the next polygon
nav.TryGetTileAndPolyByRefUnsafe(link.Reference, out nextTile, out nextPoly);
//skip off-mesh connection
if (nextPoly.PolyType == NavPolyType.OffMeshConnection)
continue;
//TODO QueryFilter
//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 = curPoly.Verts[link.Edge];
int v1 = curPoly.Verts[(link.Edge + 1) % curPoly.VertCount];
Vector3 left = curTile.Verts[v0];
Vector3 right = curTile.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 ((options & RaycastOptions.UseCosts) != 0)
{
//TODO add cost
}
if (nextRef == NavPolyId.Null)
{
//no neighbor, 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;
hit.Normal = new Vector3(dz, 0, dx);
hit.Normal.Normalize();
return true;
}
//no hit, advance to neighbor polygon
prevRef = curRef;
curRef = nextRef;
prevTile = curTile;
curTile = nextTile;
prevPoly = curPoly;
curPoly = nextPoly;
}
return true;
}
public bool IsValidPolyRef(NavPolyId reference)
{
NavTile tile;
NavPoly poly;
bool status = nav.TryGetTileAndPolyByRef(reference, out tile, out poly);
if (status == false)
return false;
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(NavPolyId from, NavPoly fromPoly, NavTile fromTile, NavPolyId to, NavPoly toPoly, NavTile toTile, ref Vector3 left, ref Vector3 right)
{
//find the link that points to the 'to' polygon
Link link = null;
foreach (Link fromLink in fromPoly.Links)
{
if (fromLink.Reference == to)
{
link = fromLink;
break;
}
}
if (link == null)
return false;
//handle off-mesh connections
if (fromPoly.PolyType == NavPolyType.OffMeshConnection)
{
//find link that points to first vertex
foreach (Link fromLink in fromPoly.Links)
{
if (fromLink.Reference == to)
{
int v = fromLink.Edge;
left = fromTile.Verts[fromPoly.Verts[v]];
right = fromTile.Verts[fromPoly.Verts[v]];
return true;
}
}
return false;
}
if (toPoly.PolyType == NavPolyType.OffMeshConnection)
{
//find link that points to first vertex
foreach (Link toLink in toPoly.Links)
{
if (toLink.Reference == from)
{
int v = toLink.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>
/// 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(NavPolyId reference, Crowds.LocalBoundary.Segment[] segmentVerts, NavPolyId[] segmentRefs, ref int segmentCount, int maxSegments)
{
segmentCount = 0;
NavTile tile;
NavPoly 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
foreach (Link link in poly.Links)
{
if (link.Edge == j)
{
if (link.Reference != NavPolyId.Null)
{
NavTile neiTile;
NavPoly neiPoly;
nav.TryGetTileAndPolyByRefUnsafe(link.Reference, out neiTile, out neiPoly);
InsertInterval(ints, ref nints, MAX_INTERVAL, link.BMin, link.BMax, link.Reference);
}
}
}
}
else
{
//internal edge
NavPolyId neiRef = NavPolyId.Null;
if (poly.Neis[j] != 0)
{
int idx = poly.Neis[j] - 1;
NavPolyId id = nav.GetTileRef(tile);
nav.IdManager.SetPolyIndex(ref id, idx, out neiRef);
}
//if the edge leads to another polygon and portals are not stored, skip
if (neiRef != NavPolyId.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, NavPolyId.Null);
InsertInterval(ints, ref nints, MAX_INTERVAL, 255, 256, NavPolyId.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 != NavPolyId.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] = NavPolyId.Null;
n++;
}
}
}
}
segmentCount = n;
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(NavPolyId from, NavPoly fromPoly, NavTile fromTile, NavPolyId to, NavPoly toPoly, NavTile toTile, ref Vector3 mid)
{
Vector3 left = new Vector3();
Vector3 right = new Vector3();
if (!GetPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, ref left, ref right))
return false;
mid = (left + right) * 0.5f;
return true;
}
/// <summary>
/// Only use this function if it is known that the provided polygon reference is valid.
/// </summary>
/// <param name="reference">Polygon reference</param>
/// <param name="tile">Resulting tile</param>
/// <param name="poly">Resulting poly</param>
public void TryGetTileAndPolyByRefUnsafe(NavPolyId reference, out NavTile tile, out NavPoly 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="NavPoint"/> struct.
/// </summary>
/// <param name="poly">The polygon that the point is on.</param>
/// <param name="pos">The 3d position of the point.</param>
public NavPoint(NavPolyId poly, Vector3 pos)
{
this.Polygon = poly;
this.Position = pos;
}
private JObject SerializeMeshTile(NavTile tile, NavPolyId 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));
JObject treeObject = new JObject();
JArray treeNodes = new JArray();
for (int i = 0; i < tile.BVTree.Count; i++)
treeNodes.Add(JToken.FromObject(tile.BVTree[i], serializer));
treeObject.Add("nodes", treeNodes);
result.Add("bvTree", treeObject);
result.Add("bvQuantFactor", JToken.FromObject(tile.BvQuantFactor, serializer));
result.Add("bvNodeCount", JToken.FromObject(tile.BvNodeCount, serializer));
result.Add("walkableClimb", JToken.FromObject(tile.WalkableClimb, serializer));
return result;
}
private NavTile DeserializeMeshTile(JToken token, NavPolyIdManager manager, out NavPolyId refId)
{
refId = token["polyId"].ToObject<NavPolyId>(serializer);
Vector2i location = token["location"].ToObject<Vector2i>(serializer);
int layer = token["layer"].ToObject<int>(serializer);
NavTile result = new NavTile(location, layer, manager, refId);
result.Salt = token["salt"].ToObject<int>(serializer);
result.Bounds = token["bounds"].ToObject<BBox3>(serializer);
result.Polys = token["polys"].ToObject<NavPoly[]>(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 treeObject = (JObject) token["bvTree"];
var nodes = treeObject.GetValue("nodes").ToObject<BVTree.Node[]>();
result.BVTree = new BVTree(nodes);
return result;
}
/// <summary>
/// Update the crowd pathfinding periodically
/// </summary>
/// <param name="dt">Th time until the next update</param>
public void Update(float dt)
{
velocitySampleCount = 0;
int numAgents = GetActiveAgents(agents);
//check that all agents have valid paths
CheckPathValidity(agents, numAgents, dt);
//update async move requests and path finder
UpdateMoveRequest();
//optimize path topology
UpdateTopologyOptimization(agents, numAgents, dt);
//register agents to proximity grid
grid.Clear();
for (int i = 0; i < numAgents; i++)
{
Agent a = agents[i];
Vector3 p = a.Position;
float r = a.Parameters.Radius;
grid.AddItem(a, p.X - r, p.Z - r, p.X + r, p.Z + r);
}
//get nearby navmesh segments and agents to collide with
for (int i = 0; i < numAgents; i++)
{
if (agents[i].State != AgentState.Walking)
continue;
//update the collision boundary after certain distance has passed or if it has become invalid
float updateThr = agents[i].Parameters.CollisionQueryRange * 0.25f;
if (Vector3Extensions.Distance2D(agents[i].Position, agents[i].Boundary.Center) > updateThr * updateThr || !agents[i].Boundary.IsValid(navQuery))
{
agents[i].Boundary.Update(agents[i].Corridor.GetFirstPoly(), agents[i].Position, agents[i].Parameters.CollisionQueryRange, navQuery);
}
//query neighbor agents
agents[i].NeighborCount = GetNeighbors(agents[i].Position, agents[i].Parameters.Height, agents[i].Parameters.CollisionQueryRange, agents[i], agents[i].Neighbors, AgentMaxNeighbors, agents, grid);
for (int j = 0; j < agents[i].NeighborCount; j++)
agents[i].Neighbors[j].Index = GetAgentIndex(agents[agents[i].Neighbors[j].Index]);
}
//find the next corner to steer to
for (int i = 0; i < numAgents; i++)
{
if (agents[i].State != AgentState.Walking)
continue;
if (agents[i].TargetState == TargetState.None ||
agents[i].TargetState == TargetState.Velocity)
continue;
//find corners for steering
agents[i].Corridor.FindCorners(agents[i].Corners, navQuery);
//check to see if the corner after the next corner is directly visible
if (((agents[i].Parameters.UpdateFlags & UpdateFlags.OptimizeVis) != 0) && agents[i].Corners.Count > 0)
{
Vector3 target = agents[i].Corners[Math.Min(1, agents[i].Corners.Count - 1)].Point.Position;
agents[i].Corridor.OptimizePathVisibility(target, agents[i].Parameters.PathOptimizationRange, navQuery);
}
}
//trigger off-mesh connections (depends on corners)
for (int i = 0; i < numAgents; i++)
{
if (agents[i].State != AgentState.Walking)
continue;
if (agents[i].TargetState == TargetState.None ||
agents[i].TargetState == TargetState.Velocity)
continue;
//check
float triggerRadius = agents[i].Parameters.Radius * 2.25f;
if (OverOffmeshConnection(agents[i], triggerRadius))
{
//prepare to off-mesh connection
int idx = i;
//adjust the path over the off-mesh connection
NavPolyId[] refs = new NavPolyId[2];
if (agents[i].Corridor.MoveOverOffmeshConnection(agents[i].Corners[agents[i].Corners.Count - 1].Point.Polygon, refs, ref agentAnims[idx].StartPos, ref agentAnims[idx].EndPos, navQuery))
{
agentAnims[idx].InitPos = agents[i].Position;
agentAnims[idx].PolyRef = refs[1];
agentAnims[idx].Active = true;
agentAnims[idx].T = 0.0f;
agentAnims[idx].TMax = (Vector3Extensions.Distance2D(agentAnims[idx].StartPos, agentAnims[idx].EndPos)
/ agents[i].Parameters.MaxSpeed) * 0.5f;
agents[i].State = AgentState.Offmesh;
agents[i].Corners.Clear();
agents[i].NeighborCount = 0;
continue;
}
}
}
//calculate steering
for (int i = 0; i < numAgents; i++)
{
if (agents[i].State != AgentState.Walking)
continue;
if (agents[i].TargetState == TargetState.None)
continue;
Vector3 dvel = new Vector3(0, 0, 0);
if (agents[i].TargetState == TargetState.Velocity)
{
dvel = agents[i].TargetPosition;
agents[i].DesiredSpeed = agents[i].TargetPosition.Length();
}
else
{
//calculate steering direction
if ((agents[i].Parameters.UpdateFlags & UpdateFlags.AnticipateTurns) != 0)
CalcSmoothSteerDirection(agents[i], ref dvel);
else
CalcStraightSteerDirection(agents[i], ref dvel);
//calculate speed scale, which tells the agent to slowdown at the end of the path
float slowDownRadius = agents[i].Parameters.Radius * 2;
float speedScale = GetDistanceToGoal(agents[i], slowDownRadius) / slowDownRadius;
agents[i].DesiredSpeed = agents[i].Parameters.MaxSpeed;
dvel = dvel * (agents[i].DesiredSpeed * speedScale);
}
//separation
if ((agents[i].Parameters.UpdateFlags & UpdateFlags.Separation) != 0)
{
float separationDist = agents[i].Parameters.CollisionQueryRange;
float invSeparationDist = 1.0f / separationDist;
float separationWeight = agents[i].Parameters.SeparationWeight;
float w = 0;
Vector3 disp = new Vector3(0, 0, 0);
for (int j = 0; j < agents[i].NeighborCount; j++)
{
Agent nei = agents[agents[i].Neighbors[j].Index];
Vector3 diff = agents[i].Position - nei.Position;
diff.Y = 0;
float distSqr = diff.LengthSquared();
if (distSqr < 0.00001f)
continue;
if (distSqr > separationDist * separationDist)
continue;
float dist = (float)Math.Sqrt(distSqr);
float weight = separationWeight * (1.0f - (dist * invSeparationDist) * (dist * invSeparationDist));
disp = disp + diff * (weight / dist);
w += 1.0f;
}
if (w > 0.0001f)
{
//adjust desired veloctiy
dvel = dvel + disp * (1.0f / w);
//clamp desired velocity to desired speed
float speedSqr = dvel.LengthSquared();
float desiredSqr = agents[i].DesiredSpeed * agents[i].DesiredSpeed;
if (speedSqr > desiredSqr)
dvel = dvel * (desiredSqr / speedSqr);
}
}
//set the desired velocity
agents[i].DesiredVel = dvel;
}
//velocity planning
for (int i = 0; i < numAgents; i++)
{
if (agents[i].State != AgentState.Walking)
continue;
if ((agents[i].Parameters.UpdateFlags & UpdateFlags.ObstacleAvoidance) != 0)
{
this.obstacleQuery.Reset();
//add neighhbors as obstacles
for (int j = 0; j < agents[i].NeighborCount; j++)
{
Agent nei = agents[agents[i].Neighbors[j].Index];
obstacleQuery.AddCircle(nei.Position, nei.Parameters.Radius, nei.Vel, nei.DesiredVel);
}
//append neighbor segments as obstacles
for (int j = 0; j < agents[i].Boundary.SegCount; j++)
{
LocalBoundary.Segment s = agents[i].Boundary.Segs[j];
if (Triangle3.Area2D(agents[i].Position, s.Start, s.End) < 0.0f)
continue;
obstacleQuery.AddSegment(s.Start, s.End);
}
//sample new safe velocity
bool adaptive = true;
int ns = 0;
ObstacleAvoidanceQuery.ObstacleAvoidanceParams parameters = obstacleQueryParams[agents[i].Parameters.ObstacleAvoidanceType];
if (adaptive)
{
ns = obstacleQuery.SampleVelocityAdaptive(agents[i].Position, agents[i].Parameters.Radius, agents[i].DesiredSpeed, agents[i].Vel, agents[i].DesiredVel, ref agents[i].NVel, parameters);
}
else
{
ns = obstacleQuery.SampleVelocityGrid(agents[i].Position, agents[i].Parameters.Radius, agents[i].DesiredSpeed, agents[i].Vel, agents[i].DesiredVel, ref agents[i].NVel, parameters);
}
this.velocitySampleCount += ns;
}
else
{
//if not using velocity planning, new velocity is directly the desired velocity
agents[i].NVel = agents[i].DesiredVel;
}
}
//integrate
for (int i = 0; i < numAgents; i++)
{
Agent ag = agents[i];
if (ag.State != AgentState.Walking)
continue;
ag.Integrate(dt);
}
//handle collisions
const float COLLISION_RESOLVE_FACTOR = 0.7f;
for (int iter = 0; iter < 4; iter++)
{
for (int i = 0; i < numAgents; i++)
{
int idx0 = GetAgentIndex(agents[i]);
if (agents[i].State != AgentState.Walking)
continue;
agents[i].Disp = new Vector3(0, 0, 0);
float w = 0;
for (int j = 0; j < agents[i].NeighborCount; j++)
{
Agent nei = agents[agents[i].Neighbors[j].Index];
int idx1 = GetAgentIndex(nei);
Vector3 diff = agents[i].Position - nei.Position;
diff.Y = 0;
float dist = diff.LengthSquared();
if (dist > (agents[i].Parameters.Radius + nei.Parameters.Radius) * (agents[i].Parameters.Radius + nei.Parameters.Radius))
continue;
dist = (float)Math.Sqrt(dist);
float pen = (agents[i].Parameters.Radius + nei.Parameters.Radius) - dist;
if (dist < 0.0001f)
{
//agents on top of each other, try to choose diverging separation directions
if (idx0 > idx1)
diff = new Vector3(-agents[i].DesiredVel.Z, 0, agents[i].DesiredVel.X);
else
diff = new Vector3(agents[i].DesiredVel.Z, 0, -agents[i].DesiredVel.X);
pen = 0.01f;
}
else
{
pen = (1.0f / dist) * (pen * 0.5f) * COLLISION_RESOLVE_FACTOR;
}
agents[i].Disp = agents[i].Disp + diff * pen;
w += 1.0f;
}
if (w > 0.0001f)
{
float iw = 1.0f / w;
agents[i].Disp = agents[i].Disp * iw;
}
}
for (int i = 0; i < numAgents; i++)
{
if (agents[i].State != AgentState.Walking)
continue;
//move along navmesh
agents[i].Corridor.MovePosition(agents[i].Position, navQuery);
//get valid constrained position back
agents[i].Position = agents[i].Corridor.Pos;
//if not using path, truncate the corridor to just one poly
if (agents[i].TargetState == TargetState.None ||
agents[i].TargetState == TargetState.Velocity)
{
agents[i].Corridor.Reset(agents[i].Corridor.GetFirstPoly(), agents[i].Position);
agents[i].IsPartial = false;
}
}
//update agents using offmesh connections
for (int i = 0; i < maxAgents; i++)
{
if (!agentAnims[i].Active)
continue;
agentAnims[i].T += dt;
if (agentAnims[i].T > agentAnims[i].TMax)
{
//reset animation
agentAnims[i].Active = false;
//prepare agent for walking
agents[i].State = AgentState.Walking;
continue;
}
//update position
float ta = agentAnims[i].TMax * 0.15f;
float tb = agentAnims[i].TMax;
if (agentAnims[i].T < ta)
{
float u = MathHelper.Normalize(agentAnims[i].T, 0.0f, ta);
Vector3 lerpOut;
Vector3.Lerp(ref agentAnims[i].InitPos, ref agentAnims[i].StartPos, u, out lerpOut);
agents[i].Position = lerpOut;
}
else
{
float u = MathHelper.Normalize(agentAnims[i].T, ta, tb);
Vector3 lerpOut;
Vector3.Lerp(ref agentAnims[i].StartPos, ref agentAnims[i].EndPos, u, out lerpOut);
agents[i].Position = lerpOut;
}
agents[i].Vel = new Vector3(0, 0, 0);
agents[i].DesiredVel = new Vector3(0, 0, 0);
}
}
}
/// <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(NavPolyId reference, Vector3 pos, ref float height)
{
if (nav == null)
return false;
NavTile tile;
NavPoly poly;
if (!nav.TryGetTileAndPolyByRef(reference, out tile, out poly))
return false;
//off-mesh connections don't have detail polygons
if (poly.PolyType == NavPolyType.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>
/// 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(NavPolyId reference, out NavTile tile, out NavPoly poly)
{
tile = null;
poly = null;
if (reference == NavPolyId.Null)
return false;
//Get tile and poly indices
int salt, polyIndex, tileIndex;
idManager.Decode(ref reference, out polyIndex, out tileIndex, out salt);
//Make sure indices are valid
if (tileIndex >= maxTiles)
return false;
if (tileList[tileIndex].Salt != salt)
return false;
if (polyIndex >= tileList[tileIndex].PolyCount)
return false;
//Retrieve tile and poly
tile = tileList[tileIndex];
poly = tileList[tileIndex].Polys[polyIndex];
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(NavPolyId from, NavPolyId to, ref Vector3 left, ref Vector3 right, ref NavPolyType fromType, ref NavPolyType toType)
{
NavTile fromTile;
NavPoly fromPoly;
if (nav.TryGetTileAndPolyByRef(from, out fromTile, out fromPoly) == false)
return false;
fromType = fromPoly.PolyType;
NavTile toTile;
NavPoly 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>
/// Check if polygon reference is valid.
/// </summary>
/// <param name="reference">Polygon reference</param>
/// <returns>True if valid</returns>
public bool IsValidPolyRef(NavPolyId reference)
{
if (reference == NavPolyId.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>
/// 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, NavPolyId 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>
/// 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(NavPolyId prevRef, NavPolyId polyRef, ref Vector3 startPos, ref Vector3 endPos)
{
int salt = 0, indexTile = 0, indexPoly = 0;
if (polyRef == NavPolyId.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;
NavTile tile = tileList[indexTile];
if (indexPoly >= tile.PolyCount)
return false;
NavPoly poly = tile.Polys[indexPoly];
if (poly.PolyType != NavPolyType.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>
/// 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(NavPolyId reference, Vector3 pos, out Vector3 closest, out bool posOverPoly)
{
posOverPoly = false;
closest = Vector3.Zero;
NavTile tile;
NavPoly poly;
if (!nav.TryGetTileAndPolyByRef(reference, out tile, out poly))
return false;
if (tile == null)
return false;
if (poly.PolyType == NavPolyType.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;
}
public void AddTileAt(NavTile tile, NavPolyId id)
{
//TODO more error checking, what if tile already exists?
Vector2i loc = tile.Location;
List<NavTile> locList;
if (!tileSet.TryGetValue(loc, out locList))
{
locList = new List<NavTile>();
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>
/// 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(NavPolyId reference, Vector3 pos, ref Vector3 closest)
{
NavTile tile;
NavPoly poly;
if (nav.TryGetTileAndPolyByRef(reference, out tile, out poly) == false)
return false;
tile.ClosestPointOnPolyBoundary(poly, pos, out closest);
return true;
}
public NavTile this[NavPolyId id]
{
get
{
int index = idManager.DecodeTileIndex(ref id);
return this[index];
}
}
public Vector2[] GetPath(Vector2 start, Vector2 end)
{
NavPoint startPt = navMeshQuery.FindNearestPoly(new SVector3(-start.X, 0, start.Y), new SharpNav.Geometry.Vector3(2f, 2f, 2f));
NavPoint endPt = navMeshQuery.FindNearestPoly(new SVector3(-end.X, 0, end.Y), new SharpNav.Geometry.Vector3(2f, 2f, 2f));
Path path = new Path();
navMeshQuery.FindPath(ref startPt, ref endPt, new NavQueryFilter(), path);
List <SVector3> smoothPath;
//find a smooth path over the mesh surface
int npolys = path.Count;
SVector3 iterPos = new SVector3();
SVector3 targetPos = new SVector3();
navMeshQuery.ClosestPointOnPoly(startPt.Polygon, startPt.Position, ref iterPos);
navMeshQuery.ClosestPointOnPoly(path[npolys - 1], endPt.Position, ref targetPos);
smoothPath = new List <SVector3>(2048);
smoothPath.Add(iterPos);
float STEP_SIZE = 0.5f;
float SLOP = 0.01f;
while (npolys > 0 && smoothPath.Count < smoothPath.Capacity)
{
//find location to steer towards
SVector3 steerPos = new SVector3();
StraightPathFlags steerPosFlag = 0;
NavPolyId steerPosRef = NavPolyId.Null;
if (!GetSteerTarget(navMeshQuery, iterPos, targetPos, SLOP, path, ref steerPos, ref steerPosFlag, ref steerPosRef))
{
break;
}
bool endOfPath = (steerPosFlag & StraightPathFlags.End) != 0 ? true : false;
bool offMeshConnection = (steerPosFlag & StraightPathFlags.OffMeshConnection) != 0 ? true : false;
//find movement delta
SVector3 delta = steerPos - iterPos;
float len = (float)Math.Sqrt(SVector3.Dot(delta, delta));
//if steer target is at end of path or off-mesh link
//don't move past location
if ((endOfPath || offMeshConnection) && len < STEP_SIZE)
{
len = 1;
}
else
{
len = STEP_SIZE / len;
}
SVector3 moveTgt = new SVector3();
VMad(ref moveTgt, iterPos, delta, len);
//move
SVector3 result = new SVector3();
List <NavPolyId> visited = new List <NavPolyId>(16);
NavPoint startPoint = new NavPoint(path[0], iterPos);
navMeshQuery.MoveAlongSurface(ref startPoint, ref moveTgt, out result, visited);
path.FixupCorridor(visited);
npolys = path.Count;
float h = 0;
navMeshQuery.GetPolyHeight(path[0], result, ref h);
result.Y = h;
iterPos = result;
//handle end of path when close enough
if (endOfPath && InRange(iterPos, steerPos, SLOP, 1.0f))
{
//reached end of path
iterPos = targetPos;
if (smoothPath.Count < smoothPath.Capacity)
{
smoothPath.Add(iterPos);
}
break;
}
//store results
if (smoothPath.Count < smoothPath.Capacity)
{
smoothPath.Add(iterPos);
}
}
return(smoothPath.Select(x => new Vector2(-x.X, x.Z)).ToArray());
}
/// <summary>
/// Finds a random point on a polygon.
/// </summary>
/// <param name="poly">Polygon to find a random point on.</param>
/// <returns>Resulting random point</returns>
public Vector3 FindRandomPointOnPoly(NavPolyId poly)
{
Vector3 result;
this.FindRandomPointOnPoly(poly, out result);
return result;
}
private void GeneratePathfinding()
{
if (!hasGenerated)
{
return;
}
NavQueryFilter filter = new NavQueryFilter();
buildData = new NavMeshBuilder(polyMesh, polyMeshDetail, new SharpNav.Pathfinding.OffMeshConnection[0], settings);
tiledNavMesh = new TiledNavMesh(buildData);
for (int i = 0; i < tiledNavMesh.Tiles.Count; ++i)
{
for (int j = 0; j < tiledNavMesh.Tiles[i].Verts.Length; ++j)
{
//if (j < tiledNavMesh.Tiles[i].Verts.Length - 1)
// Debug.DrawLine(ExportNavMeshToObj.ToUnityVector(tiledNavMesh.Tiles[i].Verts[j]), ExportNavMeshToObj.ToUnityVector(tiledNavMesh.Tiles[i].Verts[j + 1]), Color.blue, 99);
}
}
navMeshQuery = new NavMeshQuery(tiledNavMesh, 2048);
//Find random start and end points on the poly mesh
/*int startRef;
* navMeshQuery.FindRandomPoint(out startRef, out startPos);*/
//SVector3 c = new SVector3(10, 0, 0);
//SVector3 e = new SVector3(5, 5, 5);
//navMeshQuery.FindNearestPoly(ref c, ref e, out startPt);
//navMeshQuery.FindRandomPointAroundCircle(ref startPt, 1000, out endPt);
startPt = navMeshQuery.FindRandomPoint();
endPt = navMeshQuery.FindRandomPoint();
//calculate the overall path, which contains an array of polygon references
int MAX_POLYS = 256;
path = new Path();
navMeshQuery.FindPath(ref startPt, ref endPt, filter, path);
//find a smooth path over the mesh surface
int npolys = path.Count;
SVector3 iterPos = new SVector3();
SVector3 targetPos = new SVector3();
navMeshQuery.ClosestPointOnPoly(startPt.Polygon, startPt.Position, ref iterPos);
navMeshQuery.ClosestPointOnPoly(path[npolys - 1], endPt.Position, ref targetPos);
smoothPath = new List <SVector3>(2048);
smoothPath.Add(iterPos);
float STEP_SIZE = 0.5f;
float SLOP = 0.01f;
while (npolys > 0 && smoothPath.Count < smoothPath.Capacity)
{
//find location to steer towards
SVector3 steerPos = new SVector3();
StraightPathFlags steerPosFlag = 0;
NavPolyId steerPosRef = NavPolyId.Null;
if (!GetSteerTarget(navMeshQuery, iterPos, targetPos, SLOP, path, ref steerPos, ref steerPosFlag, ref steerPosRef))
{
break;
}
bool endOfPath = (steerPosFlag & StraightPathFlags.End) != 0 ? true : false;
bool offMeshConnection = (steerPosFlag & StraightPathFlags.OffMeshConnection) != 0 ? true : false;
//find movement delta
SVector3 delta = steerPos - iterPos;
float len = (float)Math.Sqrt(SVector3.Dot(delta, delta));
//if steer target is at end of path or off-mesh link
//don't move past location
if ((endOfPath || offMeshConnection) && len < STEP_SIZE)
{
len = 1;
}
else
{
len = STEP_SIZE / len;
}
SVector3 moveTgt = new SVector3();
VMad(ref moveTgt, iterPos, delta, len);
//move
SVector3 result = new SVector3();
List <NavPolyId> visited = new List <NavPolyId>(16);
NavPoint startPoint = new NavPoint(path[0], iterPos);
navMeshQuery.MoveAlongSurface(ref startPoint, ref moveTgt, out result, visited);
path.FixupCorridor(visited);
npolys = path.Count;
float h = 0;
navMeshQuery.GetPolyHeight(path[0], result, ref h);
result.Y = h;
iterPos = result;
//handle end of path when close enough
if (endOfPath && InRange(iterPos, steerPos, SLOP, 1.0f))
{
//reached end of path
iterPos = targetPos;
if (smoothPath.Count < smoothPath.Capacity)
{
smoothPath.Add(iterPos);
}
break;
}
//store results
if (smoothPath.Count < smoothPath.Capacity)
{
smoothPath.Add(iterPos);
}
}
for (int i = 0; i < smoothPath.Count; i++)
{
//if (i < smoothPath.Count - 1)
// Debug.DrawLine(ExportNavMeshToObj.ToUnityVector(smoothPath[i]), ExportNavMeshToObj.ToUnityVector(smoothPath[i + 1]), Color.red, 99);
}
}
/// <summary>
/// Finds a random point on a polygon.
/// </summary>
/// <param name="polyId">Polygon to find a radom point on.</param>
/// <param name="randomPt">Resulting random point.</param>
public void FindRandomPointOnPoly(NavPolyId polyId, out Vector3 randomPt)
{
NavTile tile;
NavPoly poly;
if (!nav.TryGetTileAndPolyByRef(polyId, out tile, out poly))
throw new ArgumentException("Invalid polygon ID", "polyId");
Vector3[] verts = new Vector3[poly.VertCount];
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, s, t, out randomPt);
//TODO bad state again.
float h = 0.0f;
if (!GetPolyHeight(polyId, randomPt, ref h))
throw new InvalidOperationException("Outside bounds?");
randomPt.Y = h;
}
private bool GetSteerTarget(NavMeshQuery navMeshQuery, SVector3 startPos, SVector3 endPos, float minTargetDist, SharpNav.Pathfinding.Path path,
ref SVector3 steerPos, ref StraightPathFlags steerPosFlag, ref NavPolyId steerPosRef)
{
StraightPath steerPath = new StraightPath();
navMeshQuery.FindStraightPath(startPos, endPos, path, steerPath, 0);
int nsteerPath = steerPath.Count;
if (nsteerPath == 0)
return false;
//find vertex far enough to steer to
int ns = 0;
while (ns < nsteerPath)
{
if ((steerPath[ns].Flags & StraightPathFlags.OffMeshConnection) != 0 ||
!InRange(steerPath[ns].Point.Position, startPos, minTargetDist, 1000.0f))
break;
ns++;
}
//failed to find good point to steer to
if (ns >= nsteerPath)
return false;
steerPos = steerPath[ns].Point.Position;
steerPos.Y = startPos.Y;
steerPosFlag = steerPath[ns].Flags;
if (steerPosFlag == StraightPathFlags.None && ns == (nsteerPath - 1))
steerPosFlag = StraightPathFlags.End; // otherwise seeks path infinitely!!!
steerPosRef = steerPath[ns].Point.Polygon;
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(NavPolyId reference, Vector3 pos, float collisionQueryRange, NavMeshQuery navquery)
{
const int MAX_SEGS_PER_POLY = PathfindingCommon.VERTS_PER_POLYGON;
if (reference == NavPolyId.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
NavPolyId[] tempArray = new NavPolyId[polys.Length];
NavPoint centerPoint = new NavPoint(reference, pos);
navquery.FindLocalNeighborhood(ref centerPoint, 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 NavPolyId[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]);
}
}
}