float GetCellCost(NativeArray <Cell> grid, NavigationCapabilities capabilities, int fromIndex, int toIndex, Neighbour neighbour, bool areNeighbours) { var target = grid[toIndex]; if (target.Blocked) { return(float.PositiveInfinity); } // If we're not neighbours, then we're a portal and can just go straight there if (!areNeighbours) { return(1); } var from = grid[fromIndex]; var heightDiff = target.Height - from.Height; var absDiff = math.abs(heightDiff); // TODO Should precompute this var dropHeight = 0; var climbHeight = 0; if (heightDiff > 0) { climbHeight = absDiff; } else { dropHeight = absDiff; } var slope = math.degrees(math.atan(absDiff / neighbour.Distance)); // TODO End precompute if ((capabilities.MaxClimbHeight < climbHeight || capabilities.MaxDropHeight < dropHeight) && capabilities.MaxSlopeAngle < slope) { return(float.PositiveInfinity); } return(1); }
public void Execute(ArchetypeChunk chunk, int chunkIndex, int firstEntityIndex) { //BufferAccessor<Cell> accessor = this.GridChunks[0].GetBufferAccessor(this.CellTypeRO); //DynamicBuffer<Cell> grid = accessor[0].Reinterpret<Cell>(); int size = DimX * DimY; BufferAccessor <Waypoint> Waypoints = chunk.GetBufferAccessor(WaypointChunkBuffer); NativeArray <PathRequest> PathRequests = chunk.GetNativeArray(PathRequestsChunkComponent); NativeArray <Translation> Translations = chunk.GetNativeArray(TranslationsChunkComponent); NativeArray <NavigationCapabilities> NavigationCapabilities = chunk.GetNativeArray(NavigationCapabilitiesChunkComponent); NativeArray <float> CostSoFar = new NativeArray <float>(size * chunk.Count, Allocator.Temp); NativeArray <int2> CameFrom = new NativeArray <int2>(size * chunk.Count, Allocator.Temp); NativeMinHeap OpenSet = new NativeMinHeap((Iterations + 1) * Neighbors.Length * chunk.Count, Allocator.Temp); for (int i = chunkIndex; i < chunk.Count; i++) { NativeSlice <float> costSoFar = CostSoFar.Slice(i * size, size); NativeSlice <int2> cameFrom = CameFrom.Slice(i * size, size); int openSetSize = (Iterations + 1) * NeighborCount; NativeMinHeap openSet = OpenSet.Slice(i * openSetSize, openSetSize); PathRequest request = PathRequests[i]; // Clear our shared data //var buffer = costSoFar.GetUnsafePtr(); //UnsafeUtility.MemClear(buffer, (long)costSoFar.Length * UnsafeUtility.SizeOf<float>()); //openSet.Clear(); Translation currentPosition = Translations[i]; NavigationCapabilities capability = NavigationCapabilities[i]; // cache these as they're used a lot int2 start = currentPosition.Value.xy.FloorToInt(); int2 goal = request.end; DynamicBuffer <float3> waypoints = Waypoints[i].Reinterpret <float3>(); waypoints.Clear(); // Special case when the start is the same point as the goal if (start.Equals(goal)) { // We just set the destination as the goal, but need to get the correct height int gridIndex = this.GetIndex(goal); Cell cell = CellArray[gridIndex]; float3 point = new float3(request.Destination.x, request.Destination.y, cell.Height); waypoints.Add(point); continue; } var stash = new InstanceStash { Grid = CellArray, CameFrom = cameFrom, CostSoFar = costSoFar, OpenSet = openSet, Request = request, Capability = capability, CurrentPosition = currentPosition, Start = start, Goal = goal, Waypoints = waypoints, }; if (this.ProcessPath(ref stash)) { this.ReconstructPath(stash); } } CostSoFar.Dispose(); CameFrom.Dispose(); OpenSet.Dispose(); }