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 org.wpilib.math.trajectory.constraint;
006
007import org.wpilib.math.geometry.Ellipse2d;
008import org.wpilib.math.geometry.Pose2d;
009
010/** Enforces a particular constraint only within an elliptical region. */
011public class EllipticalRegionConstraint implements TrajectoryConstraint {
012  private final Ellipse2d m_ellipse;
013  private final TrajectoryConstraint m_constraint;
014
015  /**
016   * Constructs a new EllipticalRegionConstraint.
017   *
018   * @param ellipse The ellipse in which to enforce the constraint.
019   * @param constraint The constraint to enforce when the robot is within the region.
020   */
021  public EllipticalRegionConstraint(Ellipse2d ellipse, TrajectoryConstraint constraint) {
022    m_ellipse = ellipse;
023    m_constraint = constraint;
024  }
025
026  @Override
027  public double getMaxVelocity(Pose2d pose, double curvature, double velocity) {
028    if (m_ellipse.contains(pose.getTranslation())) {
029      return m_constraint.getMaxVelocity(pose, curvature, velocity);
030    } else {
031      return Double.POSITIVE_INFINITY;
032    }
033  }
034
035  @Override
036  public MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity) {
037    if (m_ellipse.contains(pose.getTranslation())) {
038      return m_constraint.getMinMaxAcceleration(pose, curvature, velocity);
039    } else {
040      return new MinMax();
041    }
042  }
043}