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