diff --git a/src/Rotation.cpp b/src/Rotation.cpp index 031726c21a6b797ffced6593491bdc8f919662b5..bde05fd8f98460920aadad7467b6efa72fd5a14d 100644 --- a/src/Rotation.cpp +++ b/src/Rotation.cpp @@ -64,7 +64,8 @@ namespace ale { if (matrix.size() != 9) { throw std::invalid_argument("Rotation matrix must be 3 by 3."); } - quat = Eigen::Quaterniond(Eigen::Quaterniond::Matrix3(matrix.data())); + // 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()); } @@ -148,7 +149,9 @@ namespace ale { std::vector<double> Rotation::toRotationMatrix() const { - Eigen::Quaterniond::RotationMatrixType mat = m_impl->quat.toRotationMatrix(); + // The matrix is stored in column major, but we want to output in row semiMajor + // so take the transpose + Eigen::Quaterniond::RotationMatrixType mat = m_impl->quat.toRotationMatrix().transpose(); return std::vector<double>(mat.data(), mat.data() + mat.size()); } @@ -198,8 +201,7 @@ namespace ale { Vec3d Rotation::operator()(const Vec3d &vector) const { Eigen::Vector3d eigenVector(vector.x, vector.y, vector.z); Eigen::Vector3d rotatedVector = m_impl->quat._transformVector(eigenVector); - std::vector<double> tempVec = std::vector<double>(rotatedVector.data(), rotatedVector.data() + rotatedVector.size()); - return Vec3d(tempVec); + return Vec3d(rotatedVector[0], rotatedVector[1], rotatedVector[2]); } State Rotation::operator()( diff --git a/tests/ctests/RotationTests.cpp b/tests/ctests/RotationTests.cpp index f143070f17b8f8bda47f483f11a909337ba44311..2271ace6530d66593ab6772d0065b55b3aa0859c 100644 --- a/tests/ctests/RotationTests.cpp +++ b/tests/ctests/RotationTests.cpp @@ -30,9 +30,9 @@ TEST(RotationTest, QuaternionConstructor) { TEST(RotationTest, MatrixConstructor) { Rotation rotation( - {0.0, 0.0, 1.0, - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0}); + {0.0, 1.0, 0.0, + 0.0, 0.0, 1.0, + 1.0, 0.0, 0.0}); vector<double> quat = rotation.toQuaternion(); ASSERT_EQ(quat.size(), 4); EXPECT_NEAR(quat[0], -0.5, 1e-10); @@ -113,13 +113,13 @@ TEST(RotationTest, ToRotationMatrix) { vector<double> mat = rotation.toRotationMatrix(); ASSERT_EQ(mat.size(), 9); EXPECT_NEAR(mat[0], 0.0, 1e-10); - EXPECT_NEAR(mat[1], 0.0, 1e-10); - EXPECT_NEAR(mat[2], 1.0, 1e-10); - EXPECT_NEAR(mat[3], 1.0, 1e-10); + EXPECT_NEAR(mat[1], 1.0, 1e-10); + EXPECT_NEAR(mat[2], 0.0, 1e-10); + EXPECT_NEAR(mat[3], 0.0, 1e-10); EXPECT_NEAR(mat[4], 0.0, 1e-10); - EXPECT_NEAR(mat[5], 0.0, 1e-10); - EXPECT_NEAR(mat[6], 0.0, 1e-10); - EXPECT_NEAR(mat[7], 1.0, 1e-10); + EXPECT_NEAR(mat[5], 1.0, 1e-10); + EXPECT_NEAR(mat[6], 1.0, 1e-10); + EXPECT_NEAR(mat[7], 0.0, 1e-10); EXPECT_NEAR(mat[8], 0.0, 1e-10); }