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