// 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);
}
public Path Run <TMod>(LogLevel pathfindingLogLevel, Vector3 fromPoint, Vector3 toPoint, Constraint constraint, Graph graph, TMod pathModifier, int threadIndex = 0,
bool burstEnabled = true, bool cacheEnabled = false) where TMod : struct, IPathModifier
{
var path = new Path();
var pathResult = new PathInternal();
var navMeshGraph = (NavMeshGraph)graph;
var areas = -1;
if (constraint.checkArea == true)
{
areas = (int)constraint.areaMask;
}
System.Diagnostics.Stopwatch swPath = null;
if ((pathfindingLogLevel & LogLevel.Path) != 0)
{
swPath = System.Diagnostics.Stopwatch.StartNew();
}
var statLength = 0;
var statVisited = 0;
var query = new UnityEngine.Experimental.AI.NavMeshQuery(UnityEngine.Experimental.AI.NavMeshWorld.GetDefaultWorld(), Unity.Collections.Allocator.TempJob, PathfindingNavMeshProcessor.POOL_SIZE);
if (burstEnabled == true)
{
var results = new Unity.Collections.NativeArray <UnityEngine.Experimental.AI.NavMeshLocation>(PathfindingNavMeshProcessor.MAX_PATH_SIZE, Unity.Collections.Allocator.TempJob);
var pathResults = new Unity.Collections.NativeArray <int>(2, Unity.Collections.Allocator.TempJob);
var job = new BuildPathJob()
{
query = query,
fromPoint = fromPoint,
toPoint = toPoint,
agentTypeId = navMeshGraph.agentTypeId,
areas = areas,
pathResults = pathResults,
results = results,
};
job.Schedule().Complete();
var pathStatus = (UnityEngine.Experimental.AI.PathQueryStatus)pathResults[0];
var cornerCount = pathResults[1];
pathResults.Dispose();
if ((pathStatus & UnityEngine.Experimental.AI.PathQueryStatus.Success) != 0)
{
if (cornerCount >= 2)
{
path.navMeshPoints = PoolListCopyable <Vector3> .Spawn(cornerCount);
for (var i = 0; i < cornerCount; ++i)
{
path.navMeshPoints.Add(results[i].position);
}
if ((pathfindingLogLevel & LogLevel.Path) != 0)
{
var hash = 0;
for (var i = 0; i < cornerCount; ++i)
{
hash ^= (int)(results[i].position.x * 1000000f);
}
UnityEngine.Debug.Log("Path hash X: " + hash);
hash = 0;
for (var i = 0; i < cornerCount; ++i)
{
hash ^= (int)(results[i].position.y * 1000000f);
}
UnityEngine.Debug.Log("Path hash Y: " + hash);
hash = 0;
for (var i = 0; i < cornerCount; ++i)
{
hash ^= (int)(results[i].position.z * 1000000f);
}
UnityEngine.Debug.Log("Path hash Z: " + hash);
}
if ((pathStatus & UnityEngine.Experimental.AI.PathQueryStatus.PartialResult) != 0)
{
path.result = PathCompleteState.CompletePartial;
}
else
{
path.result = PathCompleteState.Complete;
}
}
else
{
path.result = PathCompleteState.NotExist;
}
}
results.Dispose();
query.Dispose();
return(path);
}
UnityEngine.AI.NavMesh.SamplePosition(fromPoint, out var hitFrom, 1000f, new UnityEngine.AI.NavMeshQueryFilter()
{
agentTypeID = navMeshGraph.agentTypeId,
areaMask = areas,
});
fromPoint = hitFrom.position;
var from = query.MapLocation(fromPoint, Vector3.one * 10f, navMeshGraph.agentTypeId, areas);
if (from.polygon.IsNull() == true)
{
return(path);
}
UnityEngine.AI.NavMesh.SamplePosition(toPoint, out var hitTo, 1000f, new UnityEngine.AI.NavMeshQueryFilter()
{
agentTypeID = navMeshGraph.agentTypeId,
areaMask = areas,
});
toPoint = hitTo.position;
var to = query.MapLocation(toPoint, Vector3.one * 10f, navMeshGraph.agentTypeId, areas);
if (to.polygon.IsNull() == true)
{
return(path);
}
var marker = new Unity.Profiling.ProfilerMarker("PathfindingNavMeshProcessor::Query::BuildPath");
marker.Begin();
query.BeginFindPath(from, to, areas);
query.UpdateFindPath(PathfindingNavMeshProcessor.MAX_ITERATIONS, out var performed);
marker.End();
statVisited = performed;
var result = query.EndFindPath(out var pathSize);
if ((result & UnityEngine.Experimental.AI.PathQueryStatus.Success) != 0)
{
var pathInternal = new Unity.Collections.NativeArray <UnityEngine.Experimental.AI.PolygonId>(pathSize, Unity.Collections.Allocator.Persistent);
query.GetPathResult(pathInternal);
var markerFindStraight = new Unity.Profiling.ProfilerMarker("PathfindingNavMeshProcessor::Query::FindStraightPath");
markerFindStraight.Begin();
var straightPathFlags = new Unity.Collections.NativeArray <StraightPathFlags>(PathfindingNavMeshProcessor.MAX_PATH_SIZE, Unity.Collections.Allocator.Persistent);
var vertexSide = new Unity.Collections.NativeArray <float>(PathfindingNavMeshProcessor.MAX_PATH_SIZE, Unity.Collections.Allocator.Persistent);
var results = new Unity.Collections.NativeArray <UnityEngine.Experimental.AI.NavMeshLocation>(PathfindingNavMeshProcessor.MAX_PATH_SIZE, Unity.Collections.Allocator.Persistent);
var resultStatus = new Unity.Collections.NativeArray <int>(2, Unity.Collections.Allocator.TempJob);
var job = new FindStraightPathJob()
{
query = query,
from = from,
to = to,
pathInternal = pathInternal,
pathSize = pathSize,
results = results,
straightPathFlags = straightPathFlags,
vertexSide = vertexSide,
resultStatus = resultStatus,
};
job.Schedule().Complete();
var pathStatus = (UnityEngine.Experimental.AI.PathQueryStatus)job.resultStatus[0];
var cornerCount = job.resultStatus[1];
resultStatus.Dispose();
statLength = cornerCount;
markerFindStraight.End();
if (pathStatus == UnityEngine.Experimental.AI.PathQueryStatus.Success)
{
/*for (int i = 1; i < cornerCount; ++i) {
*
* Gizmos.color = Color.green;
* Gizmos.DrawLine(results[i].position, results[i - 1].position);
*
* }*/
pathResult.pathStatus = pathStatus;
pathResult.results = results;
pathResult.corners = cornerCount;
if (cornerCount >= 2)
{
path.navMeshPoints = PoolListCopyable <Vector3> .Spawn(cornerCount);
for (var i = 0; i < cornerCount; ++i)
{
path.navMeshPoints.Add(results[i].position);
}
if ((pathfindingLogLevel & LogLevel.Path) != 0)
{
var hash = 0;
for (var i = 0; i < cornerCount; ++i)
{
hash ^= (int)(results[i].position.x * 1000000f);
}
UnityEngine.Debug.Log("Path hash X: " + hash);
hash = 0;
for (var i = 0; i < cornerCount; ++i)
{
hash ^= (int)(results[i].position.y * 1000000f);
}
UnityEngine.Debug.Log("Path hash Y: " + hash);
hash = 0;
for (var i = 0; i < cornerCount; ++i)
{
hash ^= (int)(results[i].position.z * 1000000f);
}
UnityEngine.Debug.Log("Path hash Z: " + hash);
}
path.result = PathCompleteState.Complete;
}
else
{
path.result = PathCompleteState.NotExist;
}
pathResult.Dispose();
}
else
{
path.result = PathCompleteState.NotExist;
results.Dispose();
}
vertexSide.Dispose();
straightPathFlags.Dispose();
pathInternal.Dispose();
}
else
{
path.result = PathCompleteState.NotExist;
//Debug.LogWarning("Path result: " + result + ", performed: " + performed);
}
System.Diagnostics.Stopwatch swModifier = null;
if ((pathfindingLogLevel & LogLevel.PathMods) != 0)
{
swModifier = System.Diagnostics.Stopwatch.StartNew();
}
if ((path.result & PathCompleteState.Complete) != 0)
{
path = pathModifier.Run(path, constraint);
}
if ((pathfindingLogLevel & LogLevel.Path) != 0)
{
Logger.Log(
$"Path result {path.result}, built in {(swPath.ElapsedTicks / (double)System.TimeSpan.TicksPerMillisecond).ToString("0.##")}ms. Path length: {statLength} (Visited: {statVisited})\nThread Index: {threadIndex}");
}
if ((pathfindingLogLevel & LogLevel.PathMods) != 0)
{
Logger.Log($"Path Mods: {swModifier.ElapsedMilliseconds}ms");
}
query.Dispose();
return(path);
}
