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