/*
* Copyright (c) 2005-2010 KOM – Multimedia Communications Lab
*
* This file is part of PeerfactSim.KOM.
*
* PeerfactSim.KOM is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* any later version.
*
* PeerfactSim.KOM is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with PeerfactSim.KOM. If not, see .
*
*/
package de.tud.kom.p2psim.impl.topology.movement.aerial;
import java.util.HashMap;
import java.util.LinkedList;
import java.util.Map;
import de.tud.kom.p2psim.api.topology.movement.UAVMovementModel;
import de.tud.kom.p2psim.impl.energy.components.ActuatorEnergyComponent;
import de.tud.kom.p2psim.impl.simengine.Simulator;
import de.tud.kom.p2psim.impl.topology.component.UAVTopologyComponent;
import de.tud.kom.p2psim.impl.topology.util.PositionVector;
import de.tudarmstadt.maki.simonstrator.api.Time;
import de.tudarmstadt.maki.simonstrator.api.component.sensor.location.Location;
/**
* Local movement logic specifically designs the movement for multicopter UAVs.
* This simple movement logic uses straight forward movement with the maximum speed available.
*
* @author Julian Zobel
* @version 1.0, 11.09.2018
*/
public class SimpleMutlicopterMovement implements UAVMovementModel {
private UAVTopologyComponent topologyComponent;
private final double maxCruiseSpeed;
private final double minCruiseSpeed;
private double preferredCruiseSpeed;
private double currentSpeed;
private LinkedList waypoints = new LinkedList<>();
private Map locationCallbacks = new HashMap<>(); // TODO callback interface
public SimpleMutlicopterMovement(UAVTopologyComponent topologyComponent, double maxCruiseSpeed, double minCruiseSpeed) {
this.topologyComponent = topologyComponent;
this.maxCruiseSpeed = maxCruiseSpeed;
this.minCruiseSpeed = minCruiseSpeed;
this.preferredCruiseSpeed = this.maxCruiseSpeed;
waypoints.add(new PositionVector(100,100,100));
waypoints.add(new PositionVector(600,600,15));
waypoints.add(new PositionVector(200,500,70));
}
@Override
public void setPreferredCruiseSpeed(double v_pref) {
this.preferredCruiseSpeed = v_pref;
}
@Override
public double getMaxCruiseSpeed() {
return maxCruiseSpeed;
}
@Override
public double getMinCruiseSpeed() {
return minCruiseSpeed;
}
@Override
public double getCurrentSpeed() {
return currentSpeed;
}
@Override
public void move(long timeBetweenMovementOperations) {
if(!waypoints.isEmpty() && topologyComponent.isActive() ) {
PositionVector currentPosition= topologyComponent.getRealPosition();
Location targetLocation = waypoints.getFirst();
PositionVector targetPosition = new PositionVector(targetLocation);
Double distanceToTargetPosition = targetPosition.distanceTo(currentPosition);
// If target point is reached within a 1 meter margin, we remove that point from the list
if(distanceToTargetPosition < 1 || distanceToTargetPosition < currentSpeed)
{
waypoints.add(waypoints.removeFirst());
topologyComponent.updateCurrentLocation(targetPosition, 0);
currentSpeed = 0;
return;
}
double timefactor = timeBetweenMovementOperations / Time.SECOND;
currentSpeed = Math.min(distanceToTargetPosition, preferredCruiseSpeed);
PositionVector direction = new PositionVector(targetPosition);
direction.subtract(currentPosition);
direction.normalize();
direction.multiplyScalar(currentSpeed * timefactor);
PositionVector newPosition = new PositionVector(currentPosition);
newPosition.add(direction);
topologyComponent.updateCurrentLocation(newPosition, 1);
}
// System.out.println(Simulator.getFormattedTime(Simulator.getCurrentTime()) + " | " + topologyComponent);
}
}