|
|
|
|
@ -1,5 +1,11 @@
|
|
|
|
|
package seng302.Model;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
import java.awt.*;
|
|
|
|
|
import java.awt.geom.Point2D;
|
|
|
|
|
|
|
|
|
|
import org.geotools.geometry.DirectPosition1D;
|
|
|
|
|
import org.geotools.referencing.GeodeticCalculator;
|
|
|
|
|
import seng302.GPSCoordinate;
|
|
|
|
|
import seng302.GraphCoordinate;
|
|
|
|
|
|
|
|
|
|
@ -28,30 +34,29 @@ public class ConstantVelocityRace extends Race {
|
|
|
|
|
double totalDistanceTravelled = distanceTravelled + boat.getDistanceTravelledInLeg();
|
|
|
|
|
|
|
|
|
|
boat.setDistanceTravelledInLeg(totalDistanceTravelled);
|
|
|
|
|
boat.setCurrentPosition(calculate_position(boat.getCurrentLeg().getStartGraphCoordinate(),
|
|
|
|
|
boat.setCurrentPosition(calculatePosition(boat.getCurrentLeg().getStartGraphCoordinate(),
|
|
|
|
|
totalDistanceTravelled, boat.calculateHeading()));
|
|
|
|
|
//Calculate new coordinates based on boat's heading for the leg, and distance traveled
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
protected static GPSCoordinate calculate_position(GPSCoordinate oldCoordinates, double distanceTravelled, double heading) {
|
|
|
|
|
public static GPSCoordinate calculatePosition(GPSCoordinate oldCoordinates, double distanceTravelled, double heading) {
|
|
|
|
|
|
|
|
|
|
//Find new coordinate using current heading and distance
|
|
|
|
|
|
|
|
|
|
double oldLatitude = oldCoordinates.getLatitude();
|
|
|
|
|
GeodeticCalculator geodeticCalculator = new GeodeticCalculator();
|
|
|
|
|
|
|
|
|
|
Point2D startPoint = new Point2D.Double(oldCoordinates.getLatitude(), oldCoordinates.getLongitude());
|
|
|
|
|
geodeticCalculator.setStartingGeographicPoint(startPoint);
|
|
|
|
|
|
|
|
|
|
double EARTH_RADIUS = 6371; //km
|
|
|
|
|
double oldLongitude = oldCoordinates.getLongitude();
|
|
|
|
|
double angularDistance = distanceTravelled / EARTH_RADIUS;
|
|
|
|
|
double azimuth = heading - 180;
|
|
|
|
|
geodeticCalculator.setDirection(azimuth, distanceTravelled);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
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);
|
|
|
|
|
Point2D endPoint = geodeticCalculator.getDestinationGeographicPoint();
|
|
|
|
|
|
|
|
|
|
return new GPSCoordinate(endPoint.getX(), endPoint.getY());
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|