matrix.cpp

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
*/

#include "matrix.h" // include first

#include <iostream>

using namespace sheep;

using std::endl;

Matrix Matrix::operator*(const Matrix &m) const {
    assert(m_c == m.m_r);

    //!\todo Optimize.

    Matrix res(m_r, m.m_c);

    Real *v = res.m_v;

    for(int i = 0; i < m_r; i++)
        for(int j = 0; j < m.m_c; j++) {
            Real sum = 0;

            for(int k = 0; k < m_c; k++)
                sum += operator()(i, k) * m(k, j);

            *(v++) = sum;
        }

    return res;
}

Vector Matrix::operator*(const Vector &v) const {
    assert(m_c == v.GetDimension());

    Vector res(m_r);

    for(int i = 0; i < m_r; i++) {
        Real sum = 0;

        for(int j = 0; j < m_c; j++)
            sum += operator()(i, j) * v[j];

        res[i] = sum;
    }

    return res;
}

Matrix Matrix::Transpose() const {
    Matrix res(m_c, m_r);

    Real *u = m_v;

    for(int i = 0; i < m_r; i++) {
        Real *v = res.m_v + i;

        for(int j = 0; j < m_c; j++, v += m_r)
            *v = *(u++);
    }

    return res;
}

Matrix Matrix::Inverse() const {
    assert(m_r == m_c);

    Matrix res = *this;

    int i,j,k;
                    /* Locations of pivot elements */
    int *pvt_i, *pvt_j;
    double pvt_val;                     /* Value of current pivot element */
    double hold;                        /* Temporary storage */

    pvt_i = new int[m_r];
    pvt_j = new int[m_r];

    for (k = 0; k < m_r; k++)
    {
        /* Locate k'th pivot element */
        pvt_val = res(k,k);            /* Initialize for search */
        pvt_i[k] = k;
        pvt_j[k] = k;
        for(i = k; i < m_r; i++)
            for(j = k; j < m_r; j++)
                if(fabs(res(i,j)) > fabs(pvt_val)) {
                    pvt_i[k] = i;
                    pvt_j[k] = j;
                    pvt_val = res(i,j);
                }

        if(pvt_val==0) {
            /* Matrix is singular (zero determinant). */
            delete [] pvt_i;
            delete [] pvt_j;
            assert(!"Matrix is singular.");
        }

        /* "Interchange" rows (with sign change stuff) */
        i = pvt_i[k];
        if (i != k)                     /* If rows are different */
            for (j = 0; j < m_r; j++)
            {
                hold = -res(k,j);
                res(k,j) = res(i,j);
                res(i,j) = hold;
            }

        /* "Interchange" columns */
        j = pvt_j[k];
        if (j != k)                     /* If columns are different */
            for (i = 0; i < m_r; i++)
            {
                hold = -res(i,k);
                res(i,k) = res(i,j);
                res(i,j) = hold;
            }

        /* Divide column by minus pivot value */
        for (i = 0; i < m_r; i++)
            if (i != k)                   /* Don't touch the pivot entry */
                res(i,k) /= ( -pvt_val) ;  /* (Tricky C syntax for division) */

        /* Reduce the matrix */
        for (i = 0; i < m_r; i++)
        {
            hold = res(i,k);
            for (j = 0; j < m_r; j++)
                if ( i != k && j != k )   /* Don't touch pivot. */
                    res(i,j) += hold * res(k,j);
        }

        /* Divide row by pivot */
        for (j = 0; j < m_r; j++)
            if (j != k)                   /* Don't touch the pivot! */
                res(k,j) /= pvt_val;

        /* Replace pivot by reciprocal (at last we can touch it). */
        res(k,k) = 1.0/pvt_val;
    }

    /* That was most of the work, one final pass of row/column interchange */
    /* to finish */
    for (k = m_r-2; k >= 0; k--)  /* Don't need to work with 1 by 1 corner */
    {
        i = pvt_j[k];        /* Rows to swap correspond to pivot COLUMN */
        if (i != k)                     /* If rows are different */
            for(j = 0; j < m_r; j++)
            {
                hold = res(k,j);
                res(k,j) = -res(i,j);
                res(i,j) = hold;
            }

        j = pvt_i[k];           /* Columns to swap correspond to pivot ROW */
        if (j != k)                     /* If columns are different */
            for (i = 0; i < m_r; i++)
            {
                hold = res(i,k);
                res(i,k) = -res(i,j);
                res(i,j) = hold;
            }
    }

    delete [] pvt_i;
    delete [] pvt_j;

    return res;
}

Real Matrix::Determinant() const {
    assert(m_r == m_c);

    //!\todo Implement.

    return 0;
}

htmlstream &operator<<(htmlstream &s, const Matrix &m) {
    s << "<table border=\"1\">" << endl;

    for(int i = 0; i < m.GetRows(); ++i) {
        s << "<tr>" << endl;

        for(int j = 0; j < m.GetColumns(); ++j) {
            s << "<td>" << m(i, j) << "</td>" << endl;
        }

        s << "</tr>" << endl;
    }

    s << "</table>" << endl;

    return s;
}

---