Multicopter Movement: Throw Error when calculation is not correct!

src/de/tud/kom/p2psim/impl/topology/movement/aerial/StateMulticopterMovement.java

@@ -28,8 +28,6 @@ import javax.persistence.Column;
 import javax.persistence.Entity;
 import javax.persistence.Table;
 
-import org.apache.logging.log4j.Level;
-
 import de.tud.kom.p2psim.api.topology.movement.UAVMovementModel;
 import de.tud.kom.p2psim.api.uav.MulticopterModel;
 import de.tud.kom.p2psim.impl.energy.components.ActuatorComponent;
@@ -39,7 +37,6 @@ import de.tud.kom.p2psim.impl.util.db.metric.CustomMeasurement;
 import de.tudarmstadt.maki.simonstrator.api.Monitor;
 import de.tudarmstadt.maki.simonstrator.api.Time;
 import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocationCallback;
-import umontreal.ssj.util.BitMatrix.IncompatibleDimensionException;
 
 /**
  * Complex thrust based movement model using specific models of a multicopter UAV for calculation.
@@ -302,9 +299,7 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 			Monitor.log(StateMulticopterMovement.class, Monitor.Level.WARN, 
 					"(Horizontal Flight) The Multicopter is not properly aligned to the target. Deviation: " + Math.toDegrees(horizontalAngleToPosition(currentTargetPosition))+"°", positionVector);	
 		}	
-		
-		
-		
+				
 		double v = flightVelocity();
 		if(v != velocityVector.getLength() && Math.abs(v - velocityVector.getLength()) > MARGIN) {				
 					Monitor.log(StateMulticopterMovement.class, Monitor.Level.WARN, 
@@ -347,6 +342,10 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 		
 		constTime = (double) constantPhaseTime / Time.SECOND;
 		constDist = constTime * v;
+		
+		if(constDist > distanceToTarget) {
+			throw new UnsupportedOperationException("Multicopter Movement: The approximated step distance exceeds the distance to the target");
+		}
 				
 		directionVector.normalize(); // should be valid anyways?	
 		positionVector = positionVector.plus(directionVector.scale(constDist));
@@ -405,6 +404,10 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 			vint = 0;
 		}
 		
+		if(decDist > positionVector.distanceTo(currentTargetPosition)) {
+			throw new UnsupportedOperationException("Multicopter Movement: The approximated step distance exceeds the distance to the target");
+		}
+		
 		directionVector.normalize(); // should be valid anyways?	
 		positionVector = positionVector.plus(directionVector.scale(decDist));
 		velocityVector = directionVector.scale(vint);