Clean up and document code.

src/orientation.cc

@@ -1,4 +1,4 @@
-#include <cmath>
+#include <wrmath/geom/vector.h>
 #include <wrmath/geom/orientation.h>
 
 
@@ -9,7 +9,7 @@ namespace geom {
 float
 Heading2f(Vector2f vec)
 {
-	return vec.angle(Basis2f[Basis_i]);
+	return vec.angle(Basis2f[Basis_x]);
 }
 
 
@@ -24,7 +24,7 @@ Heading3f(Vector3f vec)
 double
 Heading2d(Vector2d vec)
 {
-	return vec.angle(Basis2d[Basis_i]);
+	return vec.angle(Basis2d[Basis_x]);
 }
 
 

src/quaternion.cc

@@ -0,0 +1,108 @@
+#include <wrmath/geom/quaternion.h>
+
+
+namespace wr {
+namespace geom {
+
+
+Quaternionf
+quaternionf(Vector3f axis, float angle)
+{
+	return Quaternionf(axis.unitVector() * std::sin(angle / 2.0),
+			   std::cos(angle / 2.0));
+}
+
+
+Quaterniond
+quaterniond(Vector3d axis, double angle)
+{
+	return Quaterniond(axis.unitVector() * std::sin(angle / 2.0),
+			   std::cos(angle / 2.0));
+}
+
+
+Quaternionf
+quaternionf_from_euler(Vector3f euler)
+{
+	float x, y, z, w;
+	euler = euler / 2.0;
+
+	float cos_yaw = std::cos(euler[0]);
+	float cos_pitch = std::cos(euler[1]);
+	float cos_roll = std::cos(euler[2]);
+	float sin_yaw = std::sin(euler[0]);
+	float sin_pitch = std::sin(euler[1]);
+	float sin_roll = std::sin(euler[2]);
+
+	x = (sin_yaw * cos_pitch * cos_roll) + (cos_yaw * sin_pitch * sin_roll);
+	y = (sin_yaw * cos_pitch * sin_roll) - (cos_yaw * sin_pitch * cos_roll);
+	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});
+}
+
+
+Quaterniond
+quaterniond_from_euler(Vector3d euler)
+{
+	double x, y, z, w;
+	euler = euler / 2.0;
+
+	double cos_yaw = std::cos(euler[0]);
+	double cos_pitch = std::cos(euler[1]);
+	double cos_roll = std::cos(euler[2]);
+	double sin_yaw = std::sin(euler[0]);
+	double sin_pitch = std::sin(euler[1]);
+	double sin_roll = std::sin(euler[2]);
+
+	x = (sin_yaw * cos_pitch * cos_roll) + (cos_yaw * sin_pitch * sin_roll);
+	y = (sin_yaw * cos_pitch * sin_roll) - (cos_yaw * sin_pitch * cos_roll);
+	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});
+}
+
+
+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()
+{
+#ifndef NDEBUG
+	Vector3f		v {1.0, 0.0, 0.0};
+	Vector3f		yAxis {0.0, 1.0, 0.0};
+	float			angle = M_PI / 2;
+
+	Quaternionf		p = quaternionf(yAxis, angle);
+	Quaternionf		q;
+	Vector3f		vr {0.0, 0.0, 1.0};
+
+	assert(p.isUnitQuaternion());
+	assert(p.rotate(v) == vr);
+	assert(p * q == p);
+#endif
+}
+
+
+} // namespace geom
+} // namespace wr