Merge pull request #14 from RoboTeamTwente/robot-distance

Robot distance

src/main/java/nl/roboteamtwente/autoref/SSLAutoRef.java

@@ -54,11 +54,11 @@ public void processWorldState(StateOuterClass.State statePacket) {
         deriveRefereeMessage(game, statePacket);
         deriveBall(game, world);
         for (WorldRobotOuterClass.WorldRobot robot : world.getBlueList()) {
-            deriveRobot(game, TeamColor.BLUE, robot);
+            deriveRobot(game, TeamColor.BLUE, robot, statePacket);
         }
 
         for (WorldRobotOuterClass.WorldRobot robot : world.getYellowList()) {
-            deriveRobot(game, TeamColor.YELLOW, robot);
+            deriveRobot(game, TeamColor.YELLOW, robot, statePacket);
         }
         deriveTeamData(game, statePacket);
         deriveField(game, statePacket);
@@ -194,7 +194,7 @@ private void deriveBall(Game game, WorldOuterClass.World world) {
      * @param teamColor  team color
      * @param worldRobot robot
      */
-    private void deriveRobot(Game game, TeamColor teamColor, WorldRobotOuterClass.WorldRobot worldRobot) {
+    private void deriveRobot(Game game, TeamColor teamColor, WorldRobotOuterClass.WorldRobot worldRobot, StateOuterClass.State statePacket) {
         Robot robot = game.getTeam(teamColor).getRobotById(worldRobot.getId());
         if (robot == null) {
             robot = new Robot(worldRobot.getId());
@@ -206,6 +206,12 @@ private void deriveRobot(Game game, TeamColor teamColor, WorldRobotOuterClass.Wo
         robot.getPosition().setY(worldRobot.getPos().getY());
         robot.getVelocity().setX(worldRobot.getVel().getX());
         robot.getVelocity().setY(worldRobot.getVel().getY());
+        if (teamColor == TeamColor.BLUE) {
+            robot.setRadius(statePacket.getBlueRobotParameters().getParameters().getRadius());
+            System.out.println(robot.getRadius());
+        } else {
+            robot.setRadius(statePacket.getYellowRobotParameters().getParameters().getRadius());
+        }
         robot.setAngle(worldRobot.getAngle());
     }
 

src/main/java/nl/roboteamtwente/autoref/model/Robot.java

@@ -11,6 +11,11 @@ public class Robot extends Entity {
      */
     private final int id;
 
+    /**
+     * Maximum radius of the robot in meters
+     */
+    private float radius;
+
     /**
      * A robot has an angle that it is facing during any point of the game.
      */
@@ -123,6 +128,17 @@ public RobotIdentifier getIdentifier() {
         return new RobotIdentifier(team.getColor(), id);
     }
 
+    public void setRadius(float r) {
+        this.radius = r;
+    }
+
+    /**
+     * 
+     * @return radius of robot
+     */
+    public float getRadius() {
+        return radius;
+    }
 
     /**
      *

src/main/java/nl/roboteamtwente/autoref/validators/AttackerTooCloseToDefenseAreaValidator.java

@@ -78,25 +78,25 @@ public RuleViolation validate(Game game) {
             }
 
             // Check if robot's X is within 0.2 m of the defender area
-            if (robotX > lineX - MAX_DISTANCE) {
+            if (robotX + robot.getRadius() > lineX - MAX_DISTANCE) {
 
                 // Check if robot's Y is also within 0.2m of the defender area
                 // Can use the absolute value of the Y position as the Y coordinate is mirrored from the middle line of the field
-                if (abs(robotY) < abs(lineY) + MAX_DISTANCE) {
+                if (abs(robotY) - robot.getRadius() < abs(lineY) + MAX_DISTANCE) {
 
-                    if (abs(robotY) < abs(lineY)) {
+                    if (abs(robotY) - robot.getRadius() < abs(lineY)) {
                         // Robot is in front of the line
-                        distance = lineX - robotX;
+                        distance = lineX - (robotX + robot.getRadius());
                     } else if (robotX > lineX) {
                         // Robot is above or below the defender area, within 0.2m
-                        distance = abs(robotY) - abs(lineY);
+                        distance = abs(robotY) - robot.getRadius() - abs(lineY);
                     }
 
                     // Check if robot is within one of the corners and calculate distance to the corner
                     // Can get either p1 or p2, they should have the same coordinates when taken the absolute Y value
-                    if (robotX < lineX && abs(robotY) > abs(lineY)) {
+                    if (robotX < lineX && abs(robotY) - robot.getRadius() > abs(lineY)) {
                         // Robot is in one of the corners, use pythagorean theorem to get distance to that corner
-                        distance = (float) Math.sqrt(Math.pow(lineX - robotX, 2) + Math.pow(abs(lineY) - abs(robotY), 2));
+                        distance = (float) (Math.sqrt(Math.pow(lineX - robotX, 2) + Math.pow(abs(lineY) - abs(robotY), 2)) - robot.getRadius());
                         if (distance > MAX_DISTANCE) {
                             // Robot is not within 0.2m of the corner, so check next robot
                             continue;

src/main/java/nl/roboteamtwente/autoref/validators/BotInterferedPlacementValidator.java

@@ -27,9 +27,10 @@ public class BotInterferedPlacementValidator implements RuleValidator {
      * @param p1 - first point of the line
      * @param p2 - second point of the line
      * @param p3 - the point to calculate distance to the line
+     * @param robot - robot
      * @return the distance between the point 3 and the line defined by point 1 and 2
      */
-    public boolean calculateDistancePointToLine(Vector2 p1, Vector2 p2, Vector2 p3) {
+    public boolean calculateDistancePointToLine(Vector2 p1, Vector2 p2, Vector2 p3, Robot robot) {
         double x1 = p1.getX();
         double y1 = p1.getY();
         double x2 = p2.getX();
@@ -39,7 +40,7 @@ public boolean calculateDistancePointToLine(Vector2 p1, Vector2 p2, Vector2 p3)
 
         // If distance from p3 to p1 or p2 < threshold then it's a violation
         // Step1: Circle check
-        if ((p3.distance(p2) < MIN_DISTANCE_BETWEEN_ROBOT_AND_PLACEMENT) || (p3.distance(p2) < MIN_DISTANCE_BETWEEN_ROBOT_AND_PLACEMENT)) {
+        if ((p3.distance(p1) - robot.getRadius() < MIN_DISTANCE_BETWEEN_ROBOT_AND_PLACEMENT) || (p3.distance(p2) - robot.getRadius() < MIN_DISTANCE_BETWEEN_ROBOT_AND_PLACEMENT)) {
             return true;
         }
 
@@ -70,7 +71,7 @@ public boolean calculateDistancePointToLine(Vector2 p1, Vector2 p2, Vector2 p3)
             double numerator = Math.abs(a1 * x3 + b1 * y3 + c1);
             double denominator = Math.sqrt(a1 * a1 + b1 * b1);
             double distance = numerator / denominator;
-            if (distance < MIN_DISTANCE_BETWEEN_ROBOT_AND_PLACEMENT) {
+            if (distance - robot.getRadius() < MIN_DISTANCE_BETWEEN_ROBOT_AND_PLACEMENT) {
                 return true;
             }
         }
@@ -127,7 +128,7 @@ public RuleViolation validate(Game game) {
                 Vector2 robotPos = robot.getPosition().xy();
                 Vector2 placementPos = game.getDesignatedPosition();
                 Vector2 ballPos = game.getBall().getPosition().xy();
-                if (calculateDistancePointToLine(ballPos, placementPos, robotPos)) {
+                if (calculateDistancePointToLine(ballPos, placementPos, robotPos, robot)) {
                     if (checkViolation(robot.getIdentifier(), game.getTime())) {
                         Vector2 roundRobotPos = new Vector2(roundFloatTo1DecimalPlace(robot.getPosition().getX()), roundFloatTo1DecimalPlace(robot.getPosition().getY()));
                         return new BotInterferedPlacementValidator.BotInterferedPlacementViolation(robot.getTeam().getColor(), robot.getId(), roundRobotPos, ballPos, placementPos);

src/main/java/nl/roboteamtwente/autoref/validators/DefenderTooCloseToKickPointValidator.java

@@ -42,7 +42,7 @@ public RuleViolation validate(Game game) {
             Vector2 robotPos = robot.getPosition().xy();
 
             // Calculate distance to ball
-            float distanceToBall = ball.distance(robotPos);
+            float distanceToBall = ball.distance(robotPos) - robot.getRadius();
 
             // If robot is within 0.5m of the ball, it is too close
             if (distanceToBall < 0.5) {