diff --git a/CMakeLists.txt b/CMakeLists.txt index 3fe44c6..b130ad9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -40,7 +40,12 @@ message("${${PROJECT_NAME}_SOURCES} -> libwrmath") add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES}) add_executable(euler2quat tools/euler2quat.cc) target_link_libraries(euler2quat ${PROJECT_NAME}) -set_target_properties(${TESTNAME} PROPERTIES +set_target_properties(euler2quat PROPERTIES + FOLDER bin + RUNTIME_OUTPUT_DIRECTORY bin) +add_executable(quaternion tools/quaternion.cc) +target_link_libraries(quaternion ${PROJECT_NAME}) +set_target_properties(quaternion PROPERTIES FOLDER bin RUNTIME_OUTPUT_DIRECTORY bin) @@ -84,6 +89,7 @@ endmacro() package_add_gtest(vector_test test/vector_test.cc) package_add_gtest(orientation_test test/orientation_test.cc) package_add_gtest(quaternion_test test/quaternion_test.cc) +package_add_gtest(madgwick_test test/madgwick_test.cc) add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} --verbose DEPENDS ${TEST_EXECS}) diff --git a/include/wrmath/filter/madgwick.h b/include/wrmath/filter/madgwick.h new file mode 100644 index 0000000..8d6bdc2 --- /dev/null +++ b/include/wrmath/filter/madgwick.h @@ -0,0 +1,100 @@ +#ifndef __WRMATH_FILTER_MADGWICK_H +#define __WRMATH_FILTER_MADGWICK_H + + +#include +#include + + +namespace wr { +namespace filter { + + +/// Madgwick implements an efficient orientation filter for IMUs. +/// +/// \tparam T A floating point type. +template +class Madgwick { +public: + /// The Madgwick filter is initialised with an identity quaternion. + Madgwick() : deltaT(0.0), previousSensorFrame(), sensorFrame() {}; + + + /// The Madgwick filter is initialised with a sensor frame. + /// + /// \param sf A sensor frame; if zero, the sensor frame will be + /// initialised as an identity quaternion. + Madgwick(geom::Vector sf) : deltaT(0.0), previousSensorFrame() + { + if (!sf.isZero()) { + sensorFrame = geom::quaternion(sf, 0.0); + } + } + + + /// Initialise the filter with a sensor frame quaternion. + /// + /// \param sf A quaternion representing the current orientation. + Madgwick(geom::Quaternion sf) : + deltaT(0.0), previousSensorFrame(), sensorFrame(sf) {}; + + + /// Return the current orientation as measured by the filter. + /// + /// \return The current sensor frame. + geom::Quaternion + orientation() const + { + return this->sensorFrame; + } + + + /// Return the rate of change of the orientation of the earth frame + /// with respect to the sensor frame. + /// + /// \param gyro A three-dimensional vector containing gyro readings + /// as \f$<\omega_x, \omega_y, \omega_z\>f$. + /// \return A quaternion representing the rate of angular change. + geom::Quaternion + angularRate(const geom::Vector &gyro) const + { + return (this->sensorFrame * 0.5) * geom::Quaternion(gyro, 0.0); + } + + /// Update the sensor frame to a new frame. + /// + void + updateFrame(const geom::Quaternion &sf, T delta) + { + this->previousSensorFrame = this->sensorFrame; + this->sensorFrame = sf; + this->deltaT = delta; + } + + + /// Update the sensor frame with a gyroscope reading. + /// + /// \param gyro A three-dimensional vector containing gyro readings + /// as \f$<\omega_x, \omega_y, \omega_z\>f$. + /// \param delta The time step between readings. It must not be zero. + void + updateAngularOrientation(const geom::Vector &gyro, T delta) + { + assert(!math::WithinTolerance(delta, 0.0, 0.001)); + geom::Quaternion q = this->angularRate(gyro) * delta; + + this->updateFrame(this->sensorFrame + q, delta); + } + +private: + T deltaT; + geom::Quaternion previousSensorFrame; + geom::Quaternion sensorFrame; +}; + + +} // namespace filter +} // namespace wr + + +#endif // __WRMATH_FILTER_MADGWICK_H diff --git a/include/wrmath/geom/quaternion.h b/include/wrmath/geom/quaternion.h index 6755bfe..e27af9c 100644 --- a/include/wrmath/geom/quaternion.h +++ b/include/wrmath/geom/quaternion.h @@ -24,7 +24,7 @@ namespace geom { /// Quaternions encode rotations in three-dimensional space. While technically /// a quaternion is comprised of a real element and a complex vector<3>, for /// the purposes of this library, it is modeled as a floating point 4D vector -/// of the form , where x, y, and z represent an axis of rotation in +/// of the form , where x, y, and z represent an axis of rotation in /// R3 and w the angle, in radians, of the rotation about that axis. Where Euler /// angles are concerned, the ZYX (or yaw, pitch, roll) sequence is used. /// @@ -43,9 +43,7 @@ namespace geom { template class Quaternion { public: - /// /// The default Quaternion constructor returns an identity quaternion. - /// Quaternion() : v(Vector{0.0, 0.0, 0.0}), w(1.0) { wr::math::DefaultEpsilon(this->eps); @@ -56,6 +54,7 @@ public: /// A Quaternion may be initialised with a Vector axis of rotation /// and an angle of rotation. This doesn't do the angle transforms to simplify /// internal operations. + /// /// @param _axis A three-dimensional vector of the same type as the Quaternion. /// @param _angle The angle of rotation about the axis of rotation. Quaternion(Vector _axis, T _angle) : v(_axis), w(_angle) @@ -65,14 +64,14 @@ public: v.setEpsilon(this->eps); }; - /// + /// A Quaternion may be initialised with a Vector comprised of /// the axis of rotation followed by the angle of rotation. - /// @param vector A vector in the form . /// + /// @param vector A vector in the form . Quaternion(Vector vector) : - v(Vector{vector[0], vector[1], vector[2]}), - w(vector[3]) + v(Vector{vector[1], vector[2], vector[3]}), + w(vector[0]) { this->constrainAngle(); wr::math::DefaultEpsilon(this->eps); @@ -80,15 +79,16 @@ public: } - /// A Quaternion may be constructed with an initializer list of type T, which must have - /// exactly N element. - /// @param ilst An initial set of values in the form . + /// A Quaternion may be constructed with an initializer list of + /// type T, which must have exactly N elements. + /// + /// @param ilst An initial set of values in the form . Quaternion(std::initializer_list ilst) { auto it = ilst.begin(); - this->v = Vector{it[0], it[1], it[2]}; - this->w = it[3]; + this->v = Vector{it[1], it[2], it[3]}; + this->w = it[0]; this->constrainAngle(); wr::math::DefaultEpsilon(this->eps); @@ -97,6 +97,7 @@ public: /// Set the comparison tolerance for this quaternion. + /// /// @param epsilon A tolerance value. void setEpsilon(T epsilon) @@ -107,6 +108,7 @@ public: /// Return the axis of rotation of this quaternion. + /// /// @return The axis of rotation of this quaternion. Vector axis() const @@ -116,6 +118,7 @@ public: /// Return the angle of rotation of this quaternion. + /// /// @return the angle of rotation of this quaternion. T angle() const @@ -168,15 +171,17 @@ public: } /// Compute the conjugate of a quaternion. + /// /// @return The conjugate of this quaternion. Quaternion conjugate() const { - return Quaternion(Vector{-this->v[0], -this->v[1], -this->v[2], this->w}); + return Quaternion(Vector{this->w, -this->v[0], -this->v[1], -this->v[2]}); } /// Compute the inverse of a quaternion. + /// /// @return The inverse of this quaternion. Quaternion inverse() const @@ -187,7 +192,18 @@ public: } + /// Determine whether this is an identity quaternion. + /// + /// \return true if this is an identity quaternion. + bool + isIdentity() const { + return this->v.isZero() && + math::WithinTolerance(this->w, (T)1.0, this->eps); + } + + /// Determine whether this is a unit quaternion. + /// /// @return true if this is a unit quaternion. bool isUnitQuaternion() const @@ -198,15 +214,17 @@ public: /// Return the quaternion as a Vector, with the axis of rotation /// followed by the angle of rotation. + /// /// @return A vector representation of the quaternion. Vector asVector() const { - return Vector{this->v[0], this->v[1], this->v[2], this->w}; + return Vector{this->w, this->v[0], this->v[1], this->v[2]}; } /// Rotate vector v about this quaternion. + /// /// @param v The vector to be rotated. /// @return The rotated vector. Vector @@ -219,6 +237,7 @@ public: /// Return the Euler angles for this quaternion as a vector of /// . Users of this function should watch out /// for gimbal lock. + /// /// @return A vector containing Vector euler() const @@ -238,6 +257,7 @@ public: /// Perform quaternion addition with another quaternion. + /// /// @param other The quaternion to be added with this one. /// @return The result of adding the two quaternions together. Quaternion @@ -248,6 +268,7 @@ public: /// Perform quaternion subtraction with another quaternion. + /// /// @param other The quaternion to be subtracted from this one. /// @return The result of subtracting the other quaternion from this one. Quaternion @@ -258,6 +279,7 @@ public: /// Perform scalar multiplication. + /// /// @param k The scaling value. /// @return A scaled quaternion. Quaternion @@ -268,6 +290,7 @@ public: /// Perform scalar division. + /// /// @param k The scalar divisor. /// @return A scaled quaternion. Quaternion @@ -280,6 +303,7 @@ public: /// Perform quaternion Hamilton multiplication with a three- /// dimensional vector; this is done by treating the vector /// as a pure quaternion (e.g. with an angle of rotation of 0). + /// /// @param vector The vector to multiply with this quaternion. /// @return The Hamilton product of the quaternion and vector. Quaternion @@ -291,6 +315,7 @@ public: /// Perform quaternion Hamilton multiplication. + /// /// @param other The other quaternion to multiply with this one. /// @result The Hamilton product of the two quaternions. Quaternion @@ -317,6 +342,7 @@ public: /// Perform quaternion inequality checking. + /// /// @param other The quaternion to check inequality against. /// @return True if the two quaternions are unequal within their tolerance. bool @@ -328,6 +354,7 @@ public: /// Support stream output of a quaternion in the form `a + `. /// \todo improve the formatting. + /// /// @param outs An output stream /// @param q A quaternion /// @return The output stream @@ -375,6 +402,7 @@ typedef Quaternion Quaterniond; /// Return a float quaternion scaled appropriately from a vector and angle, /// e.g. angle = cos(angle / 2), axis.unitVector() * sin(angle / 2). +/// /// @param axis The axis of rotation. /// @param angle The angle of rotation. /// @return A quaternion. @@ -384,6 +412,7 @@ Quaternionf quaternionf(Vector3f axis, float angle); /// Return a double quaternion scaled appropriately from a vector and angle, /// e.g. angle = cos(angle / 2), axis.unitVector() * sin(angle / 2). +/// /// @param axis The axis of rotation. /// @param angle The angle of rotation. /// @return A quaternion. @@ -391,8 +420,25 @@ Quaternionf quaternionf(Vector3f axis, float angle); Quaterniond quaterniond(Vector3d axis, double angle); +/// Return a double quaternion scaled appropriately from a vector and angle, +/// e.g. angle = cos(angle / 2), axis.unitVector() * sin(angle / 2). +/// +/// @param axis The axis of rotation. +/// @param angle The angle of rotation. +/// @return A quaternion. +/// @relatesalso Quaternion +template +Quaternion +quaternion(Vector axis, T angle) +{ + return Quaternion(axis.unitVector() * std::sin(angle / (T)2.0), + std::cos(angle / (T)2.0)); +} + + /// Given a vector of Euler angles in ZYX sequence (e.g. yaw, pitch, roll), /// return a quaternion. +/// /// @param euler A vector Euler angle in ZYX sequence. /// @return A Quaternion representation of the orientation represented /// by the Euler angles. @@ -402,6 +448,7 @@ Quaternionf quaternionf_from_euler(Vector3f euler); /// Given a vector of Euler angles in ZYX sequence (e.g. yaw, pitch, roll), /// return a quaternion. +/// /// @param euler A vector Euler angle in ZYX sequence. /// @return A Quaternion representation of the orientation represented /// by the Euler angles. diff --git a/src/madgwick.cc b/src/madgwick.cc new file mode 100644 index 0000000..4967861 --- /dev/null +++ b/src/madgwick.cc @@ -0,0 +1 @@ +#include "wrmath/filter/madgwick.h" diff --git a/src/quaternion.cc b/src/quaternion.cc index 3cfa4ae..f035ae6 100644 --- a/src/quaternion.cc +++ b/src/quaternion.cc @@ -1,3 +1,4 @@ +#include #include @@ -39,7 +40,7 @@ quaternionf_from_euler(Vector3f euler) z = (cos_yaw * cos_pitch * sin_roll) + (sin_yaw * sin_pitch * cos_roll); w = (cos_yaw * cos_pitch * cos_roll) - (sin_yaw * sin_pitch * sin_roll); - return Quaternionf(Vector4f{x, y, z, w}); + return Quaternionf(Vector4f{w, x, y, z}); } @@ -61,7 +62,7 @@ quaterniond_from_euler(Vector3d euler) z = (cos_yaw * cos_pitch * sin_roll) + (sin_yaw * sin_pitch * cos_roll); w = (cos_yaw * cos_pitch * cos_roll) - (sin_yaw * sin_pitch * sin_roll); - return Quaterniond(Vector4d{x, y, z, w}); + return Quaterniond(Vector4d{w, x, y, z}); } @@ -78,6 +79,7 @@ Quaternion_SelfTest() Vector3f vr {0.0, 0.0, 1.0}; assert(p.isUnitQuaternion()); + std::cerr << p.rotate(v) << std::endl; assert(p.rotate(v) == vr); assert(p * q == p); #endif diff --git a/test/madgwick_test.cc b/test/madgwick_test.cc new file mode 100644 index 0000000..44564de --- /dev/null +++ b/test/madgwick_test.cc @@ -0,0 +1,34 @@ +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace wr; + + +TEST(MadgwickFilter, SimpleAngularOrientation) +{ + filter::Madgwick mf; + geom::Vector3d gyro {0.17453292519943295, 0.0, 0.0}; // 10° X rotation. + geom::Quaterniond frame20Deg {0.984808, 0.173648, 0, 0}; // 20° final orientation. + double delta = 0.00917; // assume 109 updates per second, as per the paper. + + // The paper specifies a minimum of 109 IMU readings to stabilize; for + // two seconds, that means 218 updates. + for (int i = 0; i < 218; i++) { + mf.updateAngularOrientation(gyro, delta); + } + + EXPECT_EQ(mf.orientation(), frame20Deg); +} + + +int +main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/quaternion_test.cc b/test/quaternion_test.cc index 9771f23..33c6afc 100644 --- a/test/quaternion_test.cc +++ b/test/quaternion_test.cc @@ -15,9 +15,9 @@ TEST(Quaternion, SelfTest) TEST(Quaterniond, Addition) { - geom::Quaterniond p(geom::Vector4d {1.0, -2.0, 1.0, 3.0}); - geom::Quaterniond q(geom::Vector4d {-1.0, 2.0, 3.0, 2.0}); - geom::Quaterniond expected(geom::Vector4d{0.0, 0.0, 4.0, 5.0}); + geom::Quaterniond p(geom::Vector4d {3.0, 1.0, -2.0, 1.0}); + geom::Quaterniond q(geom::Vector4d {2.0, -1.0, 2.0, 3.0}); + geom::Quaterniond expected(geom::Vector4d{5.0, 0.0, 0.0, 4.0}); EXPECT_EQ(p + q, expected); EXPECT_EQ(expected - q, p); @@ -27,8 +27,8 @@ TEST(Quaterniond, Addition) TEST(Quaterniond, Conjugate) { - geom::Quaterniond p(geom::Vector4d {3.0, 4.0, 5.0, 2.0}); - geom::Quaterniond q(geom::Vector4d {-3.0, -4.0, -5.0, 2.0}); + geom::Quaterniond p {2.0, 3.0, 4.0, 5.0}; + geom::Quaterniond q {2.0, -3.0, -4.0, -5.0}; EXPECT_EQ(p.conjugate(), q); } @@ -45,17 +45,18 @@ TEST(Quaterniond, Euler) TEST(Quaterniond, Identity) { - geom::Quaterniond p(geom::Vector4d {1.0, -2.0, 1.0, 3.0}); + geom::Quaterniond p {3.0, 1.0, -2.0, 1.0}; geom::Quaterniond q; + EXPECT_TRUE(q.isIdentity()); EXPECT_EQ(p * q, p); } TEST(Quaterniond, Inverse) { - geom::Quaterniond p(geom::Vector4d {3.0, 4.0, 5.0, 2.0}); - geom::Quaterniond q(geom::Vector4d {-0.05556, -0.07407, -0.09259, 0.03704 }); + geom::Quaterniond p {2.0, 3.0, 4.0, 5.0}; + geom::Quaterniond q {0.03704, -0.05556, -0.07407, -0.09259}; EXPECT_EQ(p.inverse(), q); } @@ -63,8 +64,7 @@ TEST(Quaterniond, Inverse) TEST(Quaterniond, Norm) { - geom::Quaterniond p(geom::Vector4d {0.9899139811480784, 9.387110042325054, 6.161341707794767, - 5.563199889674063}); + geom::Quaterniond p {5.563199889674063, 0.9899139811480784, 9.387110042325054, 6.161341707794767}; double norm = 12.57016663729933; EXPECT_DOUBLE_EQ(p.norm(), norm); @@ -73,9 +73,9 @@ TEST(Quaterniond, Norm) TEST(Quaterniond, Product) { - geom::Quaterniond p(geom::Vector4d {1.0, -2.0, 1.0, 3.0}); - geom::Quaterniond q(geom::Vector4d {-1.0, 2.0, 3.0, 2.0}); - geom::Quaterniond expected(geom::Vector4d{-9.0, -2.0, 11.0, 8.0}); + geom::Quaterniond p {3.0, 1.0, -2.0, 1.0}; + geom::Quaterniond q {2.0, -1.0, 2.0, 3.0}; + geom::Quaterniond expected {8.0, -9.0, -2.0, 11.0}; EXPECT_EQ(p * q, expected); } @@ -111,10 +111,10 @@ TEST(Quaterniond, ShortestSLERP) { // Our starting point is an orientation that is yawed 45° - our // orientation is pointed π/4 radians in the X axis. - geom::Quaterniond p {0.382683, 0, 0, 0.92388}; + geom::Quaterniond p {0.92388, 0.382683, 0, 0}; // Our ending point is an orientation that is yawed -45° - or // pointed -π/4 radians in the X axis. - geom::Quaterniond q {-0.382683, 0, 0, 0.92388}; + geom::Quaterniond q {0.92388, -0.382683, 0, 0}; // The halfway point should be oriented midway about the X axis. It turns // out this is an identity quaternion. geom::Quaterniond r; @@ -129,13 +129,13 @@ TEST(Quaterniond, ShortestSLERP2) { // Start with an orientation pointing forward, all Euler angles // set to 0. - geom::Quaterniond start {0.0, 0.0, 0.0, 1.0}; + geom::Quaterniond start {1.0, 0.0, 0.0, 0.0}; // The goal is to end up face up, or 90º pitch (still facing forward). - geom::Quaterniond end {0, -0.707107, 0, 0.707107}; + geom::Quaterniond end {0.707107, 0, -0.707107, 0}; // Halfway to the endpoint should be a 45º pitch. - geom::Quaterniond halfway {0, -0.382683, 0, 0.92388 }; + geom::Quaterniond halfway {0.92388, 0, -0.382683, 0}; // 2/3 of the way should be 60º pitch. - geom::Quaterniond twoThirds {0, -0.5, 0, 0.866025}; + geom::Quaterniond twoThirds {0.866025, 0, -0.5, 0}; EXPECT_EQ(ShortestSLERP(start, end, 0.0), start); EXPECT_EQ(ShortestSLERP(start, end, 1.0), end); @@ -146,7 +146,7 @@ TEST(Quaterniond, ShortestSLERP2) TEST(Quaterniond, Unit) { - geom::Quaterniond q(geom::Vector4d{0.5773502691896258, 0.5773502691896258, 0.5773502691896258, 0.0}); + geom::Quaterniond q {0.0, 0.5773502691896258, 0.5773502691896258, 0.5773502691896258}; EXPECT_TRUE(q.isUnitQuaternion()); } @@ -157,7 +157,7 @@ TEST(Quaterniond, UtilityCreator) geom::Vector3d v {1.0, 1.0, 1.0}; double w = M_PI; geom::Quaterniond p = geom::quaterniond(v, w); - geom::Quaterniond q(geom::Vector4d{0.5773502691896258, 0.5773502691896258, 0.5773502691896258, 0.0}); + geom::Quaterniond q {0.0, 0.5773502691896258, 0.5773502691896258, 0.5773502691896258}; EXPECT_EQ(p, q); } @@ -165,9 +165,9 @@ TEST(Quaterniond, UtilityCreator) TEST(Quaternionf, Addition) { - geom::Quaternionf p(geom::Vector4f {1.0, -2.0, 1.0, 3.0}); - geom::Quaternionf q(geom::Vector4f {-1.0, 2.0, 3.0, 2.0}); - geom::Quaternionf expected(geom::Vector4f{0.0, 0.0, 4.0, 5.0}); + geom::Quaternionf p {3.0, 1.0, -2.0, 1.0}; + geom::Quaternionf q {2.0, -1.0, 2.0, 3.0}; + geom::Quaternionf expected {5.0, 0.0, 0.0, 4.0}; EXPECT_EQ(p + q, expected); EXPECT_EQ(expected - q, p); @@ -177,8 +177,8 @@ TEST(Quaternionf, Addition) TEST(Quaternionf, Conjugate) { - geom::Quaternionf p(geom::Vector4f {3.0, 4.0, 5.0, 2.0}); - geom::Quaternionf q(geom::Vector4f {-3.0, -4.0, -5.0, 2.0}); + geom::Quaternionf p {2.0, 3.0, 4.0, 5.0}; + geom::Quaternionf q {2.0, -3.0, -4.0, -5.0}; EXPECT_EQ(p.conjugate(), q); } @@ -195,7 +195,7 @@ TEST(Quaternionf, Euler) TEST(Quaternionf, Identity) { - geom::Quaternionf p(geom::Vector4f {1.0, -2.0, 1.0, 3.0}); + geom::Quaternionf p {1.0, 3.0, 1.0, -2.0}; geom::Quaternionf q; EXPECT_EQ(p * q, p); @@ -204,8 +204,8 @@ TEST(Quaternionf, Identity) TEST(Quaternionf, Inverse) { - geom::Quaternionf p(geom::Vector4f {3.0, 4.0, 5.0, 2.0}); - geom::Quaternionf q(geom::Vector4f {-0.05556, -0.07407, -0.09259, 0.03704 }); + geom::Quaternionf p {2.0, 3.0, 4.0, 5.0}; + geom::Quaternionf q {0.03704, -0.05556, -0.07407, -0.09259}; EXPECT_EQ(p.inverse(), q); } @@ -213,10 +213,7 @@ TEST(Quaternionf, Inverse) TEST(Quaternionf, Norm) { - geom::Quaternionf p(geom::Vector4f {0.9899139811480784, - 9.387110042325054, - 6.161341707794767, - 5.563199889674063}); + geom::Quaternionf p {0.9899139811480784, 9.387110042325054, 6.161341707794767, 5.563199889674063}; float norm = 12.57016663729933; EXPECT_FLOAT_EQ(p.norm(), norm); @@ -225,9 +222,9 @@ TEST(Quaternionf, Norm) TEST(Quaternionf, Product) { - geom::Quaternionf p(geom::Vector4f {1.0, -2.0, 1.0, 3.0}); - geom::Quaternionf q(geom::Vector4f {-1.0, 2.0, 3.0, 2.0}); - geom::Quaternionf expected(geom::Vector4f{-9.0, -2.0, 11.0, 8.0}); + geom::Quaternionf p {3.0, 1.0, -2.0, 1.0}; + geom::Quaternionf q {2.0, -1.0, 2.0, 3.0}; + geom::Quaternionf expected {8.0, -9.0, -2.0, 11.0}; EXPECT_EQ(p * q, expected); } @@ -251,10 +248,10 @@ TEST(Quaternionf, ShortestSLERP) { // Our starting point is an orientation that is yawed 45° - our // orientation is pointed π/4 radians in the X axis. - geom::Quaternionf p {0.382683, 0, 0, 0.92388}; + geom::Quaternionf p {0.92388, 0.382683, 0, 0}; // Our ending point is an orientation that is yawed -45° - or // pointed -π/4 radians in the X axis. - geom::Quaternionf q {-0.382683, 0, 0, 0.92388}; + geom::Quaternionf q {0.92388, -0.382683, 0, 0}; // The halfway point should be oriented midway about the X axis. It turns // out this is an identity quaternion. geom::Quaternionf r; @@ -269,13 +266,13 @@ TEST(Quaternionf, ShortestSLERP2) { // Start with an orientation pointing forward, all Euler angles // set to 0. - geom::Quaternionf start {0.0, 0.0, 0.0, 1.0}; + geom::Quaternionf start {1.0, 0.0, 0.0, 0.0}; // The goal is to end up face up, or 90º pitch (still facing forward). - geom::Quaternionf end {0, -0.707107, 0, 0.707107}; + geom::Quaternionf end {0.707107, 0, -0.707107, 0}; // Halfway to the endpoint should be a 45º pitch. - geom::Quaternionf halfway {0, -0.382683, 0, 0.92388 }; + geom::Quaternionf halfway {0.92388, 0, -0.382683, 0}; // 2/3 of the way should be 60º pitch. - geom::Quaternionf twoThirds {0, -0.5, 0, 0.866025}; + geom::Quaternionf twoThirds {0.866025, 0, -0.5, 0}; EXPECT_EQ(ShortestSLERP(start, end, (float)0.0), start); EXPECT_EQ(ShortestSLERP(start, end, (float)1.0), end); @@ -286,7 +283,7 @@ TEST(Quaternionf, ShortestSLERP2) TEST(Quaternionf, Unit) { - geom::Quaternionf q(geom::Vector4f{0.5773502691896258, 0.5773502691896258, 0.5773502691896258, 0.0}); + geom::Quaternionf q {0.0, 0.5773502691896258, 0.5773502691896258, 0.5773502691896258}; EXPECT_TRUE(q.isUnitQuaternion()); } @@ -297,7 +294,7 @@ TEST(Quaternionf, UtilityCreator) geom::Vector3f v {1.0, 1.0, 1.0}; float w = M_PI; geom::Quaternionf p = geom::quaternionf(v, w); - geom::Quaternionf q(geom::Vector4f{0.5773502691896258, 0.5773502691896258, 0.5773502691896258, 0.0}); + geom::Quaternionf q {0.0, 0.5773502691896258, 0.5773502691896258, 0.5773502691896258}; EXPECT_EQ(p, q); } @@ -305,20 +302,22 @@ TEST(Quaternionf, UtilityCreator) TEST(QuaternionMiscellaneous, SanityChecks) { - geom::Vector4d q {1.0, 2.0, 3.0, 4.0}; + geom::Vector4d q {4.0, 1.0, 2.0, 3.0}; geom::Vector3d v {1.0, 2.0, 3.0}; double w = 4.0; geom::Quaterniond p(q); + geom::Quaterniond u = p.unitQuaternion(); EXPECT_EQ(p.axis(), v); EXPECT_DOUBLE_EQ(p.angle(), w); + EXPECT_TRUE(u.isUnitQuaternion()); } TEST(QuaternionMiscellaneous, OutputStream) { - geom::Quaternionf p(geom::Vector4f {1.0, 2.0, 3.0, 4.0}); - geom::Quaterniond q(geom::Vector4d {1.0, 2.0, 3.0, 4.0}); + geom::Quaternionf p {4.0, 1.0, 2.0, 3.0}; + geom::Quaterniond q {4.0, 1.0, 2.0, 3.0}; stringstream ss; ss << p; diff --git a/tools/euler2quat.cc b/tools/euler2quat.cc index 210be94..f6a6fce 100644 --- a/tools/euler2quat.cc +++ b/tools/euler2quat.cc @@ -34,12 +34,12 @@ convertEulerToQuat(char **argv) static void convertQuatToEuler(char **argv) { - double x = stod(string(argv[0])); - double y = stod(string(argv[1])); - double z = stod(string(argv[1])); - double w = stod(string(argv[1])); + double x = stod(string(argv[1])); + double y = stod(string(argv[2])); + double z = stod(string(argv[3])); + double w = stod(string(argv[0])); - geom::Quaterniond quaternion {x, y, z, w}; + geom::Quaterniond quaternion {w, x, y, z}; auto euler = quaternion.euler() * (180.0 / M_PI); cout << "Euler ZYX: " << euler << endl; diff --git a/tools/quaternion.cc b/tools/quaternion.cc new file mode 100644 index 0000000..ba41fad --- /dev/null +++ b/tools/quaternion.cc @@ -0,0 +1,28 @@ +#include +#include +#include +#include +#include + +using namespace std; +using namespace wr::geom; + + +int +main(int argc, char **argv) +{ + if (argc != 5) { + cerr << "Usage: quaternion w x y z" << endl; + return EXIT_FAILURE; + } + + double w = stod(string(argv[1])); + double x = stod(string(argv[2])); + double y = stod(string(argv[3])); + double z = stod(string(argv[4])); + + Vector3d frame {x, y, z}; + Quaterniond quat = quaternion(frame, w); + + cout << quat << endl; +} \ No newline at end of file