diff --git a/mock/src/main/java/seng302/Model/Polars.java b/mock/src/main/java/seng302/Model/Polars.java index 4de5b570..2863db48 100644 --- a/mock/src/main/java/seng302/Model/Polars.java +++ b/mock/src/main/java/seng302/Model/Polars.java @@ -35,7 +35,6 @@ public class Polars { polarValues.put(newKey, boatSpeed); } - //TODO calculate VMG from (windAngle, destAngle, windSpeed). /** * Calculates the VMG for a given wind angle, wind speed, and angle to destination. @@ -65,27 +64,37 @@ public class Polars { ArrayList windAngles = new ArrayList<>(); for (Pair key : this.polarValues.keySet()) { - windAngles.add(key.getValue()); + //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) { - //This is the velocity from the polar table at this wind speed/angle. - double estVelocity = this.polarValues.get(new Pair(polarWindSpeed, tackAngle)); - double angleBetweenDestAndTack = tackAngle - destinationAngle; - //This is the estimated velocity towards the target (e.g., angling away from the target reduces velocity). - double vmgTemp = Math.cos(angleBetweenDestAndTack) * estVelocity; - - //Check that the velocity is better. - if (vmgTemp > bestVMGVelocity) { - bestVMGVelocity = vmgTemp; - bestVMGAngle = tackAngle; + Pair key = new Pair(polarWindSpeed, tackAngle); + if (this.polarValues.containsKey(key)) { + //This is the velocity from the polar table at this wind speed/angle. + double estVelocity = this.polarValues.get(key); + double angleBetweenDestAndTack = tackAngle - destinationAngle; + //This is the estimated velocity towards the target (e.g., angling away from the target reduces velocity). + double vmgTemp = Math.cos(angleBetweenDestAndTack) * estVelocity; + + //Check that the velocity is better. + if (vmgTemp > bestVMGVelocity) { + bestVMGVelocity = vmgTemp; + bestVMGAngle = tackAngle; + } + } } + System.out.println("VMG speed = " + bestVMGVelocity + " , VMG angle = " + bestVMGAngle);//TEMP DEBUG REMOVE //Create the VMG object and return it. return new VMG(bestVMGVelocity, bestVMGAngle);