Commit 657f5807 authored by AndreR's avatar AndreR Committed by TIGERs GitLab
Browse files

Resolve "Intercept ball on the redirect ball trajectory"

Closes #1648

See merge request main/Sumatra!1394

sumatra-commit: 4e0f3b6e6ad2d164a5257a06310d6d278a8f2cc6
parent 9efd2067
Pipeline #16754 passed with stage
in 4 minutes and 8 seconds
...@@ -17,6 +17,7 @@ import lombok.Getter; ...@@ -17,6 +17,7 @@ import lombok.Getter;
import java.util.Collections; import java.util.Collections;
import java.util.List; import java.util.List;
import java.util.stream.Collectors;
/** /**
...@@ -231,7 +232,7 @@ public abstract class ABallTrajectory implements IBallTrajectory ...@@ -231,7 +232,7 @@ public abstract class ABallTrajectory implements IBallTrajectory
@Override @Override
public List<ILineSegment> getTravelLinesInterceptable() public List<ILineSegment> getTravelLinesInterceptable()
{ {
return Collections.singletonList(getTravelLineSegment()); return getTravelLineSegments();
} }
...@@ -240,4 +241,21 @@ public abstract class ABallTrajectory implements IBallTrajectory ...@@ -240,4 +241,21 @@ public abstract class ABallTrajectory implements IBallTrajectory
{ {
return Collections.emptyList(); return Collections.emptyList();
} }
@Override
public IVector2 closestPointTo(IVector2 point)
{
var closestPointsToIdealPos = getTravelLinesRolling().stream()
.map(line -> line.closestPointOnLine(point))
.collect(Collectors.toUnmodifiableList());
return point.nearestTo(closestPointsToIdealPos);
}
@Override
public double distanceTo(IVector2 point)
{
return closestPointTo(point).distanceTo(point);
}
} }
...@@ -9,6 +9,7 @@ import edu.tigers.sumatra.ball.BallState; ...@@ -9,6 +9,7 @@ import edu.tigers.sumatra.ball.BallState;
import edu.tigers.sumatra.math.IMirrorable; import edu.tigers.sumatra.math.IMirrorable;
import edu.tigers.sumatra.math.line.v2.IHalfLine; import edu.tigers.sumatra.math.line.v2.IHalfLine;
import edu.tigers.sumatra.math.line.v2.ILineSegment; import edu.tigers.sumatra.math.line.v2.ILineSegment;
import edu.tigers.sumatra.math.vector.IEuclideanDistance;
import edu.tigers.sumatra.math.vector.IVector2; import edu.tigers.sumatra.math.vector.IVector2;
import edu.tigers.sumatra.math.vector.IVector3; import edu.tigers.sumatra.math.vector.IVector3;
import edu.tigers.sumatra.planarcurve.IPlanarCurveProvider; import edu.tigers.sumatra.planarcurve.IPlanarCurveProvider;
...@@ -19,7 +20,7 @@ import java.util.List; ...@@ -19,7 +20,7 @@ import java.util.List;
/** /**
* Interface for ball trajectories. * Interface for ball trajectories.
*/ */
public interface IBallTrajectory extends IMirrorable<IBallTrajectory>, IPlanarCurveProvider public interface IBallTrajectory extends IMirrorable<IBallTrajectory>, IPlanarCurveProvider, IEuclideanDistance
{ {
/** /**
* Get ball parameters for this trajectory. * Get ball parameters for this trajectory.
...@@ -206,6 +207,15 @@ public interface IBallTrajectory extends IMirrorable<IBallTrajectory>, IPlanarCu ...@@ -206,6 +207,15 @@ public interface IBallTrajectory extends IMirrorable<IBallTrajectory>, IPlanarCu
*/ */
ILineSegment getTravelLineSegment(); ILineSegment getTravelLineSegment();
/**
* @return all lines from current ball pos to ball end pos;
* the lines are an approximation for curved shots
*/
default List<ILineSegment> getTravelLineSegments()
{
return List.of(getTravelLineSegment());
}
/** /**
* Same as getTravelLine() for a flat ball. * Same as getTravelLine() for a flat ball.
...@@ -215,6 +225,18 @@ public interface IBallTrajectory extends IMirrorable<IBallTrajectory>, IPlanarCu ...@@ -215,6 +225,18 @@ public interface IBallTrajectory extends IMirrorable<IBallTrajectory>, IPlanarCu
ILineSegment getTravelLineRolling(); ILineSegment getTravelLineRolling();
/**
* Same as getTravelLine() for a flat ball.
*
* @return all lines from the point where the ball is rolling on the ground to end pos;
* the lines are an approximation for curved shots
*/
default List<ILineSegment> getTravelLinesRolling()
{
return List.of(getTravelLineRolling());
}
/** /**
* @return all segments of the travel line where the ball is interceptable (below robot height) * @return all segments of the travel line where the ball is interceptable (below robot height)
*/ */
...@@ -228,6 +250,14 @@ public interface IBallTrajectory extends IMirrorable<IBallTrajectory>, IPlanarCu ...@@ -228,6 +250,14 @@ public interface IBallTrajectory extends IMirrorable<IBallTrajectory>, IPlanarCu
*/ */
List<IVector2> getTouchdownLocations(); List<IVector2> getTouchdownLocations();
/**
* Find the closest point on this ball trajectory for the given point.
*
* @param point some point
* @return the closest point on this trajectory
*/
IVector2 closestPointTo(IVector2 point);
/** /**
* Return a new trajectory where the initialPos is adjusted so that the position after time equals posNow. * Return a new trajectory where the initialPos is adjusted so that the position after time equals posNow.
* *
......
...@@ -9,6 +9,8 @@ import edu.tigers.sumatra.ball.BallState; ...@@ -9,6 +9,8 @@ import edu.tigers.sumatra.ball.BallState;
import edu.tigers.sumatra.ball.trajectory.ABallTrajectory; import edu.tigers.sumatra.ball.trajectory.ABallTrajectory;
import edu.tigers.sumatra.ball.trajectory.IBallTrajectory; import edu.tigers.sumatra.ball.trajectory.IBallTrajectory;
import edu.tigers.sumatra.math.SumatraMath; import edu.tigers.sumatra.math.SumatraMath;
import edu.tigers.sumatra.math.line.v2.ILineSegment;
import edu.tigers.sumatra.math.line.v2.Lines;
import edu.tigers.sumatra.math.vector.IVector2; import edu.tigers.sumatra.math.vector.IVector2;
import edu.tigers.sumatra.math.vector.IVector3; import edu.tigers.sumatra.math.vector.IVector3;
import edu.tigers.sumatra.math.vector.Vector2f; import edu.tigers.sumatra.math.vector.Vector2f;
...@@ -120,9 +122,9 @@ public class FlatBallTrajectory extends ABallTrajectory ...@@ -120,9 +122,9 @@ public class FlatBallTrajectory extends ABallTrajectory
* Create from state. * Create from state.
* *
* @param parameters * @param parameters
* @param posNow in [mm] * @param posNow in [mm]
* @param velNow in [mm/s] * @param velNow in [mm/s]
* @param spin in [rad/s] * @param spin in [rad/s]
* @return * @return
*/ */
public static FlatBallTrajectory fromState( public static FlatBallTrajectory fromState(
...@@ -309,4 +311,27 @@ public class FlatBallTrajectory extends ABallTrajectory ...@@ -309,4 +311,27 @@ public class FlatBallTrajectory extends ABallTrajectory
return tSwitch + tToVel; return tSwitch + tToVel;
} }
@Override
public List<ILineSegment> getTravelLineSegments()
{
IVector2 start = initialPos.getXYVector();
IVector2 end = getPosByTime(getTimeAtRest()).getXYVector();
if (posSwitch.equals(start))
{
return List.of(Lines.segmentFromPoints(start, end));
}
return List.of(
Lines.segmentFromPoints(start, posSwitch),
Lines.segmentFromPoints(posSwitch, end)
);
}
@Override
public List<ILineSegment> getTravelLinesRolling()
{
return getTravelLineSegments();
}
} }
...@@ -48,15 +48,15 @@ public class BallParameters ...@@ -48,15 +48,15 @@ public class BallParameters
@Configurable( @Configurable(
comment = "Amount of spin transferred during a redirect.", comment = "Amount of spin transferred during a redirect.",
defValue = "0.8", spezis = { "SUMATRA", "LAB", "TISCH", "ROBOCUP", "ANDRE", "NICOLAI", "SIMULATOR" },
spezis = { "SUMATRA", "LAB", "TISCH", "ROBOCUP", "ANDRE", "NICOLAI", "SIMULATOR" } defValueSpezis = { "0.8", "0.35", "0.35", "0.35", "0.35", "0.35", "0.8" }
) )
private double redirectSpinFactor = 0.8; private double redirectSpinFactor = 0.8;
@Configurable( @Configurable(
comment = "Restitution coefficient for redirected balls from a bot.", comment = "Restitution coefficient for redirected balls from a bot.",
defValue = "0.2", spezis = { "SUMATRA", "LAB", "TISCH", "ROBOCUP", "ANDRE", "NICOLAI", "SIMULATOR" },
spezis = { "SUMATRA", "LAB", "TISCH", "ROBOCUP", "ANDRE", "NICOLAI", "SIMULATOR" } defValueSpezis = { "0.2", "0.55", "0.55", "0.55", "0.55", "0.55", "0.2" }
) )
private double redirectRestitutionCoefficient = 0.2; private double redirectRestitutionCoefficient = 0.2;
......
...@@ -88,13 +88,14 @@ public class BallVisCalc implements IWpCalc ...@@ -88,13 +88,14 @@ public class BallVisCalc implements IWpCalc
.setColor(Color.magenta))); .setColor(Color.magenta)));
} }
ILineSegment rollLine = ball.getTrajectory().getTravelLineRolling(); ball.getTrajectory().getTravelLinesRolling().forEach(rollLine -> {
if (rollLine.directionVector().getLength2() > 1) if (rollLine.directionVector().getLength2() > 1)
{ {
DrawableLine roll = new DrawableLine(rollLine, Color.orange); DrawableLine roll = new DrawableLine(rollLine, Color.orange);
roll.setStrokeWidth(5); roll.setStrokeWidth(5);
shapeMap.get(EWpShapesLayer.BALL_PREDICTION).add(roll); shapeMap.get(EWpShapesLayer.BALL_PREDICTION).add(roll);
} }
});
wfw.getSimpleWorldFrame().getKickEvent().ifPresent(event -> shapeMap.get(EWpShapesLayer.BALL_PREDICTION) wfw.getSimpleWorldFrame().getKickEvent().ifPresent(event -> shapeMap.get(EWpShapesLayer.BALL_PREDICTION)
.add(new DrawablePoint(event.getPosition(), Color.red) .add(new DrawablePoint(event.getPosition(), Color.red)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment