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