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