Merge branch 'boat_boundarty_fix' into story36

# Conflicts:
#	mock/src/main/java/seng302/Model/Race.java
Resolved.

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

@@ -54,9 +54,6 @@ public class Angle implements Comparable<Angle> {
         return this.degrees;
     }
 
-    public void setDegrees(double degrees) {
-        this.degrees = degrees;
-    }
 
     /**
      * Returns the value of this Angle object, in radians.

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

@@ -1,10 +1,12 @@
 package seng302.Model;
 
+import javafx.util.Pair;
 import org.geotools.referencing.GeodeticCalculator;
 import org.opengis.geometry.DirectPosition;
 import seng302.Constants;
 
 import java.awt.geom.Point2D;
+import java.util.ArrayList;
 import java.util.Comparator;
 import java.util.List;
 
@@ -292,5 +294,226 @@ public class GPSCoordinate {
 
     }
 
+
+    /**
+     * Takes a list of GPS coordinates describing a course boundary, and "shrinks" it inwards by 50m.
+     * @param boundary The boundary of course.
+     * @return A copy of the course boundary list, shrunk inwards by 50m.
+     */
+    public static List<GPSCoordinate> getShrinkBoundary(List<GPSCoordinate> boundary) {
+
+
+        double shrinkDistance = 50d;
+        List<GPSCoordinate> shrunkBoundary = new ArrayList<>(boundary.size());
+        //This is a list of edges that have been shrunk/shifted inwards.
+        List<Pair<GPSCoordinate, GPSCoordinate>> shrunkEdges = new ArrayList<>();
+
+
+        //We need to invert some of our opertations depending if the boundary is clockwise or anti-clockwise.
+        boolean isClockwise = GPSCoordinate.isClockwisePolygon(boundary);
+        double clockwiseScaleFactor = 0;
+
+        if (isClockwise) {
+            clockwiseScaleFactor = 1;
+        } else {
+            clockwiseScaleFactor = -1;
+        }
+
+
+        /**
+         * Starting at a vertex, face anti-clockwise along an adjacent edge.
+         Replace the edge with a new, parallel edge placed at distance d to the "left" of the old one.
+         Repeat for all edges.
+         Find the intersections of the new edges to get the new vertices.
+         Detect if you've become a crossed polynomial and decide what to do about it. Probably add a new vertex at the crossing-point and get rid of some old ones. I'm not sure whether there's a better way to detect this than just to compare every pair of non-adjacent edges to see if their intersection lies between both pairs of vertices.
+         */
+
+        //For the first (size-1) adjacent pairs.
+        for (int i = 0; i < (boundary.size() - 1); i++) {
+
+            //Get the points.
+            GPSCoordinate firstPoint = boundary.get(i);
+            GPSCoordinate secondPoint = boundary.get(i + 1);
+
+            //Get the bearing between two adjacent points.
+            Bearing bearing = GPSCoordinate.calculateBearing(firstPoint, secondPoint);
+
+            //Calculate angle perpendicular to bearing.
+            Bearing perpindicularBearing = Bearing.fromDegrees(bearing.degrees() + (90d * clockwiseScaleFactor));
+
+            //Translate both first and second point by 50m, using this bearing. These form our inwards shifted edge.
+            GPSCoordinate firstPointTranslated = GPSCoordinate.calculateNewPosition(firstPoint, shrinkDistance, Azimuth.fromBearing(perpindicularBearing));
+            GPSCoordinate secondPointTranslated = GPSCoordinate.calculateNewPosition(secondPoint, shrinkDistance, Azimuth.fromBearing(perpindicularBearing));
+
+            //Add edge to list.
+            shrunkEdges.add(new Pair<>(firstPointTranslated, secondPointTranslated));
+
+        }
+
+        //For the final adjacent pair, between the last and first point.
+        //Get the points.
+        GPSCoordinate firstPoint = boundary.get(boundary.size() - 1);
+        GPSCoordinate secondPoint = boundary.get(0);
+
+        //Get the bearing between two adjacent points.
+        Bearing bearing = GPSCoordinate.calculateBearing(firstPoint, secondPoint);
+
+        //Calculate angle perpendicular to bearing.
+        Bearing perpindicularBearing = Bearing.fromDegrees(bearing.degrees() + (90d * clockwiseScaleFactor));
+
+        //Translate both first and second point by 50m, using this bearing. These form our inwards shifted edge.
+        GPSCoordinate firstPointTranslated = GPSCoordinate.calculateNewPosition(firstPoint, shrinkDistance, Azimuth.fromBearing(perpindicularBearing));
+        GPSCoordinate secondPointTranslated = GPSCoordinate.calculateNewPosition(secondPoint, shrinkDistance, Azimuth.fromBearing(perpindicularBearing));
+
+        //Add edge to list.
+        shrunkEdges.add(new Pair<>(firstPointTranslated, secondPointTranslated));
+
+
+        //We now have a list of edges that have been shifted inwards.
+        //We need to find the intersections between adjacent vertices in our edge list. E.g., intersection between edge1-right, and edge2-left.
+
+        //For the first (size-1) adjacent pairs.
+        for (int i = 0; i < (shrunkEdges.size() - 1); i++) {
+
+            //Get the pair of adjacent edges.
+            Pair<GPSCoordinate, GPSCoordinate> edge1 = shrunkEdges.get(i);
+            Pair<GPSCoordinate, GPSCoordinate> edge2 = shrunkEdges.get(i + 1);
+
+            //Get the x and y coordinates of first edge.
+            double x1 = edge1.getKey().getLongitude();
+            double x2 = edge1.getValue().getLongitude();
+            double y1 = edge1.getKey().getLatitude();
+            double y2 = edge1.getValue().getLatitude();
+
+            //Get the x and y coordinates of second edge.
+            double x3 = edge2.getKey().getLongitude();
+            double x4 = edge2.getValue().getLongitude();
+            double y3 = edge2.getKey().getLatitude();
+            double y4 = edge2.getValue().getLatitude();
+
+            //Find the equations for both edges.
+            // y = a*x + b
+            //First equation.
+            double a1 = (y2 - y1) / (x2 - x1);
+            double b1 = y1 - a1 * x1;
+
+            //Second equation.
+            double a2 = (y4 - y3) / (x4 - x3);
+            double b2 = y3 - a2 * x3;
+
+
+            //Find intersecting x coordinate.
+            // a1 * x + b1 = a2 * x + b2
+            double x0 = -(b1 - b2) / (a1 - a2);
+            //Find intersecting y coordinate.
+            double y0 = x0 * a1 + b1;
+
+            //Add this to shrunk boundary list.
+            GPSCoordinate coordinate = new GPSCoordinate(y0, x0);
+            shrunkBoundary.add(coordinate);
+
+        }
+
+
+        //For the final adjacent pair, between the last and first point.
+        //Get the pair of adjacent edges.
+        Pair<GPSCoordinate, GPSCoordinate> edge1 = shrunkEdges.get(shrunkEdges.size() - 1);
+        Pair<GPSCoordinate, GPSCoordinate> edge2 = shrunkEdges.get(0);
+
+        //Get the x and y coordinates of first edge.
+        double x1 = edge1.getKey().getLongitude();
+        double x2 = edge1.getValue().getLongitude();
+        double y1 = edge1.getKey().getLatitude();
+        double y2 = edge1.getValue().getLatitude();
+
+        //Get the x and y coordinates of second edge.
+        double x3 = edge2.getKey().getLongitude();
+        double x4 = edge2.getValue().getLongitude();
+        double y3 = edge2.getKey().getLatitude();
+        double y4 = edge2.getValue().getLatitude();
+
+        //Find the equations for both edges.
+        // y = a*x + b
+        //First equation.
+        double a1 = (y2 - y1) / (x2 - x1);
+        double b1 = y1 - a1 * x1;
+
+        //Second equation.
+        double a2 = (y4 - y3) / (x4 - x3);
+        double b2 = y3 - a2 * x3;
+
+
+        //Find intersecting x coordinate.
+        // a1 * x + b1 = a2 * x + b2
+        double x0 = -(b1 - b2) / (a1 - a2);
+        //Find intersecting y coordinate.
+        double y0 = x0 * a1 + b1;
+
+        //Add this to shrunk boundary list.
+        GPSCoordinate coordinate = new GPSCoordinate(y0, x0);
+        shrunkBoundary.add(coordinate);
+
+
+
+        return shrunkBoundary;
+
+    }
+
+
+    /**
+     * Determines if a list of coordinates describes a boundary polygon in clockwise or anti-clockwise order.
+     * @param boundary The list of coodinates.
+     * @return True if clockwise, false if anti-clockwise.
+     */
+    public static boolean isClockwisePolygon(List<GPSCoordinate> boundary) {
+
+        /** From https://stackoverflow.com/questions/1165647/how-to-determine-if-a-list-of-polygon-points-are-in-clockwise-order
+         * sum all pairs (x2 − x1)(y2 + y1)
+         point[0] = (5,0)   edge[0]: (6-5)(4+0) =   4
+         point[1] = (6,4)   edge[1]: (4-6)(5+4) = -18
+         point[2] = (4,5)   edge[2]: (1-4)(5+5) = -30
+         point[3] = (1,5)   edge[3]: (1-1)(0+5) =   0
+         point[4] = (1,0)   edge[4]: (5-1)(0+0) =   0
+         ---
+         -44  counter-clockwise
+         */
+
+        double sum = 0;
+
+        //For the first (size-1) adjacent pairs.
+        for (int i = 0; i < (boundary.size() - 1); i++) {
+
+            //Get the points.
+            GPSCoordinate firstPoint = boundary.get(i);
+            GPSCoordinate secondPoint = boundary.get(i + 1);
+
+            double xDelta = secondPoint.getLongitude() - firstPoint.getLongitude();
+            double ySum = secondPoint.getLatitude() + firstPoint.getLatitude();
+
+            double product = xDelta * ySum;
+
+            sum += product;
+
+        }
+
+        //For the final adjacent pair, between the last and first point.
+        //Get the points.
+        GPSCoordinate firstPoint = boundary.get(boundary.size() - 1);
+        GPSCoordinate secondPoint = boundary.get(0);
+
+        double xDelta = secondPoint.getLongitude() - firstPoint.getLongitude();
+        double ySum = secondPoint.getLatitude() + firstPoint.getLatitude();
+
+        double product = xDelta * ySum;
+
+        sum += product;
+
+
+        //sum > 0 is clockwise, sum < 0 is anticlockwise.
+        return sum > 0;
+
+
+    }
+
 }
 

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

@@ -46,6 +46,11 @@ public class Race implements Runnable {
      */
     private List<GPSCoordinate> boundary;
 
+    /**
+     * A copy of the boundary list, except "shrunk" inwards by 50m.
+     */
+    private List<GPSCoordinate> shrinkBoundary;
+
     /**
      * The elapsed time, in milliseconds, of the race.
      */
@@ -61,7 +66,7 @@ public class Race implements Runnable {
      * Frame periods are multiplied by this to get the amount of time a single frame represents.
      * E.g., frame period = 20ms, scale = 5, frame represents 20 * 5 = 100ms, and so boats are simulated for 100ms, even though only 20ms actually occurred.
      */
-    private int scaleFactor = 5;
+    private int scaleFactor = 30;
 
     /**
      * The race ID of the course.
@@ -119,6 +124,9 @@ public class Race implements Runnable {
         this.boats = FXCollections.observableArrayList(raceData.getBoats());
         this.compoundMarks = FXCollections.observableArrayList(raceData.getCompoundMarks());
         this.boundary = raceData.getBoundary();
+        this.shrinkBoundary = GPSCoordinate.getShrinkBoundary(this.boundary);
+
+
         this.legs = raceData.getLegs();
         this.legs.add(new Leg("Finish", this.legs.size()));
 
@@ -589,23 +597,17 @@ public class Race implements Runnable {
             VMG newVMG = this.calculateVMG(boat, bearingBounds);
 
 
-
-
-            Azimuth azimuth = Azimuth.fromBearing(boat.getBearing());
-            GPSCoordinate testBounds = GPSCoordinate.calculateNewPosition(boat.getCurrentPosition(),
-                    (100.0 / Constants.NMToMetersConversion), azimuth);
             //If the new vmg improves velocity, use it.
             if (improvesVelocity(boat, newVMG)) {
                 boat.setVMG(newVMG);
-            }else if (!GPSCoordinate.isInsideBoundary(testBounds, boundary)){
+            } else {
-                //checks to see if the new vmg sends the boat out of bounds and if so mirrors its direction in the wind
+                //We also need to use the new VMG if our current bearing will take us out of the course.
-                double currDegrees = newVMG.getBearing().degrees();
+                if (!this.checkBearingInsideCourse(boat.getBearing(), boat.getCurrentPosition())) {
-                double windDirectionDegrees = this.windDirection.degrees();
-                double tempHeading = (currDegrees - windDirectionDegrees +90)%360;
-                newVMG.getBearing().setDegrees(tempHeading);
                     boat.setVMG(newVMG);
                 }
 
+            }
+
 
             //Check the boats position (update leg and stuff).
             this.checkPosition(boat, totalTimeElapsed);
@@ -630,6 +632,8 @@ public class Race implements Runnable {
 
         boolean foundAnyBadBearing = false;
         boolean foundLowerBearing = false;
+        boolean foundAnyGoodBearing = false;
+        boolean stopFindingUpperBearing = false;
 
         //Check all bearings between [0, 360).
         for (double angle = 0; angle < 360; angle += 1) {
@@ -646,11 +650,14 @@ public class Race implements Runnable {
                 if (foundAnyBadBearing && !foundLowerBearing) {
                     lowerBearing = bearing;
                     foundLowerBearing = true;
+                    foundAnyGoodBearing = true;
                 }
 
                 //The upper bearing will be the last acceptable bearing before finding any unacceptable bearing.
                 if (!foundAnyBadBearing) {
                     upperBearing = bearing;
+                } else if (foundAnyBadBearing && foundAnyGoodBearing && !stopFindingUpperBearing) {
+                    upperBearing = bearing;
                 }
 
 
@@ -658,11 +665,16 @@ public class Race implements Runnable {
 
                 foundAnyBadBearing = true;
 
+                if (foundAnyBadBearing && foundAnyGoodBearing) {
+                    stopFindingUpperBearing = true;
+                }
+
             }
 
         }
 
 
+        //TODO BUG if it can't find either upper or lower, it returns (0, 359.999). Should return (boatbearing, boatbearing+0.0001)
         bearings[0] = lowerBearing;
         bearings[1] = upperBearing;
 
@@ -683,11 +695,11 @@ public class Race implements Runnable {
 
 
         //Tests to see if a point in front of the boat is out of bounds.
-        double epsilonMeters = 100d;
+        double epsilonMeters = 50d;
         GPSCoordinate testCoord = GPSCoordinate.calculateNewPosition(position, epsilonMeters, azimuth);
 
         //If it isn't inside the boundary, calculate new bearing.
-        if (GPSCoordinate.isInsideBoundary(testCoord, this.boundary)) {
+        if (GPSCoordinate.isInsideBoundary(testCoord, this.shrinkBoundary)) {
             return true;
         } else {
             return false;
@@ -865,6 +877,7 @@ public class Race implements Runnable {
         if (windDir < 0){
             windDir += 65535;
         }
+
         this.windDirection = new Bearing(BoatLocation.convertHeadingIntToDouble(windDir));
     }
 

network/src/main/java/seng302/Networking/Messages/BoatLocation.java

@@ -227,7 +227,7 @@ public class BoatLocation extends AC35Data {
      */
     public static int convertHeadingDoubleToInt(double heading) {
 
-        int headingInt = (int) ((heading / 360.0) * 65536.0);
+        int headingInt = (int) ((heading * 65536.0) / 360.0);
 
         return headingInt;
     }

network/src/main/java/seng302/Networking/Messages/RaceStatus.java

@@ -18,7 +18,7 @@ public class RaceStatus  extends AC35Data {
     private int windSpeed;
     private int raceType;
     private List<BoatStatus> boatStatuses;
-    private static final double windDirectionScalar =  360.0 / 32768.0; // 0x8000 / 360
+    private static final double windDirectionScalar =  360.0 / 65536.0; // 0x8000 / 360
 
     public RaceStatus(long currentTime, int raceID, int raceStatus, long expectedStartTime, int windDirection, int windSpeed, int raceType, List<BoatStatus> boatStatuses){
         super(MessageType.RACESTATUS);