matrix3.inl

Go to the documentation of this file.

/*
    Sheep - A Rigid Body Dynamics Engine
    Copyright (C) 2001-2004 Francois Beaune
    Contact: http://toxicengine.sourceforge.net/

    This file is part of Sheep.

    Sheep 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.

    Sheep 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 Sheep; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

inline
Matrix3::Matrix3() {}

inline
Matrix3::Matrix3(Real r) {
    m_v[0] = m_v[1] = m_v[2] =
    m_v[3] = m_v[4] = m_v[5] =
    m_v[6] = m_v[7] = m_v[8] = r;
}

inline
Matrix3::Matrix3(const Matrix3 &m) {
    m_v[0] = m.m_v[0];
    m_v[1] = m.m_v[1];
    m_v[2] = m.m_v[2];
    m_v[3] = m.m_v[3];
    m_v[4] = m.m_v[4];
    m_v[5] = m.m_v[5];
    m_v[6] = m.m_v[6];
    m_v[7] = m.m_v[7];
    m_v[8] = m.m_v[8];
}

inline
Matrix3 Matrix3::Identity() {
    Matrix3 res;

    res.m_v[0] = res.m_v[4] = res.m_v[8] = 1.0;
    res.m_v[1] = res.m_v[2] = res.m_v[3] =
    res.m_v[5] = res.m_v[6] = res.m_v[7] = 0.0;

    return res;
}

inline
Matrix3 Matrix3::Rotation(const Quaternion &q) {    // q must be normalized
    Matrix3 res;

    res.m_v[0] = 1.0 - 2.0 * (q.m_v.m_y * q.m_v.m_y + q.m_v.m_z * q.m_v.m_z);
    res.m_v[1] = 2.0 * (q.m_v.m_x * q.m_v.m_y - q.m_s * q.m_v.m_z);
    res.m_v[2] = 2.0 * (q.m_v.m_x * q.m_v.m_z + q.m_s * q.m_v.m_y);
    res.m_v[3] = 2.0 * (q.m_v.m_x * q.m_v.m_y + q.m_s * q.m_v.m_z);
    res.m_v[4] = 1.0 - 2.0 * (q.m_v.m_x * q.m_v.m_x + q.m_v.m_z * q.m_v.m_z);
    res.m_v[5] = 2.0 * (q.m_v.m_y * q.m_v.m_z - q.m_s * q.m_v.m_x);
    res.m_v[6] = 2.0 * (q.m_v.m_x * q.m_v.m_z - q.m_s * q.m_v.m_y);
    res.m_v[7] = 2.0 * (q.m_v.m_y * q.m_v.m_z + q.m_s * q.m_v.m_x);
    res.m_v[8] = 1.0 - 2.0 * (q.m_v.m_x * q.m_v.m_x + q.m_v.m_y * q.m_v.m_y);

    return res;
}

inline
Matrix3 Matrix3::Rotation(Real angle, const Vector3 &axis) {
    //!\todo Implement.
    assert(!"Not implemented yet.");
    return Matrix3();
}

inline
Matrix3 Matrix3::Rotation(Real rotx, Real roty, Real rotz) {
    const Real a = cos(rotx);
    const Real b = sin(rotx);
    const Real c = cos(roty);
    const Real d = sin(roty);
    const Real e = cos(rotz);
    const Real f = sin(rotz);

    const Real ad = a * d;
    const Real bd = b * d;

    Matrix3 res;

    res.m_v[0] = c * e;
    res.m_v[1] = -a * f + bd * e;
    res.m_v[2] = b * f + ad * e;
    res.m_v[3] = c * f;
    res.m_v[4] = a * e + bd * f;
    res.m_v[5] = -b * e + ad * f;
    res.m_v[6] = -d;
    res.m_v[7] = b * c;
    res.m_v[8] = a * c;

    return res;
}

inline
void Matrix3::GetEulerAngles(Real *rotx, Real *roty, Real *rotz) const {
    // "Computing Euler angles from a rotation matrix", Gregory G. Slabaugh.
    // http://users.ece.gatech.edu/~slabaugh/personal/research/euler/euler.pdf.

    if(m_v[6] != 1.0 && m_v[6] != -1.0) {
        const Real theta = -asin(m_v[6]);
        const Real inv_cos_theta = 1.0 / cos(theta);

        *rotx = atan2(m_v[7] * inv_cos_theta, m_v[8] * inv_cos_theta);
        *roty = theta;
        *rotz = atan2(m_v[3] * inv_cos_theta, m_v[0] * inv_cos_theta);
    } else {
        *rotx = atan2(m_v[1], m_v[2]);

        if(m_v[6] == -1.0)
            *roty = PI / 2.0;
        else *roty = -PI / 2.0;

        *rotz = 0.0;
    }
}

inline
Matrix3 Matrix3::Scale(const Vector3 &s) {
    Matrix3 res;

    res.m_v[0] = s.m_x;
    res.m_v[4] = s.m_y;
    res.m_v[8] = s.m_z;

    res.m_v[1] = res.m_v[2] = res.m_v[3] =
    res.m_v[5] = res.m_v[6] = res.m_v[7] = 0.0;

    return res;
}

inline
Matrix3 &Matrix3::operator=(Real r) {
    m_v[0] = m_v[1] = m_v[2] =
    m_v[3] = m_v[4] = m_v[5] =
    m_v[6] = m_v[7] = m_v[8] = r;

    return *this;
}

inline
Matrix3 &Matrix3::operator=(const Matrix3 &m) {
    m_v[0] = m.m_v[0];
    m_v[1] = m.m_v[1];
    m_v[2] = m.m_v[2];
    m_v[3] = m.m_v[3];
    m_v[4] = m.m_v[4];
    m_v[5] = m.m_v[5];
    m_v[6] = m.m_v[6];
    m_v[7] = m.m_v[7];
    m_v[8] = m.m_v[8];

    return *this;
}

inline
Matrix3 Matrix3::operator+(const Matrix3 &m) const {
    Matrix3 res;

    res.m_v[0] = m_v[0] + m.m_v[0];
    res.m_v[1] = m_v[1] + m.m_v[1];
    res.m_v[2] = m_v[2] + m.m_v[2];
    res.m_v[3] = m_v[3] + m.m_v[3];
    res.m_v[4] = m_v[4] + m.m_v[4];
    res.m_v[5] = m_v[5] + m.m_v[5];
    res.m_v[6] = m_v[6] + m.m_v[6];
    res.m_v[7] = m_v[7] + m.m_v[7];
    res.m_v[8] = m_v[8] + m.m_v[8];

    return res;
}

inline
Matrix3 Matrix3::operator-(const Matrix3 &m) const {
    Matrix3 res;

    res.m_v[0] = m_v[0] - m.m_v[0];
    res.m_v[1] = m_v[1] - m.m_v[1];
    res.m_v[2] = m_v[2] - m.m_v[2];
    res.m_v[3] = m_v[3] - m.m_v[3];
    res.m_v[4] = m_v[4] - m.m_v[4];
    res.m_v[5] = m_v[5] - m.m_v[5];
    res.m_v[6] = m_v[6] - m.m_v[6];
    res.m_v[7] = m_v[7] - m.m_v[7];
    res.m_v[8] = m_v[8] - m.m_v[8];

    return res;
}

inline
Matrix3 Matrix3::operator-() const {
    Matrix3 res;

    res.m_v[0] = -m_v[0];
    res.m_v[1] = -m_v[1];
    res.m_v[2] = -m_v[2];
    res.m_v[3] = -m_v[3];
    res.m_v[4] = -m_v[4];
    res.m_v[5] = -m_v[5];
    res.m_v[6] = -m_v[6];
    res.m_v[7] = -m_v[7];
    res.m_v[8] = -m_v[8];

    return res;
}

inline
Matrix3 Matrix3::operator*(Real r) const {
    Matrix3 res;

    res.m_v[0] = m_v[0] * r;
    res.m_v[1] = m_v[1] * r;
    res.m_v[2] = m_v[2] * r;
    res.m_v[3] = m_v[3] * r;
    res.m_v[4] = m_v[4] * r;
    res.m_v[5] = m_v[5] * r;
    res.m_v[6] = m_v[6] * r;
    res.m_v[7] = m_v[7] * r;
    res.m_v[8] = m_v[8] * r;

    return res;
}

inline
Matrix3 Matrix3::operator/(Real r) const {
    Real inv_r = 1.0 / r;

    Matrix3 res;

    res.m_v[0] = m_v[0] * inv_r;
    res.m_v[1] = m_v[1] * inv_r;
    res.m_v[2] = m_v[2] * inv_r;
    res.m_v[3] = m_v[3] * inv_r;
    res.m_v[4] = m_v[4] * inv_r;
    res.m_v[5] = m_v[5] * inv_r;
    res.m_v[6] = m_v[6] * inv_r;
    res.m_v[7] = m_v[7] * inv_r;
    res.m_v[8] = m_v[8] * inv_r;

    return res;
}

inline
Matrix3 &Matrix3::operator+=(const Matrix3 &m) {
    m_v[0] += m.m_v[0];
    m_v[1] += m.m_v[1];
    m_v[2] += m.m_v[2];
    m_v[3] += m.m_v[3];
    m_v[4] += m.m_v[4];
    m_v[5] += m.m_v[5];
    m_v[6] += m.m_v[6];
    m_v[7] += m.m_v[7];
    m_v[8] += m.m_v[8];

    return *this;
}

inline
Matrix3 &Matrix3::operator-=(const Matrix3 &m) {
    m_v[0] -= m.m_v[0];
    m_v[1] -= m.m_v[1];
    m_v[2] -= m.m_v[2];
    m_v[3] -= m.m_v[3];
    m_v[4] -= m.m_v[4];
    m_v[5] -= m.m_v[5];
    m_v[6] -= m.m_v[6];
    m_v[7] -= m.m_v[7];
    m_v[8] -= m.m_v[8];

    return *this;
}

inline
Matrix3 &Matrix3::operator*=(Real r) {
    m_v[0] *= r;
    m_v[1] *= r;
    m_v[2] *= r;
    m_v[3] *= r;
    m_v[4] *= r;
    m_v[5] *= r;
    m_v[6] *= r;
    m_v[7] *= r;
    m_v[8] *= r;

    return *this;
}

inline
Matrix3 &Matrix3::operator/=(Real r) {
    Real inv_r = 1.0 / r;

    m_v[0] *= inv_r;
    m_v[1] *= inv_r;
    m_v[2] *= inv_r;
    m_v[3] *= inv_r;
    m_v[4] *= inv_r;
    m_v[5] *= inv_r;
    m_v[6] *= inv_r;
    m_v[7] *= inv_r;
    m_v[8] *= inv_r;

    return *this;
}

inline
Matrix3 Matrix3::operator*(const Matrix3 &m) const {
    Matrix3 res;

    res.m_v[0] = m_v[0] * m.m_v[0] + m_v[1] * m.m_v[3] + m_v[2] * m.m_v[6];
    res.m_v[1] = m_v[0] * m.m_v[1] + m_v[1] * m.m_v[4] + m_v[2] * m.m_v[7];
    res.m_v[2] = m_v[0] * m.m_v[2] + m_v[1] * m.m_v[5] + m_v[2] * m.m_v[8];

    res.m_v[3] = m_v[3] * m.m_v[0] + m_v[4] * m.m_v[3] + m_v[5] * m.m_v[6];
    res.m_v[4] = m_v[3] * m.m_v[1] + m_v[4] * m.m_v[4] + m_v[5] * m.m_v[7];
    res.m_v[5] = m_v[3] * m.m_v[2] + m_v[4] * m.m_v[5] + m_v[5] * m.m_v[8];

    res.m_v[6] = m_v[6] * m.m_v[0] + m_v[7] * m.m_v[3] + m_v[8] * m.m_v[6];
    res.m_v[7] = m_v[6] * m.m_v[1] + m_v[7] * m.m_v[4] + m_v[8] * m.m_v[7];
    res.m_v[8] = m_v[6] * m.m_v[2] + m_v[7] * m.m_v[5] + m_v[8] * m.m_v[8];

    return res;
}

inline
Matrix3 &Matrix3::operator*=(const Matrix3 &m) {
    Real u, v;

    u = m_v[0] * m.m_v[0] + m_v[1] * m.m_v[3] + m_v[2] * m.m_v[6];
    v = m_v[0] * m.m_v[1] + m_v[1] * m.m_v[4] + m_v[2] * m.m_v[7];
    m_v[2] = m_v[0] * m.m_v[2] + m_v[1] * m.m_v[5] + m_v[2] * m.m_v[8];
    m_v[0] = u;
    m_v[1] = v;

    u = m_v[3] * m.m_v[0] + m_v[4] * m.m_v[3] + m_v[5] * m.m_v[6];
    v = m_v[3] * m.m_v[1] + m_v[4] * m.m_v[4] + m_v[5] * m.m_v[7];
    m_v[5] = m_v[3] * m.m_v[2] + m_v[4] * m.m_v[5] + m_v[5] * m.m_v[8];
    m_v[3] = u;
    m_v[4] = v;

    u = m_v[6] * m.m_v[0] + m_v[7] * m.m_v[3] + m_v[8] * m.m_v[6];
    v = m_v[6] * m.m_v[1] + m_v[7] * m.m_v[4] + m_v[8] * m.m_v[7];
    m_v[8] = m_v[6] * m.m_v[2] + m_v[7] * m.m_v[5] + m_v[8] * m.m_v[8];
    m_v[6] = u;
    m_v[7] = v;

    return *this;
}

inline
Vector3 Matrix3::operator*(const Vector3 &v) const {
    return Vector3(
        m_v[0] * v.m_x + m_v[1] * v.m_y + m_v[2] * v.m_z,
        m_v[3] * v.m_x + m_v[4] * v.m_y + m_v[5] * v.m_z,
        m_v[6] * v.m_x + m_v[7] * v.m_y + m_v[8] * v.m_z);
}

inline
Real &Matrix3::operator[](int i) {
    assert(i >= 0 && i < 9);

    return m_v[i];
}

inline
const Real &Matrix3::operator[](int i) const {
    assert(i >= 0 && i < 9);

    return m_v[i];
}

inline
Real &Matrix3::operator()(int row, int column) {
    assert(row >= 0 && row < 3);
    assert(column >= 0 && column < 3);

    return m_v[row + row + row + column];
}

inline
const Real &Matrix3::operator()(int row, int column) const {
    assert(row >= 0 && row < 3);
    assert(column >= 0 && column < 3);

    return m_v[row + row + row + column];
}

inline
Matrix3 Matrix3::Transpose() const {
    Matrix3 res;

    res.m_v[0] = m_v[0];
    res.m_v[1] = m_v[3];
    res.m_v[2] = m_v[6];
    res.m_v[3] = m_v[1];
    res.m_v[4] = m_v[4];
    res.m_v[5] = m_v[7];
    res.m_v[6] = m_v[2];
    res.m_v[7] = m_v[5];
    res.m_v[8] = m_v[8];

    return res;
}

inline
Matrix3 Matrix3::Inverse() const {
    // Compute the matrix inverse using Cramer's rule.

    Real u = m_v[4] * m_v[8] - m_v[5] * m_v[7];
    Real v = m_v[5] * m_v[6] - m_v[3] * m_v[8];
    Real w = m_v[3] * m_v[7] - m_v[4] * m_v[6];

    Matrix3 res;

    // Compute cofactors.
    res.m_v[0] = u;
    res.m_v[1] = m_v[2] * m_v[7] - m_v[1] * m_v[8];
    res.m_v[2] = m_v[1] * m_v[5] - m_v[2] * m_v[4];
    res.m_v[3] = v;
    res.m_v[4] = m_v[0] * m_v[8] - m_v[2] * m_v[6];
    res.m_v[5] = m_v[2] * m_v[3] - m_v[0] * m_v[5];
    res.m_v[6] = w;
    res.m_v[7] = m_v[1] * m_v[6] - m_v[0] * m_v[7];
    res.m_v[8] = m_v[0] * m_v[4] - m_v[1] * m_v[3];

    // Compute matrix determinant.
    Real det = m_v[0] * u + m_v[1] * v + m_v[2] * w;

    ///!\todo Throw an exception if the matrix to be inverted
    //! is singular (i.e. if fabs(det) <= EPSILON).
    assert(det != 0.0);

    // Multiply the obtained matrix by the reciprocal of the
    // input matrix determinant.
    Real inv_det = 1.0 / det;
    res.m_v[0] *= inv_det;
    res.m_v[1] *= inv_det;
    res.m_v[2] *= inv_det;
    res.m_v[3] *= inv_det;
    res.m_v[4] *= inv_det;
    res.m_v[5] *= inv_det;
    res.m_v[6] *= inv_det;
    res.m_v[7] *= inv_det;
    res.m_v[8] *= inv_det;

    return res;
}

inline
Real Matrix3::Determinant() const {
    return
        m_v[0] * (m_v[4] * m_v[8] - m_v[5] * m_v[7]) +
        m_v[1] * (m_v[5] * m_v[6] - m_v[3] * m_v[8]) +
        m_v[2] * (m_v[3] * m_v[7] - m_v[4] * m_v[6]);
}

inline
Real Matrix3::Trace() const {
    return m_v[0] + m_v[4] + m_v[8];
}

inline
Matrix3 operator*(Real r, const Matrix3 &m) {
    return m * r;
}

---