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 fb141b9ed3..0d44c095e9 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 @@ -63,7 +63,10 @@ public class Pose3d implements Interpolatable { } /** - * Transforms the pose by the given transformation and returns the new transformed pose. + * Transforms the pose by the given transformation and returns the new transformed pose. The + * transform is applied relative to the pose's frame. Note that this differs from {@link + * Pose3d#rotateBy(Rotation3d)}, which is applied relative to the global frame and around the + * origin. * * @param other The transform to transform the pose by. * @return The transformed pose. @@ -153,15 +156,19 @@ public class Pose3d implements Interpolatable { /** * Rotates the pose around the origin and returns the new pose. * - * @param other The rotation to transform the pose by. - * @return The transformed pose. + * @param other The rotation to transform the pose by, which is applied extrinsically (from the + * global frame). + * @return The rotated pose. */ public Pose3d rotateBy(Rotation3d other) { return new Pose3d(m_translation.rotateBy(other), m_rotation.rotateBy(other)); } /** - * Transforms the pose by the given transformation and returns the new pose. + * Transforms the pose by the given transformation and returns the new transformed pose. The + * transform is applied relative to the pose's frame. Note that this differs from {@link + * Pose3d#rotateBy(Rotation3d)}, which is applied relative to the global frame and around the + * origin. * * @param other The transform to transform the pose by. * @return The transformed pose. 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 25582f8fd1..952cd5c38c 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 @@ -278,9 +278,13 @@ public class Rotation3d implements Interpolatable { } /** - * Adds the new rotation to the current rotation. + * Adds the new rotation to the current rotation. The other rotation is applied intrinsically, + * which means that it rotates around the axes after applying this rotation. For example, {@code + * new Rotation3d(Units.degreesToRadians(90), 0, 0).rotateBy(new Rotation3d(0, + * Units.degreesToRadians(90), 0))} rotates by 90 degrees around the +X axis and then by 90 + * degrees around the new +Y axis (which has been moved to the +Z axis). * - * @param other The rotation to rotate by. + * @param other The intrinsic rotation to rotate by. * @return The new rotated Rotation3d. */ public Rotation3d rotateBy(Rotation3d other) { 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 c3c6b0c741..c7a5c3e689 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 @@ -6,7 +6,7 @@ package edu.wpi.first.math.geometry; import java.util.Objects; -/** Represents a transformation for a Pose2d. */ +/** Represents a transformation for a Pose2d in the pose's frame. */ public class Transform2d { private final Translation2d m_translation; private final Rotation2d m_rotation; 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 4920ef6356..0fbfb79e68 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 @@ -6,7 +6,7 @@ package edu.wpi.first.math.geometry; import java.util.Objects; -/** Represents a transformation for a Pose3d. */ +/** Represents a transformation for a Pose3d in the pose's frame. */ public class Transform3d { private final Translation3d m_translation; private final Rotation3d m_rotation; @@ -67,7 +67,8 @@ public class Transform3d { } /** - * Composes two transformations. + * Composes two transformations. The second transform is applied relative to the orientation of + * the first. * * @param other The transform to compose with this one. * @return The composition of the two transformations. diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h index cfe484913b..78fd3e886a 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose3d.h @@ -56,7 +56,9 @@ class WPILIB_DLLEXPORT Pose3d { /** * Transforms the pose by the given transformation and returns the new - * transformed pose. + * transformed pose. The transform is applied relative to the pose's frame. + * Note that this differs from Pose3d::RotateBy(const Rotation3d&), which is + * applied relative to the global frame and around the origin. * * @param other The transform to transform the pose by. * @@ -133,14 +135,18 @@ class WPILIB_DLLEXPORT Pose3d { /** * Rotates the pose around the origin and returns the new pose. * - * @param other The rotation to transform the pose by. + * @param other The rotation to transform the pose by, which is applied + * extrinsically (from the global frame). * * @return The rotated pose. */ Pose3d RotateBy(const Rotation3d& other) const; /** - * Transforms the pose by the given transformation and returns the new pose. + * Transforms the pose by the given transformation and returns the new + * transformed pose. The transform is applied relative to the pose's frame. + * Note that this differs from Pose3d::RotateBy(const Rotation3d&), which is + * applied relative to the global frame and around the origin. * * @param other The transform to transform the pose by. * diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h index ca053b2c7d..31f67e0cf8 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h @@ -138,7 +138,11 @@ class WPILIB_DLLEXPORT Rotation3d { bool operator==(const Rotation3d&) const = default; /** - * Adds the new rotation to the current rotation. + * Adds the new rotation to the current rotation. The other rotation is + * applied intrinsically, which means that it rotates around the axes after + * applying this rotation. For example, Rotation3d{90_deg, 0, 0}.RotateBy( + * Rotation3d{0, 90_deg, 0}) rotates by 90 degrees around the +X axis and then + * by 90 degrees around the new +Y axis (which has been moved to the +Z axis). * * @param other The rotation to rotate by. * diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.h b/wpimath/src/main/native/include/frc/geometry/Transform2d.h index 5395f33d68..8f6eba2879 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.h @@ -13,7 +13,7 @@ namespace frc { class WPILIB_DLLEXPORT Pose2d; /** - * Represents a transformation for a Pose2d. + * Represents a transformation for a Pose2d in the pose's frame. */ class WPILIB_DLLEXPORT Transform2d { public: diff --git a/wpimath/src/main/native/include/frc/geometry/Transform3d.h b/wpimath/src/main/native/include/frc/geometry/Transform3d.h index 91c78cfc43..cf00ecc002 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform3d.h @@ -13,7 +13,7 @@ namespace frc { class WPILIB_DLLEXPORT Pose3d; /** - * Represents a transformation for a Pose3d. + * Represents a transformation for a Pose3d in the pose's frame. */ class WPILIB_DLLEXPORT Transform3d { public: