quat.cpp

Go to the documentation of this file.

/*
 *  quat.cpp - quaternion functions 
 *  Copyright (C) 2018, D Haley 

 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.

 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.

 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <gsl/gsl_blas.h>

#include <atomprobe/helper/maths/quat.h>
#include <atomprobe/helper/aptAssert.h>

using std::string;
using std::vector;

namespace AtomProbe
{
//needed for sincos
#ifdef __LINUX__ 
#ifdef __GNUC__
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif
#endif
#endif

//quaternion multiplication, assuming q2 has no "a" component
void quat_mult_no_second_a(Quaternion *result, const Quaternion *q1, const Quaternion *q2)
{
    result->a = (-q1->b*q2->b-q1->c*q2->c -q1->d*q2->d);
    result->b = (q1->a*q2->b +q1->c*q2->d -q1->d*q2->c);
    result->c = (q1->a*q2->c -q1->b*q2->d +q1->d*q2->b);
    result->d = (q1->a*q2->d +q1->b*q2->c -q1->c*q2->b );
}

//this is a little optimisation that doesn't calculate the "a" component for
//the returned quaternion, and implicitly performs conjugation. 
//Note that the result is specific to quaternion rotation 
void quat_pointmult(Point3f *result, const Quaternion *q1, const Quaternion *q2)
{
    result->fx = (-q1->a*q2->b +q1->b*q2->a -q1->c*q2->d +q1->d*q2->c);
    result->fy = (-q1->a*q2->c +q1->b*q2->d +q1->c*q2->a -q1->d*q2->b);
    result->fz = (-q1->a*q2->d -q1->b*q2->c +q1->c*q2->b +q1->d*q2->a);

}

//Inefficient Point3D version
void quat_rot(Point3D &p, const Point3D &r, float angle)
{
    Point3f pP,rR;

    pP.fx =p[0]; pP.fy =p[1]; pP.fz =p[2];  
    rR.fx =r[0]; rR.fy =r[1]; rR.fz =r[2];

    quat_rot(&pP,&rR,angle);

    p[0] = pP.fx; p[1] =pP.fy; p[2] = pP.fz;
}


//Uses quaternion mathematics to perform a rotation around your favourite axis
//IMPORTANT: Rotvec must be normalised before passing to this function 
//failure to do so will have weird results. 
//For better performance on multiple rotations, use other function
//Note result is stored in returned point
void quat_rot(Point3f *point, const Point3f *rotVec, float angle)
{
    ASSERT(rotVec->fx*rotVec->fx + rotVec->fy*rotVec->fy + rotVec->fz*rotVec->fz - 1.0f < 
            5.0f*sqrtf(std::numeric_limits<float>::epsilon()));

    double sinCoeff;
        Quaternion rotQuat;
    Quaternion pointQuat;
    Quaternion temp;
    
    //remember this value so we don't recompute it
#ifdef _GNU_SOURCE
    double cosCoeff;
    //GNU provides sincos which is about twice the speed of sin/cos separately
    sincos(angle*0.5f,&sinCoeff,&cosCoeff);
    rotQuat.a=cosCoeff;
#else
    angle*=0.5f;
    sinCoeff=sin(angle);
    
    rotQuat.a = cos(angle);
#endif  
    rotQuat.b=sinCoeff*rotVec->fx;
    rotQuat.c=sinCoeff*rotVec->fy;
    rotQuat.d=sinCoeff*rotVec->fz;

//  pointQuat.a =0.0f; This is implied in the pointQuat multiplication function
    pointQuat.b = point->fx;
    pointQuat.c = point->fy;
    pointQuat.d = point->fz;


    //perform  rotation
    quat_mult_no_second_a(&temp,&rotQuat,&pointQuat);
    quat_pointmult(point, &temp,&rotQuat);

}

void quat_rot_array(Point3D  *pointArr, unsigned int n, 
            const Point3f *rotVec, float angle)
{
    Point3f *fArr;
    fArr = new Point3f[n];

    for(size_t ui=0;ui<n;ui++)
    {
        fArr[ui].fx = pointArr[ui][0];
        fArr[ui].fy = pointArr[ui][1];
        fArr[ui].fz = pointArr[ui][2];
    }

    quat_rot_array(fArr,n,rotVec,angle);

    for(size_t ui=0;ui<n;ui++)
    {
         pointArr[ui][0]=fArr[ui].fx; 
         pointArr[ui][1]=fArr[ui].fy;
         pointArr[ui][2]=fArr[ui].fz;
    }

    delete[] fArr;
}
void quat_rot_array(Point3f *pointArr, unsigned int n,
            const Point3f *rotVec, float angle)
{
    Quaternion rotQuat;
    Quaternion pointQuat;
    Quaternion temp;
    {
        ASSERT(rotVec->fx*rotVec->fx + rotVec->fy*rotVec->fy + rotVec->fz*rotVec->fz - 1.0f < 
                5.0f*sqrtf(std::numeric_limits<float>::epsilon()));

        double sinCoeff;
        
        //remember this value so we don't recompute it
    #ifdef _GNU_SOURCE
        double cosCoeff;
        //GNU provides sincos which is about twice the speed of sin/cos separately
        sincos(angle*0.5f,&sinCoeff,&cosCoeff);
        rotQuat.a=cosCoeff;
    #else
        angle*=0.5f;
        sinCoeff=sin(angle);
        
        rotQuat.a = cos(angle);
    #endif  
        rotQuat.b=sinCoeff*rotVec->fx;
        rotQuat.c=sinCoeff*rotVec->fy;
        rotQuat.d=sinCoeff*rotVec->fz;

        for(unsigned int ui=0;ui<n; ui++)
        {
        //  pointQuat.a =0.0f; This is implied in the pointQuat multiplication function
            pointQuat.b = pointArr[ui].fx;
            pointQuat.c = pointArr[ui].fy;
            pointQuat.d = pointArr[ui].fz;


            //perform  rotation
            quat_mult_no_second_a(&temp,&rotQuat,&pointQuat);
            quat_pointmult(pointArr+ui, &temp,&rotQuat);

        }
    }
}

//Retrieve the quaternion for repeated rotation. Pass to the quat_rot_apply_quats
void quat_get_rot_quat(const Point3f *rotVec, float angle,Quaternion *rotQuat) 
{
    ASSERT(rotVec->fx*rotVec->fx + rotVec->fy*rotVec->fy + rotVec->fz*rotVec->fz - 1.0f < 
            5.0f*sqrtf(std::numeric_limits<float>::epsilon()));
    double sinCoeff;
#ifdef _GNU_SOURCE
    double cosCoeff;
    //GNU provides sincos which is about twice the speed of sin/cos separately
    sincos(angle*0.5f,&sinCoeff,&cosCoeff);
    rotQuat->a=cosCoeff;
#else
    angle*=0.5f;
    sinCoeff=sin(angle);
    rotQuat->a = cos(angle);
#endif  
    
    rotQuat->b=sinCoeff*rotVec->fx;
    rotQuat->c=sinCoeff*rotVec->fy;
    rotQuat->d=sinCoeff*rotVec->fz;
}

//Use previously generated quats from quat_get_rot_quats to rotate a point
void quat_rot_apply_quat(Point3f *point, const Quaternion *rotQuat)
{
    Quaternion pointQuat,temp;
//  pointQuat.a =0.0f; No need to set this, as we do not use it in the multiplication function
    pointQuat.b = point->fx;
    pointQuat.c = point->fy;
    pointQuat.d = point->fz;
    //perform  rotation
    quat_mult_no_second_a(&temp,rotQuat,&pointQuat);
    quat_pointmult(point, &temp,rotQuat);
}

//Invert the given quaternion (this for example, can generate the inverse rotation) 
void quat_invert(Quaternion *quat)
{
    quat->b=-quat->b;
    quat->c=-quat->c;
    quat->d=-quat->d;
}
}