Fixed rotation matrices being transposed (#368)

* Fixed rotation matrices being transposed

* Removed extraneous copy

* consistant wording

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()(

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);
 }