[wpimath] Make geometry classes constexpr

[calcmogul]

No description provided.

[KangarooKoala]

Shouldn't the Rotation3d(const Eigen::Vector3d&, const Eigen::Vector3d&) constructor be marked constexpr?

[calcmogul]

That constructor can't be constexpr because it uses Eigen's QR decomposition, which isn't constexpr. We use that to compute an orthogonal vector when the two input vectors are antiparallel. I'd prefer a cheaper method.

Now that I think about it, we could probably pick any other non-antiparallel vector and do the cross product with that instead.

[calcmogul]

I'll see if I can make Pose3d::Exp() and Pose3d::Log() constexpr as well.

[calcmogul]

I'm not happy with the amount of code duplication in Rotation3d and Pose3d for the constexpr and non-constexpr paths. Idk what we can do about it tho other than patching Eigen to make everything constexpr. I have an Eigen branch for that, but it's annoying to keep up to date because it touches a lot of files (336 out of 746 files, 10k LOC of changes).

[KangarooKoala]

I just realized that ct_matrix will use Eigen if the call is not constant evaluated- Should we just write everything to always use ct_matrix? The downside is overhead from copying the Eigen matrices in the ct_matrix constructor.

[calcmogul]

I want to avoid runtime overhead whenever possible.