public void FindCorners(StraightPath corners, NavMeshQuery navquery) { const float MinTargetDist = 0.01f; navquery.FindStraightPath(pos, target, path, corners, 0); //prune points in the beginning of the path which are too close while (corners.Count > 0) { if (((corners[0].Flags & StraightPathFlags.OffMeshConnection) != 0) || Vector3Extensions.Distance2D(corners[0].Point.Position, pos) > MinTargetDist) break; corners.RemoveAt(0); } //prune points after an off-mesh connection for (int i = 0; i < corners.Count; i++) { if ((corners[i].Flags & StraightPathFlags.OffMeshConnection) != 0) { corners.RemoveRange(i + 1, corners.Count - i); break; } } }
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> /// 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, Path path, StraightPath straightPath, PathBuildFlags options) { straightPath.Clear(); if (path.Count == 0) return false; bool stat = false; Vector3 closestStartPos = new Vector3(); ClosestPointOnPolyBoundary(path[0], startPos, ref closestStartPos); Vector3 closestEndPos = new Vector3(); ClosestPointOnPolyBoundary(path[path.Count - 1], endPos, ref closestEndPos); stat = straightPath.AppendVertex(new StraightPathVertex(new NavPoint(path[0], closestStartPos), StraightPathFlags.Start)); if (!stat) return true; if (path.Count > 1) { Vector3 portalApex = closestStartPos; Vector3 portalLeft = portalApex; Vector3 portalRight = portalApex; int apexIndex = 0; int leftIndex = 0; int rightIndex = 0; NavPolyType leftPolyType = 0; NavPolyType rightPolyType = 0; NavPolyId leftPolyRef = path[0]; NavPolyId rightPolyRef = path[0]; for (int i = 0; i < path.Count; i++) { Vector3 left = new Vector3(); Vector3 right = new Vector3(); NavPolyType fromType = 0, toType = 0; if (i + 1 < path.Count) { //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, options); } stat = straightPath.AppendVertex(new StraightPathVertex(new NavPoint(path[i], closestEndPos), StraightPathFlags.None)); 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 = NavPolyType.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 < path.Count) ? path[i + 1] : NavPolyId.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, options); if (stat != true) return true; } portalApex = portalLeft; apexIndex = leftIndex; StraightPathFlags flags = 0; if (leftPolyRef == NavPolyId.Null) flags = StraightPathFlags.End; else if (leftPolyType == NavPolyType.OffMeshConnection) flags = StraightPathFlags.OffMeshConnection; NavPolyId reference = leftPolyRef; //append or update vertex stat = straightPath.AppendVertex(new StraightPathVertex(new NavPoint(reference, portalApex), flags)); 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 < path.Count) ? path[i + 1] : NavPolyId.Null; leftPolyType = toType; leftIndex = i; } else { if ((options & (PathBuildFlags.AreaCrossingVertices | PathBuildFlags.AllCrossingVertices)) != 0) { stat = AppendPortals(apexIndex, rightIndex, portalRight, path, straightPath, options); if (stat != true) return true; } portalApex = portalRight; apexIndex = rightIndex; StraightPathFlags flags = 0; if (rightPolyRef == NavPolyId.Null) flags = StraightPathFlags.End; else if (rightPolyType == NavPolyType.OffMeshConnection) flags = StraightPathFlags.OffMeshConnection; NavPolyId reference = rightPolyRef; //append or update vertex stat = straightPath.AppendVertex(new StraightPathVertex(new NavPoint(reference, portalApex), flags)); 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, path.Count - 1, closestEndPos, path, straightPath, options); if (stat != true) return true; } } stat = straightPath.AppendVertex(new StraightPathVertex(new NavPoint(NavPolyId.Null, closestEndPos), StraightPathFlags.End)); return true; }
/// <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, Path path, StraightPath straightPath, PathBuildFlags options) { Vector3 startPos = straightPath[straightPath.Count - 1].Point.Position; //append or update last vertex bool stat = false; for (int i = startIdx; i < endIdx; i++) { //calculate portal NavPolyId from = path[i]; NavTile fromTile; NavPoly fromPoly; if (nav.TryGetTileAndPolyByRef(from, out fromTile, out fromPoly) == false) return false; NavPolyId to = path[i + 1]; NavTile toTile; NavPoly 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 = straightPath.AppendVertex(new StraightPathVertex(new NavPoint(path[i + 1], pt), StraightPathFlags.None)); if (stat != true) return true; } } return true; }