Skip to content

Commit

Permalink
Merge pull request #23 from RoboTeamTwente/fix/ball-deflection
Browse files Browse the repository at this point in the history 
Fix/ball deflection
  • Loading branch information
@JWillegers
JWillegers authored Jul 10, 2024
2 parents d6ebae0 + ef3db51 commit d2e7a04
Show file tree
Hide file tree
Showing 11 changed files with 200 additions and 93 deletions.
21 changes: 21 additions & 0 deletions LICENSE
View file
Original file line number Diff line number Diff line change
@@ -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.
136 changes: 97 additions & 39 deletions src/main/java/nl/roboteamtwente/autoref/SSLAutoRef.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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());

Expand All @@ -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);
Expand All @@ -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 -> {
Expand Down Expand Up @@ -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");
}
}

/**
Expand Down Expand Up @@ -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);
Expand All @@ -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;
}
}
Expand All @@ -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
Expand All @@ -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);
}
}

Expand Down
4 changes: 2 additions & 2 deletions src/main/java/nl/roboteamtwente/autoref/model/Ball.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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){
Expand Down
19 changes: 17 additions & 2 deletions src/main/java/nl/roboteamtwente/autoref/model/Game.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;


/**
Expand Down Expand Up @@ -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;

Expand Down Expand Up @@ -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() {
Expand Down Expand Up @@ -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;
}

}
4 changes: 4 additions & 0 deletions src/main/java/nl/roboteamtwente/autoref/model/Touch.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,4 +85,8 @@ public int getId() {
public RobotIdentifier getBy() {
return by;
}

public double getStartTime() {
return this.startTime;
}
}
13 changes: 11 additions & 2 deletions src/main/java/nl/roboteamtwente/autoref/model/Vector2.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down
10 changes: 4 additions & 6 deletions src/main/java/nl/roboteamtwente/autoref/validators/AttackerDoubleTouchedBallValidator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion .../java/nl/roboteamtwente/autoref/validators/AttackerTouchedBallInDefenseAreaValidator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down
2 changes: 1 addition & 1 deletion src/main/java/nl/roboteamtwente/autoref/validators/BallLeftFieldGoalLineValidator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/nl/roboteamtwente/autoref/validators/BallLeftFieldTouchLineValidator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand Down
Loading

0 comments on commit d2e7a04

Please sign in to comment.