Matrix3D.cpp

Go to the documentation of this file.

/* Copyright (C) 2010 Wildfire Games.
 * This file is part of 0 A.D.
 *
 * 0 A.D. 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.
 *
 * 0 A.D. 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 0 A.D.  If not, see <http://www.gnu.org/licenses/>.
 */

/*
 * A Matrix class used for holding and manipulating transformation
 * info.
 */

#include "precompiled.h"

#include "Matrix3D.h"
#include "Quaternion.h"
#include "Vector4D.h"

//Sets the identity matrix
void CMatrix3D::SetIdentity ()
{
    _11=1.0f; _12=0.0f; _13=0.0f; _14=0.0f;
    _21=0.0f; _22=1.0f; _23=0.0f; _24=0.0f;
    _31=0.0f; _32=0.0f; _33=1.0f; _34=0.0f;
    _41=0.0f; _42=0.0f; _43=0.0f; _44=1.0f;
}

//Sets the zero matrix
void CMatrix3D::SetZero ()
{
    _11=0.0f; _12=0.0f; _13=0.0f; _14=0.0f;
    _21=0.0f; _22=0.0f; _23=0.0f; _24=0.0f;
    _31=0.0f; _32=0.0f; _33=0.0f; _34=0.0f;
    _41=0.0f; _42=0.0f; _43=0.0f; _44=0.0f;
}

void CMatrix3D::SetOrtho (float l, float r, float b, float t, float n, float f)
{
    // Based on OpenGL spec
    *this = CMatrix3D(
        2/(r-l), 0, 0, -(r+l)/(r-l),
        0, 2/(t-b), 0, -(t+b)/(t-b),
        0, 0, -2/(f-n), -(f+n)/(f-n),
        0, 0, 0, 1
    );
}

//The following clear the matrix and set the 
//rotation of each of the 3 axes

void CMatrix3D::SetXRotation (float angle)
{
    const float Cos = cosf (angle);
    const float Sin = sinf (angle);
    
    _11=1.0f; _12=0.0f; _13=0.0f; _14=0.0f;
    _21=0.0f; _22=Cos;  _23=-Sin; _24=0.0f;
    _31=0.0f; _32=Sin;  _33=Cos;  _34=0.0f;
    _41=0.0f; _42=0.0f; _43=0.0f; _44=1.0f;
}

void CMatrix3D::SetYRotation (float angle)
{
    const float Cos = cosf (angle);
    const float Sin = sinf (angle);

    _11=Cos;  _12=0.0f; _13=Sin;  _14=0.0f;
    _21=0.0f; _22=1.0f; _23=0.0f; _24=0.0f;
    _31=-Sin; _32=0.0f; _33=Cos;  _34=0.0f;
    _41=0.0f; _42=0.0f; _43=0.0f; _44=1.0f;
}

void CMatrix3D::SetZRotation (float angle)
{
    const float Cos = cosf (angle);
    const float Sin = sinf (angle);

    _11=Cos;  _12=-Sin; _13=0.0f; _14=0.0f;
    _21=Sin;  _22=Cos;  _23=0.0f; _24=0.0f;
    _31=0.0f; _32=0.0f; _33=1.0f; _34=0.0f;
    _41=0.0f; _42=0.0f; _43=0.0f; _44=1.0f;
}

//The following apply a rotation to the matrix
//about each of the axes;

void CMatrix3D::RotateX (float angle)
{
    const float Cos = cosf (angle);
    const float Sin = sinf (angle);
    const float tmp_21 = _21;
    const float tmp_22 = _22;
    const float tmp_23 = _23;
    const float tmp_24 = _24;

    _21 = Cos * _21 - Sin * _31;
    _22 = Cos * _22 - Sin * _32;
    _23 = Cos * _23 - Sin * _33;
    _24 = Cos * _24 - Sin * _34;

    _31 = Sin * tmp_21 + Cos * _31;
    _32 = Sin * tmp_22 + Cos * _32;
    _33 = Sin * tmp_23 + Cos * _33;
    _34 = Sin * tmp_24 + Cos * _34;
}

void CMatrix3D::RotateY (float angle)
{
    const float Cos = cosf (angle);
    const float Sin = sinf (angle);
    const float tmp_11 = _11;
    const float tmp_12 = _12;
    const float tmp_13 = _13;
    const float tmp_14 = _14;

    _11 = Cos * _11 + Sin * _31;
    _12 = Cos * _12 + Sin * _32;
    _13 = Cos * _13 + Sin * _33;
    _14 = Cos * _14 + Sin * _34;

    _31 = -Sin * tmp_11 + Cos * _31;
    _32 = -Sin * tmp_12 + Cos * _32;
    _33 = -Sin * tmp_13 + Cos * _33;
    _34 = -Sin * tmp_14 + Cos * _34;
}

void CMatrix3D::RotateZ (float angle)
{
    const float Cos = cosf (angle);
    const float Sin = sinf (angle);
    const float tmp_11 = _11;
    const float tmp_12 = _12;
    const float tmp_13 = _13;
    const float tmp_14 = _14;

    _11 = Cos * _11 - Sin * _21;
    _12 = Cos * _12 - Sin * _22;
    _13 = Cos * _13 - Sin * _23;
    _14 = Cos * _14 - Sin * _24;

    _21 = Sin * tmp_11 + Cos * _21;
    _22 = Sin * tmp_12 + Cos * _22;
    _23 = Sin * tmp_13 + Cos * _23;
    _24 = Sin * tmp_14 + Cos * _24;
}

//Sets the translation of the matrix
void CMatrix3D::SetTranslation (float x, float y, float z)
{
    _11=1.0f; _12=0.0f; _13=0.0f; _14=x;
    _21=0.0f; _22=1.0f; _23=0.0f; _24=y;
    _31=0.0f; _32=0.0f; _33=1.0f; _34=z;
    _41=0.0f; _42=0.0f; _43=0.0f; _44=1.0f;
}

void CMatrix3D::SetTranslation(const CVector3D& vector)
{
    SetTranslation(vector.X, vector.Y, vector.Z);   
}

//Applies a translation to the matrix
void CMatrix3D::Translate(float x, float y, float z)
{
    _14 += x;
    _24 += y;
    _34 += z;
}

void CMatrix3D::Translate(const CVector3D &vector)
{
    _14 += vector.X;
    _24 += vector.Y;
    _34 += vector.Z;
}

void CMatrix3D::PostTranslate(float x, float y, float z)
{
    // Equivalent to "m.SetTranslation(x, y, z); *this = *this * m;"
    _14 += _11*x + _12*y + _13*z;
    _24 += _21*x + _22*y + _23*z;
    _34 += _31*x + _32*y + _33*z;
    _44 += _41*x + _42*y + _43*z;
}

CVector3D CMatrix3D::GetTranslation() const
{
    return CVector3D(_14, _24, _34);
}

//Clears and sets the scaling of the matrix
void CMatrix3D::SetScaling (float x_scale, float y_scale, float z_scale)
{
    _11=x_scale; _12=0.0f;    _13=0.0f;    _14=0.0f;
    _21=0.0f;    _22=y_scale; _23=0.0f;    _24=0.0f;
    _31=0.0f;    _32=0.0f;    _33=z_scale; _34=0.0f;
    _41=0.0f;    _42=0.0f;    _43=0.0f;    _44=1.0f;
}

//Scales the matrix
void CMatrix3D::Scale (float x_scale, float y_scale, float z_scale)
{
    _11 *= x_scale;
    _12 *= x_scale;
    _13 *= x_scale;
    _14 *= x_scale;

    _21 *= y_scale;
    _22 *= y_scale;
    _23 *= y_scale;
    _24 *= y_scale;

    _31 *= z_scale;
    _32 *= z_scale;
    _33 *= z_scale;
    _34 *= z_scale;
}

//Returns the transpose of the matrix. For orthonormal
//matrices, this is the same is the inverse matrix
CMatrix3D CMatrix3D::GetTranspose() const
{
    return CMatrix3D(
        _11, _21, _31, _41,
        _12, _22, _32, _42,
        _13, _23, _33, _43,
        _14, _24, _34, _44);
}


//Get a vector which points to the left of the matrix
CVector3D CMatrix3D::GetLeft() const
{
    return CVector3D(-_11, -_21, -_31);
}

//Get a vector which points up from the matrix
CVector3D CMatrix3D::GetUp() const
{
    return CVector3D(_12, _22, _32);
}

//Get a vector which points to front of the matrix
CVector3D CMatrix3D::GetIn() const
{
    return CVector3D(_13, _23, _33);
}

///////////////////////////////////////////////////////////////////////////////
// RotateTransposed: rotate a vector by the transpose of this matrix
CVector3D CMatrix3D::RotateTransposed(const CVector3D& vector) const
{
    CVector3D result;
    RotateTransposed(vector,result);
    return result;
}

///////////////////////////////////////////////////////////////////////////////
// RotateTransposed: rotate a vector by the transpose of this matrix
void CMatrix3D::RotateTransposed(const CVector3D& vector,CVector3D& result) const
{
    result.X = _11*vector.X + _21*vector.Y + _31*vector.Z;
    result.Y = _12*vector.X + _22*vector.Y + _32*vector.Z;
    result.Z = _13*vector.X + _23*vector.Y + _33*vector.Z;
}


void CMatrix3D::GetInverse(CMatrix3D& dst) const
{
    float tmp[12];  // temp array for pairs 
    float src[16];  // array of transpose source matrix 
    float det;      // determinant 
    
    // transpose matrix 
    for (int i = 0; i < 4; ++i) {
        src[i] = _data[i*4];
        src[i + 4] = _data[i*4 + 1];
        src[i + 8] = _data[i*4 + 2];
        src[i + 12] = _data[i*4 + 3];
    }

    // calculate pairs for first 8 elements (cofactors) 
    tmp[0] = src[10] * src[15];
    tmp[1] = src[11] * src[14];
    tmp[2] = src[9] * src[15];
    tmp[3] = src[11] * src[13];
    tmp[4] = src[9] * src[14];
    tmp[5] = src[10] * src[13];
    tmp[6] = src[8] * src[15];
    tmp[7] = src[11] * src[12];
    tmp[8] = src[8] * src[14];
    tmp[9] = src[10] * src[12];
    tmp[10] = src[8] * src[13];
    tmp[11] = src[9] * src[12];
    
    // calculate first 8 elements (cofactors)
    dst._data[0] = (tmp[0]-tmp[1])*src[5] + (tmp[3]-tmp[2])*src[6] + (tmp[4]-tmp[5])*src[7];
    dst._data[1] = (tmp[1]-tmp[0])*src[4] + (tmp[6]-tmp[7])*src[6] + (tmp[9]-tmp[8])*src[7];
    dst._data[2] = (tmp[2]-tmp[3])*src[4] + (tmp[7]-tmp[6])*src[5] + (tmp[10]-tmp[11])*src[7];
    dst._data[3] = (tmp[5]-tmp[4])*src[4] + (tmp[8]-tmp[9])*src[5] + (tmp[11]-tmp[10])*src[6];
    dst._data[4] = (tmp[1]-tmp[0])*src[1] + (tmp[2]-tmp[3])*src[2] + (tmp[5]-tmp[4])*src[3];
    dst._data[5] = (tmp[0]-tmp[1])*src[0] + (tmp[7]-tmp[6])*src[2] + (tmp[8]-tmp[9])*src[3];
    dst._data[6] = (tmp[3]-tmp[2])*src[0] + (tmp[6]-tmp[7])*src[1] + (tmp[11]-tmp[10])*src[3];
    dst._data[7] = (tmp[4]-tmp[5])*src[0] + (tmp[9]-tmp[8])*src[1] + (tmp[10]-tmp[11])*src[2];
    
    // calculate pairs for second 8 elements (cofactors) 
    tmp[0] = src[2]*src[7];
    tmp[1] = src[3]*src[6];
    tmp[2] = src[1]*src[7];
    tmp[3] = src[3]*src[5];
    tmp[4] = src[1]*src[6];
    tmp[5] = src[2]*src[5];
    tmp[6] = src[0]*src[7];
    tmp[7] = src[3]*src[4];
    tmp[8] = src[0]*src[6];
    tmp[9] = src[2]*src[4];
    tmp[10] = src[0]*src[5];
    tmp[11] = src[1]*src[4];

    // calculate second 8 elements (cofactors) 
    dst._data[8] = (tmp[0]-tmp[1])*src[13] + (tmp[3]-tmp[2])*src[14] + (tmp[4]-tmp[5])*src[15];
    dst._data[9] = (tmp[1]-tmp[0])*src[12] + (tmp[6]-tmp[7])*src[14] + (tmp[9]-tmp[8])*src[15];
    dst._data[10] = (tmp[2]-tmp[3])*src[12] + (tmp[7]-tmp[6])*src[13] + (tmp[10]-tmp[11])*src[15];
    dst._data[11] = (tmp[5]-tmp[4])*src[12] + (tmp[8]-tmp[9])*src[13] + (tmp[11]-tmp[10])*src[14];
    dst._data[12] = (tmp[2]-tmp[3])*src[10] + (tmp[5]-tmp[4])*src[11] + (tmp[1]-tmp[0])*src[9];
    dst._data[13] = (tmp[7]-tmp[6])*src[10] + (tmp[8]-tmp[9])*src[11] + (tmp[0]-tmp[1])*src[8];
    dst._data[14] = (tmp[6]-tmp[7])*src[9] + (tmp[11]-tmp[10])*src[11] + (tmp[3]-tmp[2])*src[8];
    dst._data[15] = (tmp[10]-tmp[11])*src[10] + (tmp[4]-tmp[5])*src[8] + (tmp[9]-tmp[8])*src[9];

    // calculate matrix inverse 
    det=src[0]*dst._data[0]+src[1]*dst._data[1]+src[2]*dst._data[2]+src[3]*dst._data[3];
    det = 1/det;
    for ( int j = 0; j < 16; j++) {
        dst._data[j] *= det;
    }
}

CMatrix3D CMatrix3D::GetInverse() const
{
    CMatrix3D r;
    GetInverse(r);
    return r;
}

void CMatrix3D::Rotate(const CQuaternion& quat)
{
    CMatrix3D rotationMatrix=quat.ToMatrix();
    Concatenate(rotationMatrix);
}

CQuaternion CMatrix3D::GetRotation() const
{
    float tr = _data2d[0][0] + _data2d[1][1] + _data2d[2][2];

    int next[] = { 1, 2, 0 };

    float quat[4];

    if (tr > 0.f)
    {
        float s = sqrtf(tr + 1.f);
        quat[3] = s * 0.5f;
        s = 0.5f / s;
        quat[0] = (_data2d[1][2] - _data2d[2][1]) * s;
        quat[1] = (_data2d[2][0] - _data2d[0][2]) * s;
        quat[2] = (_data2d[0][1] - _data2d[1][0]) * s;
    }
    else
    {
        int i = 0;
        if (_data2d[1][1] > _data2d[0][0]) i = 1;
        if (_data2d[2][2] > _data2d[i][i]) i = 2;
        int j = next[i];
        int k = next[j];

        float s = sqrtf((_data2d[i][i] - (_data2d[j][j] + _data2d[k][k])) + 1.f);
        quat[i] = s * 0.5f;

        if (s != 0.f) s = 0.5f / s;

        quat[3] = (_data2d[j][k] - _data2d[k][j]) * s;
        quat[j] = (_data2d[i][j] + _data2d[j][i]) * s;
        quat[k] = (_data2d[i][k] + _data2d[k][i]) * s;
    }

    return CQuaternion(quat[0], quat[1], quat[2], quat[3]);
}

void CMatrix3D::SetRotation(const CQuaternion& quat)
{
    quat.ToMatrix(*this);
}

float CMatrix3D::GetYRotation() const
{
    // Project the X axis vector onto the XZ plane
    CVector3D axis = -GetLeft();
    axis.Y = 0;

    // Normalise projected vector

    float len = axis.Length();
    if (len < 0.0001f)
        return 0.f;
    axis *= 1.0f/len;

    // Negate the return angle to match the SetYRotation convention
    return -atan2(axis.Z, axis.X);
}