From 3e220906eaca02804a77d11ae95a929f7902a523 Mon Sep 17 00:00:00 2001 From: Jonathan <148167.jw@gmail.com> Date: Thu, 11 Jul 2024 11:52:43 +0200 Subject: [PATCH 1/6] ball velocity check in 3D instead of 2D --- .../autoref/validators/BotKickedBallTooFastValidator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/nl/roboteamtwente/autoref/validators/BotKickedBallTooFastValidator.java b/src/main/java/nl/roboteamtwente/autoref/validators/BotKickedBallTooFastValidator.java index 743100c..a497546 100644 --- a/src/main/java/nl/roboteamtwente/autoref/validators/BotKickedBallTooFastValidator.java +++ b/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) { From 73541209bdfa9840d8001bcb3026dca52aec146c Mon Sep 17 00:00:00 2001 From: Jonathan <148167.jw@gmail.com> Date: Thu, 11 Jul 2024 13:32:50 +0200 Subject: [PATCH 2/6] wip --- .../java/nl/roboteamtwente/autoref/SSLAutoRef.java | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/main/java/nl/roboteamtwente/autoref/SSLAutoRef.java b/src/main/java/nl/roboteamtwente/autoref/SSLAutoRef.java index c531cd7..41583e4 100644 --- a/src/main/java/nl/roboteamtwente/autoref/SSLAutoRef.java +++ b/src/main/java/nl/roboteamtwente/autoref/SSLAutoRef.java @@ -10,6 +10,7 @@ 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; @@ -282,9 +283,17 @@ private void deriveField(Game game, StateOuterClass.State statePacket) { Vector2 p1 = new Vector2(lineSegment.getP1().getX() / 1000.0f, lineSegment.getP1().getY() / 1000.0f); Vector2 p2 = new Vector2(lineSegment.getP2().getX() / 1000.0f, lineSegment.getP2().getY() / 1000.0f); FieldLine fieldLine = new FieldLine(lineSegment.getName(), p1, p2, lineSegment.getThickness() / 1000.0f); + System.out.println(fieldLine); game.getField().addLine(fieldLine); } + + // Add extra lines needed for rules around the defense area + for (Side side : Side.values()) { + String linename = side.name().toLowerCase() + "InnerMarginPenaltyStretch"; + Field line penaltyStretch = game.getField().getLineByName(side.name().toLowerCase() + "PenaltyStretch"); + FieldLine innerMarginPenaltyStretch = new FieldLine(); + } } /** @@ -341,7 +350,7 @@ private void deriveTouch(Game game) { 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 From efc8dac85e7b2c77838bb283a005415103586223 Mon Sep 17 00:00:00 2001 From: DemoLaptop Date: Thu, 11 Jul 2024 14:19:35 +0200 Subject: [PATCH 3/6] velocity magnitude --- .../java/nl/roboteamtwente/autoref/model/Vector2.java | 2 +- .../java/nl/roboteamtwente/autoref/model/Vector3.java | 8 ++++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/java/nl/roboteamtwente/autoref/model/Vector2.java b/src/main/java/nl/roboteamtwente/autoref/model/Vector2.java index b598a90..5fdcb64 100644 --- a/src/main/java/nl/roboteamtwente/autoref/model/Vector2.java +++ b/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())); } /** diff --git a/src/main/java/nl/roboteamtwente/autoref/model/Vector3.java b/src/main/java/nl/roboteamtwente/autoref/model/Vector3.java index 066ce2f..0b67fb5 100644 --- a/src/main/java/nl/roboteamtwente/autoref/model/Vector3.java +++ b/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. From ef56aa9fa840a8d95f2a0eb873d799ed0da37063 Mon Sep 17 00:00:00 2001 From: DemoLaptop Date: Thu, 11 Jul 2024 15:13:24 +0200 Subject: [PATCH 4/6] Defining the lines needed to correctly check for the rules --- .../nl/roboteamtwente/autoref/SSLAutoRef.java | 23 ++++++++++++++----- .../roboteamtwente/autoref/model/Vector2.java | 7 ++++++ 2 files changed, 24 insertions(+), 6 deletions(-) diff --git a/src/main/java/nl/roboteamtwente/autoref/SSLAutoRef.java b/src/main/java/nl/roboteamtwente/autoref/SSLAutoRef.java index 41583e4..77911e2 100644 --- a/src/main/java/nl/roboteamtwente/autoref/SSLAutoRef.java +++ b/src/main/java/nl/roboteamtwente/autoref/SSLAutoRef.java @@ -4,6 +4,7 @@ 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; @@ -283,16 +284,29 @@ private void deriveField(Game game, StateOuterClass.State statePacket) { Vector2 p1 = new Vector2(lineSegment.getP1().getX() / 1000.0f, lineSegment.getP1().getY() / 1000.0f); Vector2 p2 = new Vector2(lineSegment.getP2().getX() / 1000.0f, lineSegment.getP2().getY() / 1000.0f); FieldLine fieldLine = new FieldLine(lineSegment.getName(), p1, p2, lineSegment.getThickness() / 1000.0f); - System.out.println(fieldLine); game.getField().addLine(fieldLine); } // Add extra lines needed for rules around the defense area for (Side side : Side.values()) { + FieldLine penaltyStretch = game.getField().getLineByName(side.name().substring(0, 1) + side.name().substring(1).toLowerCase() + "PenaltyStretch"); + // check if p1 or p2 is positive + int factor = penaltyStretch.p1().getY() > penaltyStretch.p2().getY() ? 1 : -1; + String linename = side.name().toLowerCase() + "InnerMarginPenaltyStretch"; - Field line penaltyStretch = game.getField().getLineByName(side.name().toLowerCase() + "PenaltyStretch"); - FieldLine innerMarginPenaltyStretch = new FieldLine(); + 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 = side.name().toLowerCase() + "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); } } @@ -347,9 +361,6 @@ 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 && 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. diff --git a/src/main/java/nl/roboteamtwente/autoref/model/Vector2.java b/src/main/java/nl/roboteamtwente/autoref/model/Vector2.java index 5fdcb64..dded7c7 100644 --- a/src/main/java/nl/roboteamtwente/autoref/model/Vector2.java +++ b/src/main/java/nl/roboteamtwente/autoref/model/Vector2.java @@ -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. */ From b022e1d8115bc6a28823ff08bef94445f6bcd047 Mon Sep 17 00:00:00 2001 From: DemoLaptop Date: Thu, 11 Jul 2024 16:50:12 +0200 Subject: [PATCH 5/6] fixed --- .../nl/roboteamtwente/autoref/SSLAutoRef.java | 9 ++--- .../roboteamtwente/autoref/model/Field.java | 34 +++++++++++++++++++ ...ckerTouchedBallInDefenseAreaValidator.java | 3 +- .../DefenderInDefenseAreaValidator.java | 2 +- 4 files changed, 42 insertions(+), 6 deletions(-) diff --git a/src/main/java/nl/roboteamtwente/autoref/SSLAutoRef.java b/src/main/java/nl/roboteamtwente/autoref/SSLAutoRef.java index 77911e2..9dcac65 100644 --- a/src/main/java/nl/roboteamtwente/autoref/SSLAutoRef.java +++ b/src/main/java/nl/roboteamtwente/autoref/SSLAutoRef.java @@ -290,18 +290,19 @@ private void deriveField(Game game, StateOuterClass.State statePacket) { // Add extra lines needed for rules around the defense area for (Side side : Side.values()) { - FieldLine penaltyStretch = game.getField().getLineByName(side.name().substring(0, 1) + side.name().substring(1).toLowerCase() + "PenaltyStretch"); + 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 = side.name().toLowerCase() + "InnerMarginPenaltyStretch"; + 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 = side.name().toLowerCase() + "OuterMarginPenaltyStretch"; + 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(), @@ -412,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()); @@ -443,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); diff --git a/src/main/java/nl/roboteamtwente/autoref/model/Field.java b/src/main/java/nl/roboteamtwente/autoref/model/Field.java index 76f38f6..47bbe0c 100644 --- a/src/main/java/nl/roboteamtwente/autoref/model/Field.java +++ b/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 object + */ + public boolean isFullyInDefenseArea(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 object + */ + public boolean isPartiallyInDefenseArea(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()){ diff --git a/src/main/java/nl/roboteamtwente/autoref/validators/AttackerTouchedBallInDefenseAreaValidator.java b/src/main/java/nl/roboteamtwente/autoref/validators/AttackerTouchedBallInDefenseAreaValidator.java index 491145b..119c995 100644 --- a/src/main/java/nl/roboteamtwente/autoref/validators/AttackerTouchedBallInDefenseAreaValidator.java +++ b/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().isPartiallyInDefenseArea(robot.getTeam().getSide().getOpposite(), robot.getPosition().xy()) && + !game.getField().isInDefenseArea(robot.getTeam().getSide().getOpposite(), game.getBall().getPosition().xy())) { continue; } diff --git a/src/main/java/nl/roboteamtwente/autoref/validators/DefenderInDefenseAreaValidator.java b/src/main/java/nl/roboteamtwente/autoref/validators/DefenderInDefenseAreaValidator.java index f4eaa05..0f107c8 100644 --- a/src/main/java/nl/roboteamtwente/autoref/validators/DefenderInDefenseAreaValidator.java +++ b/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().isFullyInDefenseArea(robot.getTeam().getSide(), robot.getPosition().xy())) { continue; } From 548f72fd1ad53ed8a398f36ec52d91142cf9737f Mon Sep 17 00:00:00 2001 From: DemoLaptop Date: Fri, 12 Jul 2024 11:01:17 +0200 Subject: [PATCH 6/6] update function names to clarify it only works for robots and not for the ball --- src/main/java/nl/roboteamtwente/autoref/model/Field.java | 8 ++++---- .../AttackerTouchedBallInDefenseAreaValidator.java | 4 ++-- .../validators/DefenderInDefenseAreaValidator.java | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/nl/roboteamtwente/autoref/model/Field.java b/src/main/java/nl/roboteamtwente/autoref/model/Field.java index 47bbe0c..235dfb2 100644 --- a/src/main/java/nl/roboteamtwente/autoref/model/Field.java +++ b/src/main/java/nl/roboteamtwente/autoref/model/Field.java @@ -84,9 +84,9 @@ 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 object + * @param location location of the robot */ - public boolean isFullyInDefenseArea(Side side, Vector2 location) { + public boolean isRobotFullyInDefenseArea(Side side, Vector2 location) { String sideString = side == Side.LEFT ? "Left" : "Right"; FieldLine adjustedPenaltyStretch = getLineByName(sideString + "InnerMarginPenaltyStretch"); @@ -101,9 +101,9 @@ public boolean isFullyInDefenseArea(Side side, Vector2 location) { /** * @param side side of the field that the check needs to happen on - * @param location location of the object + * @param location location of the robot */ - public boolean isPartiallyInDefenseArea(Side side, Vector2 location) { + public boolean isRobotPartiallyInDefenseArea(Side side, Vector2 location) { String sideString = side == Side.LEFT ? "Left" : "Right"; FieldLine adjustedPenaltyStretch = getLineByName(sideString + "OuterMarginPenaltyStretch"); diff --git a/src/main/java/nl/roboteamtwente/autoref/validators/AttackerTouchedBallInDefenseAreaValidator.java b/src/main/java/nl/roboteamtwente/autoref/validators/AttackerTouchedBallInDefenseAreaValidator.java index 119c995..e62f03b 100644 --- a/src/main/java/nl/roboteamtwente/autoref/validators/AttackerTouchedBallInDefenseAreaValidator.java +++ b/src/main/java/nl/roboteamtwente/autoref/validators/AttackerTouchedBallInDefenseAreaValidator.java @@ -35,8 +35,8 @@ public class AttackerTouchedBallInDefenseAreaValidator implements RuleValidator @Override public RuleViolation validate(Game game) { for (Robot robot : game.getBall().getRobotsTouching()) { - if (!game.getField().isPartiallyInDefenseArea(robot.getTeam().getSide().getOpposite(), robot.getPosition().xy()) && - !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; } diff --git a/src/main/java/nl/roboteamtwente/autoref/validators/DefenderInDefenseAreaValidator.java b/src/main/java/nl/roboteamtwente/autoref/validators/DefenderInDefenseAreaValidator.java index 0f107c8..f3cdb38 100644 --- a/src/main/java/nl/roboteamtwente/autoref/validators/DefenderInDefenseAreaValidator.java +++ b/src/main/java/nl/roboteamtwente/autoref/validators/DefenderInDefenseAreaValidator.java @@ -55,7 +55,7 @@ public RuleViolation validate(Game game) { continue; } - if (!game.getField().isFullyInDefenseArea(robot.getTeam().getSide(), robot.getPosition().xy())) { + if (!game.getField().isRobotFullyInDefenseArea(robot.getTeam().getSide(), robot.getPosition().xy())) { continue; }