ModelMaths.cpp

Go to the documentation of this file.

////////////////////////////////////////////////////////////////////////////////
//    Scorched3D (c) 2000-2009
//
//    This file is part of Scorched3D.
//
//    Scorched3D 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 2 of the License, or
//    (at your option) any later version.
//
//    Scorched3D 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 Scorched3D; if not, write to the Free Software
//    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
////////////////////////////////////////////////////////////////////////////////

#include <3dsparse/ModelMaths.h>
#include <common/Defines.h>
#include <math.h>

void ModelMaths::quaternionSlerp(float p[4], float q[4], float t, float qt[4])
{
        int i;
        float omega, cosom, sinom, sclp, sclq;

        // decide if one of the quaternions is backwards
        float a = 0;
        float b = 0;
        for (i = 0; i < 4; i++) {
                a += (p[i]-q[i])*(p[i]-q[i]);
                b += (p[i]+q[i])*(p[i]+q[i]);
        }
        if (a > b) {
                for (i = 0; i < 4; i++) {
                        q[i] = -q[i];
                }
        }

        cosom = p[0]*q[0] + p[1]*q[1] + p[2]*q[2] + p[3]*q[3];

        if ((1.0 + cosom) > 0.00000001) {
                if ((1.0 - cosom) > 0.00000001) {
                        omega = (float) acos( cosom );
                        sinom = sinf( omega );
                        sclp = sinf( (1.0f - t)*omega) / sinom;
                        sclq = sinf( t*omega ) / sinom;
                }
                else {
                        sclp = 1.0f - t;
                        sclq = t;
                }
                for (i = 0; i < 4; i++) {
                        qt[i] = sclp * p[i] + sclq * q[i];
                }
        }
        else {
                qt[0] = -p[1];
                qt[1] = p[0];
                qt[2] = -p[3];
                qt[3] = p[2];
                const float Q_PI = 3.14f;
                sclp = sinf( (1.0f - t) * 0.5f * Q_PI);
                sclq = sinf( t * 0.5f * Q_PI);
                for (i = 0; i < 3; i++) {
                        qt[i] = sclp * p[i] + sclq * qt[i];
                }
        }
}

void ModelMaths::quaternionMatrix(float quaternion[4], float matrix[3][4])
{
        matrix[0][0] = 1.0f - 2.0f * quaternion[1] * quaternion[1] - 2.0f * quaternion[2] * quaternion[2];
        matrix[1][0] = 2.0f * quaternion[0] * quaternion[1] + 2.0f * quaternion[3] * quaternion[2];
        matrix[2][0] = 2.0f * quaternion[0] * quaternion[2] - 2.0f * quaternion[3] * quaternion[1];

        matrix[0][1] = 2.0f * quaternion[0] * quaternion[1] - 2.0f * quaternion[3] * quaternion[2];
        matrix[1][1] = 1.0f - 2.0f * quaternion[0] * quaternion[0] - 2.0f * quaternion[2] * quaternion[2];
        matrix[2][1] = 2.0f * quaternion[1] * quaternion[2] + 2.0f * quaternion[3] * quaternion[0];

        matrix[0][2] = 2.0f * quaternion[0] * quaternion[2] + 2.0f * quaternion[3] * quaternion[1];
        matrix[1][2] = 2.0f * quaternion[1] * quaternion[2] - 2.0f * quaternion[3] * quaternion[0];
        matrix[2][2] = 1.0f - 2.0f * quaternion[0] * quaternion[0] - 2.0f * quaternion[1] * quaternion[1];
}

void ModelMaths::angleQuaternion(const Vector &angles, float quaternion[4])
{
        float           angle;
        float           sr, sp, sy, cr, cp, cy;

        // FIXME: rescale the inputs to 1/2 angle
        angle = angles[2] * 0.5f;
        sy = sinf(angle);
        cy = cosf(angle);
        angle = angles[1] * 0.5f;
        sp = sinf(angle);
        cp = cosf(angle);
        angle = angles[0] * 0.5f;
        sr = sinf(angle);
        cr = cosf(angle);

        quaternion[0] = sr*cp*cy-cr*sp*sy; // X
        quaternion[1] = cr*sp*cy+sr*cp*sy; // Y
        quaternion[2] = cr*cp*sy-sr*sp*cy; // Z
        quaternion[3] = cr*cp*cy+sr*sp*sy; // W
}

void ModelMaths::angleMatrix(const Vector &angles, float matrix[3][4])
{
        float           angle;
        float           sr, sp, sy, cr, cp, cy;
        
        angle = angles[2] * (PI*2 / 360);
        sy = sinf(angle);
        cy = cosf(angle);
        angle = angles[1] * (PI*2 / 360);
        sp = sinf(angle);
        cp = cosf(angle);
        angle = angles[0] * (PI*2 / 360);
        sr = sinf(angle);
        cr = cosf(angle);

        // matrix = (Z * Y) * X
        matrix[0][0] = cp*cy;
        matrix[1][0] = cp*sy;
        matrix[2][0] = -sp;
        matrix[0][1] = sr*sp*cy+cr*-sy;
        matrix[1][1] = sr*sp*sy+cr*cy;
        matrix[2][1] = sr*cp;
        matrix[0][2] = (cr*sp*cy+-sr*-sy);
        matrix[1][2] = (cr*sp*sy+-sr*cy);
        matrix[2][2] = cr*cp;
        matrix[0][3] = 0.0;
        matrix[1][3] = 0.0;
        matrix[2][3] = 0.0;
}

void ModelMaths::concatTransforms(const float in1[3][4], const float in2[3][4], float out[3][4])
{
        out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] +
                                in1[0][2] * in2[2][0];
        out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] +
                                in1[0][2] * in2[2][1];
        out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] +
                                in1[0][2] * in2[2][2];
        out[0][3] = in1[0][0] * in2[0][3] + in1[0][1] * in2[1][3] +
                                in1[0][2] * in2[2][3] + in1[0][3];
        out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] +
                                in1[1][2] * in2[2][0];
        out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] +
                                in1[1][2] * in2[2][1];
        out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] +
                                in1[1][2] * in2[2][2];
        out[1][3] = in1[1][0] * in2[0][3] + in1[1][1] * in2[1][3] +
                                in1[1][2] * in2[2][3] + in1[1][3];
        out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] +
                                in1[2][2] * in2[2][0];
        out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] +
                                in1[2][2] * in2[2][1];
        out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] +
                                in1[2][2] * in2[2][2];
        out[2][3] = in1[2][0] * in2[0][3] + in1[2][1] * in2[1][3] +
                                in1[2][2] * in2[2][3] + in1[2][3];
}

void ModelMaths::vectorIRotate(const Vector &in1, const float in2[3][4], Vector &out)
{
        out[0] = in1[0]*in2[0][0] + in1[1]*in2[1][0] + in1[2]*in2[2][0];
        out[1] = in1[0]*in2[0][1] + in1[1]*in2[1][1] + in1[2]*in2[2][1];
        out[2] = in1[0]*in2[0][2] + in1[1]*in2[1][2] + in1[2]*in2[2][2];
}

---