quaternion.cpp

Go to the documentation of this file.

/*
    RenderStack  Support library for OpenGL 3+
    Copyright (C) 2010  Timo Suoranta

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

    This library 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
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public
    License along with this library; if not, write to the Free Software
    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
*/

#include "renderstack/matrix.hpp"
#include "renderstack/quaternion.hpp"

namespace renderstack {

vec3 quaternion::hpb() const
{
    float   sqx = x * x;
    float   sqy = y * y;
    float   sqz = z * z;

    float   singularity_test        = x * y + z * w;
    int     north_pole_singularity  = singularity_test >  (0.5f - RS_EPSILON);
    int     south_pole_singularity  = singularity_test < -(0.5f - RS_EPSILON);

    if(north_pole_singularity)
    {
        return vec3(
            -2.0f * atan2f(x, w),
            RS_HALF_PI,
            0.0f
        );
    }

    if(south_pole_singularity)
    {
        return vec3(
            2.0f * atan2f(x, w),
            RS_HALF_PI,
            0
        );
    }
    return vec3(
        -atan2f(2.0f * y * w - x * z, 1.0f - 2.0f * (sqy - sqz)),
         asinf (2.0f * x * y + z * w),
        -atan2f(2.0f * x * w - y * z, 1.0f - 2.0f * (sqx - sqz))
    );
}


vec3 quaternion::view_axis() const
{
    float x2 = 2.0f * x;
    float y2 = 2.0f * y;
    float z2 = 2.0f * z;
    float xx = x * x2;
    float xz = x * z2;
    float yy = y * y2;
    float yz = y * z2;
    float wx = w * x2;
    float wy = w * y2;

    return vec3(
        xz + wy,
        yz - wx,
        1.0f - (xx + yy)
    );
}


vec3 quaternion::up_axis() const
{
    float x2 = 2.0f * x;
    float y2 = 2.0f * y;
    float z2 = 2.0f * z;
    float xx = x * x2;
    float xy = x * y2;
    float yz = y * z2;
    float zz = y * z2;
    float wx = w * x2;
    float wz = w * z2;

    return vec3(
        xy - wz,
        1.0f - (xx + zz),
        yz + wx
    );
}


vec3 quaternion::right_axis() const 
{
    float y2 = 2.0f * y;
    float z2 = 2.0f * z;
    float xy = x * y2;
    float xz = x * z2;
    float yy = y * y2;
    float zz = z * z2;
    float wy = w * y2;
    float wz = w * z2;

    return vec3(
        1.0f - (yy + zz),
        xy + wz,
        xz - wy
    );
}


/*  Matrix should be orthonormalized  */ 
quaternion::quaternion(matrix const &mat)
{
    /*  Check the sum of the diagonal  */ 
    float tr = mat.m[0][0] + mat.m[1][1] + mat.m[2][2];

    if(tr > RS_EPSILON /*1e-6f*/ )
    {
        /*  The sum is positive  */ 
        float s = sqrtf(tr + 1.0f);
        w = 0.5f * s;
        s = 0.5f / s;
        x = (mat.m[1][2] - mat.m[2][1]) * s;
        y = (mat.m[2][0] - mat.m[0][2]) * s;
        z = (mat.m[0][1] - mat.m[1][0]) * s;
    }
    else
    {
        /*  The sum is negative  */ 
        float *q[3] = {&y, &z, &x};
        int const n_index[3] = {1, 2, 0};
        int i = 0;
        if(mat.m[1][1] > mat.m[i][i]) i = 1;
        if(mat.m[2][2] > mat.m[i][i]) i = 2;
        int j = n_index[i];
        int k = n_index[j];

        float s = sqrtf(
            mat.m[i][i] - (mat.m[j][j] + mat.m[k][k]) + 1.0f
        );
        *q[i] = 0.5f * s;
        if(s != 0)
        {
            s = 0.5f / s;
        }
        *q[j] = (mat.m[i][j] + mat.m[j][i]) * s;
        *q[k] = (mat.m[i][k] + mat.m[k][i]) * s;
        w     = (mat.m[j][k] + mat.m[k][j]) * s;
    }
}

quaternion quaternion::operator*(quaternion const &q2) const
{
    float E   = (x + z) * (q2.x + q2.y);
    float F   = (z - x) * (q2.x - q2.y);
    float G   = (w + y) * (q2.w - q2.z);
    float H   = (w - y) * (q2.w + q2.z);
    float A   = F - E;
    float B   = F + E;
    float GmH = G - H;
    float GpH = G + H;

    return quaternion(
        (w + x) * (q2.w + q2.x) + 0.5f * (A - GpH),
        (w - x) * (q2.y + q2.z) + 0.5f * (B + GmH),
        (y + z) * (q2.w - q2.x) + 0.5f * (B - GmH),
        (z - y) * (q2.y - q2.z) + 0.5f * (A + GpH)
    );
}

}