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  public EllipticalRegionConstraint(
027      Translation2d center,
028      double xWidth,
029      double yWidth,
030      Rotation2d rotation,
031      TrajectoryConstraint constraint) {
032    m_center = center;
033    m_radii = new Translation2d(xWidth / 2.0, yWidth / 2.0).rotateBy(rotation);
034    m_constraint = constraint;
035  }
036
037  @Override
038  public double getMaxVelocityMetersPerSecond(
039      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
040    if (isPoseInRegion(poseMeters)) {
041      return m_constraint.getMaxVelocityMetersPerSecond(
042          poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
043    } else {
044      return Double.POSITIVE_INFINITY;
045    }
046  }
047
048  @Override
049  public MinMax getMinMaxAccelerationMetersPerSecondSq(
050      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
051    if (isPoseInRegion(poseMeters)) {
052      return m_constraint.getMinMaxAccelerationMetersPerSecondSq(
053          poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
054    } else {
055      return new MinMax();
056    }
057  }
058
059  /**
060   * Returns whether the specified robot pose is within the region that the constraint is enforced
061   * in.
062   *
063   * @param robotPose The robot pose.
064   * @return Whether the robot pose is within the constraint region.
065   */
066  public boolean isPoseInRegion(Pose2d robotPose) {
067    // The region bounded by the ellipse is given by the equation:
068    //
069    // (x−h)²/Rx² + (y−k)²/Ry² ≤ 1
070    //
071    // Multiply by Rx²Ry² for efficiency reasons:
072    //
073    // (x−h)²Ry² + (y−k)²Rx² ≤ Rx²Ry²
074    //
075    // If the inequality is satisfied, then it is inside the ellipse; otherwise
076    // it is outside the ellipse.
077    return Math.pow(robotPose.getX() - m_center.getX(), 2) * Math.pow(m_radii.getY(), 2)
078            + Math.pow(robotPose.getY() - m_center.getY(), 2) * Math.pow(m_radii.getX(), 2)
079        <= Math.pow(m_radii.getX(), 2) * Math.pow(m_radii.getY(), 2);
080  }
081}