MC_quaternion.cpp

Go to the documentation of this file.

#include <cmath>
#include <MC_double_vector.hpp>
#include <MC_v3d.hpp>
#include <MC_v3d_vector.hpp>
#include <MC_matrix.hpp>

#include <MC_quaternion.hpp>

namespace mesh_conv
{

    MC_quaternion::MC_quaternion()
            :MC_v4d(1,0,0,0)
    {}
    MC_quaternion::MC_quaternion(const MC_v3d& axis,const double& angle)
            :MC_v4d(0,0,0,0)
    {
        double sin_phi=sin(angle/2.0);
        MC_v3d n_axis=axis.normalized();
        *this=MC_v4d(n_axis[0]*sin_phi,n_axis[1]*sin_phi,n_axis[2]*sin_phi,cos(angle/2.0));
    }
    MC_quaternion::MC_quaternion(const MC_v4d& v)
            :MC_v4d(v)
    {}
    MC_quaternion::MC_quaternion(const MC_quaternion& q)
            :MC_v4d(q[0],q[1],q[2],q[3])
    {}
    MC_quaternion::MC_quaternion(const double& x,const double& y,const double& z,const double& w)
            :MC_v4d(x,y,z,w)
    {}

    MC_matrix MC_quaternion::matrix() const
    {
        double x2 = (*this)[0]*(*this)[0];
        double y2 = (*this)[1]*(*this)[1];
        double z2 = (*this)[2]*(*this)[2];
        double xy = (*this)[0]*(*this)[1];
        double xz = (*this)[0]*(*this)[2];
        double yz = (*this)[1]*(*this)[2];
        double wx = (*this)[3]*(*this)[0];
        double wy = (*this)[3]*(*this)[1];
        double wz = (*this)[3]*(*this)[2];

        MC_matrix M(3,3);
        M(0,0) = 1-2*(y2+z2);
        M(1,0) =   2*(xy+wz);
        M(2,0) =   2*(xz-wy);

        M(0,1) =   2*(xy-wz);
        M(1,1) = 1-2*(x2+z2);
        M(2,1) =   2*(yz+wx);

        M(0,2) =   2*(xz+wy);
        M(1,2) =   2*(yz-wx);
        M(2,2) = 1-2*(x2+y2);

        return M;
    }

    MC_quaternion MC_quaternion::conjugated() const
    {
        MC_quaternion q2;
        for(int k_dim=0;k_dim<3;k_dim++)
            q2[k_dim]=-(*this)[k_dim];
        q2[3]=(*this)[3];
        return q2;
    }
    MC_quaternion operator*(const MC_quaternion& q0,const MC_quaternion& q1)
    {
        MC_quaternion q2;
        double w0=q0[3],w1=q1[3];
        MC_v3d v0=MC_v3d(q0[0],q0[1],q0[2]);
        MC_v3d v1=MC_v3d(q1[0],q1[1],q1[2]);

        q2[3]=w0*w1-v0.dot(v1);
        q2.set_v3d(w0*v1+w1*v0-v0.cross(v1));

        return q2;
    }
    MC_quaternion& MC_quaternion::operator*=(const MC_quaternion& q1)
    {

        double w0=(*this)[3],w1=q1[3];
        MC_v3d v0=MC_v3d((*this)[0],(*this)[1],(*this)[2]);
        MC_v3d v1=MC_v3d(q1[0],q1[1],q1[2]);

        (*this)[3]=w0*w1-v0.dot(v1);
        set_v3d(w0*v1+w1*v0-v0.cross(v1));

        return *this;
    }

    MC_v3d MC_quaternion::axis() const
    {return MC_v3d((*this)(0),(*this)(1),(*this)(2)).normalized();}
    double MC_quaternion::angle() const
    {return 2*acos((*this)[3]);}
    MC_quaternion MC_quaternion::slerp(const MC_quaternion& q0,const MC_quaternion& q1,const double& t)
    {
        double cos_theta = q0.dot(q1);
        double theta = acos(cos_theta);
        double sin_theta = sqrt(1-cos_theta*cos_theta);

        double epsilon=0.0001;
        if(fabs(sin_theta)<epsilon)
        {std::cout<<"Warning in Quaternion::SLERP, sin(theta) is zero"<<std::endl;return q0;}

        MC_quaternion interpolated = (sin((1-t)*theta)*q0 + sin(t*theta)*q1) / sin_theta;
        return interpolated;
    }

    MC_quaternion MC_quaternion::pow(const double& t) const
    {return MC_quaternion::exp( t * log());}
    MC_quaternion MC_quaternion::inverted() const
    {
        double n=norm();
        double epsilon=0.000001;
        if(n<epsilon)
        {std::cout<<"Error in Quaternion::invert(), norm is zero"<<std::endl;exit(-1);}

        MC_quaternion Q = conjugated()/(n*n);
        return Q;
    }
    MC_v3d MC_quaternion::log() const
    {

        double n=sqrt(1-(*this)[3]*(*this)[3]);
        double acos_w = acos((*this)[3]);

        double epsilon=0.0001;
        if(n<epsilon)
            return MC_v3d(0,0,0);

        return acos_w/n*MC_v3d((*this)[0],(*this)[1],(*this)[2]);
    }
    MC_quaternion MC_quaternion::exp(const MC_v3d& v)
    {
        double epsilon=0.000001;
        double n=v.norm();
        if(n<epsilon)
            return MC_quaternion(0,0,0,1);

        double a = sin(n)/n;

        return MC_quaternion(a*v[0],a*v[1],a*v[2],cos(n));
    }



    MC_quaternion MC_quaternion::quat_interp(const std::vector <MC_quaternion>& quaternion_vec,const MC_double_vector& weights)
    {
        if(int(quaternion_vec.size())!=weights.size())
        {std::cout<<"Error in MC_quaternion::quat_interp(vector<MC_quaternion>,MC_double_vector), size are not compatible ("<<quaternion_vec.size()<<","<<weights.size()<<")"<<std::endl;exit(-1);}

        MC_quaternion temp(0,0,0,0);
        int N=quaternion_vec.size();
        for(int k=0;k<N;k++)
            temp += weights[k]*quaternion_vec[k];
        temp /= temp.norm();
        return temp;
    }

    MC_matrix MC_quaternion::quat_interp(const std::vector <MC_quaternion>& quaternion_vec,const MC_double_vector& weights,const MC_v3d_vector& translation_vec)
    {
        MC_matrix res(4,4);

        if(weights.size()!=translation_vec.size())
        {std::cout<<"Error in MC_quaternion::quat_interp(vector<MC_quaternion>("<<quaternion_vec.size()<<"),MC_double_vector("<<weights.size()<<"),MC_v3d_vector("<<translation_vec.size()<<")), size are not compatible"<<std::endl;exit(-1);}

        //first do the rotation
        MC_quaternion rot = MC_quaternion::quat_interp(quaternion_vec,weights);
        res.set_block(0,2,0,2,rot.matrix());

        //then the translation by linear interpolation
        MC_v3d tr = MC_v3d_vector::sum(weights*translation_vec);
        res.set_block(0,2,3,3,tr);
        res(3,3)=1;

        return res;

    }

    MC_matrix MC_quaternion::quat_interp(const std::vector <MC_matrix>& matrix_vec,const MC_double_vector& weights)
    {
        if(static_cast<int>(matrix_vec.size()) !=weights.size())
        {std::cout<<"Error in MC_quaternion::quat_interp(vector<MC_matrix>,MC_double_vector), size are not compatible ("<<matrix_vec.size()<<","<<weights.size()<<")"<<std::endl;exit(-1);}

        int N=matrix_vec.size();

        // rotation
        std::vector <MC_quaternion> quaternion_vec(N);
        for(int k=0;k<N;k++)
            quaternion_vec[k] = MC_quaternion(matrix_vec[k]);
        MC_quaternion res = MC_quaternion::quat_interp(quaternion_vec,weights);

        // translation
        MC_v3d tr;
        for(int k=0;k<N;k++)
            tr += weights[k]*(matrix_vec[k].translation_part());


        MC_matrix interpolated_matrix(4,4);
        interpolated_matrix.set_block(0,2,0,2,res.matrix());
        interpolated_matrix.set_block(0,2,3,3,tr);


        return interpolated_matrix;

    }


}