-
Notifications
You must be signed in to change notification settings - Fork 613
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
[wpimath] Add geometry classes for Rectangle2d and Ellipse2d #6555
Conversation
|
With regard to finding the minimum distance to an ellipse, we could use the numerical optimizer we recently merged to make it easier. There's examples at https://github.com/SleipnirGroup/Sleipnir/. The problem can be formulated as "find the (x, y) pair that minimizes the sum squared distance to a test point subject to the ellipse equation". (Square roots have a limited domain and are nonlinear, so it's best to avoid them for distance calculations in favor of something quadratic.) #include <sleipnir/optimization/OptimizationProblem.hpp>
void func() {
using sleipnir::pow;
// TODO: Rotate test point around ellipse first
sleipnir::OptimizationProblem problem;
auto x = problem.DecisionVariable();
x.SetValue(testX);
auto y = problem.DecisionVariable();
y.SetValue(testY);
problem.Minimize(pow(x - testX, 2) + pow(y - testY, 2));
// (x − x_c)²/a² + (y − y_c)²/b² = 1
problem.SubjectTo(pow(x - x_c, 2) / (a * a) + pow(y - y_c, 2) / (b * b) == 1);
problem.Solve();
// TODO: Unrotate x.Value() and y.Value()
} The problem is convex with a quadratic cost function and a quadratic equality constraint. We'd have to write a wpimath JNI function to use the solver from Java, like we did for the new ArmFeedforward calculate() function. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Overall, great work! I just had a a few small comments about potential rounding error and also a question.
wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java
Outdated
Show resolved
Hide resolved
wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java
Outdated
Show resolved
Hide resolved
wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java
Outdated
Show resolved
Hide resolved
wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java
Outdated
Show resolved
Hide resolved
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Should this have some static members to account for #6563 ?
I think 0 could definitely be useful but I can't really think of any others.
What would "zero" mean in this context? The default position being the origin makes sense, but I can't think of a sensible default for the dimensions. Making them zero turns the shape into a point, which isn't useful. |
@calcmogul It looks like all the other geometry classes have some sort of default nullary constructors (eg, For example, a |
I think 1x1 sounds good. Probably any number that is less than the size of the field is reasonable |
I don't think 1x1 is a sane default that's usable for anything. The user is almost certainly going to need a different size they'll have to specify in the non-default constructor anyway, especially because the geometry types are immutable (for good reason). We should just force the user to specify a size. |
wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java
Outdated
Show resolved
Hide resolved
wpimath/src/main/java/edu/wpi/first/math/geometry/Rectangle2d.java
Outdated
Show resolved
Hide resolved
I translated the ellipse and rectangle classes to C++. Here's what's left:
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Rectangle2d.IntersectsPoint and Rectangle2d.FindNearestPoint are also failing, by the way.
Resolves #6446, resolves #4090
Adds Rectangle2d and Ellipse2d classes, for checking if points intersect certain zones. The Java implementation includes tests, structs, protobufs, and javadocs.
This also adds a
rotateAround(Translation2d other, Rotation2d rot)
method for rotating a Translation2d around another point (not the origin) by a certain amount. This is used in checking for intersection/containment of translation2ds in a rotated 2d area (ex, if checking if a point is contained in a rotated rectangle, the point is rotated around the center of the rectangle by the inverse of the rectangle's rotation, to move it into the rectangle's coordinate frame).This is marked as a draft PR because there is no c++ implementation yet. If someone has free time, feel free to contribute, otherwise I will get to it when I have time. There is also no
distanceToPoint()
method for Ellipse2d, because the math for calculating the distance to an ellipse is more complicated if that ellipse is not a perfect circle. I can leave it out, or I can change the implementation to only support perfect circles in Ellipse2d.