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;
import java.util.Collections;
import java.util.List;
import java.util.stream.Collectors;
/**
......@@ -231,7 +232,7 @@ public abstract class ABallTrajectory implements IBallTrajectory
@Override
public List<ILineSegment> getTravelLinesInterceptable()
{
return Collections.singletonList(getTravelLineSegment());
return getTravelLineSegments();
}
......@@ -240,4 +241,21 @@ public abstract class ABallTrajectory implements IBallTrajectory
{
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;
import edu.tigers.sumatra.math.IMirrorable;
import edu.tigers.sumatra.math.line.v2.IHalfLine;
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.IVector3;
import edu.tigers.sumatra.planarcurve.IPlanarCurveProvider;
......@@ -19,7 +20,7 @@ import java.util.List;
/**
* 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.
......@@ -206,6 +207,15 @@ public interface IBallTrajectory extends IMirrorable<IBallTrajectory>, IPlanarCu
*/
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.
......@@ -215,6 +225,18 @@ public interface IBallTrajectory extends IMirrorable<IBallTrajectory>, IPlanarCu
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)
*/
......@@ -228,6 +250,14 @@ public interface IBallTrajectory extends IMirrorable<IBallTrajectory>, IPlanarCu
*/
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.
*
......
......@@ -9,6 +9,8 @@ import edu.tigers.sumatra.ball.BallState;
import edu.tigers.sumatra.ball.trajectory.ABallTrajectory;
import edu.tigers.sumatra.ball.trajectory.IBallTrajectory;
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.IVector3;
import edu.tigers.sumatra.math.vector.Vector2f;
......@@ -120,9 +122,9 @@ public class FlatBallTrajectory extends ABallTrajectory
* Create from state.
*
* @param parameters
* @param posNow in [mm]
* @param velNow in [mm/s]
* @param spin in [rad/s]
* @param posNow in [mm]
* @param velNow in [mm/s]
* @param spin in [rad/s]
* @return
*/
public static FlatBallTrajectory fromState(
......@@ -309,4 +311,27 @@ public class FlatBallTrajectory extends ABallTrajectory
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
@Configurable(
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;
@Configurable(
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;
......
......@@ -88,13 +88,14 @@ public class BallVisCalc implements IWpCalc
.setColor(Color.magenta)));
}
ILineSegment rollLine = ball.getTrajectory().getTravelLineRolling();
if (rollLine.directionVector().getLength2() > 1)
{
DrawableLine roll = new DrawableLine(rollLine, Color.orange);
roll.setStrokeWidth(5);
shapeMap.get(EWpShapesLayer.BALL_PREDICTION).add(roll);
}
ball.getTrajectory().getTravelLinesRolling().forEach(rollLine -> {
if (rollLine.directionVector().getLength2() > 1)
{
DrawableLine roll = new DrawableLine(rollLine, Color.orange);
roll.setStrokeWidth(5);
shapeMap.get(EWpShapesLayer.BALL_PREDICTION).add(roll);
}
});
wfw.getSimpleWorldFrame().getKickEvent().ifPresent(event -> shapeMap.get(EWpShapesLayer.BALL_PREDICTION)
.add(new DrawablePoint(event.getPosition(), Color.red)
......
Markdown is supported
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