diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp deleted file mode 100644 index 56fe5e9e5e..0000000000 --- a/wpimath/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp +++ /dev/null @@ -1,56 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2020 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -#include "frc/trajectory/constraint/EllipticalRegionConstraint.h" - -#include - -#include "units/math.h" - -using namespace frc; - -EllipticalRegionConstraint::EllipticalRegionConstraint( - const Translation2d& center, units::meter_t xWidth, units::meter_t yWidth, - const Rotation2d& rotation, const TrajectoryConstraint& constraint) - : m_center(center), - m_radii(xWidth / 2.0, yWidth / 2.0), - m_constraint(constraint) { - m_radii = m_radii.RotateBy(rotation); -} - -units::meters_per_second_t EllipticalRegionConstraint::MaxVelocity( - const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t velocity) const { - if (IsPoseInRegion(pose)) { - return m_constraint.MaxVelocity(pose, curvature, velocity); - } else { - return units::meters_per_second_t(std::numeric_limits::infinity()); - } -} - -TrajectoryConstraint::MinMax EllipticalRegionConstraint::MinMaxAcceleration( - const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t speed) const { - if (IsPoseInRegion(pose)) { - return m_constraint.MinMaxAcceleration(pose, curvature, speed); - } else { - return {}; - } -} - -bool EllipticalRegionConstraint::IsPoseInRegion(const Pose2d& pose) const { - // The region (disk) bounded by the ellipse is given by the equation: - // ((x-h)^2)/Rx^2) + ((y-k)^2)/Ry^2) <= 1 - // If the inequality is satisfied, then it is inside the ellipse; otherwise - // it is outside the ellipse. - // Both sides have been multiplied by Rx^2 * Ry^2 for efficiency reasons. - return units::math::pow<2>(pose.X() - m_center.X()) * - units::math::pow<2>(m_radii.Y()) + - units::math::pow<2>(pose.Y() - m_center.Y()) * - units::math::pow<2>(m_radii.X()) <= - units::math::pow<2>(m_radii.X()) * units::math::pow<2>(m_radii.Y()); -} diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp deleted file mode 100644 index cf48381dd2..0000000000 --- a/wpimath/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp +++ /dev/null @@ -1,44 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2020 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -#include "frc/trajectory/constraint/RectangularRegionConstraint.h" - -#include - -using namespace frc; - -RectangularRegionConstraint::RectangularRegionConstraint( - const Translation2d& bottomLeftPoint, const Translation2d& topRightPoint, - const TrajectoryConstraint& constraint) - : m_bottomLeftPoint(bottomLeftPoint), - m_topRightPoint(topRightPoint), - m_constraint(constraint) {} - -units::meters_per_second_t RectangularRegionConstraint::MaxVelocity( - const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t velocity) const { - if (IsPoseInRegion(pose)) { - return m_constraint.MaxVelocity(pose, curvature, velocity); - } else { - return units::meters_per_second_t(std::numeric_limits::infinity()); - } -} - -TrajectoryConstraint::MinMax RectangularRegionConstraint::MinMaxAcceleration( - const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t speed) const { - if (IsPoseInRegion(pose)) { - return m_constraint.MinMaxAcceleration(pose, curvature, speed); - } else { - return {}; - } -} - -bool RectangularRegionConstraint::IsPoseInRegion(const Pose2d& pose) const { - return pose.X() >= m_bottomLeftPoint.X() && pose.X() <= m_topRightPoint.X() && - pose.Y() >= m_bottomLeftPoint.Y() && pose.Y() <= m_topRightPoint.Y(); -} diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h index e28d2aa1e3..78bc5696de 100644 --- a/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h +++ b/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h @@ -7,6 +7,8 @@ #pragma once +#include + #include "frc/geometry/Rotation2d.h" #include "frc/geometry/Translation2d.h" #include "frc/trajectory/constraint/TrajectoryConstraint.h" @@ -16,6 +18,8 @@ namespace frc { /** * Enforces a particular constraint only within an elliptical region. */ +template >> class EllipticalRegionConstraint : public TrajectoryConstraint { public: /** @@ -30,14 +34,32 @@ class EllipticalRegionConstraint : public TrajectoryConstraint { */ EllipticalRegionConstraint(const Translation2d& center, units::meter_t xWidth, units::meter_t yWidth, const Rotation2d& rotation, - const TrajectoryConstraint& constraint); + const Constraint& constraint) + : m_center(center), + m_radii(xWidth / 2.0, yWidth / 2.0), + m_constraint(constraint) { + m_radii = m_radii.RotateBy(rotation); + } units::meters_per_second_t MaxVelocity( const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t velocity) const override; + units::meters_per_second_t velocity) const override { + if (IsPoseInRegion(pose)) { + return m_constraint.MaxVelocity(pose, curvature, velocity); + } else { + return units::meters_per_second_t( + std::numeric_limits::infinity()); + } + } MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t speed) const override; + units::meters_per_second_t speed) const override { + if (IsPoseInRegion(pose)) { + return m_constraint.MinMaxAcceleration(pose, curvature, speed); + } else { + return {}; + } + } /** * Returns whether the specified robot pose is within the region that the @@ -46,11 +68,22 @@ class EllipticalRegionConstraint : public TrajectoryConstraint { * @param pose The robot pose. * @return Whether the robot pose is within the constraint region. */ - bool IsPoseInRegion(const Pose2d& pose) const; + bool IsPoseInRegion(const Pose2d& pose) const { + // The region (disk) bounded by the ellipse is given by the equation: + // ((x-h)^2)/Rx^2) + ((y-k)^2)/Ry^2) <= 1 + // If the inequality is satisfied, then it is inside the ellipse; otherwise + // it is outside the ellipse. + // Both sides have been multiplied by Rx^2 * Ry^2 for efficiency reasons. + return units::math::pow<2>(pose.X() - m_center.X()) * + units::math::pow<2>(m_radii.Y()) + + units::math::pow<2>(pose.Y() - m_center.Y()) * + units::math::pow<2>(m_radii.X()) <= + units::math::pow<2>(m_radii.X()) * units::math::pow<2>(m_radii.Y()); + } private: Translation2d m_center; Translation2d m_radii; - const TrajectoryConstraint& m_constraint; + Constraint m_constraint; }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h index 80f2a0994e..203b237a05 100644 --- a/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h +++ b/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h @@ -7,6 +7,8 @@ #pragma once +#include + #include "frc/geometry/Rotation2d.h" #include "frc/geometry/Translation2d.h" #include "frc/trajectory/constraint/TrajectoryConstraint.h" @@ -15,6 +17,8 @@ namespace frc { /** * Enforces a particular constraint only within a rectangular region. */ +template >> class RectangularRegionConstraint : public TrajectoryConstraint { public: /** @@ -29,14 +33,30 @@ class RectangularRegionConstraint : public TrajectoryConstraint { */ RectangularRegionConstraint(const Translation2d& bottomLeftPoint, const Translation2d& topRightPoint, - const TrajectoryConstraint& constraint); + const Constraint& constraint) + : m_bottomLeftPoint(bottomLeftPoint), + m_topRightPoint(topRightPoint), + m_constraint(constraint) {} units::meters_per_second_t MaxVelocity( const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t velocity) const override; + units::meters_per_second_t velocity) const override { + if (IsPoseInRegion(pose)) { + return m_constraint.MaxVelocity(pose, curvature, velocity); + } else { + return units::meters_per_second_t( + std::numeric_limits::infinity()); + } + } MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature, - units::meters_per_second_t speed) const override; + units::meters_per_second_t speed) const override { + if (IsPoseInRegion(pose)) { + return m_constraint.MinMaxAcceleration(pose, curvature, speed); + } else { + return {}; + } + } /** * Returns whether the specified robot pose is within the region that the @@ -45,11 +65,15 @@ class RectangularRegionConstraint : public TrajectoryConstraint { * @param pose The robot pose. * @return Whether the robot pose is within the constraint region. */ - bool IsPoseInRegion(const Pose2d& pose) const; + bool IsPoseInRegion(const Pose2d& pose) const { + return pose.X() >= m_bottomLeftPoint.X() && + pose.X() <= m_topRightPoint.X() && + pose.Y() >= m_bottomLeftPoint.Y() && pose.Y() <= m_topRightPoint.Y(); + } private: Translation2d m_bottomLeftPoint; Translation2d m_topRightPoint; - const TrajectoryConstraint& m_constraint; + Constraint m_constraint; }; } // namespace frc