[wpimath] Add geometry classes for Rectangle2d and Ellipse2d

[Braykoff]

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.

[calcmogul]

intersectsPoint() and containsPoint() can just be intersects() and contains(). Translation2d is clearly a point, but the shorter name also generalizes to other geometric types. Besides that, the API looks good.

[calcmogul]

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;

  sleipnir::OptimizationProblem problem;
  auto x = problem.DecisionVariable();
  auto y = problem.DecisionVariable();

  // TODO: Rotate test point around ellipse first

  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.
https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/native/cpp/jni/WPIMathJNI_ArmFeedforward.cpp
https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java#L64

[KangarooKoala]

Overall, great work! I just had a a few small comments about potential rounding error and also a question.

[spacey-sooty]

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.

[calcmogul]

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.

[Braykoff]

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, new Rotation2d() constructs a rotation of 0 degrees). Would it make sense to have a constructor like this for this 2d geometry, as "zero" doesn't make sense in this context, as you pointed out?

For example, a new Rectangle2d() constructs a rectangle at the origin of size 1x1?

[Technologyman00]

I think 1x1 sounds good. Probably any number that is less than the size of the field is reasonable

[calcmogul]

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.

[calcmogul]

We can either do this for Ellipse2d as well using the stuff from #6555 (comment) or open an issue after this PR is merged.