1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <stdexcept>
#include <gcem.hpp>
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Transform2d.h"
#include "frc/geometry/Translation2d.h"
#include "units/length.h"
#include "units/math.h"
namespace frc {
/**
* Represents a 2d ellipse space containing translational, rotational, and
* scaling components.
*/
class WPILIB_DLLEXPORT Ellipse2d {
public:
/**
* Constructs an ellipse around a center point and two semi-axes, a horizontal
* and vertical one.
*
* @param center The center of the ellipse.
* @param xSemiAxis The x semi-axis.
* @param ySemiAxis The y semi-axis.
*/
constexpr Ellipse2d(const Pose2d& center, units::meter_t xSemiAxis,
units::meter_t ySemiAxis)
: m_center{center}, m_xSemiAxis{xSemiAxis}, m_ySemiAxis{ySemiAxis} {
if (xSemiAxis <= 0_m || ySemiAxis <= 0_m) {
throw std::invalid_argument("Ellipse2d semi-axes must be positive");
}
}
/**
* Constructs a perfectly circular ellipse with the specified radius.
*
* @param center The center of the circle.
* @param radius The radius of the circle.
*/
constexpr Ellipse2d(const Translation2d& center, double radius)
: m_center{center, Rotation2d{}},
m_xSemiAxis{radius},
m_ySemiAxis{radius} {}
/**
* Returns the center of the ellipse.
*
* @return The center of the ellipse.
*/
constexpr const Pose2d& Center() const { return m_center; }
/**
* Returns the rotational component of the ellipse.
*
* @return The rotational component of the ellipse.
*/
constexpr const Rotation2d& Rotation() const { return m_center.Rotation(); }
/**
* Returns the x semi-axis.
*
* @return The x semi-axis.
*/
constexpr units::meter_t XSemiAxis() const { return m_xSemiAxis; }
/**
* Returns the y semi-axis.
*
* @return The y semi-axis.
*/
constexpr units::meter_t YSemiAxis() const { return m_ySemiAxis; }
/**
* Returns the focal points of the ellipse. In a perfect circle, this will
* always return the center.
*
* @return The focal points.
*/
constexpr wpi::array<Translation2d, 2> FocalPoints() const {
// Major semi-axis
auto a = units::math::max(m_xSemiAxis, m_ySemiAxis);
// Minor semi-axis
auto b = units::math::min(m_xSemiAxis, m_ySemiAxis);
auto c = units::math::sqrt(a * a - b * b);
if (m_xSemiAxis > m_ySemiAxis) {
return wpi::array{
(m_center + Transform2d{-c, 0_m, Rotation2d{}}).Translation(),
(m_center + Transform2d{c, 0_m, Rotation2d{}}).Translation()};
} else {
return wpi::array{
(m_center + Transform2d{0_m, -c, Rotation2d{}}).Translation(),
(m_center + Transform2d{0_m, c, Rotation2d{}}).Translation()};
}
}
/**
* Transforms the center of the ellipse and returns the new ellipse.
*
* @param other The transform to transform by.
* @return The transformed ellipse.
*/
constexpr Ellipse2d TransformBy(const Transform2d& other) const {
return Ellipse2d{m_center.TransformBy(other), m_xSemiAxis, m_ySemiAxis};
}
/**
* Rotates the center of the ellipse and returns the new ellipse.
*
* @param other The rotation to transform by.
* @return The rotated ellipse.
*/
constexpr Ellipse2d RotateBy(const Rotation2d& other) const {
return Ellipse2d{m_center.RotateBy(other), m_xSemiAxis, m_ySemiAxis};
}
/**
* Checks if a point is intersected by this ellipse's circumference.
*
* @param point The point to check.
* @return True, if this ellipse's circumference intersects the point.
*/
constexpr bool Intersects(const Translation2d& point) const {
return gcem::abs(1.0 - SolveEllipseEquation(point)) <= 1E-9;
}
/**
* Checks if a point is contained within this ellipse. This is inclusive, if
* the point lies on the circumference this will return {@code true}.
*
* @param point The point to check.
* @return True, if the point is within or on the ellipse.
*/
constexpr bool Contains(const Translation2d& point) const {
return SolveEllipseEquation(point) <= 1.0;
}
/**
* Returns the distance between the perimeter of the ellipse and the point.
*
* @param point The point to check.
* @return The distance (0, if the point is contained by the ellipse)
*/
units::meter_t Distance(const Translation2d& point) const {
return Nearest(point).Distance(point);
}
/**
* Returns the nearest point that is contained within the ellipse.
*
* @param point The point that this will find the nearest point to.
* @return A new point that is nearest to {@code point} and contained in the
* ellipse.
*/
Translation2d Nearest(const Translation2d& point) const;
/**
* Checks equality between this Ellipse2d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
constexpr bool operator==(const Ellipse2d& other) const {
return m_center == other.m_center &&
units::math::abs(m_xSemiAxis - other.m_xSemiAxis) < 1E-9_m &&
units::math::abs(m_ySemiAxis - other.m_ySemiAxis) < 1E-9_m;
}
private:
Pose2d m_center;
units::meter_t m_xSemiAxis;
units::meter_t m_ySemiAxis;
/**
* Solves the equation of an ellipse from the given point. This is a helper
* function used to determine if that point lies inside of or on an ellipse.
*
* <pre>
* (x - h)²/a² + (y - k)²/b² = 1
* </pre>
*
* @param point The point to solve for.
* @return < 1.0 if the point lies inside the ellipse, == 1.0 if a point lies
* on the ellipse, and > 1.0 if the point lies outsides the ellipse.
*/
constexpr double SolveEllipseEquation(const Translation2d& point) const {
// Rotate the point by the inverse of the ellipse's rotation
auto rotPoint =
point.RotateAround(m_center.Translation(), -m_center.Rotation());
auto x = rotPoint.X() - m_center.X();
auto y = rotPoint.Y() - m_center.Y();
// NOLINTNEXTLINE (bugprone-integer-division)
return (x * x) / (m_xSemiAxis * m_xSemiAxis) +
// NOLINTNEXTLINE (bugprone-integer-division)
(y * y) / (m_ySemiAxis * m_ySemiAxis);
}
};
} // namespace frc
#include "frc/geometry/proto/Ellipse2dProto.h"
#include "frc/geometry/struct/Ellipse2dStruct.h"