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 getMaxVelocityMetersPerSecond(
045      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
046    if (m_rectangle.contains(poseMeters.getTranslation())) {
047      return m_constraint.getMaxVelocityMetersPerSecond(
048          poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
049    } else {
050      return Double.POSITIVE_INFINITY;
051    }
052  }
053
054  @Override
055  public MinMax getMinMaxAccelerationMetersPerSecondSq(
056      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
057    if (m_rectangle.contains(poseMeters.getTranslation())) {
058      return m_constraint.getMinMaxAccelerationMetersPerSecondSq(
059          poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
060    } else {
061      return new MinMax();
062    }
063  }
064}