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.Rotation2d;
009import edu.wpi.first.math.geometry.Translation2d;
010
011/** Enforces a particular constraint only within an elliptical region. */
012public class EllipticalRegionConstraint implements TrajectoryConstraint {
013  private final Translation2d m_center;
014  private final Translation2d m_radii;
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   */
026  @SuppressWarnings("ParameterName")
027  public EllipticalRegionConstraint(
028      Translation2d center,
029      double xWidth,
030      double yWidth,
031      Rotation2d rotation,
032      TrajectoryConstraint constraint) {
033    m_center = center;
034    m_radii = new Translation2d(xWidth / 2.0, yWidth / 2.0).rotateBy(rotation);
035    m_constraint = constraint;
036  }
037
038  @Override
039  public double getMaxVelocityMetersPerSecond(
040      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
041    if (isPoseInRegion(poseMeters)) {
042      return m_constraint.getMaxVelocityMetersPerSecond(
043          poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
044    } else {
045      return Double.POSITIVE_INFINITY;
046    }
047  }
048
049  @Override
050  public MinMax getMinMaxAccelerationMetersPerSecondSq(
051      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
052    if (isPoseInRegion(poseMeters)) {
053      return m_constraint.getMinMaxAccelerationMetersPerSecondSq(
054          poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
055    } else {
056      return new MinMax();
057    }
058  }
059
060  /**
061   * Returns whether the specified robot pose is within the region that the constraint is enforced
062   * in.
063   *
064   * @param robotPose The robot pose.
065   * @return Whether the robot pose is within the constraint region.
066   */
067  public boolean isPoseInRegion(Pose2d robotPose) {
068    // The region (disk) bounded by the ellipse is given by the equation:
069    // ((x-h)^2)/Rx^2) + ((y-k)^2)/Ry^2) <= 1
070    // If the inequality is satisfied, then it is inside the ellipse; otherwise
071    // it is outside the ellipse.
072    // Both sides have been multiplied by Rx^2 * Ry^2 for efficiency reasons.
073    return Math.pow(robotPose.getX() - m_center.getX(), 2) * Math.pow(m_radii.getY(), 2)
074            + Math.pow(robotPose.getY() - m_center.getY(), 2) * Math.pow(m_radii.getX(), 2)
075        <= Math.pow(m_radii.getX(), 2) * Math.pow(m_radii.getY(), 2);
076  }
077}