/*
 * Matrix.cpp
 *
 * Copyright (C) 1999 Stephen F. White
 * 
 * This program 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.
 *
 * This program 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 this program (see the file "COPYING" for details); if 
 * not, write to the Free Software Foundation, Inc., 675 Mass Ave, 
 * Cambridge, MA 02139, USA.
 */

#include <stdio.h>
#include <string.h>
#include "stdafx.h"
#include "swt.h"

#include "Matrix.h"
#include "EulerAngles.h"

Matrix::Matrix()
{
    for (int i = 0; i < 4; i++)
       for (int k = 0; k < 4; k++)
          _mat[i][k] = 0.0f;
}

Matrix::Matrix(float mat[4][4])
{
    for (int i = 0; i < 4; i++)
       for (int k = 0; k < 4; k++)
          _mat[i][k] = mat[i][k];
}

Matrix::Matrix(Quaternion quat)
{
    HMatrix matrix;
    EulerAngles euler = Eul_FromQuat(quat, EulOrdXYZs);
    Eul_ToHMatrix(euler, matrix);
    this->getValue(matrix);
}

Matrix::Matrix(float *f)
{
    for (int i = 0; i < 4; i++)
       for (int k = 0; k < 4; k++)
          _mat[i][k] = f[i * 4 + k];
}

Matrix
Matrix::operator *(const Matrix &m) const
{
    Matrix r;

    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            r._mat[i][j] = 0.0f;
            for (int k = 0; k < 4; k++) {
                r._mat[i][j] += _mat[k][j] * m._mat[i][k];
            }
        }
    }
    return r;
}

Vec3f
Matrix::operator *(const Vec3f &v) const
{
    Vec3f r;
    float iw;

    r.x = _mat[0][0]*v.x + _mat[1][0]*v.y + _mat[2][0]*v.z + _mat[3][0];
    r.y = _mat[0][1]*v.x + _mat[1][1]*v.y + _mat[2][1]*v.z + _mat[3][1];
    r.z = _mat[0][2]*v.x + _mat[1][2]*v.y + _mat[2][2]*v.z + _mat[3][2];
    iw = 1.0f / (_mat[0][3]*v.x + _mat[1][3]*v.y + _mat[2][3]*v.z + _mat[3][3]);
    
    return r * iw;
}

void
Matrix::getValue(float mat[4][4]) const
{
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            mat[i][j] = _mat[i][j];
        }
    }
}

void
Matrix::swapRows(int i, int j)
{
    float tmp;

    for (int k = 0; k < 4; k++) {
        tmp = _mat[k][i];
        _mat[k][i] = _mat[k][j];
        _mat[k][j] = tmp;
    }
}

Matrix
Matrix::invert() const        // Gauss-Jordan elimination with partial pivoting
{
    Matrix a(*this),          // As a evolves from original mat into identity
           b(identity());     // b evolves from identity into inverse(a)
    
    int  i, j, i1, k;

    // Loop over cols of a from left to right, eliminating above and below diag
    for (j = 0; j < 4; j++) {   
        // Find largest pivot in column j among rows j..2
        i1 = j;                 // Row with largest pivot candidate
        for ( i = j + 1; i < 4; i++ )
            if ( fabs( a._mat[j][i] ) > fabs( a._mat[j][i1] ) )
                i1 = i;

        // Swap rows i1 and j in a and b to put pivot on diagonal
        if (i1 != j) {
            a.swapRows(i1, j);
            b.swapRows(i1, j);
        }

        // Scale row j to have a unit diagonal
        if (a._mat[j][j] == 0.0f) {
            swDebugf("Matrix::invert():  singular matrix, can't invert\n");
            return b;
        }

        float s = 1.0f / a._mat[j][j];

        for (i = 0; i < 4; i++) {
            b._mat[i][j] *= s;
            a._mat[i][j] *= s;
        }

        // Eliminate off-diagonal elems in col j of a, doing identical ops to b
        for (i = 0; i < 4; i++) {
            if (i != j) {
                float t = a._mat[j][i];
                for (k = 0; k < 4; k++) {
                    b._mat[k][i] -= t * b._mat[k][j];
                    a._mat[k][i] -= t * a._mat[k][j];
                }
            }
        }
    }
    return b;
}

Matrix
Matrix::identity()
{
    Matrix r;

    r._mat[0][1] = r._mat[0][2] = r._mat[0][3] = 0.0f;
    r._mat[1][0] = r._mat[1][2] = r._mat[1][3] = 0.0f;
    r._mat[2][0] = r._mat[2][1] = r._mat[2][3] = 0.0f;
    r._mat[3][0] = r._mat[3][1] = r._mat[3][2] = 0.0f;
    r._mat[0][0] = r._mat[1][1] = r._mat[2][2] = r._mat[3][3] = 1.0f;

    return r;
}

Matrixd::Matrixd()
{
    for (int i=0; i < 4; i++)
       for (int k = 0; k < 4; k++)
          _mat[i][k] = 0.0f;
}

Matrixd::Matrixd(double mat[4][4])
{
    for (int i = 0; i < 4; i++)
       for (int k = 0; k < 4; k++)
          _mat[i][k] = mat[i][k];
}

Matrixd::Matrixd(double *d)
{
    for (int i = 0; i < 4; i++)
       for (int k = 0; k < 4; k++)
          _mat[i][k] = d[i * 4 + k];
}

Matrixd
Matrixd::operator *(const Matrixd &m) const
{
    Matrixd r;

    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            r._mat[i][j] = 0.0f;
            for (int k = 0; k < 4; k++) {
                r._mat[i][j] += _mat[k][j] * m._mat[i][k];
            }
        }
    }
    return r;
}

Vec3d
Matrixd::operator *(const Vec3d &v) const
{
    Vec3d r;
    double iw;

    r.x = _mat[0][0]*v.x + _mat[1][0]*v.y + _mat[2][0]*v.z + _mat[3][0];
    r.y = _mat[0][1]*v.x + _mat[1][1]*v.y + _mat[2][1]*v.z + _mat[3][1];
    r.z = _mat[0][2]*v.x + _mat[1][2]*v.y + _mat[2][2]*v.z + _mat[3][2];
    iw = 1.0f / (_mat[0][3]*v.x + _mat[1][3]*v.y + _mat[2][3]*v.z + _mat[3][3]);
    
    return r * iw;
}

void
Matrixd::getValue(double mat[4][4]) const
{
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            mat[i][j] = _mat[i][j];
        }
    }
}

void
Matrixd::swapRows(int i, int j)
{
    double tmp;

    for (int k = 0; k < 4; k++) {
        tmp = _mat[k][i];
        _mat[k][i] = _mat[k][j];
        _mat[k][j] = tmp;
    }
}

Matrixd
Matrixd::invert() const        // Gauss-Jordan elimination with partial pivoting
{
    Matrixd a(*this),          // As a evolves from original mat into identity
           b(identity());     // b evolves from identity into inverse(a)
    
    int  i, j, i1, k;

    // Loop over cols of a from left to right, eliminating above and below diag
    for (j = 0; j < 4; j++) {   
        // Find largest pivot in column j among rows j..2
        i1 = j;                 // Row with largest pivot candidate
        for ( i = j + 1; i < 4; i++ )
            if ( fabs( a._mat[j][i] ) > fabs( a._mat[j][i1] ) )
                i1 = i;

        // Swap rows i1 and j in a and b to put pivot on diagonal
        if (i1 != j) {
            a.swapRows(i1, j);
            b.swapRows(i1, j);
        }

        // Scale row j to have a unit diagonal
        if (a._mat[j][j] == 0.0f) {
            swDebugf("Matrixd::invert():  singular Matrixd, can't invert\n");
            return b;
        }

        double s = 1.0f / a._mat[j][j];

        for (i = 0; i < 4; i++) {
            b._mat[i][j] *= s;
            a._mat[i][j] *= s;
        }

        // Eliminate off-diagonal elems in col j of a, doing identical ops to b
        for (i = 0; i < 4; i++) {
            if (i != j) {
                double t = a._mat[j][i];
                for (k = 0; k < 4; k++) {
                    b._mat[k][i] -= t * b._mat[k][j];
                    a._mat[k][i] -= t * a._mat[k][j];
                }
            }
        }
    }
    return b;
}

Matrixd
Matrixd::identity()
{
    Matrixd r;

    r._mat[0][1] = r._mat[0][2] = r._mat[0][3] = 0.0f;
    r._mat[1][0] = r._mat[1][2] = r._mat[1][3] = 0.0f;
    r._mat[2][0] = r._mat[2][1] = r._mat[2][3] = 0.0f;
    r._mat[3][0] = r._mat[3][1] = r._mat[3][2] = 0.0f;
    r._mat[0][0] = r._mat[1][1] = r._mat[2][2] = r._mat[3][3] = 1.0f;

    return r;
}

