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 e1c4b269a1..957129481f 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 @@ -4,12 +4,15 @@ package edu.wpi.first.math.geometry; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.numbers.N3; import java.util.Objects; +import org.ejml.dense.row.factory.DecompositionFactory_DDRM; /** A rotation in a 3D coordinate. */ public class Rotation3d implements Interpolatable { @@ -74,6 +77,57 @@ public class Rotation3d implements Interpolatable { m_q = new Quaternion(Math.cos(angleRadians / 2.0), v.get(0, 0), v.get(1, 0), v.get(2, 0)); } + /** + * Constructs a Rotation3d that rotates the initial vector onto the final vector. + * + *

This is useful for turning a 3D vector (final) into an orientation relative to a coordinate + * system vector (initial). + * + * @param initial The initial vector. + * @param last The final vector. + */ + public Rotation3d(Vector initial, Vector last) { + double dot = initial.dot(last); + double normProduct = initial.norm() * last.norm(); + double dotNorm = dot / normProduct; + + if (dotNorm > 1.0 - 1E-9) { + // If the dot product is 1, the two vectors point in the same direction so + // there's no rotation. The default initialization of m_q will work. + return; + } else if (dotNorm < -1.0 + 1E-9) { + // If the dot product is -1, the two vectors point in opposite directions + // so a 180 degree rotation is required. Any orthogonal vector can be used + // for it. Q in the QR decomposition is an orthonormal basis, so it + // contains orthogonal unit vectors. + @SuppressWarnings("LocalVariableName") + var X = + new MatBuilder<>(Nat.N3(), Nat.N1()) + .fill(initial.get(0, 0), initial.get(1, 0), initial.get(2, 0)); + final var qr = DecompositionFactory_DDRM.qr(3, 1); + qr.decompose(X.getStorage().getMatrix()); + final var Q = qr.getQ(null, false); + + // w = cos(θ/2) = cos(90°) = 0 + // + // For x, y, and z, we use the second column of Q because the first is + // parallel instead of orthogonal. The third column would also work. + m_q = new Quaternion(0.0, Q.get(0, 1), Q.get(1, 1), Q.get(2, 1)); + } else { + // initial x last + var axis = + VecBuilder.fill( + initial.get(1, 0) * last.get(2, 0) - last.get(1, 0) * initial.get(2, 0), + last.get(0, 0) * initial.get(2, 0) - initial.get(0, 0) * last.get(2, 0), + initial.get(0, 0) * last.get(1, 0) - last.get(0, 0) * initial.get(1, 0)); + + // https://stackoverflow.com/a/11741520 + m_q = + new Quaternion(normProduct + dot, axis.get(0, 0), axis.get(1, 0), axis.get(2, 0)) + .normalize(); + } + } + /** * Adds two rotations together. * diff --git a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp index f684b33dff..77edf2d0dd 100644 --- a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp @@ -8,6 +8,8 @@ #include +#include "Eigen/Core" +#include "Eigen/QR" #include "units/math.h" using namespace frc; @@ -43,6 +45,40 @@ Rotation3d::Rotation3d(const Vectord<3>& axis, units::radian_t angle) { m_q = Quaternion{units::math::cos(angle / 2.0), v(0), v(1), v(2)}; } +Rotation3d::Rotation3d(const Vectord<3>& initial, const Vectord<3>& final) { + double dot = initial.dot(final); + double normProduct = initial.norm() * final.norm(); + double dotNorm = dot / normProduct; + + if (dotNorm > 1.0 - 1E-9) { + // If the dot product is 1, the two vectors point in the same direction so + // there's no rotation. The default initialization of m_q will work. + return; + } else if (dotNorm < -1.0 + 1E-9) { + // If the dot product is -1, the two vectors point in opposite directions so + // a 180 degree rotation is required. Any orthogonal vector can be used for + // it. Q in the QR decomposition is an orthonormal basis, so it contains + // orthogonal unit vectors. + Eigen::Matrix X = initial; + Eigen::HouseholderQR qr{X}; + Eigen::Matrix Q = qr.householderQ(); + + // w = std::cos(θ/2) = std::cos(90°) = 0 + // + // For x, y, and z, we use the second column of Q because the first is + // parallel instead of orthogonal. The third column would also work. + m_q = Quaternion{0.0, Q(0, 1), Q(1, 1), Q(2, 1)}; + } else { + // initial x final + Eigen::Vector3d axis{initial(1) * final(2) - final(1) * initial(2), + final(0) * initial(2) - initial(0) * final(2), + initial(0) * final(1) - final(0) * initial(1)}; + + // https://stackoverflow.com/a/11741520 + m_q = Quaternion{normProduct + dot, axis(0), axis(1), axis(2)}.Normalize(); + } +} + Rotation3d Rotation3d::operator+(const Rotation3d& other) const { return RotateBy(other); } diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h index ef19bf8d3c..aada0e25f2 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h @@ -51,6 +51,18 @@ class WPILIB_DLLEXPORT Rotation3d { */ Rotation3d(const Vectord<3>& axis, units::radian_t angle); + /** + * Constructs a Rotation3d that rotates the initial vector onto the final + * vector. + * + * This is useful for turning a 3D vector (final) into an orientation relative + * to a coordinate system vector (initial). + * + * @param initial The initial vector. + * @param final The final vector. + */ + Rotation3d(const Vectord<3>& initial, const Vectord<3>& final); + /** * Adds two rotations together. * diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java index b48150ed92..e9ad4cddc6 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java @@ -16,15 +16,15 @@ class Rotation3dTest { private static final double kEpsilon = 1E-9; @Test - void testInit() { + void testInitAxisAngleAndRollPitchYaw() { @SuppressWarnings("LocalVariableName") - var xAxis = VecBuilder.fill(1.0, 0.0, 0.0); + final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0); final var rot1 = new Rotation3d(xAxis, Math.PI / 3); final var rot2 = new Rotation3d(Math.PI / 3, 0.0, 0.0); assertEquals(rot1, rot2); @SuppressWarnings("LocalVariableName") - var yAxis = VecBuilder.fill(0.0, 1.0, 0.0); + final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0); final var rot3 = new Rotation3d(yAxis, Math.PI / 3); final var rot4 = new Rotation3d(0.0, Math.PI / 3, 0.0); assertEquals(rot3, rot4); @@ -36,6 +36,66 @@ class Rotation3dTest { assertEquals(rot5, rot6); } + @Test + void testInitTwoVector() { + @SuppressWarnings("LocalVariableName") + final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0); + @SuppressWarnings("LocalVariableName") + final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0); + @SuppressWarnings("LocalVariableName") + final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); + + // 90 degree CW rotation around y-axis + final var rot1 = new Rotation3d(xAxis, zAxis); + final var expected1 = new Rotation3d(yAxis, -Math.PI / 2.0); + assertEquals(expected1, rot1); + + // 45 degree CCW rotation around z-axis + final var rot2 = new Rotation3d(xAxis, VecBuilder.fill(1.0, 1.0, 0.0)); + final var expected2 = new Rotation3d(zAxis, Math.PI / 4.0); + assertEquals(expected2, rot2); + + // 0 degree rotation of x-axes + final var rot3 = new Rotation3d(xAxis, xAxis); + assertEquals(new Rotation3d(), rot3); + + // 0 degree rotation of y-axes + final var rot4 = new Rotation3d(yAxis, yAxis); + assertEquals(new Rotation3d(), rot4); + + // 0 degree rotation of z-axes + final var rot5 = new Rotation3d(zAxis, zAxis); + assertEquals(new Rotation3d(), rot5); + + // 180 degree rotation tests. For 180 degree rotations, any quaternion with + // an orthogonal rotation axis is acceptable. The rotation axis and initial + // vector are orthogonal if their dot product is zero. + + // 180 degree rotation of x-axes + final var rot6 = new Rotation3d(xAxis, xAxis.times(-1.0)); + final var q6 = rot6.getQuaternion(); + assertEquals(0.0, q6.getW()); + assertEquals( + 0.0, + q6.getX() * xAxis.get(0, 0) + q6.getY() * xAxis.get(1, 0) + q6.getZ() * xAxis.get(2, 0)); + + // 180 degree rotation of y-axes + final var rot7 = new Rotation3d(yAxis, yAxis.times(-1.0)); + final var q7 = rot7.getQuaternion(); + assertEquals(0.0, q7.getW()); + assertEquals( + 0.0, + q7.getX() * yAxis.get(0, 0) + q7.getY() * yAxis.get(1, 0) + q7.getZ() * yAxis.get(2, 0)); + + // 180 degree rotation of z-axes + final var rot8 = new Rotation3d(zAxis, zAxis.times(-1.0)); + final var q8 = rot8.getQuaternion(); + assertEquals(0.0, q8.getW()); + assertEquals( + 0.0, + q8.getX() * zAxis.get(0, 0) + q8.getY() * zAxis.get(1, 0) + q8.getZ() * zAxis.get(2, 0)); + } + @Test void testRadiansToDegrees() { @SuppressWarnings("LocalVariableName") diff --git a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp index 5fa8f9b7e9..a92b7387a5 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp @@ -12,7 +12,7 @@ using namespace frc; -TEST(Rotation3dTest, Init) { +TEST(Rotation3dTest, InitAxisAngleAndRollPitchYaw) { const Eigen::Vector3d xAxis{1.0, 0.0, 0.0}; const Rotation3d rot1{xAxis, units::radian_t{wpi::numbers::pi / 3}}; const Rotation3d rot2{units::radian_t{wpi::numbers::pi / 3}, 0_rad, 0_rad}; @@ -29,6 +29,56 @@ TEST(Rotation3dTest, Init) { EXPECT_EQ(rot5, rot6); } +TEST(Rotation3dTest, InitTwoVector) { + const Eigen::Vector3d xAxis{1.0, 0.0, 0.0}; + const Eigen::Vector3d yAxis{0.0, 1.0, 0.0}; + const Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; + + // 90 degree CW rotation around y-axis + const Rotation3d rot1{xAxis, zAxis}; + const Rotation3d expected1{yAxis, units::radian_t{-wpi::numbers::pi / 2.0}}; + EXPECT_EQ(expected1, rot1); + + // 45 degree CCW rotation around z-axis + const Rotation3d rot2{xAxis, Eigen::Vector3d{1.0, 1.0, 0.0}}; + const Rotation3d expected2{zAxis, units::radian_t{wpi::numbers::pi / 4.0}}; + EXPECT_EQ(expected2, rot2); + + // 0 degree rotation of x-axes + const Rotation3d rot3{xAxis, xAxis}; + EXPECT_EQ(Rotation3d{}, rot3); + + // 0 degree rotation of y-axes + const Rotation3d rot4{yAxis, yAxis}; + EXPECT_EQ(Rotation3d{}, rot4); + + // 0 degree rotation of z-axes + const Rotation3d rot5{zAxis, zAxis}; + EXPECT_EQ(Rotation3d{}, rot5); + + // 180 degree rotation tests. For 180 degree rotations, any quaternion with an + // orthogonal rotation axis is acceptable. The rotation axis and initial + // vector are orthogonal if their dot product is zero. + + // 180 degree rotation of x-axes + const Rotation3d rot6{xAxis, -xAxis}; + const auto q6 = rot6.GetQuaternion(); + EXPECT_EQ(0.0, q6.W()); + EXPECT_EQ(0.0, q6.X() * xAxis(0) + q6.Y() * xAxis(1) + q6.Z() * xAxis(2)); + + // 180 degree rotation of y-axes + const Rotation3d rot7{yAxis, -yAxis}; + const auto q7 = rot7.GetQuaternion(); + EXPECT_EQ(0.0, q7.W()); + EXPECT_EQ(0.0, q7.X() * yAxis(0) + q7.Y() * yAxis(1) + q7.Z() * yAxis(2)); + + // 180 degree rotation of z-axes + const Rotation3d rot8{zAxis, -zAxis}; + const auto q8 = rot8.GetQuaternion(); + EXPECT_EQ(0.0, q8.W()); + EXPECT_EQ(0.0, q8.X() * zAxis(0) + q8.Y() * zAxis(1) + q8.Z() * zAxis(2)); +} + TEST(Rotation3dTest, RadiansToDegrees) { const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};