diff --git a/include/ale/Rotation.h b/include/ale/Rotation.h index 5b280317172c1cef7bcd1775e35deeb90c7d3d11..2df46d6d9f86af71467605f0895bc985764378ae 100644 --- a/include/ale/Rotation.h +++ b/include/ale/Rotation.h @@ -77,7 +77,7 @@ namespace ale { std::vector<double> toRotationMatrix() const; /** - * Create a state rotation matrix from the rotation and an angula velocity. + * Create a state rotation matrix from the rotation and an angular velocity. * * @param av The angular velocity vector. * diff --git a/src/Rotation.cpp b/src/Rotation.cpp index bde05fd8f98460920aadad7467b6efa72fd5a14d..07c691f2590f6555979b6fc04790ee706d490292 100644 --- a/src/Rotation.cpp +++ b/src/Rotation.cpp @@ -65,7 +65,7 @@ namespace ale { throw std::invalid_argument("Rotation matrix must be 3 by 3."); } // The data is in row major order, so take the transpose to get column major order - quat = Eigen::Quaterniond(Eigen::Quaterniond::Matrix3(matrix.data()).transpose()); + quat = Eigen::Quaterniond(Eigen::Quaterniond::Matrix3(matrix.data()).transpose()).normalized(); } @@ -81,6 +81,7 @@ namespace ale { for (size_t i = 0; i < angles.size(); i++) { quat *= Eigen::Quaterniond(Eigen::AngleAxisd(angles[i], axis(axes[i]))); } + quat = quat.normalized(); } @@ -89,7 +90,7 @@ namespace ale { throw std::invalid_argument("Rotation axis must have 3 elements."); } Eigen::Vector3d eigenAxis((double *) axis.data()); - quat = Eigen::Quaterniond(Eigen::AngleAxisd(theta, eigenAxis.normalized())); + quat = Eigen::Quaterniond(Eigen::AngleAxisd(theta, eigenAxis.normalized())).normalized(); } diff --git a/tests/ctests/RotationTests.cpp b/tests/ctests/RotationTests.cpp index 2271ace6530d66593ab6772d0065b55b3aa0859c..8e4517cb4ba2be1aeb5e08e216082f63fee5cec3 100644 --- a/tests/ctests/RotationTests.cpp +++ b/tests/ctests/RotationTests.cpp @@ -28,6 +28,18 @@ TEST(RotationTest, QuaternionConstructor) { EXPECT_NEAR(quat[3], 0.0, 1e-10); } +TEST(RotationTest, UnnormalizedQuaternion) { + Rotation rotation(1.0, 1.0, 0.0, 0.0); + vector<double> quat = rotation.toQuaternion(); + ASSERT_EQ(quat.size(), 4); + // The constructor will normalize the quaternion. + EXPECT_NEAR(quat[0], 1.0/sqrt(2), 1e-10); + EXPECT_NEAR(quat[1], 1.0/sqrt(2), 1e-10); + EXPECT_NEAR(quat[2], 0.0, 1e-10); + EXPECT_NEAR(quat[3], 0.0, 1e-10); +} + + TEST(RotationTest, MatrixConstructor) { Rotation rotation( {0.0, 1.0, 0.0,