Merge remote-tracking branch 'origin/story31.2' into story31.2

mock/src/main/java/seng302/DataInput/PolarParser.java

@@ -81,8 +81,7 @@ public class PolarParser {
 
             //For each pair of columns (the pair is angle, speed).
             //We start at column 1 since column 0 is the wind speed column.
-            // (row.length - 1) / 2    means the number of pairs of angle+speed.
+            for (int i = 1; i < row.length; i += 2) {
-            for (int i = 1; i < (row.length - 1) / 2; i += 2) {
 
                 //Add angle+speed=velocity estimate to polar table.
                 try {

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

@@ -17,6 +17,13 @@ public class Polars {
     ///Internal store of data. Maps pair<windSpeed, windAngle> to boatSpeed.
     private HashMap<Pair<Double, Double>, Double> polarValues = new HashMap<>();
 
+    ///Stores a list of angles from the polar table - this is used during the calculateVMG function.
+    ///Maps between windSpeed and a list of angles for that wind speed.
+    private HashMap<Double, ArrayList<Double>> polarAngles = new HashMap<>();
+
+
+
+
     /**
      * Ctor.
      */
@@ -26,28 +33,73 @@ public class Polars {
 
     /**
      * Adds an estimated velocity to the polar table object, for a given (windSpeed, windAngle) pair. That is, stores a mapping from (windSpeed, windAngle) to (boatVelocity).
+     * Note: an estimate means given a specific wind speed of trueWindSpeed, if the boat travels relativeWindAngle degrees towards the wind, it will move at boatSpeed knots. E.g., trueWindSpeed = 20kn, relativeWindAngle = 45 degrees, boatSpeed = 25kn. If the boat travels towards the wind, plus or minus 45 degrees either side, it will move at 25kn.
      * @param trueWindSpeed The true wind speed of the estimate.
-     * @param trueWindAngle The true wind angle of the estimate.
+     * @param relativeWindAngle The relative wind angle between the wind direction + 180 degrees and the boat's direction of the estimate.
      * @param boatSpeed The boat speed of the estimate.
      */
-    public void addEstimate(double trueWindSpeed, double trueWindAngle, double boatSpeed){
+    public void addEstimate(double trueWindSpeed, double relativeWindAngle, double boatSpeed){
-        Pair newKey = new Pair(trueWindSpeed, trueWindAngle);
+
-        polarValues.put(newKey, boatSpeed);
+        //We also add the same values with a negative angle, as the data file contains data for 0-180 degrees, but we also need 180-360 degrees. This is because it may turn out that going 5 degrees into the wind gives us, say, 9knots, but -5 into the wind may give us 10knots, towards our destination.
+
+        //Create the array to store angles for this wind speed if it doesn't exist.
+        if (!this.polarAngles.containsKey(trueWindSpeed)) {
+            this.polarAngles.put(trueWindSpeed, new ArrayList<>());
+        }
+
+        //Add estimate to map.
+        Pair newKeyPositive = new Pair(trueWindSpeed, relativeWindAngle);
+        polarValues.put(newKeyPositive, boatSpeed);
+
+        double negativeAngle = 360d - relativeWindAngle;
+        //This essentially does angle modulo 360, to get something in the interval [0, 360).
+        while (negativeAngle >= 360d) {
+            negativeAngle -= 360d;
+        }
+        //Ensure that the positive and negative angles aren't the same (e.g., pos = 0, neg = 360 - 0 = 0.
+        if (negativeAngle != relativeWindAngle) {
+            Pair newKeyNegative = new Pair(trueWindSpeed, negativeAngle);
+            polarValues.put(newKeyNegative, boatSpeed);
+        }
+
+
+        //Add angle to angle list.
+        if (!this.polarAngles.get(trueWindSpeed).contains(relativeWindAngle)) {
+            this.polarAngles.get(trueWindSpeed).add(relativeWindAngle);
         }
 
+        if (!this.polarAngles.get(trueWindSpeed).contains(negativeAngle)) {
+            this.polarAngles.get(trueWindSpeed).add(negativeAngle);
+        }
+
+
+
+    }
+
+
 
     /**
-     * Calculates the VMG for a given wind angle, wind speed, and angle to destination.
+     * Calculates the VMG for a given wind angle, wind speed, and angle to destination. Will only return VMGs that have a true bearing (angle) within a given bound - this is to ensure that you can calculate VMGs without going out of bounds. If you don't care about bearing bounds, simple pass in lower = 0, upper = 360.
      * @param trueWindAngle The current true wind angle.
      * @param trueWindSpeed The current true wind speed.
      * @param destinationAngle The angle between the boat and the destination point.
+     * @param bearingLowerBound The lowest bearing (angle) that the boat may travel on.
+     * @param bearingUpperBound The highest bearing (angle) that the boat may travel on.
      * @return
      */
-    public VMG calculateVMG(double trueWindAngle, double trueWindSpeed, double destinationAngle) {
+    public VMG calculateVMG(double trueWindAngle, double trueWindSpeed, double destinationAngle, double bearingLowerBound, double bearingUpperBound) {
 
         //Currently a fairly simple implementation where we find the wind speed that is less than or equal to the current wind speed (the lower bound), and then find the specific angle (with no interpolation) that gives the best VMG.
+        //TODO we need to add interpolation between angles for a given wind speed (e.g., we have 0 deg, 30 deg, but the optimal bearing may be 17.3 degrees).
+        //TODO we should also interpolate between wind speeds (e.g., we have 12kn and 16kn, but if the wind speed is actually 15.999kn, then we should interpolate to get a more accurate final value).
 
-        double polarWindSpeed = 0;
+        //Sorts polar angles.
+        for (ArrayList<Double> angles : this.polarAngles.values()) {
+            angles.sort(null);
+        }
+
+        //-1 indicates that we haven't found any smaller wind speeds in our map.
+        double polarWindSpeed = -1;
 
         //Find the lower bound wind speed from the polar table.
         for (Pair<Double, Double> key : this.polarValues.keySet()) {
@@ -60,46 +112,109 @@ public class Polars {
             }
         }
 
-        //We create a list of wind angles because we need (speed, angle) pairs to look into the map.
+        //If we never found a smaller speed value (e.g., smallest speed in table is 4kn, user provided 2kn), then for now we give a vector with 0 speed towards destination. Later, this should interpolate between adjacent wind speeds.
-        ArrayList<Double> windAngles = new ArrayList<>();
+        if (polarWindSpeed == -1) {
-        for (Pair<Double, Double> key : this.polarValues.keySet()) {
+            return new VMG(0, destinationAngle);
-
-            //Don't add angles multiple times.
-            double angle = key.getValue();
-            if (!windAngles.contains(angle)) {
-                windAngles.add(angle);
-            }
         }
 
+
+
         //Find the angle with the best VMG.
-        //TODO need to differentiate between windward and leeward.
+
         double bestVMGAngle = 0;
         double bestVMGVelocity = 0;
-        for (double tackAngle : windAngles) {
+        //The list of polar angles for this wind speed.
-            Pair<Double,Double> key = new Pair<Double,Double>(polarWindSpeed, tackAngle);
+        ArrayList<Double> polarAngles = this.polarAngles.get(polarWindSpeed);
-            if (this.polarValues.containsKey(key)) {
+
-                //This is the velocity from the polar table at this wind speed/angle.
+        //For all angles in the accepted interval (in 1 degree increments).
-                double estVelocity = this.polarValues.get(key);
+        for (double angle = 0; angle < 360; angle += 1) {
-                double angleBetweenDestAndTack = tackAngle - destinationAngle;
+
+            //This is the true bearing of the boat, if it went at the angle against the wind.
+            //For angle > 90 and angle < 270, it means that the boat is actually going _with_ the wind (gybe).
+            double trueBoatBearing = trueWindAngle + angle + 180d;
+            while (trueBoatBearing >= 360) {
+                trueBoatBearing -= 360;
+            }
+
+            //Check that the boat's bearing would actually be acceptable.
+            if ((trueBoatBearing <= bearingLowerBound) || (trueBoatBearing > bearingUpperBound)) {
+                //If the angle is too small or too great, don't use it - skip to the next iteration.
+                continue;
+            }
+
+
+            //Basic linear interpolation. Find the nearest two angles from the table, and interpolate between them.
+
+            //Check which pair of adjacent points the angle is between.
+            boolean foundInterval = false;
+            double lowerBound = 0;
+            double upperBound = 0;
+            for (int i = 0; i < polarAngles.size() - 1; i++) {
+                if ((angle >= polarAngles.get(i)) && (angle < polarAngles.get(i + 1))) {
+                    foundInterval = true;
+                    lowerBound = polarAngles.get(i);
+                    upperBound = polarAngles.get(i + 1);
+                    break;
+                }
+            }
+
+            //Calculate how far between those points the angle is.
+            if (!foundInterval) {
+                //If we never found the interval, then it must be the "last" interval, between the i'th and 0'th values.
+                lowerBound = polarAngles.get(polarAngles.size() - 1);
+                upperBound = polarAngles.get(0);
+            }
+
+            //This is the "distance" between the angle and its lower bound.
+            //I.e., L----A-----------U
+            //      <----> is lowerDelta.
+            double lowerDelta = angle - lowerBound;
+
+            //This is the "distance" between the upper and lower bound.
+            //I.e., L----A-----------U
+            //      <----------------> is intervalDelta.
+            //This can potentially be negative if we have, e.g., lower = 340deg, upper = 0deg, delta = -340deg.
+            double intervalDelta = upperBound - lowerBound;
+            //If it _is_ negative, modulo it to make it positive.
+            //E.g., -340deg = +20deg.
+            if (intervalDelta < 0) {
+                intervalDelta += 360d;
+            }
+
+            //This is how far between the lower and upper bounds the angle is, as a proportion (e.g., 0.5 = half-way, 0.9 = close to upper).
+            double interpolationScalar = lowerDelta / intervalDelta;
+
+            //Get the estimated boat speeds for the lower and upper angles.
+            Pair<Double,Double> lowerKey = new Pair<>(polarWindSpeed, lowerBound);
+            Pair<Double,Double> upperKey = new Pair<>(polarWindSpeed, upperBound);
+            double lowerSpeed = this.polarValues.get(lowerKey);
+            double upperSpeed = this.polarValues.get(upperKey);
+            //Get the delta between upper and lower speeds.
+            double speedDelta = upperSpeed - lowerSpeed;
+
+            //Calculate the speed at the interpolated angle.
+            double interpolatedSpeed = lowerSpeed + (speedDelta * interpolationScalar);
+
+
+
+            //This is the delta angle between the boat's true bearing and the destination.
+            double angleBetweenDestAndTack = trueBoatBearing - destinationAngle;
             //This is the estimated velocity towards the target (e.g., angling away from the target reduces velocity).
-                double vmgTemp = Math.cos(angleBetweenDestAndTack) * estVelocity;
+            double vmgTemp = Math.cos(Math.toRadians(angleBetweenDestAndTack)) * interpolatedSpeed;
+
 
             //Check that the velocity is better.
             if (vmgTemp > bestVMGVelocity) {
                 bestVMGVelocity = vmgTemp;
-                    bestVMGAngle = tackAngle;
+                bestVMGAngle = trueBoatBearing;
             }
 
         }
 
-        }
-
-        System.out.println("VMG speed = " + bestVMGVelocity + " , VMG angle = " + bestVMGAngle);//TEMP DEBUG REMOVE
 
         //Create the VMG object and return it.
         return new VMG(bestVMGVelocity, bestVMGAngle);
 
-
     }
 
 

mock/src/test/java/seng302/Model/PolarsTest.java

@@ -11,6 +11,7 @@ import static org.testng.Assert.*;
  */
 public class PolarsTest {
 
+
     @Test
     /**
      * Tests if we can parse a polar data file (stored in a string), create a polar table, and calculate VMG for a variety of values.
@@ -18,7 +19,7 @@ public class PolarsTest {
     public void testParseAndVMG() throws Exception {
 
         //Read data.
-        Polars polars;
+        Polars polars = null;
         try {
             //Parse data file.
             polars = PolarParser.parse("polars/acc_polars.csv");
@@ -27,20 +28,66 @@ public class PolarsTest {
             assertTrue(false);
         }
 
+
+
+        double angleEpsilon = 2;
+        double speedEpsilon = 2;//Are these epsilons a bit too big?
+
         //Test 1.
-        //TODO make these tests actually do something when the calculateVMG function is added.
+        //This test has a wind speed that is between two values from the table (12kn, 16kn, this is 15.9kn).
         double windAngle1 = 31.5;
         double destAngle1 = 65.32;
-        double windSpeed1 = 15;//knots
+        double windSpeed1 = 15.9;//knots
-        //double vmgAngle1 = TODO;
+        double vmgAngle1 = 88; //TODO the expected results need to be calculated and placed here.
-        //double vmgSpeed1 = TODO;
+        double vmgSpeed1 = 12;
+
+        VMG calcVMG1 = polars.calculateVMG(windAngle1, windSpeed1, destAngle1, 0, 360);
+        double calcVMGAngle1 = calcVMG1.getBearing();
+        double calcVMGSpeed1 = calcVMG1.getSpeed();
+
+        System.out.println("VMG speed = " + calcVMGSpeed1 + " , VMG angle = " + calcVMGAngle1);//TEMP DEBUG REMOVE
+
+
+        //assertEquals(calcVMGAngle1, vmgAngle1, angleEpsilon);
+        //assertEquals(calcVMGSpeed1, vmgSpeed1, speedEpsilon);
+
+
+        //Test 2.
+        //This test has a wind speed much larger than any in the table (max from table is 30kn, this is 40kn).
+        double windAngle2 = 200;
+        double destAngle2 = 35;
+        double windSpeed2 = 40;//knots
+        double vmgAngle2 = 88;
+        double vmgSpeed2 = 12;
+
+        VMG calcVMG2 = polars.calculateVMG(windAngle2, windSpeed2, destAngle2, 0, 360);
+        double calcVMGAngle2 = calcVMG2.getBearing();
+        double calcVMGSpeed2 = calcVMG2.getSpeed();
+
+        System.out.println("VMG speed = " + calcVMGSpeed2 + " , VMG angle = " + calcVMGAngle2);//TEMP DEBUG REMOVE
+
+
+        //assertEquals(calcVMGAngle2, vmgAngle2, angleEpsilon);
+        //assertEquals(calcVMGSpeed2, vmgSpeed2, speedEpsilon);
+
+
+        //Test 3.
+        //This test has a wind speed lower than any non-zero values from the table (table has 0kn, 4kn, this is 2kn).
+        double windAngle3 = 345;
+        double destAngle3 = 199;
+        double windSpeed3 = 2;//knots
+        double vmgAngle3 = 88;
+        double vmgSpeed3 = 12;
+
+        VMG calcVMG3 = polars.calculateVMG(windAngle3, windSpeed3, destAngle3, 0, 360);
+        double calcVMGAngle3 = calcVMG3.getBearing();
+        double calcVMGSpeed3 = calcVMG3.getSpeed();
+
+        System.out.println("VMG speed = " + calcVMGSpeed3 + " , VMG angle = " + calcVMGAngle3);//TEMP DEBUG REMOVE
 
-        //VMG calcVMG1 = Polars.calculateVMG(windAngle1, destAngle1, windSpeed1);
-        //double calcVMGAngle1 = calcVMG1.getKey();
-        //double calcVMGSpeed1 = calcVMG1.getValue();
 
-        //assertEquals(vmgAngle1, calcVMGAngle1, 0.1);
+        //assertEquals(calcVMGAngle3, vmgAngle3, angleEpsilon);
-        //assertEquals(vmgSpeed1, calcVMGSpeed1, 0.1);
+        //assertEquals(calcVMGSpeed3, vmgSpeed3, speedEpsilon);
 
     }