Began implementing function to calculate boat's new coordinates

-Using haversine function
-current values being produced are too small

#implement #story[9]

src/main/java/seng302/Model/ConstantVelocityRace.java

@@ -28,15 +28,29 @@ public class ConstantVelocityRace extends Race {
         double totalDistanceTravelled = distanceTravelled + boat.getDistanceTravelledInLeg();
 
         boat.setDistanceTravelledInLeg(totalDistanceTravelled);
-        boat.setCurrentPosition(calculate_position(totalDistanceTravelled, boat.calculateHeading()));
+        boat.setCurrentPosition(calculate_position(boat.getCurrentLeg().getStartGraphCoordinate(),
+                totalDistanceTravelled, boat.calculateHeading()));
         //Calculate new coordinates based on boat's heading for the leg, and distance traveled
     }
 
-    protected GPSCoordinate calculate_position(double distanceTravelled, double heading) {
+    protected static GPSCoordinate calculate_position(GPSCoordinate oldCoordinates, double distanceTravelled, double heading) {
 
         //Find new coordinate using current heading and distance
 
-        return new GPSCoordinate(-1, -1);
+        double oldLatitude = oldCoordinates.getLatitude();
+
+        double EARTH_RADIUS = 6371; //km
+        double oldLongitude = oldCoordinates.getLongitude();
+        double angularDistance = distanceTravelled / EARTH_RADIUS;
+
+
+        double newLatitude = Math.asin( Math.sin(oldLatitude)*Math.cos(angularDistance) +
+                Math.cos(oldLatitude)*Math.sin(angularDistance)*Math.cos(heading) );
+        double newLongitude = oldLongitude + Math.atan2(Math.sin(heading)*Math.sin(angularDistance)*Math.cos(oldLatitude),
+                Math.cos(angularDistance)-Math.sin(oldLatitude)*Math.sin(newLatitude));
+
+
+        return new GPSCoordinate(newLatitude, newLongitude);
 
     }
 

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

@@ -12,6 +12,7 @@ public abstract class Race {
     protected ArrayList<BoatInRace> finishingBoats = new ArrayList<>();
     protected Leg[] legs;
 
+
     private int SLEEP_TIME = 1000; //time in milliseconds to pause in a paced loop
 
     /**