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

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.
        *

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

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,