private Vector3Int GetTargetPosition(PathData path, PathGrid grid, float delta, float totalTime)
{
Vector3 toTarget = (path.target - path.currentPosition);
Vector3 direction = toTarget.normalized;
Vector3Int startGridPosition = grid.GetGridPosition(path.currentPosition);
Vector3Int targetGridPos = startGridPosition;
float moveAmount = Mathf.Min(MAX_VELOCITY, toTarget.magnitude);
int maxBoxesPerStep = Mathf.Max(1, Mathf.CeilToInt((MAX_VELOCITY * delta) / PathGrid.GridSize));
float interval = moveAmount / maxBoxesPerStep;
for (int i = 1; i <= maxBoxesPerStep; i++)
{
float amt = i * interval;
Vector3 physicalPos = path.currentPosition + (amt * direction);
Vector3Int gridPos = grid.GetGridPosition(physicalPos);
bool available = (gridPos == path.targetGridPos || grid.IsVolumeAvailable(gridPos, path.drone));
if (gridPos != targetGridPos && available)
{
targetGridPos = gridPos;
grid.OccupyVolume(gridPos, path.drone);
path.visitedGridPositions.Add(gridPos);
if (gridPos == path.targetGridPos)
{
return(targetGridPos);
}
}
}
if (targetGridPos == startGridPosition)
{
float minHeuristic = SearchHeuristic(path, targetGridPos, direction) + path.staticPenalty;
for (int i = -1; i <= 1; i++)
{
for (int j = -1; j <= 1; j++)
{
for (int k = -1; k <= 1; k++)
{
Vector3Int potentialPosition = startGridPosition + new Vector3Int(i, j, k);
if (grid.IsVolumeAvailable(potentialPosition, path.drone) && !path.visitedGridPositions.Contains(potentialPosition))
{
float currentHeuristic = SearchHeuristic(path, potentialPosition, direction);
if (currentHeuristic < minHeuristic)
{
targetGridPos = potentialPosition;
minHeuristic = currentHeuristic;
}
}
}
}
}
}
if (targetGridPos == startGridPosition)
{
path.staticPenalty += 0.05f;
Debug.Log($"Drone not moving during this time step (totalTime: {totalTime}) couldn't find valid position!! Penalty: {path.staticPenalty}");
}
else
{
path.staticPenalty = 0.0f;
}
path.visitedGridPositions.Add(targetGridPos);
grid.OccupyVolume(targetGridPos, path.drone);
return(targetGridPos);
}