IEnumerator StartQuery()
{
NavMeshWorld world = NavMeshWorld.GetDefaultWorld();
NavMeshQuery query = new NavMeshQuery(world, Allocator.Persistent, maxPath);
NavMeshLocation startLocation = query.MapLocation(start.position, Vector3.up * extents, 0);
NavMeshLocation endLocation = query.MapLocation(end.position, Vector3.up * extents, 0);
PathQueryStatus status = query.BeginFindPath(startLocation, endLocation);
yield return(new WaitWhile(() => {
status = query.UpdateFindPath(8, out int iterationsPerformed);
return status == PathQueryStatus.InProgress;
}));
status = query.EndFindPath(out int pathsize);
NativeArray <PolygonId> path = new NativeArray <PolygonId>(pathsize, Allocator.Temp);
int pathResult = query.GetPathResult(path);
//NativeArray<NavMeshLocation> pathStraight = new NativeArray<NavMeshLocation>(maxPath, Allocator.Temp);
////NativeArray<StraightPathFlag> pathStreaigthFlag = new NativeArray<StraightPathFlags>(maxPath, Allocator.Temp);
//NativeArray<float> vertexSize = new NativeArray<float>(maxPath, Allocator.Temp);
//
//int straghtPathCount = 0;
for (var i = 0; i < pathResult; i++)
{
var p = path[i];
var loc = query.CreateLocation(start.position, p);
var target = loc.position;
}
query.Dispose();
}
public void Execute()
{
nml_FromLocation = query.MapLocation(fromLocation, extents, 0);
nml_ToLocation = query.MapLocation(toLocation, extents, 0);
if (query.IsValid(nml_FromLocation) && query.IsValid(nml_ToLocation))
{
status = query.BeginFindPath(nml_FromLocation, nml_ToLocation, -1);
if (status == PathQueryStatus.InProgress)
{
status = query.UpdateFindPath(maxIteration, out int iterationPerformed);
}
if (status == PathQueryStatus.Success)
{
status = query.EndFindPath(out int polygonSize);
NativeArray <NavMeshLocation> res = new NativeArray <NavMeshLocation>(polygonSize, Allocator.Temp);
NativeArray <StraightPathFlags> straightPathFlag = new NativeArray <StraightPathFlags>(maxPathSize, Allocator.Temp);
NativeArray <float> vertexSide = new NativeArray <float>(maxPathSize, Allocator.Temp);
NativeArray <PolygonId> polys = new NativeArray <PolygonId>(polygonSize, Allocator.Temp);
int straightPathCount = 0;
query.GetPathResult(polys);
returningStatus = PathUtils.FindStraightPath(
query,
fromLocation,
toLocation,
polys,
polygonSize,
ref res,
ref straightPathFlag,
ref vertexSide,
ref straightPathCount,
maxPathSize
);
if (returningStatus == PathQueryStatus.Success)
{
int fromKey = ((int)fromLocation.x + (int)fromLocation.y + (int)fromLocation.z) * maxPathSize;
int toKey = ((int)toLocation.x + (int)toLocation.y + (int)toLocation.z) * maxPathSize;
int key = fromKey + toKey;
statusOutput[0] = 1;
statusOutput[1] = key;
statusOutput[2] = straightPathCount;
for (int i = 0; i < straightPathCount; i++)
{
result[i] = (float3)res[i].position + new float3(0, 0.75f, 0); // elevated point
ub.Add(new Unit_Buffer {
wayPoints = result[i]
});
}
}
res.Dispose();
straightPathFlag.Dispose();
polys.Dispose();
vertexSide.Dispose();
}
}
}
public static PathQueryStatus FindStraightPath(NavMeshQuery query, float3 startPos, float3 endPos, NativeSlice <PolygonId> path, int pathSize, ref NativeArray <NavMeshLocation> straightPath, ref NativeArray <StraightPathFlags> straightPathFlags, ref NativeArray <float> vertexSide, ref int straightPathCount, int maxStraightPath)
{
if (!query.IsValid(path[0]))
{
straightPath[0] = new NavMeshLocation();
return(PathQueryStatus.Failure);
}
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 = (float3)startPolyWorldToLocal.MultiplyPoint(startPos);
var left = float3.zero;
var right = float3.zero;
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(PathQueryStatus.Failure);
}
vl = polyWorldToLocal.MultiplyPoint(vl);
vr = polyWorldToLocal.MultiplyPoint(vr);
}
vl = vl - (Vector3)apex;
vr = vr - (Vector3)apex;
// Ensure left/right ordering:
if (Perp2D(vl, vr) < 0)
{
Swap(ref vl, ref vr);
}
// Terminate funnel by turning:
if (Perp2D(left, vr) < 0)
{
var polyLocalToWorld = query.PolygonLocalToWorldMatrix(path[apexIndex]);
var termPos = polyLocalToWorld.MultiplyPoint(apex + left);
n = RetracePortals(query, apexIndex, leftIndex, path, n, termPos, ref straightPath, ref straightPathFlags, maxStraightPath);
if (vertexSide.Length > 0)
{
vertexSide[n - 1] = -1;
}
if (n == maxStraightPath)
{
straightPathCount = n;
return(PathQueryStatus.Success);
}
apex = polyWorldToLocal.MultiplyPoint(termPos);
left = float3.zero;
right = float3.zero;
i = apexIndex = leftIndex;
continue;
}
if (Perp2D(right, vl) > 0)
{
var polyLocalToWorld = query.PolygonLocalToWorldMatrix(path[apexIndex]);
var termPos = polyLocalToWorld.MultiplyPoint(apex + right);
n = RetracePortals(query, apexIndex, rightIndex, path, n, termPos, ref straightPath, ref straightPathFlags, maxStraightPath);
if (vertexSide.Length > 0)
{
vertexSide[n - 1] = 1;
}
if (n == maxStraightPath)
{
straightPathCount = n;
return(PathQueryStatus.Success);
}
apex = polyWorldToLocal.MultiplyPoint(termPos);
left = float3.zero;
right = float3.zero;
i = apexIndex = rightIndex;
continue;
}
// Narrow funnel:
if (Perp2D(left, vl) >= 0)
{
left = vl;
leftIndex = i;
}
if (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 == (Vector3)endPos)
{
--n;
}
n = 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(PathQueryStatus.Success);
}
// Fix flag for final path point:
straightPathFlags[n - 1] = StraightPathFlags.End;
straightPathCount = n;
return(PathQueryStatus.Success);
}
public static unsafe ulong ReadPolygonId(this NavMeshLocation location)
{
return(*(ulong *)&location);
}
public NavAgent(NavMeshLocation navMeshLocation, PathQueryStatus pathQueryStatus)
{
NavMeshLocation = navMeshLocation;
PathQueryStatus = pathQueryStatus;
}
public NavMeshLocationComponent(NavMeshLocation navMeshLocation)
{
NavMeshLocation = navMeshLocation;
}
public static PathQueryStatus FindStraightPath(NavMeshQuery query, Vector3 startPos, Vector3 endPos
, NativeSlice <PolygonId> path, int pathSize
, ref NativeArray <NavMeshLocation> straightPath
, ref NativeArray <StraightPathFlags> straightPathFlags
, ref NativeArray <float> vertexSide
, ref int straightPathCount
, int maxStraightPath)
{
#if DEBUG_CROWDSYSTEM_ASSERTS
Assert.IsTrue(pathSize > 0, "FindStraightPath: The path cannot be empty");
Assert.IsTrue(path.Length >= pathSize, "FindStraightPath: The array of path polygons must fit at least the size specified");
Assert.IsTrue(maxStraightPath > 1, "FindStraightPath: At least two corners need to be returned, the start and end");
Assert.IsTrue(straightPath.Length >= maxStraightPath, "FindStraightPath: The array of returned corners cannot be smaller than the desired maximum corner count");
Assert.IsTrue(straightPathFlags.Length >= straightPath.Length, "FindStraightPath: The array of returned flags must not be smaller than the array of returned corners");
#endif
if (!query.IsValid(path[0]))
{
straightPath[0] = new NavMeshLocation(); // empty terminator
return(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(PathQueryStatus.Failure); // | PathQueryStatus.InvalidParam;
}
#if DEBUG_CROWDSYSTEM_ASSERTS
Assert.IsTrue(query.IsValid(path[i - 1]));
Assert.IsTrue(query.IsValid(path[i]));
#endif
vl = polyWorldToLocal.MultiplyPoint(vl);
vr = polyWorldToLocal.MultiplyPoint(vr);
}
vl = vl - apex;
vr = vr - apex;
// Ensure left/right ordering
if (Perp2D(vl, vr) < 0)
{
Swap(ref vl, ref vr);
}
// Terminate funnel by turning
if (Perp2D(left, vr) < 0)
{
var polyLocalToWorld = query.PolygonLocalToWorldMatrix(path[apexIndex]);
var termPos = polyLocalToWorld.MultiplyPoint(apex + left);
n = 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(PathQueryStatus.Success); // | PathQueryStatus.BufferTooSmall;
}
apex = polyWorldToLocal.MultiplyPoint(termPos);
left.Set(0, 0, 0);
right.Set(0, 0, 0);
i = apexIndex = leftIndex;
continue;
}
if (Perp2D(right, vl) > 0)
{
var polyLocalToWorld = query.PolygonLocalToWorldMatrix(path[apexIndex]);
var termPos = polyLocalToWorld.MultiplyPoint(apex + right);
n = 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(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 (Perp2D(left, vl) >= 0)
{
left = vl;
leftIndex = i;
}
if (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 = 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(PathQueryStatus.Success); // | PathQueryStatus.BufferTooSmall;
}
// Fix flag for final path point
straightPathFlags[n - 1] = StraightPathFlags.End;
straightPathCount = n;
return(PathQueryStatus.Success);
}
