37 units::meter_t ySemiAxis)
38 : m_center{center}, m_xSemiAxis{xSemiAxis}, m_ySemiAxis{ySemiAxis} {
39 if (xSemiAxis <= 0_m || ySemiAxis <= 0_m) {
40 throw std::invalid_argument(
"Ellipse2d semi-axes must be positive");
53 m_ySemiAxis{radius} {}
74 constexpr units::meter_t
XSemiAxis()
const {
return m_xSemiAxis; }
81 constexpr units::meter_t
YSemiAxis()
const {
return m_ySemiAxis; }
98 if (m_xSemiAxis > m_ySemiAxis) {
136 return gcem::abs(1.0 - SolveEllipseEquation(point)) <= 1E-9;
147 return SolveEllipseEquation(point) <= 1.0;
174 return m_center == other.m_center &&
181 units::meter_t m_xSemiAxis;
182 units::meter_t m_ySemiAxis;
196 constexpr double SolveEllipseEquation(
const Translation2d& point)
const {
201 auto x = rotPoint.
X() - m_center.
X();
202 auto y = rotPoint.Y() - m_center.
Y();
204 return (x * x) / (m_xSemiAxis * m_xSemiAxis) +
205 (y * y) / (m_ySemiAxis * m_ySemiAxis);
#define WPILIB_DLLEXPORT
Definition: SymbolExports.h:36
Represents a 2d ellipse space containing translational, rotational, and scaling components.
Definition: Ellipse2d.h:26
constexpr bool operator==(const Ellipse2d &other) const
Checks equality between this Ellipse2d and another object.
Definition: Ellipse2d.h:173
constexpr bool Intersects(const Translation2d &point) const
Checks if a point is intersected by this ellipse's circumference.
Definition: Ellipse2d.h:135
constexpr const Rotation2d & Rotation() const
Returns the rotational component of the ellipse.
Definition: Ellipse2d.h:67
constexpr units::meter_t YSemiAxis() const
Returns the y semi-axis.
Definition: Ellipse2d.h:81
Translation2d FindNearestPoint(const Translation2d &point) const
Returns the nearest point that is contained within the ellipse.
constexpr Ellipse2d(const Pose2d ¢er, units::meter_t xSemiAxis, units::meter_t ySemiAxis)
Constructs an ellipse around a center point and two semi-axes, a horizontal and vertical one.
Definition: Ellipse2d.h:36
constexpr Ellipse2d TransformBy(const Transform2d &other) const
Transforms the center of the ellipse and returns the new ellipse.
Definition: Ellipse2d.h:115
constexpr wpi::array< Translation2d, 2 > FocalPoints() const
Returns the focal points of the ellipse.
Definition: Ellipse2d.h:89
constexpr bool Contains(const Translation2d &point) const
Checks if a point is contained within this ellipse.
Definition: Ellipse2d.h:146
constexpr Ellipse2d RotateBy(const Rotation2d &other) const
Rotates the center of the ellipse and returns the new ellipse.
Definition: Ellipse2d.h:125
constexpr units::meter_t XSemiAxis() const
Returns the x semi-axis.
Definition: Ellipse2d.h:74
constexpr const Pose2d & Center() const
Returns the center of the ellipse.
Definition: Ellipse2d.h:60
units::meter_t Distance(const Translation2d &point) const
Returns the distance between the perimeter of the ellipse and the point.
constexpr Ellipse2d(const Translation2d ¢er, double radius)
Constructs a perfectly circular ellipse with the specified radius.
Definition: Ellipse2d.h:50
Represents a 2D pose containing translational and rotational elements.
Definition: Pose2d.h:23
constexpr const Rotation2d & Rotation() const
Returns the underlying rotation.
Definition: Pose2d.h:103
constexpr units::meter_t Y() const
Returns the Y component of the pose's translation.
Definition: Pose2d.h:96
constexpr units::meter_t X() const
Returns the X component of the pose's translation.
Definition: Pose2d.h:89
constexpr const Translation2d & Translation() const
Returns the underlying translation.
Definition: Pose2d.h:82
A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
Definition: Rotation2d.h:23
Represents a translation in 2D space.
Definition: Translation2d.h:28
constexpr Translation2d RotateAround(const Translation2d &other, const Rotation2d &rot) const
Rotates this translation around another translation in 2D space.
Definition: Translation2d.inc:33
constexpr units::meter_t X() const
Returns the X component of the translation.
Definition: Translation2d.h:79
This class is a wrapper around std::array that does compile time size checking.
Definition: array.h:26
constexpr UnitType abs(const UnitType x) noexcept
Compute absolute value.
Definition: math.h:726
constexpr auto sqrt(const UnitType &value) noexcept -> unit_t< square_root< typename units::traits::unit_t_traits< UnitType >::unit_type >, typename units::traits::unit_t_traits< UnitType >::underlying_type, linear_scale >
computes the square root of value
Definition: math.h:485
Definition: AprilTagDetector_cv.h:11
constexpr T abs(const T x) noexcept
Compile-time absolute value function.
Definition: abs.hpp:40
static constexpr const velocity::meters_per_second_t c(299792458.0)
Speed of light in vacuum.
constexpr UnitTypeLhs() min(const UnitTypeLhs &lhs, const UnitTypeRhs &rhs)
Definition: base.h:3413
constexpr UnitTypeLhs() max(const UnitTypeLhs &lhs, const UnitTypeRhs &rhs)
Definition: base.h:3421