Start quaternions.

CMakeLists.txt

@@ -68,6 +68,7 @@ endmacro()
 # define the tests
 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)
 
 add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} --verbose DEPENDS ${TEST_EXECS})
 

include/wrmath/geom/quaternion.h

@@ -0,0 +1,89 @@
+#ifndef __WRMATH_QUATERNION_H
+#define __WRMATH_QUATERNION_H
+
+
+#include <cassert>
+#include <cmath>
+#include <initializer_list>
+#include <wrmath/geom/vector.h>
+#include <wrmath/math.h>
+
+
+namespace wr {
+namespace geom {
+
+
+template <typename T>
+class Quaternion {
+public:
+	// The default constructor returns the identity quaternion.
+	Quaternion() : v(Vector<T, 3>()), w(1.0)
+	{
+		wr::math::DefaultEpsilon(this->eps);
+	};
+
+
+	Quaternion(Vector<T, 3> axis, T angle) : v(axis), w(angle)
+	{
+		wr::math::DefaultEpsilon(this->eps);
+	};
+
+
+	Quaternion
+	operator+(const Quaternion<T> &other) const
+	{
+		return Quaternion(this->v + other.v, (this->w + other.w) % this->maxRotation);
+	}
+
+
+	Quaternion
+	operator-(const Quaternion<T> &other) const
+	{
+	    return Quaternion(this->v - other.v, (this->w - other.w) % this->maxRotation);
+	}
+
+
+	Quaternion
+	operator*(const Quaternion<T> &other) const
+	{
+		T		angle = (this->w * other.w) -
+					(this->v * other.v);
+		Vector<T, 3>	axis = (other.v * this->w) +
+				       (this->v * other.w) +
+				       (this->v.cross(other.v));
+		return Quaternion(axis, angle);
+	}
+
+
+	bool
+	operator==(const Quaternion<T> &other) const
+	{
+		return (this->v == other.v) &&
+		       (wr::math::WithinTolerance(this->w, other.w, this->eps));
+	}
+
+
+	bool
+	operator!=(const Quaternion<T> &other) const
+	{
+		return !(*this == other);
+	}
+
+private:
+	static constexpr T	maxRotation = 4 * M_PI;
+
+	Vector<T, 3>	v; // axis of rotation
+	T		w; // angle of rotation
+	T		eps;
+};
+
+
+typedef Quaternion<float>	Quaternionf;
+typedef Quaternion<double>	Quaterniond;
+
+
+} // namespace geom
+} // namespace wr
+
+
+#endif // WRMATH_QUATERNION_H

include/wrmath/geom/vector.h

@@ -211,7 +211,7 @@ public:
 	 * @return The cross product vector.
 	 */
 	Vector
-	cross(const Vector<T, N> &other)
+	cross(const Vector<T, N> &other) const
 	{
 		assert(N == 3);
 		return Vector<T, N> {
@@ -228,7 +228,9 @@ public:
 	 * @return A new vector that is the result of adding this and the
 	 *         other vector.
 	 */
-	Vector operator+(const Vector<T, N> &other) const {
+	Vector
+	operator+(const Vector<T, N> &other) const
+	{
 		Vector<T, N>	vec;
 
 		for (size_t i = 0; i < N; i++) {
@@ -245,7 +247,9 @@ public:
 	 * @return A new vector that is the result of subtracting the
 	 *         other vector from this one.
 	 */
-	Vector operator-(const Vector<T, N> &other) const {
+	Vector
+	operator-(const Vector<T, N> &other) const
+	{
 		Vector<T, N>	vec;
 
 		for (size_t i = 0; i < N; i++) {
@@ -261,7 +265,9 @@ public:
 	 * @param k The scaling value.
 	 * @return A new vector that is this vector scaled by k.
 	 */
-	Vector operator*(const T k) const {
+	Vector
+	operator*(const T k) const
+	{
 		Vector<T, N>	vec;
 
 		for (size_t i = 0; i < N; i++) {
@@ -277,7 +283,9 @@ public:
 	 * @param k The scaling value
 	 * @return A new vector that is this vector scaled by 1/k.
 	 */
-	Vector operator/(const T k) const {
+	Vector
+	operator/(const T k) const
+	{
 		Vector<T, N>	vec;
 
 		for (size_t i = 0; i < N; i++) {
@@ -293,7 +301,9 @@ public:
 	 * @param other The other vector.
 	 * @return A scalar value that is the dot product of the two vectors.
 	 */
-	T operator*(const Vector<T, N> &other) const {
+	T
+	operator*(const Vector<T, N> &other) const
+	{
 		T	result = 0;
 
 		for (size_t i = 0; i < N; i++) {
@@ -310,7 +320,9 @@ public:
 	 * @return Return true if all the components of both vectors are
 	 *         within the tolerance value.
 	 */
-	bool operator==(const Vector<T, N> &other) const {
+	bool
+	operator==(const Vector<T, N> &other) const
+	{
 		for (size_t i = 0; i<N; i++) {
 			if (!wr::math::WithinTolerance(this->arr[i], other.arr[i], this->epsilon)) {
 				return false;
@@ -326,7 +338,9 @@ public:
 	 * @return Return true if any of the components of both vectors are
 	 *         not within the tolerance value.
 	 */
-	bool operator!=(const Vector<T, N> &other) const {
+	bool
+	operator!=(const Vector<T, N> &other) const
+	{
 		return !(*this == other);
 	}
 
@@ -336,7 +350,9 @@ public:
 	 * @param i The component index.
 	 * @return The value of the vector component at i.
 	 */
-	T operator[](size_t i) const {
+	T
+	operator[](size_t i) const
+	{
 		return this->arr[i];
 	}
 
@@ -347,7 +363,9 @@ public:
 	 * @param vec The vector to be formatted.
 	 * @return The output stream.
 	 */
-	friend std::ostream& operator<<(std::ostream& outs, const Vector<T, N>& vec) {
+	friend std::ostream&
+	operator<<(std::ostream& outs, const Vector<T, N>& vec)
+	{
 		outs << "<";
 		for (size_t i = 0; i < N; i++) {
 			outs << vec.arr[i];

test/quaternion_test.cc

@@ -0,0 +1,23 @@
+#include <gtest/gtest.h>
+#include <wrmath/geom/quaternion.h>
+
+using namespace std;
+using namespace wr;
+
+
+TEST(Quaterniond, Addition)
+{
+	geom::Quaterniond	p(geom::Vector3d {1.0, -2.0, 1.0}, 3.0);
+	geom::Quaterniond	q(geom::Vector3d {-1.0, 2.0, 3.0}, 2.0);
+	geom::Quaterniond	expected(geom::Vector3d{-9.0, -2.0, 11.0}, 8.0);
+
+	EXPECT_EQ(p * q, expected);
+}
+
+
+int
+main(int argc, char **argv)
+{
+    ::testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}