Minor bugfix

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

@@ -39,6 +39,7 @@ 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.
@@ -504,6 +505,7 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 	
 		PositionVector tangentPoint = calculateCircleTangentExitPoint(circleCenter, turnRadius, currentTargetPosition, angleToTarget);
 		
+		try {
 		// calculate circular path from the current location to the destination tangent point
 		double circleAngle = 2 * Math.asin(positionVector.distanceTo(tangentPoint) / (2 * turnRadius));
 		double circleDistance = circleAngle * turnRadius;		
@@ -533,13 +535,14 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 		positionVector = new PositionVector(circleLocationX, circleLocationY, positionVector.getZ());
 						
 		currentAngleOfAttack = uav.pitchAngleForHorizontalVelocity(curveVelocity);
-		
+	
 		// direction vector is the tangent at the new position
 		// x' = x cos θ − y sin θ
 		// y' = x sin θ + y cos θ
 		double xn = directionVector.getX() * Math.cos(circleAngle) - directionVector.getY() * Math.sin(circleAngle);
 		double yn = directionVector.getX() * Math.sin(circleAngle) + directionVector.getY() * Math.cos(circleAngle);
-				
+		
+			
 		directionVector = new PositionVector(xn, yn);
 		directionVector.normalize();		
 		
@@ -559,6 +562,11 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 		
 		// remaining time
 		return time - constantPhaseTime;
+		}
+		catch(Exception e) {
+			System.out.println();
+			return 0;
+		}
 	}
 	
 	
@@ -945,6 +953,14 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 		PositionVector T1 = new PositionVector(T1x, T1y, positionVector.getZ());
 		PositionVector T2 = new PositionVector(T2x, T2y, positionVector.getZ());
 		
+		// pick the other one, if one is the current location
+		if(T1.equals(positionVector)) {
+			return T2;
+		}
+		else if(T2.equals(positionVector)) {
+			return T1;
+		}
+				
 		// now define which point to use for exiting	
 		double aT1 = horizontalAngleToPosition(T1);
 		double aT2 = horizontalAngleToPosition(T2);
@@ -979,6 +995,13 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 				tangentPoint = Math.abs(aT1) < Math.abs(aT2) ? T1 : T2;
 			}	
 		}
+		else if(angleToTarget == 0) {
+			throw new AssertionError();
+		}
+		
+		if(tangentPoint == null) {
+			throw new AssertionError();
+		}
 		
 		return tangentPoint;
 	}