Cleaned up updatePosition in Mock.Race to simplify time estimation.

#story[875]

mock/src/main/java/seng302/Model/Race.java

@@ -239,6 +239,28 @@ public class Race implements Runnable {
         return new GPSCoordinate(endPoint.getY(), endPoint.getX());
     }
 
+    private VMG calculateHeading(Boat boat) {
+        //How fast a boat can turn, in degrees per millisecond.
+        double turnRate = 0.03;
+
+        //How much the boat is allowed to turn, considering how long since it last turned.
+        double turnAngle = turnRate * boat.getTimeSinceTackChange();
+
+        //Find the bounds on what angle the boat is allowed to travel at. The bounds cap out at [0, 360).
+        double bound1 = Math.max(boat.getHeading() - turnAngle, 0);
+        double bound2 = Math.min(boat.getHeading() + turnAngle, 360);
+
+        return boat.getPolars().calculateVMG(this.windDirection, this.windSpeed, boat.calculateBearingToDestination(), bound1, bound2);
+    }
+
+    private boolean improvesVelocity(Boat boat, VMG newHeading) {
+        double angleBetweenDestAndHeading = boat.getHeading() - boat.calculateBearingToDestination();
+        double angleBetweenDestAndNewVMG = newHeading.getBearing() - boat.calculateBearingToDestination();
+        double currentVelocity = cos(Math.toRadians(angleBetweenDestAndHeading)) * boat.getVelocity();
+        double vmgVelocity = cos(Math.toRadians(angleBetweenDestAndNewVMG)) * newHeading.getSpeed();
+        return vmgVelocity > currentVelocity;
+    }
+
     /**
      * Calculates the distance a boat has travelled and updates its current position according to this value.
      *
@@ -249,85 +271,40 @@ public class Race implements Runnable {
 
         //distanceTravelled = velocity (nm p hr) * time taken to update loop
         double distanceTravelled = (boat.getVelocity() * this.scaleFactor * millisecondsElapsed) / 3600000;
-        double totalDistanceTravelled = distanceTravelled + boat.getDistanceTravelledInLeg();
+        double totalDistanceTravelled;
 
         boolean finish = boat.getCurrentLeg().getName().equals("Finish");
         if (!finish) {
-
-
             double totalDistanceTravelledInTack = distanceTravelled + boat.getDistanceTravelledInTack();
 
-            double bound1 = (boat.calculateBearingToDestination() - 90) % 360;
-            double bound2 = (boat.calculateBearingToDestination() + 90) % 360;
-            //TODO the actual bearing bounds need to be the interval in which the boat won't go out of bounds.
-            bound1 = 0;
-            bound2 = 360;
-
             boat.setTimeSinceTackChange(boat.getTimeSinceTackChange() + this.scaleFactor * millisecondsElapsed);
 
-
+            VMG newHeading = calculateHeading(boat);
-            //How fast a boat can turn, in degrees per millisecond.
-            double turnRate = 0.03; //Roughly 30 per second, or 12 seconds per revolution.
-
-            //How much the boat is allowed to turn, considering how long since it last turned.
-            double turnAngle = turnRate * boat.getTimeSinceTackChange();
-
-
-            //System.out.println("boat " + boat.getAbbrev() + " turn angle is " + turnAngle + ".");//TEMP DEBUG REMOVE
-
-            //Find the bounds on what angle the boat is allowed to travel at.
-            bound1 = boat.getHeading() - turnAngle;
-            bound2 = boat.getHeading() + turnAngle;
-
-            //The bounds cap out at [0, 360).
-            bound1 = Math.max(bound1, 0);
-            bound2 = Math.min(bound2, 360);
-
-            VMG newHeading = boat.getPolars().calculateVMG(this.windDirection, this.windSpeed,
-                    boat.calculateBearingToDestination(), bound1, bound2);
 
             if (!GPSCoordinate.isInsideBoundary(boat.getCurrentPosition(), boundary)){
                 double tempHeading = (newHeading.getBearing() - this.windDirection +90)%360;
                 newHeading.setBearing(tempHeading);
             }
 
-            //Is this new VMG better than the current VMG?
+            if (improvesVelocity(boat, newHeading)) {
-
-            double angleBetweenDestAndHeading = boat.getHeading() - boat.calculateBearingToDestination();
-            double angleBetweenDestAndNewVMG = newHeading.getBearing() - boat.calculateBearingToDestination();
-            double currentVelocity = cos(Math.toRadians(angleBetweenDestAndHeading)) * boat.getVelocity();
-            double vmgVelocity = cos(Math.toRadians(angleBetweenDestAndNewVMG)) * newHeading.getSpeed();
-            //System.out.println("boat " + boat.getAbbrev() + " current velocity is " + currentVelocity + " knots, possible VMG is " + vmgVelocity + " knots.");//TEMP DEBUG REMOVE
-            if (vmgVelocity > currentVelocity) {
                 boat.setHeading(newHeading.getBearing());
                 boat.setVelocity(newHeading.getSpeed());
                 boat.setTimeSinceTackChange(0);
-
-                //System.out.println("boat " + boat.getAbbrev() + " has a new bearing " + boat.getHeading() + " degrees, and is " + boat.calculateDistanceToNextMarker() + " nautical miles to the next marker. Velocity to next marker is " + boat.getVelocity() + " knots.");//TEMP DEBUG REMOVE
             }
 
-
-
-
             //TODO one way to fix the boat's rapid turning it to only update the velocity/heading every X seconds (e.g., every 5 seconds).
             //TODO may need a lower tack period
             //TODO another way would be to allow boats use a better VMG if it is within turnRate * timeSinceTackChange. E.g., after 100ms a boat can select a more optimal VMG within 5deg of their current bearing. After 500ms VMG can be within 25deg of current bearing. After 2sec it can be 100deg of bearing, etc...
-
             //calc the distance travelled in a straight line to windward
             //double angleBetweenDestAndHeading = boat.getHeading() - boat.calculateBearingToDestination();
-            totalDistanceTravelled = cos(Math.toRadians(angleBetweenDestAndHeading))*totalDistanceTravelledInTack;
+            totalDistanceTravelled = cos(Math.toRadians(boat.getHeading() - boat.calculateBearingToDestination()))*totalDistanceTravelledInTack;
             boat.setDistanceTravelledInLeg(totalDistanceTravelled);
 
 
             //Calculate boat's new position by adding the distance travelled onto the start point of the leg
             double azimuth = boat.getHeading();
-            if (azimuth > 180) {
+            azimuth = azimuth > 180? azimuth - 360 : azimuth;
-                azimuth = azimuth - 360;
+            boat.setCurrentPosition(calculatePosition(boat.getCurrentPosition(), totalDistanceTravelledInTack, azimuth));
-            }
-            boat.setCurrentPosition(calculatePosition(boat.getCurrentPosition(),
-                    totalDistanceTravelledInTack, azimuth));
-
-
         }
     }