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; }
/// <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> /// 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> /// 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; }