diff --git a/mock/src/main/java/seng302/Model/Angle.java b/mock/src/main/java/seng302/Model/Angle.java index 0fa6db0c..8fa767dd 100644 --- a/mock/src/main/java/seng302/Model/Angle.java +++ b/mock/src/main/java/seng302/Model/Angle.java @@ -54,9 +54,6 @@ public class Angle implements Comparable { return this.degrees; } - public void setDegrees(double degrees) { - this.degrees = degrees; - } /** * Returns the value of this Angle object, in radians. diff --git a/mock/src/main/java/seng302/Model/GPSCoordinate.java b/mock/src/main/java/seng302/Model/GPSCoordinate.java index 3f9527e2..a185b865 100644 --- a/mock/src/main/java/seng302/Model/GPSCoordinate.java +++ b/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 getShrinkBoundary(List boundary) { + + + double shrinkDistance = 50d; + List shrunkBoundary = new ArrayList<>(boundary.size()); + //This is a list of edges that have been shrunk/shifted inwards. + List> 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 edge1 = shrunkEdges.get(i); + Pair 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 edge1 = shrunkEdges.get(shrunkEdges.size() - 1); + Pair 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 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; + + + } + } diff --git a/mock/src/main/java/seng302/Model/Race.java b/mock/src/main/java/seng302/Model/Race.java index f0810eaf..1a9b447d 100644 --- a/mock/src/main/java/seng302/Model/Race.java +++ b/mock/src/main/java/seng302/Model/Race.java @@ -46,6 +46,11 @@ public class Race implements Runnable { */ private List boundary; + /** + * A copy of the boundary list, except "shrunk" inwards by 50m. + */ + private List 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,21 +597,15 @@ 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)){ - //checks to see if the new vmg sends the boat out of bounds and if so mirrors its direction in the wind - double currDegrees = newVMG.getBearing().degrees(); - double windDirectionDegrees = this.windDirection.degrees(); - double tempHeading = (currDegrees - windDirectionDegrees +90)%360; - newVMG.getBearing().setDegrees(tempHeading); - boat.setVMG(newVMG); + } else { + //We also need to use the new VMG if our current bearing will take us out of the course. + if (!this.checkBearingInsideCourse(boat.getBearing(), boat.getCurrentPosition())) { + boat.setVMG(newVMG); + } + } @@ -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)); } @@ -884,4 +897,4 @@ public class Race implements Runnable { protected int getWind(){ return windDir; } -} \ No newline at end of file +} diff --git a/network/src/main/java/seng302/Networking/Messages/BoatLocation.java b/network/src/main/java/seng302/Networking/Messages/BoatLocation.java index 9cb0bc78..fe7f2161 100644 --- a/network/src/main/java/seng302/Networking/Messages/BoatLocation.java +++ b/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; } diff --git a/network/src/main/java/seng302/Networking/Messages/RaceStatus.java b/network/src/main/java/seng302/Networking/Messages/RaceStatus.java index 89c7b90c..599411ac 100644 --- a/network/src/main/java/seng302/Networking/Messages/RaceStatus.java +++ b/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 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 boatStatuses){ super(MessageType.RACESTATUS);