// Retrace portals between corners and register if type of polygon changes
public static int RetracePortals(UnityEngine.Experimental.AI.NavMeshQuery query, int startIndex, int endIndex,
Unity.Collections.NativeSlice <UnityEngine.Experimental.AI.PolygonId> path, int n, Vector3 termPos,
ref Unity.Collections.NativeArray <UnityEngine.Experimental.AI.NavMeshLocation> straightPath,
ref Unity.Collections.NativeArray <StraightPathFlags> straightPathFlags, int maxStraightPath)
{
for (var k = startIndex; k < endIndex - 1; ++k)
{
var type1 = query.GetPolygonType(path[k]);
var type2 = query.GetPolygonType(path[k + 1]);
if (type1 != type2)
{
Vector3 l, r;
var status = query.GetPortalPoints(path[k], path[k + 1], out l, out r);
Unity.Mathematics.float3 cpa1, cpa2;
GeometryUtils.SegmentSegmentCPA(out cpa1, out cpa2, l, r, straightPath[n - 1].position, termPos);
straightPath[n] = query.CreateLocation(cpa1, path[k + 1]);
straightPathFlags[n] = type2 == UnityEngine.Experimental.AI.NavMeshPolyTypes.OffMeshConnection ? StraightPathFlags.OffMeshConnection : 0;
if (++n == maxStraightPath)
{
return(maxStraightPath);
}
}
}
straightPath[n] = query.CreateLocation(termPos, path[endIndex]);
straightPathFlags[n] = query.GetPolygonType(path[endIndex]) == UnityEngine.Experimental.AI.NavMeshPolyTypes.OffMeshConnection
? StraightPathFlags.OffMeshConnection
: 0;
return(++n);
}
public static UnityEngine.Experimental.AI.PathQueryStatus FindStraightPath(UnityEngine.Experimental.AI.NavMeshQuery query, Vector3 startPos, Vector3 endPos,
Unity.Collections.NativeSlice <UnityEngine.Experimental.AI.PolygonId> path, int pathSize,
ref Unity.Collections.NativeArray <UnityEngine.Experimental.AI.NavMeshLocation> straightPath,
ref Unity.Collections.NativeArray <StraightPathFlags> straightPathFlags,
ref Unity.Collections.NativeArray <float> vertexSide, ref int straightPathCount,
int maxStraightPath)
{
if (!query.IsValid(path[0]))
{
straightPath[0] = new UnityEngine.Experimental.AI.NavMeshLocation(); // empty terminator
return(UnityEngine.Experimental.AI.PathQueryStatus.Failure); // | PathQueryStatus.InvalidParam;
}
straightPath[0] = query.CreateLocation(startPos, path[0]);
straightPathFlags[0] = StraightPathFlags.Start;
var apexIndex = 0;
var n = 1;
if (pathSize > 1)
{
var startPolyWorldToLocal = query.PolygonWorldToLocalMatrix(path[0]);
var apex = startPolyWorldToLocal.MultiplyPoint(startPos);
var left = new Vector3(0, 0, 0); // Vector3.zero accesses a static readonly which does not work in burst yet
var right = new Vector3(0, 0, 0);
var leftIndex = -1;
var rightIndex = -1;
for (var i = 1; i <= pathSize; ++i)
{
var polyWorldToLocal = query.PolygonWorldToLocalMatrix(path[apexIndex]);
Vector3 vl, vr;
if (i == pathSize)
{
vl = vr = polyWorldToLocal.MultiplyPoint(endPos);
}
else
{
var success = query.GetPortalPoints(path[i - 1], path[i], out vl, out vr);
if (!success)
{
return(UnityEngine.Experimental.AI.PathQueryStatus.Failure); // | PathQueryStatus.InvalidParam;
}
vl = polyWorldToLocal.MultiplyPoint(vl);
vr = polyWorldToLocal.MultiplyPoint(vr);
}
vl = vl - apex;
vr = vr - apex;
// Ensure left/right ordering
if (PathUtils.Perp2D(vl, vr) < 0)
{
PathUtils.Swap(ref vl, ref vr);
}
// Terminate funnel by turning
if (PathUtils.Perp2D(left, vr) < 0)
{
var polyLocalToWorld = query.PolygonLocalToWorldMatrix(path[apexIndex]);
var termPos = polyLocalToWorld.MultiplyPoint(apex + left);
n = PathUtils.RetracePortals(query, apexIndex, leftIndex, path, n, termPos, ref straightPath, ref straightPathFlags, maxStraightPath);
if (vertexSide.Length > 0)
{
vertexSide[n - 1] = -1;
}
//Debug.Log("LEFT");
if (n == maxStraightPath)
{
straightPathCount = n;
return(UnityEngine.Experimental.AI.PathQueryStatus.Success); // | PathQueryStatus.BufferTooSmall;
}
apex = polyWorldToLocal.MultiplyPoint(termPos);
left.Set(0, 0, 0);
right.Set(0, 0, 0);
i = apexIndex = leftIndex;
continue;
}
if (PathUtils.Perp2D(right, vl) > 0)
{
var polyLocalToWorld = query.PolygonLocalToWorldMatrix(path[apexIndex]);
var termPos = polyLocalToWorld.MultiplyPoint(apex + right);
n = PathUtils.RetracePortals(query, apexIndex, rightIndex, path, n, termPos, ref straightPath, ref straightPathFlags, maxStraightPath);
if (vertexSide.Length > 0)
{
vertexSide[n - 1] = 1;
}
//Debug.Log("RIGHT");
if (n == maxStraightPath)
{
straightPathCount = n;
return(UnityEngine.Experimental.AI.PathQueryStatus.Success); // | PathQueryStatus.BufferTooSmall;
}
apex = polyWorldToLocal.MultiplyPoint(termPos);
left.Set(0, 0, 0);
right.Set(0, 0, 0);
i = apexIndex = rightIndex;
continue;
}
// Narrow funnel
if (PathUtils.Perp2D(left, vl) >= 0)
{
left = vl;
leftIndex = i;
}
if (PathUtils.Perp2D(right, vr) <= 0)
{
right = vr;
rightIndex = i;
}
}
}
// Remove the the next to last if duplicate point - e.g. start and end positions are the same
// (in which case we have get a single point)
if (n > 0 && straightPath[n - 1].position == endPos)
{
n--;
}
n = PathUtils.RetracePortals(query, apexIndex, pathSize - 1, path, n, endPos, ref straightPath, ref straightPathFlags, maxStraightPath);
if (vertexSide.Length > 0)
{
vertexSide[n - 1] = 0;
}
if (n == maxStraightPath)
{
straightPathCount = n;
return(UnityEngine.Experimental.AI.PathQueryStatus.Success); // | PathQueryStatus.BufferTooSmall;
}
// Fix flag for final path point
straightPathFlags[n - 1] = StraightPathFlags.End;
straightPathCount = n;
return(UnityEngine.Experimental.AI.PathQueryStatus.Success);
}
