Skip to content
Snippets Groups Projects
Commit 069b5768 authored by Kristin's avatar Kristin Committed by GitHub
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 aa6957ea
Branches
No related tags found
No related merge requests found
......@@ -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.
*
......
......@@ -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();
}
......
......@@ -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,
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment