Quaternion.hpp

Go to the documentation of this file.

#ifndef SPIDAR_QUATERNION_HPP
#define SPIDAR_QUATERNION_HPP

#include <cmath>

#include "Vector.hpp"

namespace Spidar
{
namespace Math
 {
template <class T>
struct Quaternion
{
    typedef T               ValueType;      
    typedef Vector<T, 3>    VectorType;     

    ValueType x, y, z, w;                   

    // コンストラクタ
    Quaternion(void)
    {
        x = y = z = 0;
        w = 1;
    }

    Quaternion(const Quaternion& other)
    {
        x = other.x;
        y = other.y;
        z = other.z;
        w = other.w;
    }

    explicit Quaternion(const VectorType& v)
    {
        w = 0;
        x = v[0];
        y = v[1];
        z = v[2];
    }

    template <class T>
    explicit Quaternion(T x, T y, T z, T w)
    {
        this->x = static_cast<ValueType>(x);
        this->y = static_cast<ValueType>(y);
        this->z = static_cast<ValueType>(z);
        this->w = static_cast<ValueType>(w);
    }

    Quaternion(ValueType angle, const VectorType &axis)
    {
        w = static_cast<ValueType>(cos(0.5*angle));

        ValueType s = static_cast<ValueType>(sin(0.5*angle) / axis.norm());

        x = axis[0] * s;
        y = axis[1] * s;
        z = axis[2] * s;
    }

    template <class OtherType>
    explicit Quaternion(const OtherType &other)
    {
        x = static_cast<ValueType>(other.x);
        y = static_cast<ValueType>(other.y);
        z = static_cast<ValueType>(other.z);
        w = static_cast<ValueType>(other.w);
    }

    Quaternion& operator=(const Quaternion &other)
    {
        x = other.x;
        y = other.y;
        z = other.z;
        w = other.w;

        return *this;
    }

    template <class OtherType>
    Quaternion& operator=(const OtherType &other)
    {
        x = static_cast<ValueType>(other.x);
        y = static_cast<ValueType>(other.y);
        z = static_cast<ValueType>(other.z);
        w = static_cast<ValueType>(other.w);

        return *this;
    }

    Quaternion& operator+=(const Quaternion &other)
    {
        x += static_cast<ValueType>(other.x);
        y += static_cast<ValueType>(other.y);
        z += static_cast<ValueType>(other.z);
        w += static_cast<ValueType>(other.w);

        return *this;
    }

    Quaternion& operator-=(const Quaternion &other)
    {
        x -= static_cast<ValueType>(other.x);
        y -= static_cast<ValueType>(other.y);
        z -= static_cast<ValueType>(other.z);
        w -= static_cast<ValueType>(other.w);

        return *this;
    }

    Quaternion& operator*=(double value)
    {
        x = static_cast<ValueType>(value * x);
        y = static_cast<ValueType>(value * y);
        z = static_cast<ValueType>(value * z);
        w = static_cast<ValueType>(value * w);

        return *this;
    }

    Quaternion& operator*=(const Quaternion &other)
    {
        Quaternion temp;

        temp.x = w * other.x + x * other.w + y * other.z - z * other.y;
        temp.y = w * other.y + y * other.w + z * other.x - x * other.z;
        temp.z = w * other.z + z * other.w + x * other.y - y * other.x;
        temp.w = w * other.w - x * other.x - y * other.y - z * other.z;

        *this = temp;

        return *this;
    }

    Quaternion& operator/=(double value)
    {
        x = static_cast<ValueType>(x / value);
        y = static_cast<ValueType>(y / value);
        z = static_cast<ValueType>(z / value);
        w = static_cast<ValueType>(w / value);

        return *this;
    }

    //
    ValueType angle(void) const
    {
        double temp = w;
        double check = 1;
        temp = std::min(temp, check);
        check = -1;
        temp = std::max(temp, check);
        return static_cast<ValueType>(2.0 * std::acos(temp));
    }

    // 
    VectorType axis(void) const
    {
        VectorType &r = static_cast<VectorType>(*this);
        ValueType norm = r.norm();

        if (norm > 0)   return r / norm;
        else            return VectorType(1, 0, 0);
    }

    //
    const ValueType dot(const Quaternion &other) const
    {
        return x * other.x + y * other.y + z * other.z + w * other.w;
    }

    //
    const ValueType norm(void) const
    {
        return static_cast<ValueType>(std::sqrt(static_cast<double>(dot(*this))));
    }

    //
    const Quaternion unit(void) const
    {
        double norm = std::sqrt(static_cast<double>(dot(*this)));

        if (norm>0)     return *this / norm;
        else            return *this;
    }

    //
    Quaternion conjugated(void) const 
    {
        Quaternion q(*this);
        q.x = -q.x;
        q.y = -q.y;
        q.z = -q.z;
        return q;
    }
    
    //
    Quaternion inv(void) const
    {
        Quaternion q(conjugated());

        ValueType temp = dot(*this);
        
        q.x /= temp;
        q.y /= temp;
        q.z /= temp;
        q.w /= temp;

        return q;   
    }

    //
    const Quaternion derivative(const VectorType &avel) const
    {
        return 0.5 * Quaternion(avel) * (*this);
    }

    //
    const VectorType angularVelocity(const Quaternion &qd) const
    {
        return *reinterpret_cast<const VectorType*>(&(qd * conjugated())) * 2.0;
    }

    //
    const VectorType rotate(const VectorType &v) const
    {
        return *reinterpret_cast<const VectorType*>(&(*this * Quaternion(v) * conjugated()));
    }

}; // end of struct Quaternion.

//
template <class T>
std::ostream& operator<<(std::ostream &os, const Quaternion<T> &q)
{
    os << "(" << q.w << " " << q.x << " " << q.y << " " << q.z << ")";

    return os;
}

// 
template <class T>
const Quaternion<T> operator+(Quaternion<T> lhs, const Quaternion<T> &rhs)
{
    return lhs += rhs;
}

// 
template <class T>
const Quaternion<T> operator-(Quaternion<T> lhs, const Quaternion<T> &rhs)
{
    return lhs -= rhs;
}

// 
template <class T>
const Quaternion<T> operator*(Quaternion<T> lhs, double rhs)
{
    return lhs *= rhs;
}

// 
template <class T>
const Quaternion<T> operator*(double lhs, Quaternion<T> rhs)
{
    return rhs *= lhs;
}

// 
template <class T>
const Quaternion<T> operator*(Quaternion<T> lhs, const Quaternion<T> &rhs)
{
    return lhs *= rhs;
}

// 
template <class T>
const Quaternion<T> operator/(Quaternion<T> lhs, double rhs)
{
    return lhs /= rhs;
}

//
template <class T>
const Quaternion<T> slerp(const Quaternion<T> &from, const Quaternion<T> &to, double t)
{
    typedef Quaternion<T> QuaternionType;
    typedef typename QuaternionType::ValueType ValueType;

    QuaternionType q;

    double dot = from.dot(to);
   
    if (dot < 0)
    {
        dot = -dot;
        q = Quaternion<T>(-to.x, -to.y, -to.z, -to.w);
    }
    else q = to;
    
    if (dot >  1)   dot = 1;
    if (dot < -1)   dot = -1;
    
    double angle = static_cast<ValueType>(std::acos(dot));
    ValueType sina = static_cast<ValueType>(std::sin(angle));
    ValueType sinat = static_cast<ValueType>(std::sin(angle * t));
    ValueType sinaomt = static_cast<ValueType>(std::sin(angle * (1.0-t)));
    
    if (sina)
    {
        return QuaternionType((from.x * sinaomt + q.x * sinat) / sina,
                              (from.y * sinaomt + q.y * sinat) / sina,
                              (from.z * sinaomt + q.z * sinat) / sina,
                              (from.w * sinaomt + q.w * sinat) / sina);
    }
    else
    {
        return from;
    }
}

} // end of namespace Math.
} // end of namespace Spidar.

#endif // SPIDAR_QUATERNION_HPP

// end of file.