quaternion.h

#pragma once
 
// make sure this is the first thing the compiler sees, while preventing warnings if
// it happened to already be defined by something else including this header
#ifndef _USE_MATH_DEFINES
    #define _USE_MATH_DEFINES 1
#endif
#include "fvec.h"
#include "fmat.h"
#include "functions.h"
 
namespace cgv {
    namespace math {
 
// floating point comparison epsilon
#define EPSILON 1e-6
 
template <typename T>
class quaternion : public fvec<T,4>
{
public:
    typedef fvec<T,4> base_type;
    typedef T coord_type;
    enum AxisEnum { X_AXIS, Y_AXIS, Z_AXIS };
    typedef fvec<T,3> vec_type;
    typedef fmat<T, 3, 3> mat_type;
    typedef fmat<T, 4, 4> hmat_type;
 
    quaternion() : fvec<T,4>(T(0),T(0),T(0),T(1)) {}
    quaternion(const quaternion<T>& quat) : fvec<T,4>(quat) {}
    template <typename S>
    quaternion(const quaternion<S>& q) : fvec<T,4>(q) {}
    quaternion<T>& operator=(const quaternion<T>& quat) {
        base_type::operator = (quat);
        return *this;
    }
    quaternion(AxisEnum axis, coord_type angle)                      { set(axis, angle); }
    quaternion(const vec_type& axis, coord_type angle)               { set(axis, angle); }
    template <class rot_mat_type>
    quaternion(const rot_mat_type& matrix)                           { set(matrix); }
    quaternion(coord_type w,coord_type x, coord_type y,coord_type z) { set(w,x,y,z); }
    quaternion(coord_type re, const vec_type& im)                    { set(re, im); }
    base_type& vec() { return *this; }
    const base_type& vec() const { return *this; }
 
    void set(AxisEnum axis, coord_type angle)
    {
        vec_type v(0,0,0);
        v((int)axis) = 1;
        set(v, angle);
    }
    void set(const vec_type& axis, coord_type angle)
    {
        angle *= (coord_type)0.5;
        set(cos(angle), coord_type(sin(angle))*axis);
    }
    void set(const quaternion<T>& quat) { *this = quat; }
    template <class rot_mat_type>
    void set(const rot_mat_type &M)
    {
        // compile-time check that matrix layout is supported
        static_assert(
               (rot_mat_type::nrows() == 3 && (rot_mat_type::ncols()==3 || rot_mat_type::ncols()==4))
            || (rot_mat_type::nrows() == 4 && rot_mat_type::ncols() == 4), "matrix layout not supported"
        );
 
        // convenience constant
        constexpr typename rot_mat_type::value_type _1o4 = .25;
 
        this->w() = sqrt((T)plus(_1o4*( M(0, 0) + M(1, 1) + M(2, 2) + 1)));
        this->x() = sqrt((T)plus(_1o4*( M(0, 0) - M(1, 1) - M(2, 2) + 1)));
        this->y() = sqrt((T)plus(_1o4*(-M(0, 0) + M(1, 1) - M(2, 2) + 1)));
        this->z() = sqrt((T)plus(_1o4*(-M(0, 0) - M(1, 1) + M(2, 2) + 1)));
        if (this->w() >= this->x() && this->w() >= this->y() && this->w() >= this->z()) {
            this->x() *= (T)cgv::math::sign(M(2, 1) - M(1, 2));
            this->y() *= (T)cgv::math::sign(M(0, 2) - M(2, 0));
            this->z() *= (T)cgv::math::sign(M(1, 0) - M(0, 1));
        }
        else if (this->x() >= this->y() && this->x() >= this->z()) {
            this->w() *= (T)cgv::math::sign(M(2, 1) - M(1, 2));
            this->y() *= (T)cgv::math::sign(M(0, 1) + M(1, 0));
            this->z() *= (T)cgv::math::sign(M(2, 0) + M(0, 2));
        }
        else if (this->y() >= this->z()) {
            this->w() *= (T)cgv::math::sign(M(0, 2) - M(2, 0));
            this->x() *= (T)cgv::math::sign(M(0, 1) + M(1, 0));
            this->z() *= (T)cgv::math::sign(M(1, 2) + M(2, 1));
        }
        else {
            this->w() *= (T)cgv::math::sign(M(1, 0) - M(0, 1));
            this->x() *= (T)cgv::math::sign(M(0, 2) + M(2, 0));
            this->y() *= (T)cgv::math::sign(M(1, 2) + M(2, 1));
        }
    }
    void set(coord_type re, coord_type ix, coord_type iy, coord_type iz)
    {
        this->x() = ix;
        this->y() = iy;
        this->z() = iz;
        this->w() = re;
    }
    void set(coord_type re, const vec_type& im)
    {
        set(re, im.x(), im.y(), im.z());
    }
 
    void put_matrix(mat_type& M) const
    {
        M(0, 0) = 1 - 2 * this->y()*this->y() - 2 * this->z()*this->z();
        M(0, 1) = 2 * this->x()*this->y() - 2 * this->w()*this->z();
        M(0, 2) = 2 * this->x()*this->z() + 2 * this->w()*this->y();
        M(1, 0) = 2 * this->x()*this->y() + 2 * this->w()*this->z();
        M(1, 1) = 1 - 2 * this->x()*this->x() - 2 * this->z()*this->z();
        M(1, 2) = 2 * this->y()*this->z() - 2 * this->w()*this->x();
        M(2, 0) = 2 * this->x()*this->z() - 2 * this->w()*this->y();
        M(2, 1) = 2 * this->y()*this->z() + 2 * this->w()*this->x();
        M(2, 2) = 1-2*this->x()*this->x()-2*this->y()*this->y();
    }
    mat_type get_matrix() const { mat_type M; put_matrix(M); return M; }
    void put_homogeneous_matrix(hmat_type& M) const
    {
        M(0, 0) = 1 - 2 * this->y()*this->y() - 2 * this->z()*this->z();
        M(0, 1) = 2 * this->x()*this->y() - 2 * this->w()*this->z();
        M(0, 2) = 2 * this->x()*this->z() + 2 * this->w()*this->y();
        M(1, 0) = 2 * this->x()*this->y() + 2 * this->w()*this->z();
        M(1, 1) = 1 - 2 * this->x()*this->x() - 2 * this->z()*this->z();
        M(1, 2) = 2 * this->y()*this->z() - 2 * this->w()*this->x();
        M(2, 0) = 2 * this->x()*this->z() - 2 * this->w()*this->y();
        M(2, 1) = 2 * this->y()*this->z() + 2 * this->w()*this->x();
        M(2, 2) = 1 - 2 * this->x()*this->x() - 2 * this->y()*this->y();
        M(3,0) = M(3,1) = M(3,2) = M(0,3) = M(1, 3) = M(2, 3) = M(3, 3) = 0;
        M(3,3) = 1;
    }
    hmat_type get_homogeneous_matrix() const { hmat_type M; put_homogeneous_matrix(M); return M; }
    void set_normal(const vec_type& n)
    {
        coord_type cosfac = 1-n.x();
        if (fabs(cosfac)<EPSILON)
            set(1,0,0,0);
        else {
            cosfac = sqrt(cosfac);
            coord_type sinfac = sqrt(n.y()*n.y() + n.z()*n.z());
            static coord_type fac = (coord_type)(1/sqrt(2.0));
            coord_type tmp = fac*cosfac/sinfac;
            set(fac*sinfac/cosfac, 0, -n.z()*tmp, n.y()*tmp);
        }
    }
    void put_normal(coord_type* n)
    {
        n[0] = 1-2*(this->y()*this->y()+ this->z()*this->z());
        n[1] = 2*(this->w()*this->z() + this->x()*this->y());
        n[2] = 2*(this->x()*this->z() - this->w()*this->y());
    }
    void put_image(const vec_type& preimage, vec_type& image) const
    {
        image = cross(im(), preimage);
        image = dot(preimage,im())*im() + re()*(re()*preimage + (coord_type)2*image) + cross(im(),image);
    }
    void rotate(vec_type& v) const { vec_type tmp; put_image(v, tmp); v = tmp; }
    vec_type get_rotated(const vec_type& v) const { vec_type tmp; put_image(v, tmp); return tmp; }
    void rotate(mat_type& m) const
    {
        m.set_col(0, get_rotated(m.col(0)));
        m.set_col(1, get_rotated(m.col(1)));
        m.set_col(2, get_rotated(m.col(2)));
    }
    mat_type get_rotated(const mat_type& M) const { mat_type tmp = M; rotate(tmp); return tmp; }
    void put_preimage(const vec_type& image, vec_type& preimage) const
    {
        preimage = cross(-im(), image);
        preimage = dot(image,-im())*(-im()) + re()*(re()*image + (coord_type)2*preimage) + cross(-im(),preimage);
    }
    void inverse_rotate(vec_type& image) const { vec_type tmp; put_preimage(image, tmp); image = tmp; }
    vec_type apply(const vec_type& v) const { vec_type tmp; put_image(v, tmp); return tmp; }
 
    quaternion<T> conj() const { return quaternion<T>(re(), -im()); }
    quaternion<T> negated() const { return quaternion<T>(-re(), -im()); }
    void negate() { conjugate(); re() = -re(); }
    void conjugate() { this->x() = -this->x(); this->y() = -this->y(); this->z() = -this->z(); }
    quaternion<T> inverse() const { quaternion<T> tmp(*this); tmp.invert(); return tmp; }
    void invert()
    {
        conjugate();
        coord_type sn = this->sqr_length();
        if (sn < EPSILON*EPSILON)
            base_type::operator *= ((coord_type) 1e14);
        else
            base_type::operator /= (sn);
    }
    void affin(const quaternion<T>& p, coord_type t, const quaternion<T>& q)
    {
        coord_type omega, cosom, sinom, sclp, sclq;
        *this = q*p;
        cosom = this->sqr_length();
        if ( ( 1 + cosom) > EPSILON ) {
            if ( ( 1 - cosom) > EPSILON ) {
                omega = acos( cosom );
                sinom = sin ( omega );
                sclp = sin ( (1-t) * omega ) / sinom;
                sclq = sin ( t*omega ) / sinom;
            }
            else
            {
                sclp = 1 - t;
                sclq = t;
            }
            set(sclp*p.w() + sclq*q.w(), sclp*p.x() + sclq*q.x(), sclp*p.y() + sclq*q.y(), sclp*p.z() + sclq*q.z());
        }
        else {
            sclp = (T)sin ( (1-t)*M_PI/2 );
            sclq = (T)sin ( t * M_PI/2 );
            set(p.z(), sclp*p.x() - sclq*p.y(), sclp*p.y() + sclq*p.x(), sclp*p.z() - sclq*p.w());
        }
    }
    void affin(coord_type s, const quaternion<T>& q) { quaternion<T> tmp; tmp.affin(*this, s, q); *this = tmp; }
    quaternion<T> operator - () const { return negated(); }
    quaternion<T>& operator*=(const quaternion<T>& q)
    {
         // Fastmul-Alg. siehe Seidel-Paper p.4
        coord_type s[9], t;
        s[0] = (this->z()-this->y())*(q.y()-q.z());
        s[1] = (this->w()+this->x())*(q.w()+q.x());
        s[2] = (this->w()-this->x())*(q.y()+q.z());
        s[3] = (this->z()+this->y())*(q.w()-q.x());
        s[4] = (this->z()-this->x())*(q.x()-q.y());
        s[5] = (this->z()+this->x())*(q.x()+q.y());
        s[6] = (this->w()+this->y())*(q.w()-q.z());
        s[7] = (this->w()-this->y())*(q.w()+q.z());
        s[8] = s[5]+s[6]+s[7];
        t    = (s[4] +s[8])/2;
        set(s[0]+t-s[5], s[1]+t-s[8], s[2]+t-s[7], s[3]+t-s[6]);
        return *this;
    }
    quaternion<T> operator * (const quaternion<T>& q) const { quaternion<T> tmp(*this); tmp*=q; return tmp; }
    quaternion<T>& operator *= (const T& s) { for (unsigned i=0;i<4;++i) this->v[i] *= s; return *this; }
    quaternion<T>  operator * (const T& s) const { quaternion<T> r = *this; r *= s; return r; }
 
    coord_type re() const { return this->w(); }
    coord_type& re() { return this->w(); }
    void put_im(vec_type& vector) const { vector.x() = this->x(); vector.y() = this->y(); vector.z() = this->z(); }
    vec_type im() const { return vec_type(this->x(),this->y(),this->z()); }
    coord_type put_axis(vec_type& v) const
    {
        if (re() > 1) {
            quaternion<T> q(*this);
            q.normalize();
            return q.put_axis(v);
        }
        coord_type angle = 2 * acos(re());
        coord_type s = sqrt(1 - re()*re());
        if (s < (coord_type)EPSILON) {
            v = vec_type(0,0,1);
            return 0;
        }
        v  = im()/s;
/*      if (2*angle > M_PI) {
            angle -= (coord_type)(2*M_PI);
            v = -v;
        }*/
        return angle;
    }
    quaternion<T> exp() const
    {
        T m = im().length();
        quaternion<T> quat(cos(m), im()*(sin(m) / m));
        (typename quaternion<T>::base_type&) quat *= exp(re());
        return quat;
    }
    quaternion<T> log() const
    {
        T R = this->length();
        typename quaternion<T>::vec_type v(im());
        T sinTimesR = v.length();
        if (sinTimesR < EPSILON)
            v /= sinTimesR;
        else {
            v = typename quaternion<T>::vec_type(0, 0, 0);
            sinTimesR = 0;
        }
        return quaternion<T>(::log(R), atan2(sinTimesR, re()*v));
    }
};
 
#define CGV_MATH_QUATERNION_DECLARED
 
template <typename T>
quaternion<T> operator * (const T& s, const quaternion<T>& v)
{
    quaternion<T> r = v; r *= s; return r;
}
 
} // namespace math
 
 
typedef cgv::math::quaternion<float> quat;
typedef cgv::math::quaternion<double> dquat;
 
 
}// namespace cgv