scsl/include/scmp/geom/Quaternion.h

///
/// \file include/scmp/geom/Quaternion.h
/// \author K. Isom <kyle@imap.cc>
/// \date 2019-08-05
/// \brief Quaternion implementation suitable for navigation in R3.
///
/// Copyright 2019 K. Isom <kyle@imap.cc>
///
/// Permission to use, copy, modify, and/or distribute this software for
/// any purpose with or without fee is hereby granted, provided that
/// the above copyright notice and this permission notice appear in all /// copies.
///
/// THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
/// WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
/// WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE
/// AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL
/// DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA
/// OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER
/// TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
/// PERFORMANCE OF THIS SOFTWARE.
///

#ifndef SCMATH_GEOM_QUATERNION_H
#define SCMATH_GEOM_QUATERNION_H


#include <cassert>
#include <cmath>
#include <initializer_list>
#include <iostream>
#include <ostream>

#include <scmp/Math.h>
#include <scmp/geom/Vector.h>


namespace scmp {
namespace geom {


/// \brief Quaternions provide a representation of Orientation
///        and rotations in three dimensions.
///
/// 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 <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.
///
/// For information on the underlying vector type, see the
/// documentation for scmp::geom::Vector.
///
/// Like vectors, quaternions carry an internal tolerance value ε that
/// is used for floating point comparisons. The math namespace contains
/// the default values used for this; generally, a tolerance of 0.0001
/// is considered appropriate for the uses of this library. The
/// tolerance can be explicitly set with the SetEpsilon method.
template<typename T>
class Quaternion {
public:
	/// \brief Construct an identity Quaternion.
	Quaternion() : v(Vector<T, 3>{0.0, 0.0, 0.0}), w(1.0)
	{
		scmp::DefaultEpsilon(this->eps);
		v.SetEpsilon(this->eps);
	};


	/// \brief Construct a Quaternion with an Axis and Angle of
	///        rotation.
	///
	/// 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)
	{
		this->constrainAngle();
		scmp::DefaultEpsilon(this->eps);
		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 <w, x, y, z>.
	Quaternion(Vector<T, 4> vector) :
		v(Vector<T, 3>{vector[1], vector[2], vector[3]}),
		w(vector[0])
	{
		this->constrainAngle();
		scmp::DefaultEpsilon(this->eps);
		v.SetEpsilon(this->eps);
	}

	
	/// \brief An initializer list containing values for w, x, y,
	///        and z.
	///
	/// \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[1], it[2], it[3]};
		this->w = it[0];

		this->constrainAngle();
		scmp::DefaultEpsilon(this->eps);
		v.SetEpsilon(this->eps);
	}

	
	/// \brief Set the comparison tolerance for this Quaternion.
	///
	/// \param epsilon A tolerance value.
	void
	SetEpsilon(T epsilon)
	{
		this->eps = epsilon;
		this->v.SetEpsilon(epsilon);
	}


	/// \brief Return the Axis of rotation of this Quaternion.
	///
	/// \return The Axis of rotation of this Quaternion.
	Vector<T, 3>
	Axis() const
	{
		return this->v;
	}


	/// \brief Return the Angle of rotation of this Quaternion.
	///
	/// \return the Angle of rotation of this Quaternion.
	T
	Angle() const
	{
		return this->w;
	}


	/// \brief 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;
	}


	/// \brief Compute the Norm of a Quaternion.
	///
	/// Treating the Quaternion as a Vector<T, 4>, this is the same
	/// process as computing the Magnitude.
	///
	/// \return A non-negative real number.
	T
	Norm() const
	{
		T n = 0;

		n += (this->v[0] * this->v[0]);
		n += (this->v[1] * this->v[1]);
		n += (this->v[2] * this->v[2]);
		n += (this->w * this->w);

		return std::sqrt(n);
	}


	/// \brief Return the unit Quaternion.
	///
	/// \return The unit Quaternion.
	Quaternion
	UnitQuaternion()
	{
		return *this / this->Norm();
	}

	/// \brief Compute the Conjugate of a Quaternion.
	///
	/// \return The Conjugate of this Quaternion.
	Quaternion
	Conjugate() const
	{
		return Quaternion(Vector<T, 4>{this->w, -this->v[0], -this->v[1], -this->v[2]});
	}


	/// \brief Compute the Inverse of a Quaternion.
	///
	/// \return The Inverse of this Quaternion.
	Quaternion
	Inverse() const
	{
		T _norm = this->Norm();

		return this->Conjugate() / (_norm * _norm);
	}


	/// \brief Determine whether this is an identity Quaternion.
	///
	/// \return true if this is an identity Quaternion.
	bool
	IsIdentity() const {
		return this->v.IsZero() &&
		       scmp::WithinTolerance(this->w, (T)1.0, this->eps);
	}


	/// \brief Determine whether this is a unit Quaternion.
	///
	/// \return true if this is a unit Quaternion.
	bool
	IsUnitQuaternion() const
	{
		auto normal = this->Norm();
		return scmp::WithinTolerance(normal, (T) 1.0, this->eps);
	}


	/// \brief Convert to Vector form.
	///
	/// 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->w, this->v[0], this->v[1], this->v[2]};
	}


	/// \brief Rotate Vector vr about this Quaternion.
	///
	/// \param vr The vector to be rotated.
	/// \return The rotated vector.
	Vector<T, 3>
	Rotate(Vector<T, 3> vr) const
	{
		return (this->Conjugate() * vr * (*this)).Axis();
	}


	/// \brief Return Euler angles for this Quaternion.
	///
	/// Return the Euler angles for this Quaternion as a vector of
	/// <yaw, pitch, roll>.
	///
	/// \warning Users of this function should watch out for gimbal
	///          lock.
	///
	/// \return A vector<T, 3> containing <yaw, pitch, roll>
	Vector<T, 3>
	Euler() const
	{
		T yaw, pitch, roll;
		T a = this->w, a2 = a * a;
		T b = this->v[0], b2 = b * b;
		T c = this->v[1], c2 = c * c;
		T d = this->v[2], d2 = d * d;

		yaw = std::atan2(2 * ((a * b) + (c * d)), a2 - b2 - c2 + d2);
		pitch = std::asin(2 * ((b * d) - (a * c)));
		roll = std::atan2(2 * ((a * d) + (b * c)), a2 + b2 - c2 - d2);

		return Vector<T, 3>{yaw, pitch, roll};
	}


	/// \brief Quaternion addition.
	///
	/// \param other The Quaternion to be added with this one.
	/// \return The result of adding the two quaternions together.
	Quaternion
	operator+(const Quaternion<T> &other) const
	{
		return Quaternion(this->v + other.v, this->w + other.w);
	}


	/// \brief Quaternion subtraction.
	///
	/// \param other The Quaternion to be subtracted from this one.
	/// \return The result of subtracting the other Quaternion from this one.
	Quaternion
	operator-(const Quaternion<T> &other) const
	{
		return Quaternion(this->v - other.v, this->w - other.w);
	}


	/// \brief Scalar multiplication.
	///
	/// \param k The scaling value.
	/// \return A scaled Quaternion.
	Quaternion
	operator*(const T k) const
	{
		return Quaternion(this->v * k, this->w * k);
	}


	/// \brief Scalar division.
	///
	/// \param k The scalar divisor.
	/// \return A scaled Quaternion.
	Quaternion
	operator/(const T k) const
	{
		return Quaternion(this->v / k, this->w / k);
	}


	/// \brief 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
	operator*(const Vector<T, 3> &vector) const
	{
		return Quaternion(vector * this->w + this->v.Cross(vector),
				  (T) 0.0);
	}


	/// \brief Quaternion Hamilton multiplication.
	///
	/// \param other The other Quaternion to multiply with this one.
	/// @result The Hamilton product of the two quaternions.
	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);
	}


	/// \brief Quaternion equivalence.
	///
	/// \param other The Quaternion to check equality against.
	/// \return True if the two quaternions are equal within their tolerance.
	bool
	operator==(const Quaternion<T> &other) const
	{
		return (this->v == other.v) &&
		       (scmp::WithinTolerance(this->w, other.w, this->eps));
	}


	/// \brief Quaternion non-equivalence.
	///
	/// \param other The Quaternion to check inequality against.
	/// \return True if the two quaternions are unequal within their tolerance.
	bool
	operator!=(const Quaternion<T> &other) const
	{
		return !(*this == other);
	}


	/// \brief Output a Quaternion to a stream in the form
	///        `a + <i, j, k>`.
	///
	/// \todo improve the formatting.
	///
	/// \param outs An output stream
	/// \param q A Quaternion
	/// \return The output stream
	friend std::ostream &
	operator<<(std::ostream &outs, const Quaternion<T> &q)
	{
		outs << q.w << " + " << q.v;
		return outs;
	}

private:
	static constexpr T minRotation = -4 * M_PI;
	static constexpr T maxRotation = 4 * M_PI;

	Vector<T, 3> v; // Axis of rotation
	T w; // Angle of rotation
	T eps;

	void
	constrainAngle()
	{
		if (this->w < 0.0) {
			this->w = std::fmod(this->w, this->minRotation);
		}
		else {
			this->w = std::fmod(this->w, this->maxRotation);
		}
	}
};


///
/// \defgroup quaternion_aliases Quaternion type aliases.
/// Type aliases are provided for float and double quaternions.
///

/// \ingroup quaternion_aliases
/// \brief Type alias for a float Quaternion.
typedef Quaternion<float> Quaternionf;

/// \ingroup quaternion_aliases
/// \brief Type alias for a double Quaternion.
typedef Quaternion<double> Quaterniond;


/// \brief Convenience Quaternion construction function.
///
/// 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.
/// \relatesalso Quaternion
Quaternionf	MakeQuaternion(Vector3F axis, float angle);

/// \brief Convience Quaternion construction function.
///
/// 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
Quaterniond	MakeQuaternion(Vector3D axis, double angle);


/// \brief Convience Quaternion construction function.
///
/// 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>
MakeQuaternion(Vector<T, 3> axis, T angle)
{
	return Quaternion<T>(axis.UnitVector() * std::sin(angle / (T)2.0),
			     std::cos(angle / (T)2.0));
}


/// \brief COnstruct a Quaternion from Euler angles.
///
/// 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.
/// \relatesalso Quaternion
Quaternionf	FloatQuaternionFromEuler(Vector3F euler);


/// \brief COnstruct a Quaternion from Euler angles.
///
/// 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.
/// \relatesalso Quaternion
Quaterniond	DoubleQuaternionFromEuler(Vector3D euler);


/// \brief Linear interpolation for two Quaternions.
///
/// LERP computes the linear interpolation of two quaternions At some
/// fraction of the distance between them.
///
/// \tparam T
/// \param p The starting Quaternion.
/// \param q The ending Quaternion.
/// \param t The fraction of the distance between the two quaternions to
///	     interpolate.
/// \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)
{
	return (p + (q - p) * t).UnitQuaternion();
}


/// \brief Shortest distance spherical linear interpolation.
///
/// ShortestSLERP computes the shortest distance spherical linear
/// interpolation between two unit quaternions At some fraction of the
/// distance between them.
///
/// \tparam T
/// \param p The starting Quaternion.
/// \param q The ending Quaternion.
/// \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)
{
	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);
}


/// \brief Internal consistency check.
///
/// Run a quick self test to exercise basic functionality of the Quaternion
/// class to verify correct operation. Note that if \#NDEBUG is defined, the
/// self test is disabled.
void		QuaternionSelfTest();


} // namespace geom
} // namespace wr


#endif // SCMATH_GEOM_QUATERNION_H