private void StepForward(PathData pathData, PathGrid grid, float deltaTime, float totalTime)
{
OccupyPhysicalPosition(pathData.currentPosition, pathData.drone);
if (pathData.complete)
{
return;
}
Vector3Int newGridPos = GetTargetPosition(pathData, grid, deltaTime, totalTime);
if (newGridPos == grid.GetGridPosition(pathData.currentPosition))
{
//Debug.Log("CHECK 2 | not moving during this time step, couldn't find valid position!!");
}
if (newGridPos == pathData.targetGridPos)
{
pathData.complete = true;
AddPositionToPath(pathData, pathData.target, totalTime);
return;
}
else
{
AddNodeToPath(pathData, newGridPos, totalTime);
}
}
private void StartPath(PathData path, PathGrid grid)
{
GameObject drone = path.drone;
path.currentPosition = drone.transform.position;
path.currentDistance = Vector3.Distance(path.currentPosition, path.target);
path.targetGridPos = grid.GetGridPosition(path.target);
AddPositionToPath(path, drone.transform.position, 0.0f);
}
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);
}
private void OccupyPhysicalPosition(Vector3 position, GameObject drone)
{
Vector3Int gridPos = grid.GetGridPosition(position);
grid.OccupyVolume(gridPos, drone);
}