Fix/ball deflection

LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2024 RoboTeam Twente
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

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

@@ -13,6 +13,7 @@
 
 public class SSLAutoRef {
     private static final float BALL_TOUCHING_DISTANCE = 0.025f;
+    private static final float BALL_ANGLE_NOISE_RANGE = 5.0f;
 
     private final Referee referee;
 
@@ -88,6 +89,7 @@ public void processWorldState(StateOuterClass.State statePacket) {
      * @param statePacket packet AutoRef got from World
      */
     private void deriveRefereeMessage(Game game, StateOuterClass.State statePacket) {
+        game.setKickPoint(game.getPrevious().getKickPoint());
         if (game.getState() == null || statePacket.getReferee().getCommandCounter() != commands) {
             game.setState(game.getPrevious().getState());
 
@@ -113,6 +115,7 @@ private void deriveRefereeMessage(Game game, StateOuterClass.State statePacket)
                 case NORMAL_START -> {
                     // Normal start starts the current stage of the game.
                     if (game.getPrevious().getState() == GameState.PREPARE_KICKOFF) {
+                        game.setKickPoint(game.getPrevious().getBall().getPosition().xy());
                         game.setState(GameState.KICKOFF);
                     } else if (game.getPrevious().getState() == GameState.PREPARE_PENALTY) {
                         game.setState(GameState.PENALTY);
@@ -121,6 +124,7 @@ private void deriveRefereeMessage(Game game, StateOuterClass.State statePacket)
                 //noinspection deprecation
                 case INDIRECT_FREE_YELLOW, INDIRECT_FREE_BLUE, DIRECT_FREE_YELLOW, DIRECT_FREE_BLUE -> {
                     // Free kick is always triggered.
+                    game.setKickPoint(game.getPrevious().getBall().getPosition().xy());
                     game.setState(GameState.FREE_KICK);
                 }
                 case PREPARE_KICKOFF_YELLOW, PREPARE_KICKOFF_BLUE -> {
@@ -196,6 +200,20 @@ private void deriveBall(Game game, WorldOuterClass.World world) {
         game.getBall().getVelocity().setY(world.getBall().getVel().getY());
         game.getBall().getVelocity().setZ(world.getBall().getZVel());
         game.getBall().setVisible(world.getBall().getVisible());
+
+        game.setKickIntoPlay(game.getPrevious().getKickIntoPlay());
+
+        // if this happened during kickoff or a free kick, this is the kick into play
+        if ((game.getState() == GameState.KICKOFF || game.getState() == GameState.FREE_KICK) && 
+            game.getBall().getPosition().xy().distance(game.getKickPoint()) >= 0.05f) {
+            game.setKickType(game.getState() == GameState.KICKOFF ? KickType.KICKOFF : KickType.FREE_KICK);
+            game.setKickIntoPlay(game.getPrevious().getLastStartedTouch());
+
+            // we change the state to running
+            game.setState(GameState.RUN);
+
+            System.out.println("ball kicked into play");
+        }
     }
 
     /**
@@ -278,7 +296,6 @@ private void deriveTouch(Game game) {
         // copy variables from previous game
         game.getBall().setLastTouchStarted(game.getPrevious().getBall().getLastTouchStarted());
         game.setKickType(game.getPrevious().getKickType());
-        game.setKickIntoPlay(game.getPrevious().getKickIntoPlay());
         for (Touch touch : game.getPrevious().getTouches()) {
             if (!touch.isFinished()) {
                 game.getTouches().add(touch);
@@ -294,12 +311,11 @@ private void deriveTouch(Game game) {
 
         //check if ball has randomly teleported
         if (game.getBall().getLastTouchStarted() != null && !game.getBall().getLastTouchStarted().isFinished()) { // a robot is still touching the ball
-            Ball ball_ = game.getBall();
-            Touch touch_ = ball_.getLastTouchStarted();
+            Touch touch_ = ball.getLastTouchStarted();
             Robot robot_ = game.getRobot(touch_.getBy());
             //if distance between robot and ball is greater than 15m/s * 60Hz + robot radius there is a false positive
             float f = (15.0f / 60.0f + robot_.getRadius());
-            if (ball_.getPosition().xy().distance(robot_.getPosition().xy()) > (15.0f / 60.0f + robot_.getRadius())) {
+            if (ball.getPosition().xy().distance(robot_.getPosition().xy()) > (15.0f / 60.0f + robot_.getRadius())) {
                 return;
             }
         }
@@ -311,22 +327,70 @@ private void deriveTouch(Game game) {
             }
         }
 
-        for (Robot robot : game.getRobots()) {
-            Robot oldRobot = game.getPrevious().getRobot(robot.getIdentifier());
+        if (ball.isVisible()) {
+            Ball previousBall = game.getPrevious().getBall();
+            float angle = 0.0f;
+            float distance = 0.0f;
+            RobotIdentifier deflectedBy = null;
+            float deflectedMinDistance = 1.0f;
+            if (previousBall.isVisible()) {
+                angle = Math.abs(ball.getVelocity().xy().angle(previousBall.getVelocity().xy()));
+            }
+
+            // 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) {
+                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
+                    if (ball.getVelocity().xy().magnitude() > previousBall.getVelocity().xy().magnitude() + 0.1f && robot.getVelocity().xy().magnitude() > 0.1f) {
+                        // robot is in (robot radius + speed ball + 0.03 margin) meters from the ball
+                        // robot is traveling towards to ball
+                        distance = ball.getPosition().xy().distance(robot.getPosition().xy());
+                        if (distance < robot.getRadius() + ball.getVelocity().xy().magnitude() / 80.0f + 0.03f 
+                        && (robot.getVelocity().xy().angle(ball.getVelocity().xy()) < 30 || robot.getVelocity().xy().angle(ball.getVelocity().xy()) > 330)
+                        && distance < deflectedMinDistance) {
+                            deflectedMinDistance = distance;
+                            deflectedBy = robot.getIdentifier();
+                        }
+
+                    }
 
-            // copy over old values
-            if (oldRobot != null) {
-                robot.setTouch(oldRobot.getTouch());
-                robot.setJustTouchedBall(oldRobot.hasJustTouchedBall());
+                    // case: ball bounces of a robot, changing its direction of travel
+                    if (angle > BALL_ANGLE_NOISE_RANGE && angle < 360.0f - BALL_ANGLE_NOISE_RANGE && !previousBall.getRobotsTouching().contains(robot)) {
+                        for (int i = 0; i <= 100; i++) {
+                            Vector2 ballPosAdjusted = previousBall.getPosition().xy().add(previousBall.getVelocity().xy().multiply(i/100.0f));
+                            distance = robot.getPosition().xy().distance(ballPosAdjusted);
+                            if (distance < robot.getRadius() + 0.022 && distance < deflectedMinDistance) {
+                                deflectedMinDistance = distance;
+                                deflectedBy = robot.getIdentifier();
+                            } else if (distance >= 1.0f) {
+                                break;
+                            } else if (robot.getIdentifier() == deflectedBy && distance > deflectedMinDistance) {
+                                break;
+                            } 
+                        }
+                    }
+                }
             }
 
-            Touch touch = robot.getTouch();
-            float distance = robot.getPosition().xy().distance(ballPosition.xy());
+            for (Robot robot : game.getRobots()) {
+                Robot oldRobot = game.getPrevious().getRobot(robot.getIdentifier());
+
+                // copy over old values
+                if (oldRobot != null) {
+                    robot.setTouch(oldRobot.getTouch());
+                    robot.setJustTouchedBall(oldRobot.hasJustTouchedBall());
+                }
+
+                Touch touch = robot.getTouch();
+                distance = robot.getPosition().xy().distance(ballPosition.xy());
 
-            if (ball.isVisible()) {
                 // detect if there's a touch
-                if (distance <= robot.getTeam().getRobotRadius() + BALL_TOUCHING_DISTANCE && ball.getPosition().getZ()
-                        <= robot.getTeam().getRobotHeight() + BALL_TOUCHING_DISTANCE) {
+                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
@@ -343,40 +407,34 @@ private void deriveTouch(Game game) {
                         touch.setEndTime(game.getTime());
                         touch.setEndVelocity(ball.getVelocity());
 
+                        System.out.println("End of touch #" + touch.getId() + " at [x,y] :" + touch.getEndLocation().getX() + ", " + touch.getEndLocation().getY());
+
                         // if this touch is the kick into play, we update that too
                         if (Objects.equals(touch, game.getKickIntoPlay())) {
                             game.setKickIntoPlay(touch);
                         }
                     }
                 }
-            }
-
-            if (robot.hasJustTouchedBall()) {
-                // we create a new partial touch
-                touch = new Touch(nextTouchId++, ballPosition, game.getTime(), ball.getVelocity(), robotsCloseToBall, robot.getIdentifier());
-                ball.setLastTouchStarted(touch);
-                robot.setTouch(touch);
 
-                System.out.print("touch #" + touch.getId() + " by " + robot.getIdentifier());
-
-                // if this happened during kickoff or a free kick, this is the kick into play
-                if (game.getState() == GameState.KICKOFF || game.getState() == GameState.FREE_KICK) {
-                    game.setKickType(game.getState() == GameState.KICKOFF ? KickType.KICKOFF : KickType.FREE_KICK);
-                    game.setKickIntoPlay(touch);
-
-                    // we change the state to running
-                    game.setState(GameState.RUN);
-
-                    System.out.print(" (kick into play)");
+                if (robot.hasJustTouchedBall()) {
+                    // we create a new partial touch
+                    touch = new Touch(nextTouchId++, ballPosition, game.getTime(), ball.getVelocity(), robotsCloseToBall, robot.getIdentifier());
+                    ball.setLastTouchStarted(touch);
+                    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);
                 }
-            } else if (touch != null) {
-                touch.updatePercentages(ball.isVisible(), robotsCloseToBall);
-            }
 
-            // to conclude, we add the touch to the game
-            if (touch != null && !game.getTouches().contains(touch)) {
-                game.getTouches().add(touch);
+                // to conclude, we add the touch to the game
+                if (touch != null && !game.getTouches().contains(touch)) {
+                    game.getTouches().add(touch);
+                }
             }
+        } else if (game.getLastStartedTouch() != null && !game.getLastStartedTouch().isFinished()) {
+            game.getLastStartedTouch().updatePercentages(ball.isVisible(), robotsCloseToBall);
         }
     }
 

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

@@ -32,11 +32,11 @@ public void setLastTouchStarted(Touch lastTouchStarted) {
      * @return a list of robots which are currently touching the ball.
      */
     public List<Robot> getRobotsTouching() {
-        return robotsTouching;
+        return this.robotsTouching;
     }
 
     public boolean isVisible(){
-        return visible;
+        return this.visible;
     }
 
     public void setVisible(boolean vis){

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

@@ -5,6 +5,8 @@
 import java.util.ArrayList;
 import java.util.List;
 import java.util.stream.Collectors;
+import java.util.Comparator;
+import java.util.Optional;
 
 
 /**
@@ -61,9 +63,12 @@ public class Game {
 
     private Game previous;
 
+    private Vector2 kickPoint;
     private Touch kickIntoPlay;
     private KickType kickType;
     private final List<Touch> touches;
+    private final Comparator<Touch> endTimeComparator = Comparator.comparing(Touch::getEndTime);
+    private final Comparator<Touch> startTimeComparator = Comparator.comparing(Touch::getStartTime);
 
     private boolean forceStarted;
 
@@ -218,11 +223,13 @@ public List<Touch> getFinishedTouches() {
     }
 
     public Touch getLastStartedTouch() {
-        return touches.isEmpty() ? null : touches.get(touches.size() - 1);
+        Optional<Touch> optionalTouch = touches.stream().filter(Touch::isFinished).max(startTimeComparator);
+        return optionalTouch.isPresent() ? optionalTouch.get() : null;
     }
 
     public Touch getLastFinishedTouch() {
-        return touches.stream().filter(Touch::isFinished).reduce((first, second) -> second).orElse(null);
+        Optional<Touch> optionalTouch = touches.stream().filter(Touch::isFinished).max(endTimeComparator);
+        return optionalTouch.isPresent() ? optionalTouch.get() : null;
     }
 
     public Touch getKickIntoPlay() {
@@ -261,4 +268,12 @@ public boolean isBallInPlay() {
         return getState() == GameState.RUN || (getState() == GameState.PENALTY && getKickIntoPlay() != null);
     }
 
+    public void setKickPoint(Vector2 point) {
+        this.kickPoint = point;
+    }
+
+    public Vector2 getKickPoint() {
+        return this.kickPoint;
+    }
+
 }

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

@@ -85,4 +85,8 @@ public int getId() {
     public RobotIdentifier getBy() {
         return by;
     }
+
+    public double getStartTime() {
+        return this.startTime;
+    }
 }

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

@@ -74,16 +74,25 @@ public Vector2 add(Vector2 other) {
 
     /**
      * Subtract the dimensions of another vector to the one in the current object.
-     * @param other the vector to add.
+     * @param other the vector to substract.
      * @return the new Vector object.
      */
     public Vector2 subtract(Vector2 other) {
         return new Vector2(this.getX() - other.getX(), this.getY() - other.getY());
     }
 
+    /**
+     * Multiply vector by a constant
+     * @param constant the constant the vertor needs to be multiplied with
+     * @return the new Vector object
+     */
+    public Vector2 multiply(float constant) {
+        return new Vector2(constant * this.getX(), constant * this.getY());
+    }
+
     /**
      * Calculate the dot product of the current object with other object.
-     * @param other the vector to add.
+     * @param other the second vector.
      * @return the new Vector object.
      */
     public float dotProduct(Vector2 other) {

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

@@ -20,19 +20,17 @@ public class AttackerDoubleTouchedBallValidator implements RuleValidator {
     @Override
     public RuleViolation validate(Game game) {
         Touch kickIntoPlay = game.getKickIntoPlay();
-        if (kickIntoPlay == null || !kickIntoPlay.equals(game.getLastFinishedTouch())) {
+        if (kickIntoPlay == null || 
+        !(kickIntoPlay.equals(game.getLastFinishedTouch()) || kickIntoPlay.equals(game.getLastStartedTouch()))) {
             return null;
         }
 
         Robot robot = game.getRobot(kickIntoPlay.getBy());
         Touch currentTouch = robot.getTouch();
 
-        // Ball should move 0.05 meters before "in play", then another 0.05 meters before it's a violation.
-        float distance = kickIntoPlay.isFinished() ? 0.05f : 0.10f;
-
-        if (!triggered && currentTouch != null && game.getBall().getPosition().distance(kickIntoPlay.getStartLocation()) >= distance) {
+        if (!triggered && currentTouch != null && game.getBall().getPosition().xy().distance(game.getKickPoint()) >= 0.05f) {
             triggered = true;
-            return new Violation(robot.getTeam().getColor(),robot.getIdentifier(), game.getKickIntoPlay().getStartLocation().xy());
+            return new Violation(robot.getTeam().getColor(),robot.getIdentifier(), game.getKickPoint());
         }
 
         return null;

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

@@ -35,7 +35,7 @@ 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(), robot.getPosition().xy())) {
+            if (!game.getField().isInDefenseArea(robot.getTeam().getSide().getOpposite(), game.getBall().getPosition().xy())) {
                 continue;
             }
 

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

@@ -39,7 +39,7 @@ public RuleViolation validate(Game game) {
                 return null;
             }
 
-            RobotIdentifier byBot = game.getLastStartedTouch().getBy();
+            RobotIdentifier byBot = game.getBall().getLastTouchStarted().getBy();
             if (game.getTime() - lastViolations > GRACE_PERIOD) {
                 lastViolations = game.getTime();
                 return new Violation(byBot.teamColor(), byBot.id(), ball.xy());

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

@@ -35,11 +35,11 @@ public RuleViolation validate(Game game) {
         }
 
         if (ball.getY() > topTouchLine.p1().getY() || ball.getY() < bottomTouchLine.p1().getY()) {
-            if (game.getLastStartedTouch() == null) {
+            if (game.getBall().getLastTouchStarted() == null) {
                 return null;
             }
 
-            RobotIdentifier byBot = game.getLastStartedTouch().getBy();
+            RobotIdentifier byBot = game.getBall().getLastTouchStarted().getBy();
             if (game.getTime() - lastViolations > GRACE_PERIOD) {
                 lastViolations = game.getTime();
                 return new Violation(byBot.teamColor(), byBot.id(), ball.xy());