Commit 6c3d066a authored by Jesse Mapel's avatar Jesse Mapel
Browse files

Fixed rotation matrices being transposed (#368) 

* Fixed rotation matrices being transposed

* Removed extraneous copy

* consistant wording
parent c1abf9fa

src/Rotation.cpp

+6 −4
Original line number Diff line number Diff line
@@ -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()(

tests/ctests/RotationTests.cpp

+9 −9
Original line number Diff line number Diff line
@@ -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);

}