[wpimath] Add RotateBy() function to pose classes (#5491)

Fixes #5472.
2026-06-20 00:51:42 +00:00 · 2023-07-31 21:16:44 -07:00
parent 26d6e68c8f
commit 35a8b129d9
10 changed files with 139 additions and 0 deletions
--- a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp
@@ -60,6 +60,10 @@ Pose3d Pose3d::operator/(double scalar) const {
  return *this * (1.0 / scalar);
 }

+Pose3d Pose3d::RotateBy(const Rotation3d& other) const {
+  return {m_translation.RotateBy(other), m_rotation.RotateBy(other)};
+}
+
 Pose3d Pose3d::TransformBy(const Transform3d& other) const {
  return {m_translation + (other.Translation().RotateBy(m_rotation)),
          other.Rotation() + m_rotation};