Quaternion.h

Go to the documentation of this file.

 
 
#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 {
 
 
template<typename T>
class Quaternion {
public:
    Quaternion() : v(Vector<T, 3>{0.0, 0.0, 0.0}), w(1.0)
    {
        scmp::DefaultEpsilon(this->eps);
        v.SetEpsilon(this->eps);
    };
 
 
    Quaternion(Vector<T, 3> _axis, T _angle) : v(_axis), w(_angle)
    {
        this->constrainAngle();
        scmp::DefaultEpsilon(this->eps);
        v.SetEpsilon(this->eps);
    };
 
 
    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);
    }
 
    
    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);
    }
 
    
    void
    SetEpsilon(T epsilon)
    {
        this->eps = epsilon;
        this->v.SetEpsilon(epsilon);
    }
 
 
    Vector<T, 3>
    Axis() const
    {
        return this->v;
    }
 
 
    T
    Angle() const
    {
        return this->w;
    }
 
 
    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;
    }
 
 
    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);
    }
 
 
    Quaternion
    UnitQuaternion()
    {
        return *this / this->Norm();
    }
 
    Quaternion
    Conjugate() const
    {
        return Quaternion(Vector<T, 4>{this->w, -this->v[0], -this->v[1], -this->v[2]});
    }
 
 
    Quaternion
    Inverse() const
    {
        T _norm = this->Norm();
 
        return this->Conjugate() / (_norm * _norm);
    }
 
 
    bool
    IsIdentity() const {
        return this->v.IsZero() &&
               scmp::WithinTolerance(this->w, (T)1.0, this->eps);
    }
 
 
    bool
    IsUnitQuaternion() const
    {
        auto normal = this->Norm();
        return scmp::WithinTolerance(normal, (T) 1.0, this->eps);
    }
 
 
    Vector<T, 4>
    AsVector() const
    {
        return Vector<T, 4>{this->w, this->v[0], this->v[1], this->v[2]};
    }
 
 
    Vector<T, 3>
    Rotate(Vector<T, 3> vr) const
    {
        return (this->Conjugate() * vr * (*this)).Axis();
    }
 
 
    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};
    }
 
 
    Quaternion
    operator+(const Quaternion<T> &other) const
    {
        return Quaternion(this->v + other.v, this->w + other.w);
    }
 
 
    Quaternion
    operator-(const Quaternion<T> &other) const
    {
        return Quaternion(this->v - other.v, this->w - other.w);
    }
 
 
    Quaternion
    operator*(const T k) const
    {
        return Quaternion(this->v * k, this->w * k);
    }
 
 
    Quaternion
    operator/(const T k) const
    {
        return Quaternion(this->v / k, this->w / k);
    }
 
 
    Quaternion
    operator*(const Vector<T, 3> &vector) const
    {
        return Quaternion(vector * this->w + this->v.Cross(vector),
                  (T) 0.0);
    }
 
 
    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) &&
               (scmp::WithinTolerance(this->w, other.w, this->eps));
    }
 
 
    bool
    operator!=(const Quaternion<T> &other) const
    {
        return !(*this == other);
    }
 
 
    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);
        }
    }
};
 
 
 
typedef Quaternion<float> Quaternionf;
 
typedef Quaternion<double> Quaterniond;
 
 
Quaternionf MakeQuaternion(Vector3F axis, float angle);
 
Quaterniond MakeQuaternion(Vector3D axis, double angle);
 
 
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));
}
 
 
Quaternionf FloatQuaternionFromEuler(Vector3F euler);
 
 
Quaterniond DoubleQuaternionFromEuler(Vector3D euler);
 
 
template <typename T>
Quaternion<T>
LERP(Quaternion<T> p, Quaternion<T> q, T t)
{
    return (p + (q - p) * t).UnitQuaternion();
}
 
 
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);
}
 
 
void        QuaternionSelfTest();
 
 
} // namespace geom
} // namespace wr
 
 
#endif // SCMATH_GEOM_QUATERNION_H