Add SLERP test and euler2quat.

CMakeLists.txt

@@ -29,11 +29,16 @@ include_directories(include)
 file(GLOB_RECURSE ${PROJECT_NAME}_HEADERS include/**.h)
 file(GLOB_RECURSE ${PROJECT_NAME}_SOURCES src/*.cc)
 
-message("${PROJECT_NAME}_SOURCES -> libwrmath")
+message("${${PROJECT_NAME}_SOURCES}	 -> libwrmath")
 
 ## BUILD
 
-add_library(lib${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES})
+add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES})
+add_executable(euler2quat tools/euler2quat.cc)
+target_link_libraries(euler2quat ${PROJECT_NAME})
+set_target_properties(${TESTNAME} PROPERTIES
+		FOLDER bin
+		RUNTIME_OUTPUT_DIRECTORY bin)
 
 ## INSTALL
 
@@ -62,7 +67,7 @@ include(CTest)
 set(TEST_EXECS)
 macro(package_add_gtest TESTNAME)
 	add_executable(${TESTNAME} ${ARGN})
-	target_link_libraries(${TESTNAME} gtest_main lib${PROJECT_NAME})
+	target_link_libraries(${TESTNAME} gtest_main ${PROJECT_NAME})
 	target_compile_options(${TESTNAME} PUBLIC ${GTEST_CFLAGS})
 	add_test(NAME ${TESTNAME} COMMAND ${TESTNAME})
 	set_target_properties(${TESTNAME} PROPERTIES

docs/CMakeLists.txt

@@ -1,7 +1,7 @@
 find_package(Doxygen REQUIRED)
 
 # Find all the public headers
-get_target_property(WRMATH_PUBLIC_HEADER_DIR libwrmath INTERFACE_INCLUDE_DIRECTORIES)
+get_target_property(WRMATH_PUBLIC_HEADER_DIR wrmath INTERFACE_INCLUDE_DIRECTORIES)
 file(GLOB_RECURSE WRMATH_PUBLIC_HEADERS ${WRMATH_PUBLIC_HEADER_DIR}/*.h)
 
 #This will be the main output of our command

include/wrmath/geom/quaternion.h

@@ -7,6 +7,7 @@
 #include <cassert>
 #include <cmath>
 #include <initializer_list>
+#include <iostream>
 #include <ostream>
 #include <wrmath/geom/vector.h>
 #include <wrmath/math.h>
@@ -123,8 +124,25 @@ public:
 	}
 
 
+	/// Compute the dot product of two quaternions.
+	///
+	/// \param other Another quaternion.
+	/// \return The dot product between the two quaternions.
+	T
+	dot(const Quaternion<T> &other) const
+	{
+		double	innerProduct = this->v[0] * other.v[0];
+
+		innerProduct += (this->v[1] * other.v[1]);
+		innerProduct += (this->v[2] * other.v[2]);
+		innerProduct += (this->w * other.w);
+		return innerProduct;
+	}
+
+
 	/// Compute the norm of a quaternion. Treating the Quaternion as a
 	/// Vector<T, 4>, it's the same as computing the magnitude.
+	///
 	/// @return A non-negative real number.
 	T
 	norm() const
@@ -140,6 +158,15 @@ public:
 	}
 
 
+	/// Return the unit quaternion.
+	///
+	/// \return The unit quaternion.
+	Quaternion
+	unitQuaternion()
+	{
+		return *this / this->norm();
+	}
+
 	/// Compute the conjugate of a quaternion.
 	/// @return The conjugate of this quaternion.
 	Quaternion
@@ -393,7 +420,11 @@ Quaterniond	quaterniond_from_euler(Vector3d euler);
 /// \return A Quaternion representing the linear interpolation of the
 ///	    two quaternions.
 template <typename T>
-Quaternion<T>	LERP(Quaternion<T> p, Quaternion<T> q, T t);
+Quaternion<T>
+LERP(Quaternion<T> p, Quaternion<T> q, T t)
+{
+	return (p + (q - p) * t).unitQuaternion();
+}
 
 
 /// ShortestSLERP computes the shortest distance spherical linear
@@ -402,13 +433,30 @@ Quaternion<T>	LERP(Quaternion<T> p, Quaternion<T> q, T t);
 ///
 /// \tparam T
 /// \param p The starting quaternion.
-/// \param q The ending quaternion.
+/// \param q The ending quaternion.Short
 /// \param t The fraction of the distance between the two quaternions
 ///	     to interpolate.
 /// \return A Quaternion representing the shortest path between two
 ///         quaternions.
 template <typename T>
-Quaternion<T>	ShortestSLERP(Quaternion<T> p, Quaternion<T> q, T t);
+Quaternion<T>
+ShortestSLERP(Quaternion<T> p, Quaternion<T> q, T t)
+{
+	assert(p.isUnitQuaternion());
+	assert(q.isUnitQuaternion());
+
+	T	dp = p.dot(q);
+	T	sign = dp < 0.0 ? -1.0 : 1.0;
+	T	omega = std::acos(dp * sign);
+	T	sin_omega = std::sin(omega); // Compute once.
+
+	if (dp > 0.99999) {
+		return LERP(p, q * sign, t);
+	}
+
+	return (p * std::sin((1.0 - t) * omega) / sin_omega) +
+	       (q * sign * std::sin(omega*t) / sin_omega);
+}
 
 
 /// Run a quick self test to exercise basic functionality of the Quaternion

src/quaternion.cc

@@ -65,26 +65,6 @@ quaterniond_from_euler(Vector3d euler)
 }
 
 
-template <typename T>
-Quaternion<T>
-LERP(Quaternion<T> p, Quaternion<T> q, T t)
-{
-	return p + (q - p) * t;
-}
-
-
-template <typename T>
-Quaternion<T>
-ShortestSLERP(Quaternion<T> p, Quaternion<T> q, T t)
-{
-	T	innerProduct = p.dot(q);
-	T	sign = innerProduct >= 0.0 ? -1.0 : 1.0;
-	T	acip = std::acos(innerProduct);
-
-	return (p * std::sin((T)1.0 - t) * acip + p * sign * std::sin(t * acip)) / std::sin(acip);
-}
-
-
 void
 Quaternion_SelfTest()
 {

test/quaternion_test.cc

@@ -107,6 +107,19 @@ TEST(Quaterniond, Rotate)
 }
 
 
+TEST(Quaterniond, ShortestSLERP)
+{
+	geom::Quaterniond	p = geom::Quaterniond {0.382683, 0, 0, 0.92388};
+	geom::Quaterniond	q = geom::Quaterniond {-0.382683, 0, 0, 0.92388};
+	geom::Quaterniond	r = geom::Quaterniond {0, 0, 0, 1};
+
+
+	EXPECT_EQ(geom::ShortestSLERP(p, q, 0.0), p);
+	EXPECT_EQ(geom::ShortestSLERP(p, q, 1.0), q);
+	EXPECT_EQ(geom::ShortestSLERP(p, q, 0.5), r);
+}
+
+
 TEST(Quaterniond, Unit)
 {
 	geom::Quaterniond	q(geom::Vector4d{0.5773502691896258, 0.5773502691896258, 0.5773502691896258, 0.0});

tools/euler2quat.cc

@@ -0,0 +1,64 @@
+#include <cstdlib>
+#include <iostream>
+#include <string>
+#include <wrmath/math.h>
+#include <wrmath/geom/quaternion.h>
+
+using namespace std;
+using namespace wr;
+
+
+static void
+usage(ostream& outs)
+{
+	outs << "Print conversions between Euler angles and quaternions." << endl;
+	outs << "Usage: euler2quat yaw pitch roll" << endl;
+	outs << "       euler2quat x y z w" << endl;
+}
+
+
+static void
+convertEulerToQuat(char **argv)
+{
+	double		yaw = math::DegreesToRadiansD(stod(string(argv[0])));
+	double		pitch = math::DegreesToRadiansD(stod(string(argv[1])));
+	double		roll = math::DegreesToRadiansD(stod(string(argv[2])));
+
+	geom::Vector3d	euler {yaw, pitch, roll};
+	auto		quaternion = geom::quaterniond_from_euler(euler);
+
+	cout << "Quaternion: " << quaternion.asVector() << endl;
+}
+
+
+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]));
+
+	geom::Quaterniond	quaternion {x, y, z, w};
+	auto			euler = quaternion.euler() * (180.0 / M_PI);
+
+	cout << "Euler ZYX: " << euler << endl;
+}
+
+
+int
+main(int argc, char **argv)
+{
+	if ((argc != 4) && (argc != 5)) {
+		usage(cerr);
+		return EXIT_FAILURE;
+	}
+
+	argv++;
+	if (argc == 4) {
+		convertEulerToQuat(argv);
+	}
+	else {
+		convertQuatToEuler(argv);
+	}
+}