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.
022   * @param yWidth The height of the ellipse in which to enforce the constraint.
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 getMaxVelocityMetersPerSecond(
050      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
051    if (m_ellipse.contains(poseMeters.getTranslation())) {
052      return m_constraint.getMaxVelocityMetersPerSecond(
053          poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
054    } else {
055      return Double.POSITIVE_INFINITY;
056    }
057  }
058
059  @Override
060  public MinMax getMinMaxAccelerationMetersPerSecondSq(
061      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
062    if (m_ellipse.contains(poseMeters.getTranslation())) {
063      return m_constraint.getMinMaxAccelerationMetersPerSecondSq(
064          poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
065    } else {
066      return new MinMax();
067    }
068  }
069}