From 38bb23eb18f9acc0dbd8f7cbce459deaaaa36a18 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 28 Sep 2022 21:34:29 -0700 Subject: [PATCH] [wpimath] Add scalar multiply and divide operators to all geometry classes (#4438) Closes #4435. --- .../edu/wpi/first/math/geometry/Pose2d.java | 20 +++++++++++++++++++ .../edu/wpi/first/math/geometry/Pose3d.java | 20 +++++++++++++++++++ .../wpi/first/math/geometry/Rotation2d.java | 10 ++++++++++ .../wpi/first/math/geometry/Rotation3d.java | 10 ++++++++++ .../wpi/first/math/geometry/Transform2d.java | 12 ++++++++++- .../wpi/first/math/geometry/Transform3d.java | 12 ++++++++++- .../src/main/native/cpp/geometry/Pose2d.cpp | 8 ++++++++ .../src/main/native/cpp/geometry/Pose3d.cpp | 8 ++++++++ .../main/native/cpp/geometry/Rotation2d.cpp | 4 ++++ .../main/native/cpp/geometry/Rotation3d.cpp | 4 ++++ .../main/native/include/frc/geometry/Pose2d.h | 18 +++++++++++++++++ .../main/native/include/frc/geometry/Pose3d.h | 18 +++++++++++++++++ .../native/include/frc/geometry/Rotation2d.h | 10 ++++++++++ .../native/include/frc/geometry/Rotation3d.h | 10 ++++++++++ .../native/include/frc/geometry/Transform2d.h | 10 +++++++++- .../native/include/frc/geometry/Transform3d.h | 10 +++++++++- 16 files changed, 180 insertions(+), 4 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java index cb5d2272ac..227ba33ade 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java @@ -115,6 +115,26 @@ public class Pose2d implements Interpolatable { return m_rotation; } + /** + * Multiplies the current pose by a scalar. + * + * @param scalar The scalar. + * @return The new scaled Pose2d. + */ + public Pose2d times(double scalar) { + return new Pose2d(m_translation.times(scalar), m_rotation.times(scalar)); + } + + /** + * Divides the current pose by a scalar. + * + * @param scalar The scalar. + * @return The new scaled Pose2d. + */ + public Pose2d div(double scalar) { + return times(1.0 / scalar); + } + /** * Transforms the pose by the given transformation and returns the new pose. See + operator for * the matrix multiplication performed. diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java index f2f4eb8b7c..803a3469cd 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java @@ -114,6 +114,26 @@ public class Pose3d implements Interpolatable { return m_rotation; } + /** + * Multiplies the current pose by a scalar. + * + * @param scalar The scalar. + * @return The new scaled Pose3d. + */ + public Pose3d times(double scalar) { + return new Pose3d(m_translation.times(scalar), m_rotation.times(scalar)); + } + + /** + * Divides the current pose by a scalar. + * + * @param scalar The scalar. + * @return The new scaled Pose3d. + */ + public Pose3d div(double scalar) { + return times(1.0 / scalar); + } + /** * Transforms the pose by the given transformation and returns the new pose. See + operator for * the matrix multiplication performed. diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java index c9e76ced8b..b4bbe0042c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java @@ -136,6 +136,16 @@ public class Rotation2d implements Interpolatable { return new Rotation2d(m_value * scalar); } + /** + * Divides the current rotation by a scalar. + * + * @param scalar The scalar. + * @return The new scaled Rotation2d. + */ + public Rotation2d div(double scalar) { + return times(1.0 / scalar); + } + /** * Adds the new rotation to the current rotation using a rotation matrix. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java index 3bc002ac7d..91daf486d8 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java @@ -245,6 +245,16 @@ public class Rotation3d implements Interpolatable { } } + /** + * Divides the current rotation by a scalar. + * + * @param scalar The scalar. + * @return The new scaled Rotation3d. + */ + public Rotation3d div(double scalar) { + return times(1.0 / scalar); + } + /** * Adds the new rotation to the current rotation. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java index dd35670367..c3c6b0c741 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java @@ -47,7 +47,7 @@ public class Transform2d { } /** - * Scales the transform by the scalar. + * Multiplies the transform by the scalar. * * @param scalar The scalar. * @return The scaled Transform2d. @@ -56,6 +56,16 @@ public class Transform2d { return new Transform2d(m_translation.times(scalar), m_rotation.times(scalar)); } + /** + * Divides the transform by the scalar. + * + * @param scalar The scalar. + * @return The scaled Transform2d. + */ + public Transform2d div(double scalar) { + return times(1.0 / scalar); + } + /** * Composes two transformations. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java index a80fbb2171..4920ef6356 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java @@ -47,7 +47,7 @@ public class Transform3d { } /** - * Scales the transform by the scalar. + * Multiplies the transform by the scalar. * * @param scalar The scalar. * @return The scaled Transform3d. @@ -56,6 +56,16 @@ public class Transform3d { return new Transform3d(m_translation.times(scalar), m_rotation.times(scalar)); } + /** + * Divides the transform by the scalar. + * + * @param scalar The scalar. + * @return The scaled Transform3d. + */ + public Transform3d div(double scalar) { + return times(1.0 / scalar); + } + /** * Composes two transformations. * diff --git a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp index c856f17d1b..3a6f530510 100644 --- a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp @@ -33,6 +33,14 @@ bool Pose2d::operator!=(const Pose2d& other) const { return !operator==(other); } +Pose2d Pose2d::operator*(double scalar) const { + return Pose2d{m_translation * scalar, m_rotation * scalar}; +} + +Pose2d Pose2d::operator/(double scalar) const { + return *this * (1.0 / scalar); +} + Pose2d Pose2d::TransformBy(const Transform2d& other) const { return {m_translation + (other.Translation().RotateBy(m_rotation)), m_rotation + other.Rotation()}; diff --git a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp index 422b6a6194..f3bb96e42f 100644 --- a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp @@ -54,6 +54,14 @@ bool Pose3d::operator!=(const Pose3d& other) const { return !operator==(other); } +Pose3d Pose3d::operator*(double scalar) const { + return Pose3d{m_translation * scalar, m_rotation * scalar}; +} + +Pose3d Pose3d::operator/(double scalar) const { + return *this * (1.0 / scalar); +} + Pose3d Pose3d::TransformBy(const Transform3d& other) const { return {m_translation + (other.Translation().RotateBy(m_rotation)), m_rotation + other.Rotation()}; diff --git a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp index fcafa80b33..fa438f73f2 100644 --- a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp @@ -50,6 +50,10 @@ Rotation2d Rotation2d::operator*(double scalar) const { return Rotation2d{m_value * scalar}; } +Rotation2d Rotation2d::operator/(double scalar) const { + return *this * (1.0 / scalar); +} + bool Rotation2d::operator==(const Rotation2d& other) const { return std::hypot(m_cos - other.m_cos, m_sin - other.m_sin) < 1E-9; } diff --git a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp index 81bb8ab240..258c4c6ae1 100644 --- a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp @@ -165,6 +165,10 @@ Rotation3d Rotation3d::operator*(double scalar) const { } } +Rotation3d Rotation3d::operator/(double scalar) const { + return *this * (1.0 / scalar); +} + bool Rotation3d::operator==(const Rotation3d& other) const { return m_q == other.m_q; } diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h index 34cda3e8b8..8c92fcf95e 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.h @@ -112,6 +112,24 @@ class WPILIB_DLLEXPORT Pose2d { */ const Rotation2d& Rotation() const { return m_rotation; } + /** + * Multiplies the current pose by a scalar. + * + * @param scalar The scalar. + * + * @return The new scaled Pose2d. + */ + Pose2d operator*(double scalar) const; + + /** + * Divides the current pose by a scalar. + * + * @param scalar The scalar. + * + * @return The new scaled Pose2d. + */ + Pose2d operator/(double scalar) const; + /** * Transforms the pose by the given transformation and returns the new pose. * See + operator for the matrix multiplication performed. diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h index 23c75d38a8..8e9633f968 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose3d.h @@ -112,6 +112,24 @@ class WPILIB_DLLEXPORT Pose3d { */ const Rotation3d& Rotation() const { return m_rotation; } + /** + * Multiplies the current pose by a scalar. + * + * @param scalar The scalar. + * + * @return The new scaled Pose2d. + */ + Pose3d operator*(double scalar) const; + + /** + * Divides the current pose by a scalar. + * + * @param scalar The scalar. + * + * @return The new scaled Pose2d. + */ + Pose3d operator/(double scalar) const; + /** * Transforms the pose by the given transformation and returns the new pose. * See + operator for the matrix multiplication performed. diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h index c206d5f892..3b09fa003b 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h @@ -84,12 +84,22 @@ class WPILIB_DLLEXPORT Rotation2d { /** * Multiplies the current rotation by a scalar. + * * @param scalar The scalar. * * @return The new scaled Rotation2d. */ Rotation2d operator*(double scalar) const; + /** + * Divides the current rotation by a scalar. + * + * @param scalar The scalar. + * + * @return The new scaled Rotation2d. + */ + Rotation2d operator/(double scalar) const; + /** * Checks equality between this Rotation2d and another object. * diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h index ee5aeca8fc..fe74ba433d 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h @@ -99,12 +99,22 @@ class WPILIB_DLLEXPORT Rotation3d { /** * Multiplies the current rotation by a scalar. + * * @param scalar The scalar. * * @return The new scaled Rotation3d. */ Rotation3d operator*(double scalar) const; + /** + * Divides the current rotation by a scalar. + * + * @param scalar The scalar. + * + * @return The new scaled Rotation3d. + */ + Rotation3d operator/(double scalar) const; + /** * Checks equality between this Rotation3d and another object. * diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.h b/wpimath/src/main/native/include/frc/geometry/Transform2d.h index 3d5e1d6d4e..956db1490f 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.h @@ -74,7 +74,7 @@ class WPILIB_DLLEXPORT Transform2d { Transform2d Inverse() const; /** - * Scales the transform by the scalar. + * Multiplies the transform by the scalar. * * @param scalar The scalar. * @return The scaled Transform2d. @@ -83,6 +83,14 @@ class WPILIB_DLLEXPORT Transform2d { return Transform2d(m_translation * scalar, m_rotation * scalar); } + /** + * Divides the transform by the scalar. + * + * @param scalar The scalar. + * @return The scaled Transform2d. + */ + Transform2d operator/(double scalar) const { return *this * (1.0 / scalar); } + /** * Composes two transformations. * diff --git a/wpimath/src/main/native/include/frc/geometry/Transform3d.h b/wpimath/src/main/native/include/frc/geometry/Transform3d.h index cb89dee70c..88fa8fca2f 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform3d.h @@ -81,7 +81,7 @@ class WPILIB_DLLEXPORT Transform3d { Transform3d Inverse() const; /** - * Scales the transform by the scalar. + * Multiplies the transform by the scalar. * * @param scalar The scalar. * @return The scaled Transform3d. @@ -90,6 +90,14 @@ class WPILIB_DLLEXPORT Transform3d { return Transform3d(m_translation * scalar, m_rotation * scalar); } + /** + * Divides the transform by the scalar. + * + * @param scalar The scalar. + * @return The scaled Transform3d. + */ + Transform3d operator/(double scalar) const { return *this * (1.0 / scalar); } + /** * Composes two transformations. *