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