Renamed MutlicopterMovement to SimpleMulticopterMovement (because there may be more!)

And UAVLOcationActuator to the SimUAVLocationActuator for peerfact

src/de/tud/kom/p2psim/api/topology/movement/UAVLocationActuator.java → src/de/tud/kom/p2psim/api/topology/movement/SimUAVLocationActuator.java

@@ -20,10 +20,12 @@
 
 package de.tud.kom.p2psim.api.topology.movement;
 
+import de.tud.kom.p2psim.impl.energy.components.ActuatorEnergyComponent;
+import de.tudarmstadt.maki.simonstrator.api.component.sensor.location.ControllableLocationActuator;
 
-
-public interface UAVLocationActuator extends SimLocationActuator {
+public interface SimUAVLocationActuator extends SimLocationActuator, ControllableLocationActuator {
 	
 	public UAVMovementModel getUAVMovement();
 	public void setUAVMovement(UAVMovementModel uavMovement);
+	public ActuatorEnergyComponent getActuatorEnergyComponent();
 }

src/de/tud/kom/p2psim/impl/topology/movement/aerial/MutlicopterMovement.java → src/de/tud/kom/p2psim/impl/topology/movement/aerial/SimpleMutlicopterMovement.java

@@ -22,35 +22,47 @@ package de.tud.kom.p2psim.impl.topology.movement.aerial;
 
 import java.util.HashMap;
 import java.util.LinkedList;
-import java.util.List;
 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.common.graph.INodeID;
+import de.tudarmstadt.maki.simonstrator.api.Time;
+
 import de.tudarmstadt.maki.simonstrator.api.component.sensor.location.Location;
 
-public class MutlicopterMovement implements UAVMovementModel  {
+/**
+ * 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 List<Location> waypoints = new LinkedList<>();
+	private LinkedList<Location> waypoints = new LinkedList<>();
 	private Map<Location, ?> locationCallbacks = new HashMap<>();  // TODO callback interface
+		
 	
-	
-	public MutlicopterMovement(UAVTopologyComponent topologyComponent, double maxCruiseSpeed, double minCruiseSpeed) {
+	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;		
@@ -74,9 +86,41 @@ public class MutlicopterMovement implements UAVMovementModel  {
 	@Override
 	public void move(long timeBetweenMovementOperations) {
 		
-		System.out.println(Simulator.getFormattedTime(Simulator.getCurrentTime()) + " | " +  topologyComponent);
+		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);		
 	}
 
+
+	
 }