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.Pose2d;
008import edu.wpi.first.math.geometry.Rectangle2d;
009import edu.wpi.first.math.geometry.Translation2d;
010
011/** Enforces a particular constraint only within a rectangular region. */
012public class RectangularRegionConstraint implements TrajectoryConstraint {
013  private final Rectangle2d m_rectangle;
014  private final TrajectoryConstraint m_constraint;
015
016  /**
017   * Constructs a new RectangularRegionConstraint.
018   *
019   * @param bottomLeftPoint The bottom left point of the rectangular region in which to enforce the
020   *     constraint.
021   * @param topRightPoint The top right point of the rectangular region in which to enforce the
022   *     constraint.
023   * @param constraint The constraint to enforce when the robot is within the region.
024   * @deprecated Use constructor taking Rectangle2d instead.
025   */
026  @Deprecated(since = "2025", forRemoval = true)
027  public RectangularRegionConstraint(
028      Translation2d bottomLeftPoint, Translation2d topRightPoint, TrajectoryConstraint constraint) {
029    this(new Rectangle2d(bottomLeftPoint, topRightPoint), constraint);
030  }
031
032  /**
033   * Constructs a new RectangularRegionConstraint.
034   *
035   * @param rectangle The rectangular region in which to enforce the constraint.
036   * @param constraint The constraint to enforce when the robot is within the region.
037   */
038  public RectangularRegionConstraint(Rectangle2d rectangle, TrajectoryConstraint constraint) {
039    m_rectangle = rectangle;
040    m_constraint = constraint;
041  }
042
043  @Override
044  public double getMaxVelocity(Pose2d pose, double curvature, double velocity) {
045    if (m_rectangle.contains(pose.getTranslation())) {
046      return m_constraint.getMaxVelocity(pose, curvature, velocity);
047    } else {
048      return Double.POSITIVE_INFINITY;
049    }
050  }
051
052  @Override
053  public MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity) {
054    if (m_rectangle.contains(pose.getTranslation())) {
055      return m_constraint.getMinMaxAcceleration(pose, curvature, velocity);
056    } else {
057      return new MinMax();
058    }
059  }
060}