matrix.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"

#include <math.h>

namespace renderstack {


void matrix::set_row(int row, vec4 const &v)
{
    m[0][row] = v.x;
    m[1][row] = v.y;
    m[2][row] = v.z;
    m[3][row] = v.w;
}

vec4 matrix::row(int row) const
{
    return vec4(
        m[0][row],
        m[1][row],
        m[2][row],
        m[3][row]
    );
}

vec3 matrix::column3(int col) const
{
    return vec3(
        m[col][0],
        m[col][1],
        m[col][2]
    );
}

vec4 matrix::column(int col) const
{
    return vec4(
        m[col][0],
        m[col][1],
        m[col][2],
        m[col][3]
    );
}

void matrix::set_column(int col, vec4 const &v)
{
    m[col][0] = v.x;
    m[col][1] = v.y;
    m[col][2] = v.z;
    m[col][3] = v.w;
}

vec3 matrix::transform_vector(vec3 const &v) const
{
    return vec3(
        m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0],
        m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1],
        m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2]
    );
}

vec4 matrix::transform_vector(vec4 const &v) const
{
    return vec4(
        m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w,
        m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w,
        m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w,
        m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w
    );
}

vec3 matrix::uniform_transform_direction(vec3 const &v) const
{
    return vec3(
        m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z,
        m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z,
        m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z
    );
}


matrix::matrix(quaternion const &q)
{
    float x2 = 2.0f * q.x;
    float y2 = 2.0f * q.y;
    float z2 = 2.0f * q.z;
    float xx = q.x * x2;
    float xy = q.x * y2;
    float xz = q.x * z2;
    float yy = q.y * y2;
    float yz = q.y * z2;
    float zz = q.z * z2;
    float wx = q.w * x2;
    float wy = q.w * y2;
    float wz = q.w * z2;

    m[0][0] = 1.0f - yy - zz;   m[1][0] = xy - wz;          m[2][0] = xz + wy;          m[3][0] = 0.0f; 
    m[0][1] = xy + wz;          m[1][1] = 1.0f - xx - zz;   m[2][1] = yz - wx;          m[3][1] = 0.0f; 
    m[0][2] = xz - wy;          m[1][2] = yz + wx;          m[2][2] = 1.0f - xx - yy;   m[3][2] = 0.0f;
    m[0][3] = 0.0f;             m[1][3] = 0.0f;             m[2][3] = 0.0f;             m[3][3] = 1.0f;

}

matrix matrix::operator*(matrix const &other) const
{
    return matrix(
        m[0][0] * other.m[0][0] + m[1][0] * other.m[0][1] + m[2][0] * other.m[0][2] + m[3][0] * other.m[0][3],
        m[0][0] * other.m[1][0] + m[1][0] * other.m[1][1] + m[2][0] * other.m[1][2] + m[3][0] * other.m[1][3],
        m[0][0] * other.m[2][0] + m[1][0] * other.m[2][1] + m[2][0] * other.m[2][2] + m[3][0] * other.m[2][3],
        m[0][0] * other.m[3][0] + m[1][0] * other.m[3][1] + m[2][0] * other.m[3][2] + m[3][0] * other.m[3][3],

        m[0][1] * other.m[0][0] + m[1][1] * other.m[0][1] + m[2][1] * other.m[0][2] + m[3][1] * other.m[0][3],
        m[0][1] * other.m[1][0] + m[1][1] * other.m[1][1] + m[2][1] * other.m[1][2] + m[3][1] * other.m[1][3],
        m[0][1] * other.m[2][0] + m[1][1] * other.m[2][1] + m[2][1] * other.m[2][2] + m[3][1] * other.m[2][3],
        m[0][1] * other.m[3][0] + m[1][1] * other.m[3][1] + m[2][1] * other.m[3][2] + m[3][1] * other.m[3][3],

        m[0][2] * other.m[0][0] + m[1][2] * other.m[0][1] + m[2][2] * other.m[0][2] + m[3][2] * other.m[0][3],
        m[0][2] * other.m[1][0] + m[1][2] * other.m[1][1] + m[2][2] * other.m[1][2] + m[3][2] * other.m[1][3],
        m[0][2] * other.m[2][0] + m[1][2] * other.m[2][1] + m[2][2] * other.m[2][2] + m[3][2] * other.m[2][3],
        m[0][2] * other.m[3][0] + m[1][2] * other.m[3][1] + m[2][2] * other.m[3][2] + m[3][2] * other.m[3][3],

        m[0][3] * other.m[0][0] + m[1][3] * other.m[0][1] + m[2][3] * other.m[0][2] + m[3][3] * other.m[0][3],
        m[0][3] * other.m[1][0] + m[1][3] * other.m[1][1] + m[2][3] * other.m[1][2] + m[3][3] * other.m[1][3],
        m[0][3] * other.m[2][0] + m[1][3] * other.m[2][1] + m[2][3] * other.m[2][2] + m[3][3] * other.m[2][3],
        m[0][3] * other.m[3][0] + m[1][3] * other.m[3][1] + m[2][3] * other.m[3][2] + m[3][3] * other.m[3][3]
    );
}


matrix inverse(matrix const &m)
{
    vec4  x  = m.column(0);
    vec4  y  = m.column(1);
    vec4  z  = m.column(2);
    vec4  w  = m.column(3);
    vec4  x1 = cross(y, z, w);
    vec4  y1 = cross(z, w, x);
    vec4  z1 = cross(w, x, y);
    vec4  w1 = cross(x, y, z);
    float lx = dot(x, x1);
    float ly = dot(y, y1);
    float lz = dot(z, z1);
    float lw = dot(w, w1);

    if(
        RS_NEAR_ZERO(lx) ||
        RS_NEAR_ZERO(ly) ||
        RS_NEAR_ZERO(lz) ||
        RS_NEAR_ZERO(lw)
    )
    {
        throw matrix_not_invertible_exception();
    }

    x1[0] /= lx; 
    y1[0] /= ly; 
    z1[0] /= lz; 
    w1[0] /= lw; 

    return matrix(x1, y1, z1, w1);
}


matrix make_matrix_frustum_centered(
    float width,
    float height,
    float nearv,
    float farv
)
{
    float far_plus_near  = farv + nearv;
    float far_minus_near = farv - nearv;
    float far_times_near = farv * nearv;

    float x = RS_NEAR_ZERO(width         ) ? 1.0f : ( 2.0f *  nearv          / width);
    float y = RS_NEAR_ZERO(height        ) ? 1.0f : ( 2.0f *  nearv          / height);
    float c = RS_NEAR_ZERO(far_minus_near) ? 1.0f : (        -far_plus_near  / far_minus_near);
    float d = RS_NEAR_ZERO(far_minus_near) ? 1.0f : (-2.0f *  far_times_near / far_minus_near);

    return matrix(
        x,     0.0f,   0.0f,  0.0f,
        0.0f,  y,      0.0f,  0.0f,
        0.0f,  0.0f,   c,     d,
        0.0f,  0.0f,  -1.0f,  0.0f
    );
}


matrix make_matrix_frustum(
    float left,
    float right,
    float bottom,
    float top,
    float nearv,
    float farv
)
{
    float right_plus_left  = right + left;
    float top_plus_bottom  = top   + bottom;
    float far_plus_near    = farv  + nearv;
    float right_minus_left = right - left;
    float top_minus_bottom = top   - bottom;
    float far_minus_near   = farv  - nearv;
    float far_times_near   = farv  * nearv;

    float x = RS_NEAR_ZERO(right_minus_left) ? 1.0f : ( 2.0f * nearv          / right_minus_left);
    float y = RS_NEAR_ZERO(top_minus_bottom) ? 1.0f : ( 2.0f * nearv          / top_minus_bottom);
    float a = RS_NEAR_ZERO(right_minus_left) ? 1.0f : ( right_plus_left       / right_minus_left);
    float b = RS_NEAR_ZERO(top_minus_bottom) ? 1.0f : ( top_plus_bottom       / top_minus_bottom);
    float c = RS_NEAR_ZERO(far_minus_near  ) ? 1.0f : (-far_plus_near         / far_minus_near);
    float d = RS_NEAR_ZERO(far_minus_near  ) ? 1.0f : (-2.0f * far_times_near / far_minus_near);

    return matrix(
        x,     0.0f,   a,     0.0f,
        0.0f,  y,      b,     0.0f,
        0.0f,  0.0f,   c,     d,
        0.0f,  0.0f,  -1.0f,  0.0f
    );
}


matrix make_matrix_perspective_vertical(
    float fov_y_radians,
    float aspect,
    float near_val,
    float far_val
)
{
    float fov_tangent = tanf(fov_y_radians * 0.5f);
    float height      = 2.0f * near_val * fov_tangent;
    float width       = height * aspect;

    return make_matrix_frustum_centered(width, height, near_val, far_val);
}

matrix make_matrix_perspective_horizontal(
    float fov_x_radians,
    float aspect,
    float near_val,
    float far_val
)
{
    float fov_tangent = tanf(fov_x_radians * 0.5f);
    float width       = 2.0f * near_val * fov_tangent;
    float height      = width / aspect;

    return make_matrix_frustum_centered(width, height, near_val, far_val);
}

matrix make_matrix_orthogonal_centered(
    float width,
    float height,
    float near_val,
    float far_val
)
{
    float x = RS_NEAR_ZERO(width            ) ? 1.0f : ( 2.0f / (float)(width)             );
    float y = RS_NEAR_ZERO(height           ) ? 1.0f : ( 2.0f / (float)(height)            );
    float z = RS_TOLERANCE(near_val, far_val) ? 1.0f : (-2.0f / (float)(far_val - near_val));

    return matrix(
        x,     0.0f,  0.0f,  0.0f,
        0.0f,  y,     0.0f,  0.0f,
        0.0f,  0.0f,  z,     0.0f,
        0.0f,  0.0f,  0.0f,  1.0f
    );
}

matrix make_matrix_orthogonal(
    float left,
    float right,
    float bottom,
    float top,
    float near_val,
    float far_val
)
{
    float r_plus_l  = right   + left;
    float t_plus_b  = top     + bottom;
    float f_plus_n  = far_val + near_val;
    float r_minus_l = right   - left;
    float t_minus_b = top     - bottom;
    float f_minus_n = far_val - near_val;

    float x  = RS_NEAR_ZERO(r_minus_l) ? 1.0f : ( 2.0f     / r_minus_l);
    float y  = RS_NEAR_ZERO(t_minus_b) ? 1.0f : ( 2.0f     / t_minus_b);
    float z  = RS_NEAR_ZERO(f_minus_n) ? 1.0f : (-2.0f     / f_minus_n);
    float tx = RS_NEAR_ZERO(r_minus_l) ? 0.0f : (-r_plus_l / r_minus_l);
    float ty = RS_NEAR_ZERO(t_minus_b) ? 0.0f : (-t_plus_b / t_minus_b);
    float tz = RS_NEAR_ZERO(f_minus_n) ? 0.0f : (-f_plus_n / f_minus_n);

    return matrix(
        x,     0.0f,  0,     tx,
        0.0f,  y,     0,     ty,
        0.0f,  0.0f,  z,     tz,
        0.0f,  0.0f,  0.0f,  1.0f
    );
}

void make_matrix_quaternion_xyz(
    matrix           &m,
    matrix           &mi,
    quaternion const &q,
    vec3       const &t
)
{
    float x2 = 2.0f * q.x;
    float y2 = 2.0f * q.y;
    float z2 = 2.0f * q.z;
    float xx = q.x * x2;
    float xy = q.x * y2;
    float xz = q.x * z2;
    float yy = q.y * y2;
    float yz = q.y * z2;
    float zz = q.z * z2;
    float wx = q.w * x2;
    float wy = q.w * y2;
    float wz = q.w * z2;

    mi.m[0][0] = m.m[0][0] = 1.0f - yy - zz;
    mi.m[0][1] = m.m[1][0] = xy - wz;
    mi.m[0][2] = m.m[2][0] = xz + wy;
                 m.m[3][0] = t.x;

    mi.m[1][0] = m.m[0][1] = xy + wz;
    mi.m[1][1] = m.m[1][1] = 1.0f - xx - zz;
    mi.m[1][2] = m.m[2][1] = yz - wx;
                 m.m[3][1] = t.y;

    mi.m[2][0] = m.m[0][2] = xz - wy;
    mi.m[2][1] = m.m[1][2] = yz + wx;
    mi.m[2][2] = m.m[2][2] = 1.0f - xx - yy;
                 m.m[3][2] = t.z;

    mi.m[3][0] = mi.m[0][0] * -t.x + mi.m[1][0] * -t.y + mi.m[2][0] * -t.z;
    mi.m[3][1] = mi.m[0][1] * -t.x + mi.m[1][1] * -t.y + mi.m[2][1] * -t.z;
    mi.m[3][2] = mi.m[0][2] * -t.x + mi.m[1][2] * -t.y + mi.m[2][2] * -t.z;

    mi.m[3][3] = m.m[3][3] = 1.0f;
}

void make_matrix_quaternion_xyz(
    matrix           &m,
    matrix           &mi,
    quaternion const &q,
    vec3       const &t,
    vec3       const &s
)
{
    float x2 = 2.0f * q.x;
    float y2 = 2.0f * q.y;
    float z2 = 2.0f * q.z;
    float xx = q.x * x2;
    float xy = q.x * y2;
    float xz = q.x * z2;
    float yy = q.y * y2;
    float yz = q.y * z2;
    float zz = q.z * z2;
    float wx = q.w * x2;
    float wy = q.w * y2;
    float wz = q.w * z2;

    mi.m[0][0] = m.m[0][0] = 1.0f - yy - zz;
    mi.m[0][1] = m.m[1][0] = xy - wz;
    mi.m[0][2] = m.m[2][0] = xz + wy;
                 m.m[3][0] = t.x;

    mi.m[1][0] = m.m[0][1] = xy + wz;
    mi.m[1][1] = m.m[1][1] = 1.0f - xx - zz;
    mi.m[1][2] = m.m[2][1] = yz - wx;
                 m.m[3][1] = t.y;

    mi.m[2][0] = m.m[0][2] = xz - wy;
    mi.m[2][1] = m.m[1][2] = yz + wx;
    mi.m[2][2] = m.m[2][2] = 1.0f - xx - yy;
                 m.m[3][2] = t.z;

    mi.m[3][0] = mi.m[0][0] * -t.x + mi.m[1][0] * -t.y + mi.m[2][0] * -t.z;
    mi.m[3][1] = mi.m[0][1] * -t.x + mi.m[1][1] * -t.y + mi.m[2][1] * -t.z;
    mi.m[3][2] = mi.m[0][2] * -t.x + mi.m[1][2] * -t.y + mi.m[2][2] * -t.z;

    m.m[0][0] = m.m[0][0] * s.x;
    m.m[1][0] = m.m[1][0] * s.y;
    m.m[2][0] = m.m[2][0] * s.z;

    m.m[0][1] = m.m[0][1] * s.x;
    m.m[1][1] = m.m[1][1] * s.y;
    m.m[2][1] = m.m[2][1] * s.z;

    m.m[0][2] = m.m[0][2] * s.x;
    m.m[1][2] = m.m[1][2] * s.y;
    m.m[2][2] = m.m[2][2] * s.z;

    {
        float isx = RS_NEAR_ZERO(s.x) ? 0.0f : 1.0f / s.x;
        float isy = RS_NEAR_ZERO(s.y) ? 0.0f : 1.0f / s.y;
        float isz = RS_NEAR_ZERO(s.z) ? 0.0f : 1.0f / s.z;

        mi.m[0][0] = mi.m[0][0] * isx;
        mi.m[1][0] = mi.m[1][0] * isy;
        mi.m[2][0] = mi.m[2][0] * isz;

        mi.m[0][1] = mi.m[0][1] * isx;
        mi.m[1][1] = mi.m[1][1] * isy;
        mi.m[2][1] = mi.m[2][1] * isz;

        mi.m[0][2] = mi.m[0][2] * isx;
        mi.m[1][2] = mi.m[1][2] * isy;
        mi.m[2][2] = mi.m[2][2] * isz;
    }
    m.m[0][3] = mi.m[0][3] = 0.0f;
    m.m[1][3] = mi.m[1][3] = 0.0f;
    m.m[2][3] = mi.m[2][3] = 0.0f;
    m.m[3][3] = mi.m[3][3] = 1.0f;
}

matrix matrix_make_translate_scale(
    vec3 const &t,
    vec3 const &s
)
{
    return matrix(
        s.x,  0.0f, 0.0f, t.x,
        0.0f, s.y,  0.0f, t.y,
        0.0f, 0.0f, s.z,  t.z,
        0.0f, 0.0f, 0.0f, 1.0f
    );
}

matrix matrix_make_scale(
    vec3 const &s
)
{
    return matrix(
        s.x,  0.0f, 0.0f, 0.0f,
        0.0f, s.y,  0.0f, 0.0f,
        0.0f, 0.0f, s.z,  0.0f,
        0.0f, 0.0f, 0.0f, 1.0f
    );
}

matrix matrix_make_translate(
    vec3 const &t
)
{
    return matrix(
        1.0f, 0.0f, 0.0f, t.x,
        0.0f, 1.0f, 0.0f, t.y,
        0.0f, 0.0f, 1.0f, t.z,
        0.0f, 0.0f, 0.0f, 1.0f
    );
}

void make_matrix_hpb_xyz(
    matrix       &m,
    matrix       &mi,
    vec3   const &hpb,
    vec3   const &t
)
{
    float A = cosf(hpb.y);
    float B = sinf(hpb.y);
    float C = cosf(hpb.x);
    float D = sinf(hpb.x);
    float E = cosf(hpb.z);
    float F = sinf(hpb.z);

    {
        float DB = D * B;

        mi.m[0][0] = m.m[0][0] =  C *  E + -DB * F;
        mi.m[0][1] = m.m[1][0] =  C * -F + -DB * E;
        mi.m[0][2] = m.m[2][0] = -D *  A;
                     m.m[3][0] =  t.x;
    }

    {
        mi.m[1][0] = m.m[0][1] =  A * F;
        mi.m[1][1] = m.m[1][1] =  A * E;
        mi.m[1][2] = m.m[2][1] = -B;
                     m.m[3][1] =  t.y;
    }

    {
        float CB = C * B;

        mi.m[2][0] = m.m[0][2] =  D *  E + CB * F;
        mi.m[2][1] = m.m[1][2] =  D * -F + CB * E;
        mi.m[2][2] = m.m[2][2] =  C *  A;
                     m.m[3][2] =  t.z;
    }

    mi.m[3][0] = mi.m[0][0] * -t.x + mi.m[1][0] * -t.y + mi.m[2][0] * -t.z;
    mi.m[3][1] = mi.m[0][1] * -t.x + mi.m[1][1] * -t.y + mi.m[2][1] * -t.z;
    mi.m[3][2] = mi.m[0][2] * -t.x + mi.m[1][2] * -t.y + mi.m[2][2] * -t.z;
    mi.m[3][3] = 1.0f;
    m.m[0][3] = 0.0f;
    m.m[1][3] = 0.0f;
    m.m[2][3] = 0.0f;
    m.m[3][3] = 1.0f;
}

void make_matrix_hpb_xyz_scale(
    matrix       &m,
    matrix       &mi,
    vec3   const &hpb,
    vec3   const &t,
    vec3   const &s
)
{
    float A = cosf(hpb.y);
    float B = sinf(hpb.y);
    float C = cosf(hpb.x);
    float D = sinf(hpb.x);
    float E = cosf(hpb.z);
    float F = sinf(hpb.z);

    {
        float DB = D * B;

        mi.m[0][0] = m.m[0][0] =  C *  E + -DB * F;
        mi.m[0][1] = m.m[1][0] =  C * -F + -DB * E;
        mi.m[0][2] = m.m[2][0] = -D *  A;
                     m.m[3][0] =  t.x;
    }

    {
        mi.m[1][0] = m.m[0][1] =  A * F;
        mi.m[1][1] = m.m[1][1] =  A * E;
        mi.m[1][2] = m.m[2][1] = -B;
                     m.m[3][1] =  t.y;
    }

    {
        float CB = C * B;

        mi.m[2][0] = m.m[0][2] =  D *  E + CB * F;
        mi.m[2][1] = m.m[1][2] =  D * -F + CB * E;
        mi.m[2][2] = m.m[2][2] =  C *  A;
                     m.m[3][2] =  t.z;
    }

    mi.m[3][0] = mi.m[0][0] * -t.x + mi.m[1][0] * -t.y + mi.m[2][0] * -t.z;
    mi.m[3][1] = mi.m[0][1] * -t.x + mi.m[1][1] * -t.y + mi.m[2][1] * -t.z;
    mi.m[3][2] = mi.m[0][2] * -t.x + mi.m[1][2] * -t.y + mi.m[2][2] * -t.z;

    m.m[0][0] = m.m[0][0] * s.x;
    m.m[1][0] = m.m[1][0] * s.y;
    m.m[2][0] = m.m[2][0] * s.z;

    m.m[0][1] = m.m[0][1] * s.x;
    m.m[1][1] = m.m[1][1] * s.y;
    m.m[2][1] = m.m[2][1] * s.z;

    m.m[0][2] = m.m[0][2] * s.x;
    m.m[1][2] = m.m[1][2] * s.y;
    m.m[2][2] = m.m[2][2] * s.z;

    {
        float isx = RS_NEAR_ZERO(s.x) ? 0.0f : 1.0f / s.x; 
        float isy = RS_NEAR_ZERO(s.y) ? 0.0f : 1.0f / s.y;
        float isz = RS_NEAR_ZERO(s.z) ? 0.0f : 1.0f / s.z;

        mi.m[0][0] = mi.m[0][0] * isx;
        mi.m[1][0] = mi.m[1][0] * isy;
        mi.m[2][0] = mi.m[2][0] * isz;
        mi.m[3][0] = mi.m[3][0] * isz;

        mi.m[0][1] = mi.m[0][1] * isx;
        mi.m[1][1] = mi.m[1][1] * isy;
        mi.m[2][1] = mi.m[2][1] * isz;
        mi.m[3][1] = mi.m[3][1] * isz;

        mi.m[0][2] = mi.m[0][2] * isx;
        mi.m[1][2] = mi.m[1][2] * isy;
        mi.m[2][2] = mi.m[2][2] * isz;
        mi.m[3][2] = mi.m[3][2] * isz;
    }

    mi.m[3][3] = 1.0f;
    m.m[3][3] = 1.0f;
}

}