diff --git a/wpimath/src/main/java/org/wpilib/math/geometry/CoordinateSystem.java b/wpimath/src/main/java/org/wpilib/math/geometry/CoordinateSystem.java index af65808c5c..79a4dacb33 100644 --- a/wpimath/src/main/java/org/wpilib/math/geometry/CoordinateSystem.java +++ b/wpimath/src/main/java/org/wpilib/math/geometry/CoordinateSystem.java @@ -86,7 +86,7 @@ public class CoordinateSystem { public static Translation3d convert( Translation3d translation, CoordinateSystem from, CoordinateSystem to) { // Convert to NWU, then convert to the new coordinate system - return translation.rotateBy(from.m_rotation).rotateBy(to.m_rotation.unaryMinus()); + return translation.rotateBy(from.m_rotation).rotateBy(to.m_rotation.inverse()); } /** @@ -100,7 +100,7 @@ public class CoordinateSystem { public static Rotation3d convert( Rotation3d rotation, CoordinateSystem from, CoordinateSystem to) { // Convert to NWU, then convert to the new coordinate system - return rotation.rotateBy(from.m_rotation).rotateBy(to.m_rotation.unaryMinus()); + return rotation.rotateBy(from.m_rotation).rotateBy(to.m_rotation.inverse()); } /** @@ -128,7 +128,7 @@ public class CoordinateSystem { Transform3d transform, CoordinateSystem from, CoordinateSystem to) { // coordRot is the rotation that converts between the coordinate systems when applied // extrinsically. It first converts to NWU, then converts to the new coordinate system. - var coordRot = from.m_rotation.rotateBy(to.m_rotation.unaryMinus()); + var coordRot = from.m_rotation.rotateBy(to.m_rotation.inverse()); // The new rotation is the extrinsic rotation from convert(zero) to // convert(transformRot). That is, applying convertedRot extrinsically to // convert(zero) produces convert(transformRot). In the below snippet, we @@ -140,9 +140,9 @@ public class CoordinateSystem { // = (coordRot transformRot) coordRot⁻¹ // // In code, the equivalent for rotA rotB is rotB.rotateBy(rotA) (note the - // change in order), and the equivalent for rot⁻¹ is rot.unaryMinus(). + // change in order). return new Transform3d( convert(transform.getTranslation(), from, to), - coordRot.unaryMinus().rotateBy(transform.getRotation().rotateBy(coordRot))); + coordRot.inverse().rotateBy(transform.getRotation().rotateBy(coordRot))); } } diff --git a/wpimath/src/main/java/org/wpilib/math/geometry/Pose3d.java b/wpimath/src/main/java/org/wpilib/math/geometry/Pose3d.java index 1db6cdd75a..f27a6367ac 100644 --- a/wpimath/src/main/java/org/wpilib/math/geometry/Pose3d.java +++ b/wpimath/src/main/java/org/wpilib/math/geometry/Pose3d.java @@ -338,7 +338,7 @@ public class Pose3d implements Interpolatable, ProtobufSerializable, Str Comparator.comparing( (Pose3d other) -> this.getTranslation().getDistance(other.getTranslation())) .thenComparing( - (Pose3d other) -> this.getRotation().minus(other.getRotation()).getAngle())); + (Pose3d other) -> this.getRotation().relativeTo(other.getRotation()).getAngle())); } @Override diff --git a/wpimath/src/main/java/org/wpilib/math/geometry/Quaternion.java b/wpimath/src/main/java/org/wpilib/math/geometry/Quaternion.java index fa8bf61124..1334b8fcb4 100644 --- a/wpimath/src/main/java/org/wpilib/math/geometry/Quaternion.java +++ b/wpimath/src/main/java/org/wpilib/math/geometry/Quaternion.java @@ -216,16 +216,6 @@ public class Quaternion implements ProtobufSerializable, StructSerializable { return this.log().times(t).exp(); } - /** - * Matrix exponential of a quaternion. - * - * @param adjustment the "Twist" that will be applied to this quaternion. - * @return The quaternion product of exp(adjustment) * this - */ - public Quaternion exp(Quaternion adjustment) { - return adjustment.exp().times(this); - } - /** * Matrix exponential of a quaternion. * @@ -260,16 +250,6 @@ public class Quaternion implements ProtobufSerializable, StructSerializable { getZ() * axial_scalar * scalar); } - /** - * Log operator of a quaternion. - * - * @param end The quaternion to map this quaternion onto. - * @return The "Twist" that maps this quaternion to the argument. - */ - public Quaternion log(Quaternion end) { - return end.times(this.inverse()).log(); - } - /** * The Log operator of a general quaternion. * diff --git a/wpimath/src/main/java/org/wpilib/math/geometry/Rotation2d.java b/wpimath/src/main/java/org/wpilib/math/geometry/Rotation2d.java index 0525bb8a96..6a3bdd1c90 100644 --- a/wpimath/src/main/java/org/wpilib/math/geometry/Rotation2d.java +++ b/wpimath/src/main/java/org/wpilib/math/geometry/Rotation2d.java @@ -203,7 +203,7 @@ public class Rotation2d } /** - * Subtracts the new rotation from the current rotation and returns the new rotation. + * Returns this rotation relative to another rotation. * *

For example, Rotation2d.fromDegrees(10).minus(Rotation2d.fromDegrees(100)) * equals Rotation2d(-Math.PI/2.0) diff --git a/wpimath/src/main/java/org/wpilib/math/geometry/Rotation3d.java b/wpimath/src/main/java/org/wpilib/math/geometry/Rotation3d.java index ce24c36efa..e0e9a43fb9 100644 --- a/wpimath/src/main/java/org/wpilib/math/geometry/Rotation3d.java +++ b/wpimath/src/main/java/org/wpilib/math/geometry/Rotation3d.java @@ -326,49 +326,12 @@ public class Rotation3d this(0.0, 0.0, rotation.getRadians()); } - /** - * Adds two rotations together. The other rotation is applied extrinsically to this rotation, - * which is equivalent to this rotation being applied intrinsically to the other rotation. See the - * class comment for definitions of extrinsic and intrinsic rotations. - * - *

Note that {@code a.minus(b).plus(b)} always equals {@code a}, but {@code b.plus(a.minus(b))} - * sometimes doesn't. To apply a rotation offset, use either {@code offset = - * measurement.unaryMinus().plus(actual); newAngle = angle.plus(offset);} or {@code offset = - * actual.minus(measurement); newAngle = offset.plus(angle);}, depending on how the corrected - * angle needs to change as the input angle changes. - * - * @param other The rotation to add (applied extrinsically). - * @return The sum of the two rotations. - */ - public Rotation3d plus(Rotation3d other) { - return rotateBy(other); - } - - /** - * Subtracts the other rotation from the current rotation and returns the new rotation. The new - * rotation is from the perspective of the other rotation (like {@link Pose3d#minus}), so it needs - * to be applied intrinsically. See the class comment for definitions of extrinsic and intrinsic - * rotations. - * - *

Note that {@code a.minus(b).plus(b)} always equals {@code a}, but {@code b.plus(a.minus(b))} - * sometimes doesn't. To apply a rotation offset, use either {@code offset = - * measurement.unaryMinus().plus(actual); newAngle = angle.plus(offset);} or {@code offset = - * actual.minus(measurement); newAngle = offset.plus(angle);}, depending on how the corrected - * angle needs to change as the input angle changes. - * - * @param other The rotation to subtract. - * @return The difference between the two rotations, from the perspective of the other rotation. - */ - public Rotation3d minus(Rotation3d other) { - return rotateBy(other.unaryMinus()); - } - /** * Takes the inverse of the current rotation. * * @return The inverse of the current rotation. */ - public Rotation3d unaryMinus() { + public Rotation3d inverse() { return new Rotation3d(m_q.inverse()); } @@ -379,16 +342,7 @@ public class Rotation3d * @return The new scaled Rotation3d. */ public Rotation3d times(double scalar) { - // https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp - if (m_q.getW() >= 0.0) { - return new Rotation3d( - VecBuilder.fill(m_q.getX(), m_q.getY(), m_q.getZ()), - 2.0 * scalar * Math.acos(m_q.getW())); - } else { - return new Rotation3d( - VecBuilder.fill(-m_q.getX(), -m_q.getY(), -m_q.getZ()), - 2.0 * scalar * Math.acos(-m_q.getW())); - } + return Rotation3d.kZero.interpolate(this, scalar); } /** @@ -638,7 +592,18 @@ public class Rotation3d @Override public Rotation3d interpolate(Rotation3d endValue, double t) { - return endValue.minus(this).times(Math.clamp(t, 0, 1)).plus(this); + // https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp + // + // slerp(q₀, q₁, t) = (q₁q₀⁻¹)ᵗq₀ + // + // We negate the delta quaternion if necessary to take the shortest path + var q0 = m_q; + var q1 = endValue.m_q; + var delta = q1.times(q0.inverse()); + if (delta.getW() < 0.0) { + delta = new Quaternion(-delta.getW(), -delta.getX(), -delta.getY(), -delta.getZ()); + } + return new Rotation3d(delta.pow(t).times(q0)); } /** Rotation3d protobuf for serialization. */ diff --git a/wpimath/src/main/java/org/wpilib/math/geometry/Transform3d.java b/wpimath/src/main/java/org/wpilib/math/geometry/Transform3d.java index e65e2dd2f2..b1c7c55fab 100644 --- a/wpimath/src/main/java/org/wpilib/math/geometry/Transform3d.java +++ b/wpimath/src/main/java/org/wpilib/math/geometry/Transform3d.java @@ -45,7 +45,7 @@ public class Transform3d implements ProtobufSerializable, StructSerializable { m_translation = last.getTranslation() .minus(initial.getTranslation()) - .rotateBy(initial.getRotation().unaryMinus()); + .rotateBy(initial.getRotation().inverse()); m_rotation = last.getRotation().relativeTo(initial.getRotation()); } @@ -290,8 +290,7 @@ public class Transform3d implements ProtobufSerializable, StructSerializable { // using a clockwise rotation matrix. This transforms the global // delta into a local delta (relative to the initial pose). return new Transform3d( - getTranslation().unaryMinus().rotateBy(getRotation().unaryMinus()), - getRotation().unaryMinus()); + getTranslation().unaryMinus().rotateBy(getRotation().inverse()), getRotation().inverse()); } @Override diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/Odometry3d.java b/wpimath/src/main/java/org/wpilib/math/kinematics/Odometry3d.java index c2554dc9cc..1b686a2e6b 100644 --- a/wpimath/src/main/java/org/wpilib/math/kinematics/Odometry3d.java +++ b/wpimath/src/main/java/org/wpilib/math/kinematics/Odometry3d.java @@ -57,7 +57,7 @@ public class Odometry3d { m_pose = initialPose; // When applied extrinsically, m_gyroOffset cancels the current gyroAngle and // then rotates to m_poseMeters.getRotation() - m_gyroOffset = gyroAngle.unaryMinus().rotateBy(m_pose.getRotation()); + m_gyroOffset = gyroAngle.inverse().rotateBy(m_pose.getRotation()); m_previousAngle = m_pose.getRotation(); m_previousWheelPositions = m_kinematics.copy(wheelPositions); } @@ -76,7 +76,7 @@ public class Odometry3d { m_pose = pose; // When applied extrinsically, m_gyroOffset cancels the current gyroAngle and // then rotates to m_poseMeters.getRotation() - m_gyroOffset = gyroAngle.unaryMinus().rotateBy(m_pose.getRotation()); + m_gyroOffset = gyroAngle.inverse().rotateBy(m_pose.getRotation()); m_previousAngle = m_pose.getRotation(); m_kinematics.copyInto(wheelPositions, m_previousWheelPositions); } @@ -89,7 +89,7 @@ public class Odometry3d { public void resetPose(Pose3d pose) { // Cancel the previous m_pose.Rotation() and then rotate to the new angle m_gyroOffset = - m_gyroOffset.rotateBy(m_pose.getRotation().unaryMinus()).rotateBy(pose.getRotation()); + m_gyroOffset.rotateBy(m_pose.getRotation().inverse()).rotateBy(pose.getRotation()); m_pose = pose; m_previousAngle = m_pose.getRotation(); } @@ -110,7 +110,7 @@ public class Odometry3d { */ public void resetRotation(Rotation3d rotation) { // Cancel the previous m_pose.Rotation() and then rotate to the new angle - m_gyroOffset = m_gyroOffset.rotateBy(m_pose.getRotation().unaryMinus()).rotateBy(rotation); + m_gyroOffset = m_gyroOffset.rotateBy(m_pose.getRotation().inverse()).rotateBy(rotation); m_pose = new Pose3d(m_pose.getTranslation(), rotation); m_previousAngle = m_pose.getRotation(); } @@ -136,7 +136,7 @@ public class Odometry3d { */ public Pose3d update(Rotation3d gyroAngle, T wheelPositions) { var angle = gyroAngle.rotateBy(m_gyroOffset); - var angle_difference = angle.minus(m_previousAngle).toVector(); + var angle_difference = angle.relativeTo(m_previousAngle).toVector(); var twist2d = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions); var twist = diff --git a/wpimath/src/main/native/include/wpi/math/geometry/CoordinateSystem.hpp b/wpimath/src/main/native/include/wpi/math/geometry/CoordinateSystem.hpp index 353a76faa3..ded8b55273 100644 --- a/wpimath/src/main/native/include/wpi/math/geometry/CoordinateSystem.hpp +++ b/wpimath/src/main/native/include/wpi/math/geometry/CoordinateSystem.hpp @@ -86,7 +86,8 @@ class WPILIB_DLLEXPORT CoordinateSystem { const CoordinateSystem& from, const CoordinateSystem& to) { // Convert to NWU, then convert to the new coordinate system - return translation.RotateBy(from.m_rotation).RotateBy(-to.m_rotation); + return translation.RotateBy(from.m_rotation) + .RotateBy(to.m_rotation.Inverse()); } /** @@ -101,7 +102,7 @@ class WPILIB_DLLEXPORT CoordinateSystem { const CoordinateSystem& from, const CoordinateSystem& to) { // Convert to NWU, then convert to the new coordinate system - return rotation.RotateBy(from.m_rotation).RotateBy(-to.m_rotation); + return rotation.RotateBy(from.m_rotation).RotateBy(to.m_rotation.Inverse()); } /** @@ -133,7 +134,7 @@ class WPILIB_DLLEXPORT CoordinateSystem { // coordRot is the rotation that converts between the coordinate systems // when applied extrinsically. It first converts to NWU, then converts to // the new coordinate system. - const auto coordRot = from.m_rotation.RotateBy(-to.m_rotation); + const auto coordRot = from.m_rotation.RotateBy(to.m_rotation.Inverse()); // The new rotation is the extrinsic rotation from convert(zero) to // convert(transformRot). That is, applying convertedRot extrinsically to // convert(zero) produces convert(transformRot). In the below snippet, we @@ -145,10 +146,10 @@ class WPILIB_DLLEXPORT CoordinateSystem { // = (coordRot transformRot) coordRot⁻¹ // // In code, the equivalent for rotA rotB is rotB.RotateBy(rotA) (note the - // change in order), and the equivalent for rot⁻¹ is -rot. + // change in order). return Transform3d{ Convert(transform.Translation(), from, to), - (-coordRot).RotateBy(transform.Rotation().RotateBy(coordRot))}; + coordRot.Inverse().RotateBy(transform.Rotation().RotateBy(coordRot))}; } private: diff --git a/wpimath/src/main/native/include/wpi/math/geometry/Pose3d.hpp b/wpimath/src/main/native/include/wpi/math/geometry/Pose3d.hpp index 9963ef4150..7f288e5ce0 100644 --- a/wpimath/src/main/native/include/wpi/math/geometry/Pose3d.hpp +++ b/wpimath/src/main/native/include/wpi/math/geometry/Pose3d.hpp @@ -256,9 +256,14 @@ class WPILIB_DLLEXPORT Pose3d { // If the distances are equal sort by difference in rotation if (aDistance == bDistance) { - return gcem::abs( - (this->Rotation() - a.Rotation()).Angle().value()) < - gcem::abs((this->Rotation() - b.Rotation()).Angle().value()); + return gcem::abs(this->Rotation() + .RelativeTo(a.Rotation()) + .Angle() + .value()) < + gcem::abs(this->Rotation() + .RelativeTo(b.Rotation()) + .Angle() + .value()); } return aDistance < bDistance; }); @@ -281,9 +286,14 @@ class WPILIB_DLLEXPORT Pose3d { // If the distances are equal sort by difference in rotation if (aDistance == bDistance) { - return gcem::abs( - (this->Rotation() - a.Rotation()).Angle().value()) < - gcem::abs((this->Rotation() - b.Rotation()).Angle().value()); + return gcem::abs(this->Rotation() + .RelativeTo(a.Rotation()) + .Angle() + .value()) < + gcem::abs(this->Rotation() + .RelativeTo(b.Rotation()) + .Angle() + .value()); } return aDistance < bDistance; }); diff --git a/wpimath/src/main/native/include/wpi/math/geometry/Quaternion.hpp b/wpimath/src/main/native/include/wpi/math/geometry/Quaternion.hpp index b25e5f32a9..14dcbae211 100644 --- a/wpimath/src/main/native/include/wpi/math/geometry/Quaternion.hpp +++ b/wpimath/src/main/native/include/wpi/math/geometry/Quaternion.hpp @@ -157,15 +157,6 @@ class WPILIB_DLLEXPORT Quaternion { */ constexpr Quaternion Pow(double t) const { return (Log() * t).Exp(); } - /** - * Matrix exponential of a quaternion. - * - * @param other the "Twist" that will be applied to this quaternion. - */ - constexpr Quaternion Exp(const Quaternion& other) const { - return other.Exp() * *this; - } - /** * Matrix exponential of a quaternion. * @@ -196,15 +187,6 @@ class WPILIB_DLLEXPORT Quaternion { Y() * axial_scalar * scalar, Z() * axial_scalar * scalar); } - /** - * Log operator of a quaternion. - * - * @param other The quaternion to map this quaternion onto - */ - constexpr Quaternion Log(const Quaternion& other) const { - return (other * Inverse()).Log(); - } - /** * Log operator of a quaternion. * diff --git a/wpimath/src/main/native/include/wpi/math/geometry/Rotation2d.hpp b/wpimath/src/main/native/include/wpi/math/geometry/Rotation2d.hpp index 8f77a2e0e3..3f64a2e14e 100644 --- a/wpimath/src/main/native/include/wpi/math/geometry/Rotation2d.hpp +++ b/wpimath/src/main/native/include/wpi/math/geometry/Rotation2d.hpp @@ -118,8 +118,7 @@ class WPILIB_DLLEXPORT Rotation2d { } /** - * Subtracts the new rotation from the current rotation and returns the new - * rotation. + * Returns this rotation relative to another rotation. * * For example, Rotation2d{10_deg} - Rotation2d{100_deg} equals * Rotation2d{wpi::units::radian_t{-std::numbers::pi/2.0}} diff --git a/wpimath/src/main/native/include/wpi/math/geometry/Rotation3d.hpp b/wpimath/src/main/native/include/wpi/math/geometry/Rotation3d.hpp index f1430ad744..513182bf62 100644 --- a/wpimath/src/main/native/include/wpi/math/geometry/Rotation3d.hpp +++ b/wpimath/src/main/native/include/wpi/math/geometry/Rotation3d.hpp @@ -282,53 +282,12 @@ class WPILIB_DLLEXPORT Rotation3d { constexpr explicit Rotation3d(const Rotation2d& rotation) : Rotation3d{0_rad, 0_rad, rotation.Radians()} {} - /** - * Adds two rotations together. The other rotation is applied extrinsically to - * this rotation, which is equivalent to this rotation being applied - * intrinsically to the other rotation. See the class comment for definitions - * of extrinsic and intrinsic rotations. - * - * Note that `a - b + b` always equals `a`, but `b + (a - b)` - * sometimes doesn't. To apply a rotation offset, use either `offset = - * -measurement + actual; newAngle = angle + offset;` or `offset = actual - - * measurement; newAngle = offset + angle;`, depending on how the corrected - * angle needs to change as the input angle changes. - * - * @param other The rotation to add (applied extrinsically). - * - * @return The sum of the two rotations. - */ - constexpr Rotation3d operator+(const Rotation3d& other) const { - return RotateBy(other); - } - - /** - * Subtracts the new rotation from the current rotation and returns the new - * rotation. The new rotation is from the perspective of the other rotation - * (like Pose3d::operator-), so it needs to be applied intrinsically. See the - * class comment for definitions of extrinsic and intrinsic rotations. - * - * Note that `a - b + b` always equals `a`, but `b + (a - b)` sometimes - * doesn't. To apply a rotation offset, use either `offset = -measurement + - * actual; newAngle = angle + offset;` or `offset = actual - measurement; - * newAngle = offset + angle;`, depending on how the corrected angle needs to - * change as the input angle changes. - * - * @param other The rotation to subtract. - * - * @return The difference between the two rotations, from the perspective of - * the other rotation. - */ - constexpr Rotation3d operator-(const Rotation3d& other) const { - return *this + -other; - } - /** * Takes the inverse of the current rotation. * * @return The inverse of the current rotation. */ - constexpr Rotation3d operator-() const { return Rotation3d{m_q.Inverse()}; } + constexpr Rotation3d Inverse() const { return Rotation3d{m_q.Inverse()}; } /** * Multiplies the current rotation by a scalar. @@ -338,16 +297,7 @@ class WPILIB_DLLEXPORT Rotation3d { * @return The new scaled Rotation3d. */ constexpr Rotation3d operator*(double scalar) const { - // https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp - if (m_q.W() >= 0.0) { - return Rotation3d{ - Eigen::Vector3d{{m_q.X(), m_q.Y(), m_q.Z()}}, - 2.0 * wpi::units::radian_t{scalar * gcem::acos(m_q.W())}}; - } else { - return Rotation3d{ - Eigen::Vector3d{{-m_q.X(), -m_q.Y(), -m_q.Z()}}, - 2.0 * wpi::units::radian_t{scalar * gcem::acos(-m_q.W())}}; - } + return Rotation3d{}.Interpolate(*this, scalar); } /** @@ -406,6 +356,29 @@ class WPILIB_DLLEXPORT Rotation3d { return Rotation3d{other.m_q.Inverse() * m_q}; } + /** + * Interpolates between this rotation and another. + * + * @param endValue The other rotation. + * @param t How far between the two rotations we are (0 means this rotation, 1 + * means other rotation). + * @return The interpolated value. + */ + constexpr Rotation3d Interpolate(Rotation3d endValue, double t) const { + // https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp + // + // slerp(q₀, q₁, t) = (q₁q₀⁻¹)ᵗq₀ + // + // We negate the delta quaternion if necessary to take the shortest path + const auto& q0 = m_q; + const auto& q1 = endValue.m_q; + auto delta = q1 * q0.Inverse(); + if (delta.W() < 0.0) { + delta = Quaternion{-delta.W(), -delta.X(), -delta.Y(), -delta.Z()}; + } + return Rotation3d{delta.Pow(t) * q0}; + } + /** * Returns the quaternion representation of the Rotation3d. */ @@ -537,7 +510,7 @@ template <> constexpr wpi::math::Rotation3d wpi::util::Lerp( const wpi::math::Rotation3d& startValue, const wpi::math::Rotation3d& endValue, double t) { - return (endValue - startValue) * t + startValue; + return startValue.Interpolate(endValue, t); } #include "wpi/math/geometry/proto/Rotation3dProto.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/geometry/Transform3d.hpp b/wpimath/src/main/native/include/wpi/math/geometry/Transform3d.hpp index a353c64e5e..7b0f62e2c3 100644 --- a/wpimath/src/main/native/include/wpi/math/geometry/Transform3d.hpp +++ b/wpimath/src/main/native/include/wpi/math/geometry/Transform3d.hpp @@ -153,7 +153,8 @@ class WPILIB_DLLEXPORT Transform3d { // We are rotating the difference between the translations // using a clockwise rotation matrix. This transforms the global // delta into a local delta (relative to the initial pose). - return Transform3d{(-Translation()).RotateBy(-Rotation()), -Rotation()}; + return Transform3d{(-Translation()).RotateBy(Rotation().Inverse()), + Rotation().Inverse()}; } /** @@ -207,7 +208,7 @@ constexpr Transform3d::Transform3d(const Pose3d& initial, const Pose3d& final) { // To transform the global translation delta to be relative to the initial // pose, rotate by the inverse of the initial pose's orientation. m_translation = (final.Translation() - initial.Translation()) - .RotateBy(-initial.Rotation()); + .RotateBy(initial.Rotation().Inverse()); m_rotation = final.Rotation().RelativeTo(initial.Rotation()); } diff --git a/wpimath/src/main/native/include/wpi/math/interpolation/TimeInterpolatableBuffer.hpp b/wpimath/src/main/native/include/wpi/math/interpolation/TimeInterpolatableBuffer.hpp index deb6043570..4050893e64 100644 --- a/wpimath/src/main/native/include/wpi/math/interpolation/TimeInterpolatableBuffer.hpp +++ b/wpimath/src/main/native/include/wpi/math/interpolation/TimeInterpolatableBuffer.hpp @@ -36,7 +36,7 @@ class TimeInterpolatableBuffer { /** * Create a new TimeInterpolatableBuffer. * - * @param historySize The history size of the buffer. + * @param historySize The history size of the buffer. * @param func The function used to interpolate between values. */ TimeInterpolatableBuffer(wpi::units::second_t historySize, @@ -45,15 +45,20 @@ class TimeInterpolatableBuffer { /** * Create a new TimeInterpolatableBuffer. By default, the interpolation - * function is wpi::util::Lerp except for Pose2d, which uses the pose - * exponential. + * function is wpi::util::Lerp. If the arithmetic operators aren't supported + * (usually because they wouldn't be commutative), Interpolate() is used + * instead. * - * @param historySize The history size of the buffer. + * @param historySize The history size of the buffer. */ explicit TimeInterpolatableBuffer(wpi::units::second_t historySize) : m_historySize(historySize), m_interpolatingFunc([](const T& start, const T& end, double t) { - return wpi::util::Lerp(start, end, t); + if constexpr (requires(T a, T b, double t) { a + (b - a) * t; }) { + return wpi::util::Lerp(start, end, t); + } else { + return start.Interpolate(end, t); + } }) {} /** diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/Odometry3d.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/Odometry3d.hpp index 3ea8231320..e1562f8d6b 100644 --- a/wpimath/src/main/native/include/wpi/math/kinematics/Odometry3d.hpp +++ b/wpimath/src/main/native/include/wpi/math/kinematics/Odometry3d.hpp @@ -49,7 +49,7 @@ class WPILIB_DLLEXPORT Odometry3d { m_previousAngle = m_pose.Rotation(); // When applied extrinsically, m_gyroOffset cancels the // current gyroAngle and then rotates to m_pose.Rotation() - m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation()); + m_gyroOffset = gyroAngle.Inverse().RotateBy(m_pose.Rotation()); } /** @@ -68,7 +68,7 @@ class WPILIB_DLLEXPORT Odometry3d { m_previousAngle = pose.Rotation(); // When applied extrinsically, m_gyroOffset cancels the // current gyroAngle and then rotates to m_pose.Rotation() - m_gyroOffset = (-gyroAngle).RotateBy(m_pose.Rotation()); + m_gyroOffset = gyroAngle.Inverse().RotateBy(m_pose.Rotation()); m_previousWheelPositions = wheelPositions; } @@ -79,8 +79,8 @@ class WPILIB_DLLEXPORT Odometry3d { */ void ResetPose(const Pose3d& pose) { // Cancel the previous m_pose.Rotation() and then rotate to the new angle - m_gyroOffset = - m_gyroOffset.RotateBy(-m_pose.Rotation()).RotateBy(pose.Rotation()); + m_gyroOffset = m_gyroOffset.RotateBy(m_pose.Rotation().Inverse()) + .RotateBy(pose.Rotation()); m_pose = pose; m_previousAngle = pose.Rotation(); } @@ -101,7 +101,8 @@ class WPILIB_DLLEXPORT Odometry3d { */ void ResetRotation(const Rotation3d& rotation) { // Cancel the previous m_pose.Rotation() and then rotate to the new angle - m_gyroOffset = m_gyroOffset.RotateBy(-m_pose.Rotation()).RotateBy(rotation); + m_gyroOffset = + m_gyroOffset.RotateBy(m_pose.Rotation().Inverse()).RotateBy(rotation); m_pose = Pose3d{m_pose.Translation(), rotation}; m_previousAngle = rotation; } @@ -126,7 +127,7 @@ class WPILIB_DLLEXPORT Odometry3d { const Pose3d& Update(const Rotation3d& gyroAngle, const WheelPositions& wheelPositions) { auto angle = gyroAngle.RotateBy(m_gyroOffset); - auto angle_difference = (angle - m_previousAngle).ToVector(); + auto angle_difference = angle.RelativeTo(m_previousAngle).ToVector(); auto twist2d = m_kinematics.ToTwist2d(m_previousWheelPositions, wheelPositions); diff --git a/wpimath/src/main/python/semiwrap/Rotation3d.yml b/wpimath/src/main/python/semiwrap/Rotation3d.yml index 6f580dfcab..d01c56d51d 100644 --- a/wpimath/src/main/python/semiwrap/Rotation3d.yml +++ b/wpimath/src/main/python/semiwrap/Rotation3d.yml @@ -26,15 +26,12 @@ classes: keepalive: [] const Rotation2d&: keepalive: [] - operator+: - operator-: - overloads: - const Rotation3d& [const]: - '[const]': + Inverse: operator*: operator/: operator==: RotateBy: + Interpolate: GetQuaternion: X: Y: diff --git a/wpimath/src/test/java/org/wpilib/math/geometry/QuaternionTest.java b/wpimath/src/test/java/org/wpilib/math/geometry/QuaternionTest.java index 31325dbfa8..122b6fcf9f 100644 --- a/wpimath/src/test/java/org/wpilib/math/geometry/QuaternionTest.java +++ b/wpimath/src/test/java/org/wpilib/math/geometry/QuaternionTest.java @@ -219,8 +219,8 @@ class QuaternionTest { var start = new Quaternion(1, 2, 3, 4); var expect = new Quaternion(5, 6, 7, 8); - var twist = start.log(expect); - var actual = start.exp(twist); + var twist = expect.times(start.inverse()).log(); + var actual = twist.exp().times(start); assertEquals(expect, actual); } diff --git a/wpimath/src/test/java/org/wpilib/math/geometry/Rotation3dTest.java b/wpimath/src/test/java/org/wpilib/math/geometry/Rotation3dTest.java index 6ef74d535a..58dff1ad3b 100644 --- a/wpimath/src/test/java/org/wpilib/math/geometry/Rotation3dTest.java +++ b/wpimath/src/test/java/org/wpilib/math/geometry/Rotation3dTest.java @@ -24,7 +24,7 @@ class Rotation3dTest { var rot1 = new Rotation3d(0, 0, Math.PI / 2); var rot2 = new Rotation3d(Math.PI, 0, 0); var rot3 = new Rotation3d(-Math.PI / 2, 0, 0); - final var result1 = rot1.plus(rot2).plus(rot3); + final var result1 = rot1.rotateBy(rot2).rotateBy(rot3); final var expected1 = new Rotation3d(0, -Math.PI / 2, Math.PI / 2); assertAll( () -> assertEquals(expected1, result1), @@ -34,7 +34,7 @@ class Rotation3dTest { rot1 = new Rotation3d(0, 0, Math.PI / 2); rot2 = new Rotation3d(-Math.PI, 0, 0); rot3 = new Rotation3d(Math.PI / 2, 0, 0); - final var result2 = rot1.plus(rot2).plus(rot3); + final var result2 = rot1.rotateBy(rot2).rotateBy(rot3); final var expected2 = new Rotation3d(0, Math.PI / 2, Math.PI / 2); assertAll( () -> assertEquals(expected2, result2), @@ -44,7 +44,7 @@ class Rotation3dTest { rot1 = new Rotation3d(0, 0, Math.PI / 2); rot2 = new Rotation3d(0, Math.PI / 3, 0); rot3 = new Rotation3d(-Math.PI / 2, 0, 0); - final var result3 = rot1.plus(rot2).plus(rot3); + final var result3 = rot1.rotateBy(rot2).rotateBy(rot3); final var expected3 = new Rotation3d(0, Math.PI / 2, Math.PI / 6); assertAll( () -> assertEquals(expected3, result3), @@ -196,22 +196,22 @@ class Rotation3dTest { void testRotationLoop() { var rot = Rotation3d.kZero; - rot = rot.plus(new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0)); + rot = rot.rotateBy(new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0)); var expected = new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0); assertEquals(expected, rot); - rot = rot.plus(new Rotation3d(0.0, Units.degreesToRadians(90.0), 0.0)); + rot = rot.rotateBy(new Rotation3d(0.0, Units.degreesToRadians(90.0), 0.0)); expected = new Rotation3d( VecBuilder.fill(1.0 / Math.sqrt(3), 1.0 / Math.sqrt(3), -1.0 / Math.sqrt(3)), Units.degreesToRadians(120.0)); assertEquals(expected, rot); - rot = rot.plus(new Rotation3d(0.0, 0.0, Units.degreesToRadians(90.0))); + rot = rot.rotateBy(new Rotation3d(0.0, 0.0, Units.degreesToRadians(90.0))); expected = new Rotation3d(0.0, Units.degreesToRadians(90.0), 0.0); assertEquals(expected, rot); - rot = rot.plus(new Rotation3d(0.0, Units.degreesToRadians(-90.0), 0.0)); + rot = rot.rotateBy(new Rotation3d(0.0, Units.degreesToRadians(-90.0), 0.0)); assertEquals(Rotation3d.kZero, rot); } @@ -253,7 +253,7 @@ class Rotation3dTest { final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0); var rot = new Rotation3d(xAxis, Units.degreesToRadians(90.0)); - rot = rot.plus(new Rotation3d(xAxis, Units.degreesToRadians(30.0))); + rot = rot.rotateBy(new Rotation3d(xAxis, Units.degreesToRadians(30.0))); var expected = new Rotation3d(xAxis, Units.degreesToRadians(120.0)); assertEquals(expected, rot); @@ -264,7 +264,7 @@ class Rotation3dTest { final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0); var rot = new Rotation3d(yAxis, Units.degreesToRadians(90.0)); - rot = rot.plus(new Rotation3d(yAxis, Units.degreesToRadians(30.0))); + rot = rot.rotateBy(new Rotation3d(yAxis, Units.degreesToRadians(30.0))); var expected = new Rotation3d(yAxis, Units.degreesToRadians(120.0)); assertEquals(expected, rot); @@ -275,7 +275,7 @@ class Rotation3dTest { final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); var rot = new Rotation3d(zAxis, Units.degreesToRadians(90.0)); - rot = rot.plus(new Rotation3d(zAxis, Units.degreesToRadians(30.0))); + rot = rot.rotateBy(new Rotation3d(zAxis, Units.degreesToRadians(30.0))); var expected = new Rotation3d(zAxis, Units.degreesToRadians(120.0)); assertEquals(expected, rot); @@ -297,16 +297,6 @@ class Rotation3dTest { assertEquals(expected, result); } - @Test - void testMinus() { - final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); - - var rot1 = new Rotation3d(zAxis, Units.degreesToRadians(70.0)); - var rot2 = new Rotation3d(zAxis, Units.degreesToRadians(30.0)); - - assertEquals(rot1.minus(rot2).getZ(), Units.degreesToRadians(40.0), kEpsilon); - } - @Test void testEquality() { final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); diff --git a/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp b/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp index ccde8705e7..97d03d7d02 100644 --- a/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp @@ -200,8 +200,8 @@ TEST(QuaternionTest, LogarithmAndExponentialInverse) { Quaternion start{1, 2, 3, 4}; Quaternion expect{5, 6, 7, 8}; - auto twist = start.Log(expect); - auto actual = start.Exp(twist); + auto twist = (expect * start.Inverse()).Log(); + auto actual = twist.Exp() * start; EXPECT_EQ(expect, actual); } diff --git a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp index 0b067dd33c..762f396807 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp @@ -20,7 +20,7 @@ TEST(Rotation3dTest, GimbalLockAccuracy) { auto rot2 = Rotation3d{wpi::units::radian_t{std::numbers::pi}, 0_rad, 0_rad}; auto rot3 = Rotation3d{-wpi::units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad}; - const auto result1 = rot1 + rot2 + rot3; + const auto result1 = rot1.RotateBy(rot2).RotateBy(rot3); const auto expected1 = Rotation3d{0_rad, -wpi::units::radian_t{std::numbers::pi / 2}, wpi::units::radian_t{std::numbers::pi / 2}}; @@ -31,7 +31,7 @@ TEST(Rotation3dTest, GimbalLockAccuracy) { rot1 = Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi / 2}}; rot2 = Rotation3d{wpi::units::radian_t{-std::numbers::pi}, 0_rad, 0_rad}; rot3 = Rotation3d{wpi::units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad}; - const auto result2 = rot1 + rot2 + rot3; + const auto result2 = rot1.RotateBy(rot2).RotateBy(rot3); const auto expected2 = Rotation3d{0_rad, wpi::units::radian_t{std::numbers::pi / 2}, wpi::units::radian_t{std::numbers::pi / 2}}; @@ -42,7 +42,7 @@ TEST(Rotation3dTest, GimbalLockAccuracy) { rot1 = Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi / 2}}; rot2 = Rotation3d{0_rad, wpi::units::radian_t{std::numbers::pi / 3}, 0_rad}; rot3 = Rotation3d{-wpi::units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad}; - const auto result3 = rot1 + rot2 + rot3; + const auto result3 = rot1.RotateBy(rot2).RotateBy(rot3); const auto expected3 = Rotation3d{0_rad, wpi::units::radian_t{std::numbers::pi / 2}, wpi::units::radian_t{std::numbers::pi / 6}}; @@ -184,20 +184,20 @@ TEST(Rotation3dTest, DegreesToRadians) { TEST(Rotation3dTest, RotationLoop) { Rotation3d rot; - rot = rot + Rotation3d{90_deg, 0_deg, 0_deg}; + rot = rot.RotateBy(Rotation3d{90_deg, 0_deg, 0_deg}); Rotation3d expected{90_deg, 0_deg, 0_deg}; EXPECT_EQ(expected, rot); - rot = rot + Rotation3d{0_deg, 90_deg, 0_deg}; + rot = rot.RotateBy(Rotation3d{0_deg, 90_deg, 0_deg}); expected = Rotation3d{ {1.0 / std::sqrt(3), 1.0 / std::sqrt(3), -1.0 / std::sqrt(3)}, 120_deg}; EXPECT_EQ(expected, rot); - rot = rot + Rotation3d{0_deg, 0_deg, 90_deg}; + rot = rot.RotateBy(Rotation3d{0_deg, 0_deg, 90_deg}); expected = Rotation3d{0_deg, 90_deg, 0_deg}; EXPECT_EQ(expected, rot); - rot = rot + Rotation3d{0_deg, -90_deg, 0_deg}; + rot = rot.RotateBy(Rotation3d{0_deg, -90_deg, 0_deg}); EXPECT_EQ(Rotation3d{}, rot); } @@ -205,7 +205,7 @@ TEST(Rotation3dTest, RotateByFromZeroX) { const Eigen::Vector3d xAxis{1.0, 0.0, 0.0}; const Rotation3d zero; - auto rotated = zero + Rotation3d{xAxis, 90_deg}; + auto rotated = zero.RotateBy(Rotation3d{xAxis, 90_deg}); Rotation3d expected{xAxis, 90_deg}; EXPECT_EQ(expected, rotated); @@ -215,7 +215,7 @@ TEST(Rotation3dTest, RotateByFromZeroY) { const Eigen::Vector3d yAxis{0.0, 1.0, 0.0}; const Rotation3d zero; - auto rotated = zero + Rotation3d{yAxis, 90_deg}; + auto rotated = zero.RotateBy(Rotation3d{yAxis, 90_deg}); Rotation3d expected{yAxis, 90_deg}; EXPECT_EQ(expected, rotated); @@ -225,7 +225,7 @@ TEST(Rotation3dTest, RotateByFromZeroZ) { const Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; const Rotation3d zero; - auto rotated = zero + Rotation3d{zAxis, 90_deg}; + auto rotated = zero.RotateBy(Rotation3d{zAxis, 90_deg}); Rotation3d expected{zAxis, 90_deg}; EXPECT_EQ(expected, rotated); @@ -235,7 +235,7 @@ TEST(Rotation3dTest, RotateByNonZeroX) { const Eigen::Vector3d xAxis{1.0, 0.0, 0.0}; auto rot = Rotation3d{xAxis, 90_deg}; - rot = rot + Rotation3d{xAxis, 30_deg}; + rot = rot.RotateBy(Rotation3d{xAxis, 30_deg}); Rotation3d expected{xAxis, 120_deg}; EXPECT_EQ(expected, rot); @@ -245,7 +245,7 @@ TEST(Rotation3dTest, RotateByNonZeroY) { const Eigen::Vector3d yAxis{0.0, 1.0, 0.0}; auto rot = Rotation3d{yAxis, 90_deg}; - rot = rot + Rotation3d{yAxis, 30_deg}; + rot = rot.RotateBy(Rotation3d{yAxis, 30_deg}); Rotation3d expected{yAxis, 120_deg}; EXPECT_EQ(expected, rot); @@ -255,7 +255,7 @@ TEST(Rotation3dTest, RotateByNonZeroZ) { const Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; auto rot = Rotation3d{zAxis, 90_deg}; - rot = rot + Rotation3d{zAxis, 30_deg}; + rot = rot.RotateBy(Rotation3d{zAxis, 30_deg}); Rotation3d expected{zAxis, 120_deg}; EXPECT_EQ(expected, rot); @@ -276,15 +276,6 @@ TEST(Rotation3dTest, RelativeTo) { EXPECT_EQ(expected, result); } -TEST(Rotation3dTest, Minus) { - const Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; - - const auto rot1 = Rotation3d{zAxis, 70_deg}; - const auto rot2 = Rotation3d{zAxis, 30_deg}; - - EXPECT_DOUBLE_EQ(40.0, wpi::units::degree_t{(rot1 - rot2).Z()}.value()); -} - TEST(Rotation3dTest, AxisAngle) { const Eigen::Vector3d xAxis{1.0, 0.0, 0.0}; const Eigen::Vector3d yAxis{0.0, 1.0, 0.0}; @@ -376,7 +367,7 @@ TEST(Rotation3dTest, Interpolate) { rot2 = Rotation3d{yAxis, -160_deg}; interpolated = wpi::util::Lerp(rot1, rot2, 0.5); EXPECT_DOUBLE_EQ(180.0, wpi::units::degree_t{interpolated.X()}.value()); - EXPECT_DOUBLE_EQ(-5.0, wpi::units::degree_t{interpolated.Y()}.value()); + EXPECT_NEAR(-5.0, wpi::units::degree_t{interpolated.Y()}.value(), 1e-9); EXPECT_DOUBLE_EQ(180.0, wpi::units::degree_t{interpolated.Z()}.value()); // 50 + (70 - 50) * 0.5 = 60 diff --git a/wpimath/src/test/python/geometry/test_quaternion.py b/wpimath/src/test/python/geometry/test_quaternion.py index 9709466cd6..483b847732 100644 --- a/wpimath/src/test/python/geometry/test_quaternion.py +++ b/wpimath/src/test/python/geometry/test_quaternion.py @@ -204,8 +204,8 @@ def test_logarithm_and_exponential_inverse(): start = Quaternion(1, 2, 3, 4) expect = Quaternion(5, 6, 7, 8) - twist = start.log(expect) - actual = start.exp(twist) + twist = (expect * start.inverse()).log() + actual = twist.exp() * start assert actual == expect diff --git a/wpimath/src/test/python/geometry/test_rotation3d.py b/wpimath/src/test/python/geometry/test_rotation3d.py index c0da59e1f0..e3f35f8ac5 100644 --- a/wpimath/src/test/python/geometry/test_rotation3d.py +++ b/wpimath/src/test/python/geometry/test_rotation3d.py @@ -9,7 +9,7 @@ def test_gimbal_lock_accuracy(): rot1 = Rotation3d(roll=0, pitch=0, yaw=math.pi / 2) rot2 = Rotation3d(roll=math.pi, pitch=0, yaw=0) rot3 = Rotation3d(roll=-math.pi / 2, pitch=0, yaw=0) - result1 = rot1 + rot2 + rot3 + result1 = rot1.rotateBy(rot2).rotateBy(rot3) expected1 = Rotation3d(roll=0, pitch=-math.pi / 2, yaw=math.pi / 2) assert expected1 == result1 assert (result1.x + result1.z) == pytest.approx(math.pi / 2) @@ -18,7 +18,7 @@ def test_gimbal_lock_accuracy(): rot1 = Rotation3d(roll=0, pitch=0, yaw=math.pi / 2) rot2 = Rotation3d(roll=-math.pi, pitch=0, yaw=0) rot3 = Rotation3d(roll=math.pi / 2, pitch=0, yaw=0) - result2 = rot1 + rot2 + rot3 + result2 = rot1.rotateBy(rot2).rotateBy(rot3) expected2 = Rotation3d(roll=0, pitch=math.pi / 2, yaw=math.pi / 2) assert expected2 == result2 assert (result2.z - result2.x) == pytest.approx(math.pi / 2) @@ -27,7 +27,7 @@ def test_gimbal_lock_accuracy(): rot1 = Rotation3d(roll=0, pitch=0, yaw=math.pi / 2) rot2 = Rotation3d(roll=0, pitch=math.pi / 3, yaw=0) rot3 = Rotation3d(roll=-math.pi / 2, pitch=0, yaw=0) - result3 = rot1 + rot2 + rot3 + result3 = rot1.rotateBy(rot2).rotateBy(rot3) expected3 = Rotation3d(roll=0, pitch=math.pi / 2, yaw=math.pi / 6) assert expected3 == result3 assert (result3.z - result3.x) == pytest.approx(math.pi / 6) @@ -170,22 +170,22 @@ def test_degrees_to_radians(): def test_rotation_loop(): rot = Rotation3d() - rot = rot + Rotation3d(math.radians(90), 0, 0) + rot = rot.rotateBy(Rotation3d(math.radians(90), 0, 0)) expected = Rotation3d(math.radians(90), 0, 0) assert expected == rot - rot = rot + Rotation3d(0, math.radians(90), 0) + rot = rot.rotateBy(Rotation3d(0, math.radians(90), 0)) expected = Rotation3d( np.array([1.0 / math.sqrt(3), 1.0 / math.sqrt(3), -1.0 / math.sqrt(3)]), math.radians(120), ) assert expected == rot - rot = rot + Rotation3d(0, 0, math.radians(90)) + rot = rot.rotateBy(Rotation3d(0, 0, math.radians(90))) expected = Rotation3d(0, math.radians(90), 0) assert expected == rot - rot = rot + Rotation3d(0, math.radians(-90), 0) + rot = rot.rotateBy(Rotation3d(0, math.radians(-90), 0)) assert Rotation3d() == rot @@ -193,7 +193,7 @@ def test_rotate_by_from_zero_x(): x_axis = np.array([1.0, 0.0, 0.0]) zero = Rotation3d() - rotated = zero + Rotation3d(x_axis, math.radians(90)) + rotated = zero.rotateBy(Rotation3d(x_axis, math.radians(90))) expected = Rotation3d(x_axis, math.radians(90)) assert expected == rotated @@ -203,7 +203,7 @@ def test_rotate_by_from_zero_y(): y_axis = np.array([0.0, 1.0, 0.0]) zero = Rotation3d() - rotated = zero + Rotation3d(y_axis, math.radians(90)) + rotated = zero.rotateBy(Rotation3d(y_axis, math.radians(90))) expected = Rotation3d(y_axis, math.radians(90)) assert expected == rotated @@ -213,7 +213,7 @@ def test_rotate_by_from_zero_z(): z_axis = np.array([0.0, 0.0, 1.0]) zero = Rotation3d() - rotated = zero + Rotation3d(z_axis, math.radians(90)) + rotated = zero.rotateBy(Rotation3d(z_axis, math.radians(90))) expected = Rotation3d(z_axis, math.radians(90)) assert expected == rotated @@ -223,7 +223,7 @@ def test_rotate_by_non_zero_x(): x_axis = np.array([1.0, 0.0, 0.0]) rot = Rotation3d(x_axis, math.radians(90)) - rot = rot + Rotation3d(x_axis, math.radians(30)) + rot = rot.rotateBy(Rotation3d(x_axis, math.radians(30))) expected = Rotation3d(x_axis, math.radians(120)) assert expected == rot @@ -233,7 +233,7 @@ def test_rotate_by_non_zero_y(): y_axis = np.array([0.0, 1.0, 0.0]) rot = Rotation3d(y_axis, math.radians(90)) - rot = rot + Rotation3d(y_axis, math.radians(30)) + rot = rot.rotateBy(Rotation3d(y_axis, math.radians(30))) expected = Rotation3d(y_axis, math.radians(120)) assert expected == rot @@ -243,21 +243,12 @@ def test_rotate_by_non_zero_z(): z_axis = np.array([0.0, 0.0, 1.0]) rot = Rotation3d(z_axis, math.radians(90)) - rot = rot + Rotation3d(z_axis, math.radians(30)) + rot = rot.rotateBy(Rotation3d(z_axis, math.radians(30))) expected = Rotation3d(z_axis, math.radians(120)) assert expected == rot -def test_minus(): - z_axis = np.array([0.0, 0.0, 1.0]) - - rot1 = Rotation3d(z_axis, math.radians(70)) - rot2 = Rotation3d(z_axis, math.radians(30)) - - assert math.degrees((rot1 - rot2).z) == pytest.approx(40.0) - - def test_axis_angle(): x_axis = np.array([1.0, 0.0, 0.0]) y_axis = np.array([0.0, 1.0, 0.0])