diff --git a/mock/src/main/java/seng302/Model/Boat.java b/mock/src/main/java/seng302/Model/Boat.java index 5a85a0d4..9b81d975 100644 --- a/mock/src/main/java/seng302/Model/Boat.java +++ b/mock/src/main/java/seng302/Model/Boat.java @@ -14,6 +14,8 @@ public class Boat { private int sourceID; private Leg currentLeg; private double distanceTravelledInLeg; + private double distanceTravelledInTack; + private double distanceForTack; private GPSCoordinate currentPosition; private long timeFinished = -1; private boolean started = false; @@ -42,11 +44,9 @@ public class Boat { * * @return the direction that the boat is heading towards in degrees (-180 to 180). */ - public double calculateAzimuth() { + public double calculateAzimuth(GPSCoordinate start, GPSCoordinate end) { GeodeticCalculator calc = new GeodeticCalculator(); - GPSCoordinate start = currentLeg.getStartMarker().getAverageGPSCoordinate(); - GPSCoordinate end = currentLeg.getEndMarker().getAverageGPSCoordinate(); calc.setStartingGeographicPoint(start.getLongitude(), start.getLatitude()); calc.setDestinationGeographicPoint(end.getLongitude(), end.getLatitude()); @@ -59,7 +59,23 @@ public class Boat { * @return The azimuth value which is greater than 0 */ public double calculateHeading() { - double azimuth = this.calculateAzimuth(); + double azimuth = this.calculateAzimuth(currentLeg.getStartMarker().getAverageGPSCoordinate(), + currentLeg.getEndMarker().getAverageGPSCoordinate()); + + if (azimuth >= 0) { + return azimuth; + } else { + return azimuth + 360; + } + } + + /** + * Calculate the heding depending on the calculated azimuth value + * @return The azimuth value which is greater than 0 + */ + public double calculateBearingToDestination() { + double azimuth = this.calculateAzimuth(this.currentPosition, + currentLeg.getEndMarker().getAverageGPSCoordinate()); if (azimuth >= 0) { return azimuth; @@ -125,6 +141,22 @@ public class Boat { this.distanceTravelledInLeg = distanceTravelledInLeg; } + public double getDistanceTravelledInTack() { + return distanceTravelledInTack; + } + + public void setDistanceTravelledInTack(double distanceTravelledInTack) { + this.distanceTravelledInTack = distanceTravelledInTack; + } + + public double getDistanceForTack() { + return distanceForTack; + } + + public void setDistanceForTack(double distanceForTack) { + this.distanceForTack = distanceForTack; + } + public GPSCoordinate getCurrentPosition() { return currentPosition; } diff --git a/mock/src/main/java/seng302/Model/Race.java b/mock/src/main/java/seng302/Model/Race.java index d4e92480..a1ae7f8c 100644 --- a/mock/src/main/java/seng302/Model/Race.java +++ b/mock/src/main/java/seng302/Model/Race.java @@ -19,6 +19,10 @@ import java.util.ArrayList; import java.util.List; import java.util.Random; +import static java.lang.Math.cos; +import static java.lang.Math.max; +import static java.lang.Math.min; + /** * Parent class for races @@ -41,6 +45,7 @@ public class Race implements Runnable { private MockOutput mockOutput; private static int boatOffset = 0; private int finished = 0; + private List boundary; /** @@ -50,7 +55,7 @@ public class Race implements Runnable { * @param legs Number of marks in order that the boats pass in order to complete the race. * @param scaleFactor for race */ - public Race(List boats, List legs, int raceID, int scaleFactor, MockOutput mockOutput) { + public Race(List boats, List legs, int raceID, int scaleFactor, MockOutput mockOutput, List boundary) { this.startingBoats = FXCollections.observableArrayList(boats); this.legs = legs; @@ -58,6 +63,7 @@ public class Race implements Runnable { this.raceId = raceID; this.scaleFactor = scaleFactor; this.mockOutput = mockOutput; + this.boundary = boundary; //TODO refactor this.startTime = System.currentTimeMillis() + (this.PRERACE_TIME / this.scaleFactor); @@ -69,7 +75,7 @@ public class Race implements Runnable { } public Race(RaceDataSource raceData, int scaleFactor, MockOutput mockOutput) { - this(raceData.getBoats(), raceData.getLegs(), raceData.getRaceId(), scaleFactor, mockOutput); + this(raceData.getBoats(), raceData.getLegs(), raceData.getRaceId(), scaleFactor, mockOutput, raceData.getBoundary()); } /** @@ -298,16 +304,57 @@ public class Race implements Runnable { boolean finish = boat.getCurrentLeg().getName().equals("Finish"); if (!finish) { - boat.setHeading(boat.calculateHeading()); - //update boat's distance travelled - boat.setDistanceTravelledInLeg(totalDistanceTravelled); - //Calculate boat's new position by adding the distance travelled onto the start point of the leg - boat.setCurrentPosition(calculatePosition(boat.getCurrentLeg().getStartMarker().getAverageGPSCoordinate(), - totalDistanceTravelled, boat.calculateAzimuth())); + int windAngle = 360;//todo - get the wind speed from somewhere, using 360 for now + if(boat.getCurrentLeg().getName().endsWith("to Windward Gate")){//todo something is broken in this if statement, not sure what + double totalDistanceTravelledInTack = distanceTravelled + boat.getDistanceTravelledInTack(); + + double bound1 = (boat.calculateBearingToDestination()-90)%360; + double bound2 = (boat.calculateBearingToDestination()+90)%360; + + VMG newHeading = boat.getPolars().calculateVMG(windAngle, 30, + boat.calculateBearingToDestination(), min(bound1, bound2), max(bound1,bound2)); + + + double azimuth = newHeading.bearing; + if (newHeading.bearing > 180){ + azimuth = newHeading.bearing -360; + } + +// if (!GPSCoordinate.isInsideBoundary(calculatePosition(boat.getCurrentPosition(), +// 1, azimuth), boundary)){ +// System.out.println("LDFSGSDFG"); +// double tempHeading = (newHeading.bearing+90)%360; +// newHeading.bearing = tempHeading; +// } + + boat.setHeading(newHeading.bearing); + boat.setVelocity(newHeading.speed); + + //calc the distance travelled in a straight line to windward + double angleBetweenDestAndHeading = newHeading.bearing - boat.calculateBearingToDestination(); + totalDistanceTravelled = cos(angleBetweenDestAndHeading)*totalDistanceTravelledInTack; + boat.setDistanceTravelledInLeg(totalDistanceTravelled); + + //Calculate boat's new position by adding the distance travelled onto the start point of the leg + boat.setCurrentPosition(calculatePosition(boat.getCurrentPosition(), + totalDistanceTravelledInTack, azimuth)); + + }else{ + boat.setHeading(boat.calculateHeading()); + //update boat's distance travelled + boat.setDistanceTravelledInLeg(totalDistanceTravelled); + //Calculate boat's new position by adding the distance travelled onto the start point of the leg + boat.setCurrentPosition(calculatePosition(boat.getCurrentLeg().getStartMarker().getAverageGPSCoordinate(), + totalDistanceTravelled, boat.calculateAzimuth(boat.getCurrentLeg().getStartMarker().getAverageGPSCoordinate(), + boat.getCurrentLeg().getEndMarker().getAverageGPSCoordinate()))); + } } } protected void checkPosition(Boat boat, long timeElapsed) { + System.out.println(boat.getDistanceTravelledInLeg()); + System.out.println(boat.getCurrentLeg().getDistance()); + System.out.println(" "); if (boat.getDistanceTravelledInLeg() > boat.getCurrentLeg().getDistance()) { //boat has passed onto new leg if (boat.getCurrentLeg().getName().equals("Finish")) { diff --git a/mock/src/test/java/seng302/Model/BoatTest.java b/mock/src/test/java/seng302/Model/BoatTest.java index 47c0e9c2..83d47e6d 100644 --- a/mock/src/test/java/seng302/Model/BoatTest.java +++ b/mock/src/test/java/seng302/Model/BoatTest.java @@ -22,7 +22,7 @@ public class BoatTest { Marker endMarker = new Marker(new GPSCoordinate(50, 0)); Leg start = new Leg("Start", startMarker, endMarker, 0); TEST_BOAT.setCurrentLeg(start); - assertEquals(TEST_BOAT.calculateAzimuth(), 0, 1e-8); + assertEquals(TEST_BOAT.calculateAzimuth(startMarker.getAverageGPSCoordinate(), endMarker.getAverageGPSCoordinate()), 0, 1e-8); } @Test @@ -31,7 +31,7 @@ public class BoatTest { Marker endMarker = new Marker(new GPSCoordinate(-50, 0)); Leg start = new Leg("Start", startMarker, endMarker, 0); TEST_BOAT.setCurrentLeg(start); - assertEquals(TEST_BOAT.calculateAzimuth(), 180, 1e-8); + assertEquals(TEST_BOAT.calculateAzimuth(startMarker.getAverageGPSCoordinate(), endMarker.getAverageGPSCoordinate()), 180, 1e-8); } @@ -42,7 +42,7 @@ public class BoatTest { Marker endMarker = new Marker(new GPSCoordinate(0, 50)); Leg start = new Leg("Start", startMarker, endMarker, 0); TEST_BOAT.setCurrentLeg(start); - assertEquals(TEST_BOAT.calculateAzimuth(), 90, 1e-8); + assertEquals(TEST_BOAT.calculateAzimuth(startMarker.getAverageGPSCoordinate(), endMarker.getAverageGPSCoordinate()), 90, 1e-8); } @@ -52,7 +52,7 @@ public class BoatTest { Marker endMarker = new Marker(new GPSCoordinate(0, -50)); Leg start = new Leg("Start", startMarker, endMarker, 0); TEST_BOAT.setCurrentLeg(start); - assertEquals(TEST_BOAT.calculateAzimuth(), -90, 1e-8); + assertEquals(TEST_BOAT.calculateAzimuth(startMarker.getAverageGPSCoordinate(), endMarker.getAverageGPSCoordinate()), -90, 1e-8); }