001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.trajectory.constraint;
006
007import edu.wpi.first.math.geometry.Ellipse2d;
008import edu.wpi.first.math.geometry.Pose2d;
009import edu.wpi.first.math.geometry.Rotation2d;
010import edu.wpi.first.math.geometry.Translation2d;
011
012/** Enforces a particular constraint only within an elliptical region. */
013public class EllipticalRegionConstraint implements TrajectoryConstraint {
014  private final Ellipse2d m_ellipse;
015  private final TrajectoryConstraint m_constraint;
016
017  /**
018   * Constructs a new EllipticalRegionConstraint.
019   *
020   * @param center The center of the ellipse in which to enforce the constraint.
021   * @param xWidth The width of the ellipse in which to enforce the constraint in meters.
022   * @param yWidth The height of the ellipse in which to enforce the constraint in meters.
023   * @param rotation The rotation to apply to all radii around the origin.
024   * @param constraint The constraint to enforce when the robot is within the region.
025   * @deprecated Use constructor taking Ellipse2d instead.
026   */
027  @Deprecated(since = "2025", forRemoval = true)
028  public EllipticalRegionConstraint(
029      Translation2d center,
030      double xWidth,
031      double yWidth,
032      Rotation2d rotation,
033      TrajectoryConstraint constraint) {
034    this(new Ellipse2d(new Pose2d(center, rotation), xWidth / 2.0, yWidth / 2.0), constraint);
035  }
036
037  /**
038   * Constructs a new EllipticalRegionConstraint.
039   *
040   * @param ellipse The ellipse in which to enforce the constraint.
041   * @param constraint The constraint to enforce when the robot is within the region.
042   */
043  public EllipticalRegionConstraint(Ellipse2d ellipse, TrajectoryConstraint constraint) {
044    m_ellipse = ellipse;
045    m_constraint = constraint;
046  }
047
048  @Override
049  public double getMaxVelocity(Pose2d pose, double curvature, double velocity) {
050    if (m_ellipse.contains(pose.getTranslation())) {
051      return m_constraint.getMaxVelocity(pose, curvature, velocity);
052    } else {
053      return Double.POSITIVE_INFINITY;
054    }
055  }
056
057  @Override
058  public MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity) {
059    if (m_ellipse.contains(pose.getTranslation())) {
060      return m_constraint.getMinMaxAcceleration(pose, curvature, velocity);
061    } else {
062      return new MinMax();
063    }
064  }
065}