left curve now correct

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

@@ -224,7 +224,7 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 		if(intrastate == IntraState.STOP || intrastate == IntraState.ADJUST) {
 			
 			// not directly in front of the UAV... turn or curve
-			if(getHorizontalAngleToTarget() != 0) {
+			if(horizontalAngleToPosition(currentTargetPosition) != 0) {
 				if(velocityVector.getLength() == 0) {
 					System.out.println("turn before acc");
 					intrastate = IntraState.TURN;
@@ -245,7 +245,7 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 		}
 		else if(intrastate == IntraState.TURN) {
 					
-			double rot = getHorizontalAngleToTarget();			
+			double rot = horizontalAngleToPosition(currentTargetPosition);;			
 			double turnTime = Math.abs(rot) / maximumTurnAngle;						
 			long turnPhaseTime = (long) (turnTime * Time.SECOND);
 			
@@ -282,7 +282,7 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 		{
 			// acceleration
 			assert velocityVector.getZ() == 0;
-			assert getHorizontalAngleToTarget() == 0;
+			assert horizontalAngleToPosition(currentTargetPosition) == 0;
 			
 			double distanceToTarget = positionVector.distanceTo(currentTargetPosition);
 			
@@ -353,8 +353,8 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 		{			
 			assert velocityVector.getZ() == 0;
 			
-			if(getHorizontalAngleToTarget() != 0) {
-				System.out.println(Math.toDegrees(getHorizontalAngleToTarget()));
+			if(horizontalAngleToPosition(currentTargetPosition) != 0) {
+				System.out.println(Math.toDegrees(horizontalAngleToPosition(currentTargetPosition)));
 				System.out.println();
 			}
 			
@@ -437,8 +437,8 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 		{
 			assert velocityVector.getZ() == 0;
 			
-			if(getHorizontalAngleToTarget() != 0) {
-				System.out.println(getHorizontalAngleToTarget());
+			if(horizontalAngleToPosition(currentTargetPosition) != 0) {
+				System.out.println(horizontalAngleToPosition(currentTargetPosition));
 			}
 			
 			double v = velocityVector.getLength();
@@ -517,7 +517,8 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 			 */
 			
 			// calculate turn circle
-			double angle = getHorizontalAngleToTarget();
+			double angle = horizontalAngleToPosition(currentTargetPosition);
+
 			PositionVector circleCenter = null;
 			
 			
@@ -567,24 +568,47 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 			PositionVector T1 = new PositionVector(T1x, T1y, positionVector.getZ());
 			PositionVector T2 = new PositionVector(T2x, T2y, positionVector.getZ());
 			
+			// now define which point to use...		
+			double aT1 = horizontalAngleToPosition(T1);
+			double aT2 = horizontalAngleToPosition(T2);
+			
+			System.out.println(Math.toDegrees(horizontalAngleToPosition(T1)));
+			System.out.println(Math.toDegrees(horizontalAngleToPosition(T2)));
+			
 			System.out.println(T1);
 			System.out.println(T2);
 			System.out.println();
 			
-			// now define which point to use...
-			double bearingT1 = positionVector.bearingTo(T1);
-			double bearingT2 = positionVector.bearingTo(T2);
-			
-			System.out.println(Math.toDegrees(bearingT1));
-			System.out.println(Math.toDegrees(bearingT2));
-			
-			PositionVector tangentPoint;
+						
+			PositionVector tangentPoint = null;
 			
-			if(Math.abs(bearingT1) < Math.abs(bearingT2)) {
-				tangentPoint = T1;
+			// left -> neg angles
+			if(angle < 0) {
+				assert aT1 < 0 || aT2 < 0;
+				
+				if(aT1 < 0 && aT2 > 0) {
+					tangentPoint = T1;
+				}
+				else if(aT1 > 0 && aT2 < 0) {
+					tangentPoint = T2;
+				}
+				else if(aT1 < 0 && aT2 < 0) {
+					tangentPoint = Math.abs(aT1) < Math.abs(aT2) ? T1 : T2;
+				}				
 			}
-			else {
-				tangentPoint = T2;
+			// right
+			else if(angle > 0) {
+				assert aT1 > 0 || aT2 > 0;
+				
+				if(aT1 < 0 && aT2 > 0) {
+					tangentPoint = T2;
+				}
+				else if(aT1 > 0 && aT2 < 0) {
+					tangentPoint = T1;
+				}
+				else if(aT1 > 0 && aT2 > 0) {
+					tangentPoint = Math.abs(aT1) < Math.abs(aT2) ? T1 : T2;
+				}	
 			}
 			
 			double circleAngle = 2 * Math.asin(positionVector.distanceTo(tangentPoint) / (2 * turnRadius));
@@ -607,8 +631,13 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 			}
 			
 			constTime = (double) constantPhaseTime / Time.SECOND;
-			circleDistance = constTime * curveVelocity;
+			circleDistance = constTime * curveVelocity;			
 			circleAngle = circleDistance / turnRadius;
+			
+			// left
+			if(angle < 0) {
+				circleAngle = -circleAngle;
+			}
 					
 			System.out.println(Math.toDegrees(circleAngle));
 			
@@ -1028,57 +1057,19 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 	
 	///////////
 	///////////
-	
+
 	/**
 	 * 
 	 * @return >0 -> right; <0 -> left  
 	 */
-	private double getHorizontalAngleToTarget() {
-		PositionVector directionToTarget = currentTargetPosition.minus(positionVector);
-		directionToTarget.normalize();		
-		
-		double num = (directionToTarget.getX() * directionVector.getX() + directionToTarget.getY() * directionVector.getY());
-		double den = (directionToTarget.getLength() * directionVector.getLength());
-		double cos =  num / den;
-		
+	private double horizontalAngleToPosition(PositionVector target) {
+		PositionVector directionToTarget = target.minus(positionVector);
+		directionToTarget.normalize();				
+				
 		double dot1 = directionVector.getX() * directionToTarget.getX() + directionVector.getY() * directionToTarget.getY(); //dot product between [x1, y1] and [x2, y2]
 		double det1 = directionVector.getX() * directionToTarget.getY() - directionVector.getY() * directionToTarget.getX(); // determinant
 		double angle2 = Math.atan2(det1, dot1);  // atan2(y, x) or atan2(sin, cos)
-		System.out.println(Math.toDegrees(angle2));
-		
-		if(cos > 1) {
-			if(cos > 1 + Math.pow(10, -3)) {
-				throw new UnsupportedOperationException("cannot be bigger than 1!");
-			}
-			else {
-				cos = 1;
-			}
-		}
-		else if(cos < -1) {
-			if(cos < -1 - Math.pow(10, -3)) {
-				throw new UnsupportedOperationException("cannot be bigger than 1!");
-			}
-			else {
-				cos = -1;
-			}
-		}
-				
-		/*
-		double dot = directionVector.getX() * - directionToTarget.getY() + directionVector.getY() * directionToTarget.getX();
-		  
-		if(dot > 0)
-			System.out.println("b on the left of a");
-		else if(dot < 0)
-			System.out.println("b on the right of a");
-		else
-			System.out.println("b parallel/antiparallel to a");
-		*/
-		
-		double angle = Math.acos(cos);
-		angle = -angle;
-		
-		System.out.println(Math.toDegrees(angle));
-					
+									
 		if(Math.abs(angle2) < Math.pow(10, -3)) {
 			return 0;
 		}
@@ -1087,7 +1078,6 @@ public class StateMulticopterMovement implements UAVMovementModel  {
 		}
 	}
 	
-	
 	//////////
 	//////////