private float calculateTotalPotential(Vector3f dronePosition, Vector3f goal, APFObstacle nearestObstacle, bool collisionDetection = false)
{
//Calculate uAttract = pAttract * dGoal
float distanceToGoal = Vector3.Distance(goal.ToUnityVector(), dronePosition.ToUnityVector());
float uAttract = AttractionConst * distanceToGoal;
float uRepel;
if (nearestObstacle.type == APFObstacleType.NONE)
{
uRepel = 0;
}
else
{
float distanceToNearestObstacle = Vector3.Distance(dronePosition.ToUnityVector(), nearestObstacle.position.ToUnityVector());
uRepel = distanceToNearestObstacle < InfuentialDistanceConstant
? uRepel = RepulsionConst / (distanceToNearestObstacle - DroneRadius)
: 0;
}
Vector3f previousTarget = DroneDataWriter.Data.previousTarget;
float uRet = previousTarget.y < 0
? 0
: ReturnConstant *Vector3.Distance(dronePosition.ToUnityVector(), DroneDataWriter.Data.previousTarget.ToUnityVector());
return(uAttract + uRepel + uRet);
}
private Vector3 CalculateDirection()
{
Vector3f dp = transform.position.ToSpatialVector3f();
Vector3f goal = DroneDataWriter.Data.target;
APFObstacle nearest = GetNearestObstacle(dp.ToUnityVector(), true);
if (nearest.type == APFObstacleType.NONE)
{
Vector3 direction = goal.ToUnityVector() - dp.ToUnityVector();
return(direction.normalized);
}
float potentialAtDrone = calculateTotalPotential(dp, goal, nearest);
Vector3f xDpos = new Vector3f(dp.x + 1, dp.y, dp.z);
Vector3f yDpos = new Vector3f(dp.x, dp.y + 1, dp.z);
Vector3f zDpos = new Vector3f(dp.x, dp.y, dp.z + 1);
float xD = calculateTotalPotential(xDpos, goal, GetNearestObstacle(xDpos.ToUnityVector(), false)) - potentialAtDrone;
float yD = calculateTotalPotential(yDpos, goal, GetNearestObstacle(yDpos.ToUnityVector(), false)) - potentialAtDrone;
float zD = calculateTotalPotential(zDpos, goal, GetNearestObstacle(zDpos.ToUnityVector(), false)) - potentialAtDrone;
// TODO: Add modifying factor here in order to disencourage changes in altitude
return(-1 * new Vector3(xD, yD, zD).normalized);
}
private void MoveDrone(APFObstacle obstacle)
{
nearestStaticObstacle = obstacle;
Vector3 direction = CalculateDirection();
DroneDataWriter.Send(new DroneData.Update().SetDirection(direction.ToSpatialVector3f()));
}
private void OnEnable()
{
RepulsionConst = SimulationSettings.RepulsionConst;
AttractionConst = SimulationSettings.AttractionConst;
InfuentialDistanceConstant = SimulationSettings.InfuentialDistanceConstant;
ReturnConstant = SimulationSettings.ReturnConstant;
DroneRadius = SimulationSettings.DroneRadius;
droneCalcReady = false;
dummyObstacle = new APFObstacle(APFObstacleType.NONE, new Vector3f(0, -1, 0));
nearestDrone = new APFObstacle(APFObstacleType.DRONE, new Vector3f(0, -1, 0));
radiusOfInfluence = 2 * DroneDataWriter.Data.speed * SimulationSettings.DroneUpdateInterval;
nearestDroneDistance = radiusOfInfluence;
}