Commit 473aeebc authored by Kristin's avatar Kristin Committed by Jesse Mapel
Browse files

Normalize quaternions in Rotations (#377) 

* Update format of range conversion coefficients isd output to include a separate list for times. Also update driver, test data, and remove unneeded semicolons

* Fix documentaion error

* fix other documentation error

* Add quaternion normalization

* Add test that makes sure an unnormalized quaternion is normalized in the rotation constructor
parent 7fd08abb

include/ale/Rotation.h

+1 −1
Original line number Original line Diff line number Diff line
@@ -77,7 +77,7 @@ namespace ale { 
      std::vector<double> toRotationMatrix() const;

      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.

       * @param av The angular velocity vector.

       *

       *

src/Rotation.cpp

+3 −2
Original line number Original line Diff line number Diff line
@@ -65,7 +65,7 @@ namespace ale { 
          throw std::invalid_argument("Rotation matrix must be 3 by 3.");

          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

        // 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++) {

        for (size_t i = 0; i < angles.size(); i++) {

          quat *= Eigen::Quaterniond(Eigen::AngleAxisd(angles[i], axis(axes[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.");

          throw std::invalid_argument("Rotation axis must have 3 elements.");

        }

        }

        Eigen::Vector3d eigenAxis((double *) axis.data());

        Eigen::Vector3d eigenAxis((double *) axis.data());

        quat = Eigen::Quaterniond(Eigen::AngleAxisd(theta, eigenAxis.normalized()));

        quat = Eigen::Quaterniond(Eigen::AngleAxisd(theta, eigenAxis.normalized())).normalized();

      }

      }

tests/ctests/RotationTests.cpp

+12 −0
Original line number Original line Diff line number Diff line
@@ -28,6 +28,18 @@ TEST(RotationTest, QuaternionConstructor) { 
  EXPECT_NEAR(quat[3], 0.0, 1e-10);

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

TEST(RotationTest, MatrixConstructor) {

  Rotation rotation(

  Rotation rotation(

        {0.0, 1.0, 0.0,

        {0.0, 1.0, 0.0,