Merge pull request #24 from RoboTeamTwente/update/defense-area-rules

Update/defense area rules

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

@@ -4,12 +4,14 @@
 import nl.roboteamtwente.proto.StateOuterClass;
 import nl.roboteamtwente.proto.WorldOuterClass;
 import nl.roboteamtwente.proto.WorldRobotOuterClass;
+
 import org.robocup.ssl.proto.SslVisionGeometry;
 
 import java.util.EnumSet;
 import java.util.List;
 import java.util.Objects;
 import java.util.function.Consumer;
+import java.util.Arrays;
 
 public class SSLAutoRef {
     private static final float BALL_TOUCHING_DISTANCE = 0.025f;
@@ -285,6 +287,28 @@ private void deriveField(Game game, StateOuterClass.State statePacket) {
 
             game.getField().addLine(fieldLine);
         }
+
+        // Add extra lines needed for rules around the defense area
+        for (Side side : Side.values()) {
+            String sideString = side == Side.LEFT ? "Left" : "Right";
+            FieldLine penaltyStretch = game.getField().getLineByName(sideString + "PenaltyStretch");
+            // check if p1 or p2 is positive
+            int factor = penaltyStretch.p1().getY() > penaltyStretch.p2().getY() ? 1 : -1;
+
+            String linename = sideString + "InnerMarginPenaltyStretch";
+            FieldLine innerMarginPenaltyStretch = new FieldLine(linename, 
+            penaltyStretch.p1().add(new Vector2(side.getCardinality()*0.09f, factor*-0.09f)).roundTo3Decimals(), 
+            penaltyStretch.p2().add(new Vector2(side.getCardinality()*0.09f, factor*0.09f)).roundTo3Decimals(),
+            penaltyStretch.thickness());
+            game.getField().addLine(innerMarginPenaltyStretch);
+
+            linename = sideString + "OuterMarginPenaltyStretch";
+            FieldLine outerMarginPenaltyStretch = new FieldLine(linename, 
+            penaltyStretch.p1().add(new Vector2(side.getCardinality()*-0.09f, factor*0.09f)).roundTo3Decimals(), 
+            penaltyStretch.p2().add(new Vector2(side.getCardinality()*-0.09f, factor*-0.09f)).roundTo3Decimals(), 
+            penaltyStretch.thickness());
+            game.getField().addLine(outerMarginPenaltyStretch);
+        }
     }
 
     /**
@@ -338,10 +362,7 @@ private void deriveTouch(Game game) {
             }
 
             // checks for ball bouncing of robots
-            if (game.isBallInPlay()) {
-                System.out.println("ANGLE: " + angle + "; ball pos: " + ball.getPosition().xy() + "; magnitude: " + ball.getVelocity().xy().magnitude());
-            }
-            if (ball.getVelocity().xy().magnitude() > 0.01f) {
+            if (ball.getVelocity().xy().magnitude() > 0.01f && ball.getPosition().getZ() < 0.15f) {
                 for (Robot robot : game.getRobots()) {
                     // case: ball is rolling, robot has velocity in the same direct to try and grab the ball.
                     // but ball bounces off the robot
@@ -392,7 +413,6 @@ private void deriveTouch(Game game) {
                 if ((distance <= robot.getTeam().getRobotRadius() + BALL_TOUCHING_DISTANCE && ball.getPosition().getZ()
                         <= robot.getTeam().getRobotHeight() + BALL_TOUCHING_DISTANCE) || robot.getIdentifier().equals(deflectedBy)) {
                     ball.getRobotsTouching().add(robot);
-
                     // it just started touching ball, either when its the first frame or when
                     // in the previous frame the robot was not touching the ball.
                     robot.setJustTouchedBall(oldRobot == null || !oldRobot.isTouchingBall());
@@ -423,6 +443,7 @@ private void deriveTouch(Game game) {
                     robot.setTouch(touch);
                     game.getTouches().add(touch);
 
+
                     System.out.println("touch #" + touch.getId() + " by " + robot.getIdentifier() + " at " + ball.getPosition().getX() + ", " + ball.getPosition().getY());
                 } else if (touch != null) {
                     touch.updatePercentages(ball.isVisible(), robotsCloseToBall);

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

@@ -82,6 +82,40 @@ public boolean isInDefenseArea(Side side, Vector2 location) {
 
     }
 
+    /**
+     * @param side side of the field that the check needs to happen on
+     * @param location location of the robot
+     */
+    public boolean isRobotFullyInDefenseArea(Side side, Vector2 location) {
+        String sideString = side == Side.LEFT ? "Left" : "Right";
+
+        FieldLine adjustedPenaltyStretch = getLineByName(sideString + "InnerMarginPenaltyStretch");
+        if (location.getX() * side.getCardinality() < adjustedPenaltyStretch.p1().getX() * side.getCardinality()) {
+            return false;
+        }
+
+        // check if p1 or p2 is positive
+        int factor = adjustedPenaltyStretch.p1().getY() > adjustedPenaltyStretch.p2().getY() ? 1 : -1;
+        return (location.getY() > adjustedPenaltyStretch.p2().getY() * factor && location.getY() < adjustedPenaltyStretch.p1().getY() * factor);
+    }
+
+    /**
+     * @param side side of the field that the check needs to happen on
+     * @param location location of the robot
+     */
+    public boolean isRobotPartiallyInDefenseArea(Side side, Vector2 location) {
+        String sideString = side == Side.LEFT ? "Left" : "Right";
+
+        FieldLine adjustedPenaltyStretch = getLineByName(sideString + "OuterMarginPenaltyStretch");
+        if (location.getX() * side.getCardinality() < adjustedPenaltyStretch.p1().getX() * side.getCardinality()) {
+            return false;
+        }
+
+        // check if p1 or p2 is positive
+        int factor = adjustedPenaltyStretch.p1().getY() > adjustedPenaltyStretch.p2().getY() ? 1 : -1;
+        return (location.getY() > adjustedPenaltyStretch.p2().getY() * factor && location.getY() < adjustedPenaltyStretch.p1().getY() * factor);
+    }
+
     public boolean isInOwnHalf(Side side, Vector2 location){
         FieldLine halfway = getLineByName("HalfwayLine");
 //        if (location.getX()  * side.getCardinality() > halfway.p1().getX() * side.getCardinality()){

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

@@ -104,7 +104,7 @@ public float dotProduct(Vector2 other) {
      * @return the length of vector.
      */
     public float magnitude() {
-        return ((float) Math.sqrt(this.getX()*this.getX() + this.getY()* this.getY()));
+        return ((float) Math.sqrt(this.getX()*this.getX() + this.getY()*this.getY()));
     }
 
     /**
@@ -136,6 +136,13 @@ public float angle(Vector2 other) {
         return (float) Math.toDegrees(Math.acos(dotProduct(other) / (magnitude() * other.magnitude())));
     }
 
+    /**
+     * Round the values of the vector to 3 decimals
+     */
+    public Vector2 roundTo3Decimals() {
+        return new Vector2(Math.round(1000*this.x)/1000.0f, Math.round(1000*this.y)/1000.0f);
+    }
+
     /**
      * @return the string value of the Vector2 object.
      */

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

@@ -105,6 +105,14 @@ public float distance(Vector3 other) {
         return (float) Math.sqrt(Math.pow(x - other.x, 2) + Math.pow(y - other.y, 2) + Math.pow(z - other.z, 2));
     }
 
+    /**
+     * Calculate the length/magnitude of the current vector.
+     * @return the length of vector.
+     */
+    public float magnitude() {
+        return ((float) Math.sqrt(this.getX()*this.getX() + this.getY()*this.getY() + this.getZ()*this.getZ()));
+    }
+
 
     /**
      * @return the string value of the Vector3 object.

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

@@ -35,7 +35,8 @@ public class AttackerTouchedBallInDefenseAreaValidator implements RuleValidator
     @Override
     public RuleViolation validate(Game game) {
         for (Robot robot : game.getBall().getRobotsTouching()) {
-            if (!game.getField().isInDefenseArea(robot.getTeam().getSide().getOpposite(), game.getBall().getPosition().xy())) {
+            if (!(game.getField().isRobotPartiallyInDefenseArea(robot.getTeam().getSide().getOpposite(), robot.getPosition().xy()) &&
+            game.getField().isInDefenseArea(robot.getTeam().getSide().getOpposite(), game.getBall().getPosition().xy()))) {
                 continue;
             }
 

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

@@ -28,7 +28,7 @@ public class BotKickedBallTooFastValidator implements RuleValidator {
     public RuleViolation validate(Game game) {
         // Ball speed in m/s
         Ball ball = game.getBall();
-        float speed = ball.getVelocity().xy().magnitude();
+        float speed = ball.getVelocity().magnitude();
 
         // If speed in one frame is higher than 6.5 m/s, ball was kicked too fast by the bot.
         if (speed > 6.5) {

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

@@ -55,7 +55,7 @@ public RuleViolation validate(Game game) {
                 continue;
             }
 
-            if (!game.getField().isInDefenseArea(robot.getTeam().getSide(), robot.getPosition().xy())) {
+            if (!game.getField().isRobotFullyInDefenseArea(robot.getTeam().getSide(), robot.getPosition().xy())) {
                 continue;
             }