Start Madwick filters.

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

include/wrmath/filter/madgwick.h

@@ -0,0 +1,100 @@
+#ifndef __WRMATH_FILTER_MADGWICK_H
+#define __WRMATH_FILTER_MADGWICK_H
+
+
+#include <wrmath/geom/vector.h>
+#include <wrmath/geom/quaternion.h>
+
+
+namespace wr {
+namespace filter {
+
+
+/// Madgwick implements an efficient orientation filter for IMUs.
+///
+/// \tparam T A floating point type.
+template <typename T>
+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<T, 3> 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<T> sf) :
+		deltaT(0.0), previousSensorFrame(), sensorFrame(sf) {};
+
+
+	/// Return the current orientation as measured by the filter.
+	///
+	/// \return The current sensor frame.
+	geom::Quaternion<T>
+	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<T>
+	angularRate(const geom::Vector<T, 3> &gyro) const
+	{
+		return (this->sensorFrame * 0.5) * geom::Quaternion<T>(gyro, 0.0);
+	}
+
+	/// Update the sensor frame to a new frame.
+	///
+	void
+	updateFrame(const geom::Quaternion<T> &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<T, 3> &gyro, T delta)
+	{
+		assert(!math::WithinTolerance(delta, 0.0, 0.001));
+		geom::Quaternion<T>	q = this->angularRate(gyro) * delta;
+
+		this->updateFrame(this->sensorFrame + q, delta);
+	}
+
+private:
+	T			deltaT;
+	geom::Quaternion<T>	previousSensorFrame;
+	geom::Quaternion<T>	sensorFrame;
+};
+
+
+} // namespace filter
+} // namespace wr
+
+
+#endif // __WRMATH_FILTER_MADGWICK_H

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 <x, y, z, w>, where x, y, and z represent an axis of rotation in
+/// of the form <w, x, y, z>, 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<typename T>
 class Quaternion {
 public:
-	///
 	/// The default Quaternion constructor returns an identity quaternion.
-	///
 	Quaternion() : v(Vector<T, 3>{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<T, 3> 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<T, 3> _axis, T _angle) : v(_axis), w(_angle)
@@ -65,14 +64,14 @@ public:
 		v.setEpsilon(this->eps);
 	};
 
-	///
+
 	/// A Quaternion may be initialised with a Vector<T, 4> comprised of
 	/// the axis of rotation followed by the angle of rotation.
-	/// @param vector A vector in the form <x, y, z, w>.
 	///
+	/// @param vector A vector in the form <w, x, y, z>.
 	Quaternion(Vector<T, 4> vector) :
-		v(Vector<T, 3>{vector[0], vector[1], vector[2]}),
+		v(Vector<T, 3>{vector[1], vector[2], vector[3]}),
-		w(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
+	/// A Quaternion may be constructed with an initializer list of
-	/// exactly N element.
+	/// type T, which must have exactly N elements.
-	/// @param ilst An initial set of values in the form <x, y, z, w>.
+	///
+	/// @param ilst An initial set of values in the form <w, x, y, z>.
 	Quaternion(std::initializer_list<T> ilst)
 	{
 		auto it = ilst.begin();
 
-		this->v = Vector<T, 3>{it[0], it[1], it[2]};
+		this->v = Vector<T, 3>{it[1], it[2], it[3]};
-		this->w = 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<T, 3>
 	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<T, 4>{-this->v[0], -this->v[1], -this->v[2], this->w});
+		return Quaternion(Vector<T, 4>{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<T, 4>, with the axis of rotation
 	/// followed by the angle of rotation.
+	///
 	/// @return A vector representation of the quaternion.
 	Vector<T, 4>
 	asVector() const
 	{
-		return Vector<T, 4>{this->v[0], this->v[1], this->v[2], this->w};
+		return Vector<T, 4>{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<T, 3>
@@ -219,6 +237,7 @@ public:
 	/// Return the Euler angles for this quaternion as a vector of
 	/// <yaw, pitch, roll>. Users of this function should watch out
 	/// for gimbal lock.
+	///
 	/// @return A vector<T, 3> containing <yaw, pitch, roll>
 	Vector<T, 3>
 	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 + <i, j, k>`.
 	/// \todo improve the formatting.
+	///
 	/// @param outs An output stream
 	/// @param q A quaternion
 	/// @return The output stream
@@ -375,6 +402,7 @@ typedef Quaternion<double> 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 <typename T>
+Quaternion<T>
+quaternion(Vector<T, 3> axis, T angle)
+{
+	return Quaternion<T>(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.

src/madgwick.cc

@@ -0,0 +1 @@
+#include "wrmath/filter/madgwick.h"

src/quaternion.cc

@@ -1,3 +1,4 @@
+#include <iostream>
 #include <wrmath/geom/quaternion.h>
 
 
@@ -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

test/madgwick_test.cc

@@ -0,0 +1,34 @@
+#include <cmath>
+#include <sstream>
+#include <gtest/gtest.h>
+#include <wrmath/geom/vector.h>
+#include <wrmath/geom/quaternion.h>
+#include <wrmath/filter/madgwick.h>
+
+using namespace std;
+using namespace wr;
+
+
+TEST(MadgwickFilter, SimpleAngularOrientation)
+{
+	filter::Madgwick<double>	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();
+}

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	p(geom::Vector4d {3.0, 1.0, -2.0, 1.0});
-	geom::Quaterniond	q(geom::Vector4d {-1.0, 2.0, 3.0, 2.0});
+	geom::Quaterniond	q(geom::Vector4d {2.0, -1.0, 2.0, 3.0});
-	geom::Quaterniond	expected(geom::Vector4d{0.0, 0.0, 4.0, 5.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	p {2.0, 3.0, 4.0, 5.0};
-	geom::Quaterniond	q(geom::Vector4d {-3.0, -4.0, -5.0, 2.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	p {2.0, 3.0, 4.0, 5.0};
-	geom::Quaterniond	q(geom::Vector4d {-0.05556, -0.07407, -0.09259, 0.03704 });
+	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,
+	geom::Quaterniond	p {5.563199889674063, 0.9899139811480784, 9.387110042325054, 6.161341707794767};
-						  5.563199889674063});
 	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	p {3.0, 1.0, -2.0, 1.0};
-	geom::Quaterniond	q(geom::Vector4d {-1.0, 2.0, 3.0, 2.0});
+	geom::Quaterniond	q {2.0, -1.0, 2.0, 3.0};
-	geom::Quaterniond	expected(geom::Vector4d{-9.0, -2.0, 11.0, 8.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	p {3.0, 1.0, -2.0, 1.0};
-	geom::Quaternionf	q(geom::Vector4f {-1.0, 2.0, 3.0, 2.0});
+	geom::Quaternionf	q {2.0, -1.0, 2.0, 3.0};
-	geom::Quaternionf	expected(geom::Vector4f{0.0, 0.0, 4.0, 5.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	p {2.0, 3.0, 4.0, 5.0};
-	geom::Quaternionf	q(geom::Vector4f {-3.0, -4.0, -5.0, 2.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	p {2.0, 3.0, 4.0, 5.0};
-	geom::Quaternionf	q(geom::Vector4f {-0.05556, -0.07407, -0.09259, 0.03704 });
+	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,
+	geom::Quaternionf	p {0.9899139811480784, 9.387110042325054, 6.161341707794767, 5.563199889674063};
-						  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	p {3.0, 1.0, -2.0, 1.0};
-	geom::Quaternionf	q(geom::Vector4f {-1.0, 2.0, 3.0, 2.0});
+	geom::Quaternionf	q {2.0, -1.0, 2.0, 3.0};
-	geom::Quaternionf	expected(geom::Vector4f{-9.0, -2.0, 11.0, 8.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::Quaternionf	p {4.0, 1.0, 2.0, 3.0};
-	geom::Quaterniond	q(geom::Vector4d {1.0, 2.0, 3.0, 4.0});
+	geom::Quaterniond	q {4.0, 1.0, 2.0, 3.0};
 	stringstream		ss;
 
 	ss << p;

tools/euler2quat.cc

@@ -34,12 +34,12 @@ convertEulerToQuat(char **argv)
 static void
 convertQuatToEuler(char **argv)
 {
-	double	x = stod(string(argv[0]));
+	double	x = stod(string(argv[1]));
-	double	y = stod(string(argv[1]));
+	double	y = stod(string(argv[2]));
-	double	z = stod(string(argv[1]));
+	double	z = stod(string(argv[3]));
-	double	w = stod(string(argv[1]));
+	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;

tools/quaternion.cc

@@ -0,0 +1,28 @@
+#include <cstdlib>
+#include <iostream>
+#include <string>
+#include <wrmath/geom/vector.h>
+#include <wrmath/geom/quaternion.h>
+
+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;
+}