/*@@@**************************************************************************
* \file  3DMatrix_inline.h
* \author Hernan Badino
* \date  Thu Jun 18 09:52:19 EDT 2009
* \notes 
*******************************************************************************
******************************************************************************/

#include <stdio.h>
#include <math.h>
#include "3DRowVector.h"

/// Set to 0 all elements.
inline void
C3DMatrix::clear ()
{
    memset(m_elems_p, 0, 9 * sizeof(double));
    
    //for (int i = 0; i < 9; ++i)
    //    m_elems_p[i] = 0.;
}

/// Set values from vector.
inline void
C3DMatrix::set ( const double * const f_vals_p )
{ 
    memcpy ( m_elems_p, f_vals_p, 9 * sizeof(double) ); 
}

inline double &
C3DMatrix::at ( int f_row_i, int f_col_i )
{ 
    return m_elems_p[f_row_i*3+f_col_i]; 
}

inline double
C3DMatrix::at ( int f_row_i, int f_col_i ) const
{ 
    return m_elems_p[f_row_i*3+f_col_i]; 
}

inline double &
C3DMatrix::at ( int f_elem_i )
{ 
    return m_elems_p[f_elem_i];
}

inline double
C3DMatrix::at ( int f_elem_i ) const
{ 
    return m_elems_p[f_elem_i]; 
}

inline void
C3DMatrix::loadIdentity ()
{
    for (int i = 0; i < 9; ++i)
        m_elems_p[i] = ((i%4)==0);
}

inline void
C3DMatrix::diagonalize ( double f_val_d )
{
    memset(m_elems_p, 0, 9 * sizeof(double));
    m_elems_p[0] = m_elems_p[4] = m_elems_p[8] = f_val_d;

    //for (int i = 0; i < 9; ++i)
    //    m_elems_p[i] = ((i%4)==0)?f_val_d:0;
}

inline void
C3DMatrix::print () const
{
    printf("\n%.15f %.15f %.15f\n%.15f %.15f %.15f\n%.15f %.15f %.15f\n", 
           m_elems_p[0], m_elems_p[1], m_elems_p[2],
           m_elems_p[3], m_elems_p[4], m_elems_p[5],
           m_elems_p[6], m_elems_p[7], m_elems_p[8] );
}

inline C3DMatrix 
C3DMatrix::operator * ( const C3DMatrix & f_other ) const
{
    C3DMatrix result;

    for(int i = 0; i < 3; ++i)
        for(int j = 0; j < 3; ++j)
        {
            result.m_elems_p[i*3+j] = m_elems_p[i*3] * f_other.m_elems_p[j];
            for(int k = 1; k < 3; ++k)
                result.m_elems_p[i*3+j] += m_elems_p[i*3+k] * f_other.m_elems_p[k*3+j];
        }
    
    return result;
}

inline C3DMatrix 
C3DMatrix::operator + ( const C3DMatrix & f_other ) const
{
    C3DMatrix result;

    for (int i = 0; i < 9; ++i)
        result.m_elems_p[i] = m_elems_p[i] + f_other.m_elems_p[i];

    return result;
}

inline C3DMatrix 
C3DMatrix::operator - ( const C3DMatrix & f_other ) const
{
    C3DMatrix result;

    for (int i = 0; i < 9; ++i)
        result.m_elems_p[i] = m_elems_p[i] - f_other.m_elems_p[i];

    return result;
}

inline C3DMatrix &
C3DMatrix::operator *= ( const C3DMatrix & f_other )
{
    C3DMatrix auxiliar;

    for(int i = 0; i < 3; ++i)
        for(int j = 0; j < 3; ++j)
        {
            auxiliar.m_elems_p[i*3+j] = m_elems_p[i*3] * f_other.m_elems_p[j];
            for(int k = 1; k < 3; ++k)
                auxiliar.m_elems_p[i*3+j] += m_elems_p[i*3+k] * f_other.m_elems_p[k*3+j];
        }
    
    *this = auxiliar;
    return *this;
}

inline C3DMatrix &
C3DMatrix::operator += ( const C3DMatrix & f_other )
{
    for(int i = 0; i < 9; ++i)
    {
        m_elems_p[i] += f_other.m_elems_p[i];
    }
    
    return *this;
}

inline C3DMatrix &
C3DMatrix::operator -= ( const C3DMatrix & f_other )
{
    for(int i = 0; i < 9; ++i)
    {
        m_elems_p[i] -= f_other.m_elems_p[i];
    }
    
    return *this;
}

inline C3DMatrix
C3DMatrix::getInverse ( ) const
{
    // from row 1 to row 4
    C3DMatrix aux = *this;
    C3DMatrix res;
    res.loadIdentity();

    for (int i = 0; i < 3; i++)
    {
        // normalize the diagonal elements
        double e = aux.m_elems_p[i*3+i];
        for (int j = 0; j < 3; j++)
        {
            aux.m_elems_p[i*3+j] /= e;
            res.m_elems_p[i*3+j] /= e;
        }
        
        // make other elements to be 0
        for (int k = 0; k < 3; k++)
        {
            if (k != i)
            {
                e = -aux.m_elems_p[k*3+i];
                for (int j = 0; j < 3; j++)
                {
                    aux.m_elems_p[k*3+j] += e * aux.m_elems_p[i*3+j];
                    res.m_elems_p[k*3+j] += e * res.m_elems_p[i*3+j];
                }
            }
        }
    }
    
    return res;
}

inline C3DMatrix
C3DMatrix::getTranspose ( ) const
{
    C3DMatrix result;
    result.m_elems_p[0] = m_elems_p[0];
    result.m_elems_p[1] = m_elems_p[3];
    result.m_elems_p[2] = m_elems_p[6];
    result.m_elems_p[3] = m_elems_p[1];
    result.m_elems_p[4] = m_elems_p[4];
    result.m_elems_p[5] = m_elems_p[7];
    result.m_elems_p[6] = m_elems_p[2];
    result.m_elems_p[7] = m_elems_p[5];
    result.m_elems_p[8] = m_elems_p[8];
    
    return result;
}

inline void
C3DMatrix::invert ( )
{
#if 0 // algorithm
    C3DMatrix res;
    res.loadIdentity();

    printf("C3DMatrix res;\nres.loadIdentity();\n");
    
    for (int i = 0; i < 3; i++)
    {
        // normalize the diagonal elements
        double e = m_elems_p[i*3+i];
        printf("e = m_elems_p[%i];\n", i*3+i );
        
        for (int j = 0; j < 3; j++)
        {
            printf("m_elems_p[%i] /= e;\n", i*3+j);            
            m_elems_p[i*3+j] /= e;
            printf("res.m_elems_p[%i] /= e;\n", i*3+j);
            res.m_elems_p[i*3+j] /= e;
        }
        
        // make other elements to be 0
        for (int k = 0; k < 3; k++)
        {
            if (k != i)
            {
                e = -m_elems_p[k*3+i];
                printf("e = -m_elems_p[%i];\n", k*3+i);            
                for (int j = 0; j < 3; j++)
                {
                    printf ("m_elems_p[%i] += e * m_elems_p[%i];\n", k*3+j , i*3+j);
                    m_elems_p[k*3+j] += e * m_elems_p[i*3+j];
                    printf("res.m_elems_p[%i] += e * res.m_elems_p[%i];\n", k*3+j, i*3+j);
                    res.m_elems_p[k*3+j] += e * res.m_elems_p[i*3+j];
                }
            }
        }
    }
    
    printf("*this = res;\n");
    *this = res;
#else
    C3DMatrix res;

    res.loadIdentity();

    res.m_elems_p[0] /= m_elems_p[0];
    m_elems_p[1]     /= m_elems_p[0];
    res.m_elems_p[1] /= m_elems_p[0];
    m_elems_p[2]     /= m_elems_p[0];
    res.m_elems_p[2] /= m_elems_p[0];

    res.m_elems_p[3] -= m_elems_p[3] * res.m_elems_p[0];
    m_elems_p[4]     -= m_elems_p[3] * m_elems_p[1];
    res.m_elems_p[4] -= m_elems_p[3] * res.m_elems_p[1];
    m_elems_p[5]     -= m_elems_p[3] * m_elems_p[2];
    res.m_elems_p[5] -= m_elems_p[3] * res.m_elems_p[2];

    res.m_elems_p[6] -= m_elems_p[6] * res.m_elems_p[0];
    m_elems_p[7]     -= m_elems_p[6] * m_elems_p[1];
    res.m_elems_p[7] -= m_elems_p[6] * res.m_elems_p[1];
    m_elems_p[8]     -= m_elems_p[6] * m_elems_p[2];
    res.m_elems_p[8] -= m_elems_p[6] * res.m_elems_p[2];

    ////////////////////////////

    res.m_elems_p[3] /= m_elems_p[4];
    res.m_elems_p[4] /= m_elems_p[4];
    m_elems_p[5]     /= m_elems_p[4];
    res.m_elems_p[5] /= m_elems_p[4];

    res.m_elems_p[0] -= m_elems_p[1] * res.m_elems_p[3];
    res.m_elems_p[1] -= m_elems_p[1] * res.m_elems_p[4];
    m_elems_p[2]     -= m_elems_p[1] * m_elems_p[5];
    res.m_elems_p[2] -= m_elems_p[1] * res.m_elems_p[5];

    res.m_elems_p[6] -= m_elems_p[7] * res.m_elems_p[3];
    res.m_elems_p[7] -= m_elems_p[7] * res.m_elems_p[4];
    m_elems_p[8]     -= m_elems_p[7] * m_elems_p[5];
    res.m_elems_p[8] -= m_elems_p[7] * res.m_elems_p[5];

    /////////////////////////////

    res.m_elems_p[6] /= m_elems_p[8];
    res.m_elems_p[7] /= m_elems_p[8];
    res.m_elems_p[8] /= m_elems_p[8];

    res.m_elems_p[0] -= m_elems_p[2] * res.m_elems_p[6];
    res.m_elems_p[1] -= m_elems_p[2] * res.m_elems_p[7];
    res.m_elems_p[2] -= m_elems_p[2] * res.m_elems_p[8];

    res.m_elems_p[3] -= m_elems_p[5] * res.m_elems_p[6];
    res.m_elems_p[4] -= m_elems_p[5] * res.m_elems_p[7];
    res.m_elems_p[5] -= m_elems_p[5] * res.m_elems_p[8];
    
    *this = res;
#endif
}

inline void
C3DMatrix::transpose ( )
{
    std::swap(m_elems_p[1], m_elems_p[3]);
    std::swap(m_elems_p[2], m_elems_p[6]);
    std::swap(m_elems_p[5], m_elems_p[7]);    
}


inline C3DRowVector 
C3DMatrix::multiply ( const C3DRowVector & f_vector ) const
{
    return operator * ( f_vector );    
}

inline C3DRowVector 
C3DMatrix::operator * ( const C3DRowVector & f_vector ) const
{
    C3DRowVector result;

    for (int i = 0; i < 3; i++)
    {
        result.m_elems_p[i]  = m_elems_p[i*3+0] * f_vector.m_elems_p[0];
        result.m_elems_p[i] += m_elems_p[i*3+1] * f_vector.m_elems_p[1];    
        result.m_elems_p[i] += m_elems_p[i*3+2] * f_vector.m_elems_p[2];    
    }
    
    return result;
}

/// Multiply transposed matrix with vector.
inline C3DRowVector 
C3DMatrix::multiplyTransposed ( const C3DRowVector & f_vector ) const
{
    C3DRowVector result;

    for (int i = 0; i < 3; i++)
    {
        result.m_elems_p[i]  = m_elems_p[0*3+i] * f_vector.m_elems_p[0];
        result.m_elems_p[i] += m_elems_p[1*3+i] * f_vector.m_elems_p[1];    
        result.m_elems_p[i] += m_elems_p[2*3+i] * f_vector.m_elems_p[2];    
    }
    
    return result;
}

inline void
C3DMatrix::rotateX ( double f_angle_d )
{
    C3DMatrix rotation;
    
    double sin_d = sin(f_angle_d);
    double cos_d = cos(f_angle_d);
    
    rotation.m_elems_p[0] = 1.;
    rotation.m_elems_p[1] = 0.;
    rotation.m_elems_p[2] = 0.;
    rotation.m_elems_p[3] = 0.;
    rotation.m_elems_p[4] = cos_d;
    rotation.m_elems_p[5] = -sin_d;
    rotation.m_elems_p[6] = 0.;
    rotation.m_elems_p[7] = sin_d;
    rotation.m_elems_p[8] = cos_d;
    
    *this *= rotation;
}

inline void
C3DMatrix::rotateY ( double f_angle_d )
{
    C3DMatrix rotation;
    
    double sin_d = sin(f_angle_d);
    double cos_d = cos(f_angle_d);
    
    rotation.m_elems_p[0] = cos_d;
    rotation.m_elems_p[1] = 0.;
    rotation.m_elems_p[2] = sin_d;
    rotation.m_elems_p[3] = 0.;
    rotation.m_elems_p[4] = 1.;
    rotation.m_elems_p[5] = 0;
    rotation.m_elems_p[6] = -sin_d;
    rotation.m_elems_p[7] = 0.;
    rotation.m_elems_p[8] = cos_d;
    
    *this *= rotation;
}

inline void
C3DMatrix::rotateZ ( double f_angle_d )
{
    C3DMatrix rotation;
    
    double sin_d = sin(f_angle_d);
    double cos_d = cos(f_angle_d);
    
    rotation.m_elems_p[0] = cos_d;
    rotation.m_elems_p[1] = -sin_d;
    rotation.m_elems_p[2] = 0.;
    rotation.m_elems_p[3] = sin_d;
    rotation.m_elems_p[4] = cos_d;
    rotation.m_elems_p[5] = 0.;
    rotation.m_elems_p[6] = 0.;
    rotation.m_elems_p[7] = 0.;
    rotation.m_elems_p[8] = 1.;
    
    *this *= rotation;
}

/// Not tested.
inline void 
C3DMatrix::rotateAxis ( const C3DRowVector &f_axis, 
                        double             f_angle_d )
{
#if 0
    C3DMatrix rotation;
    const double cos_d   = cos( f_angle_d );
    const double sin_d   = sin( f_angle_d );
    const double icos_d  = 1.0 - cos_d;

    const double xy_d    = f_axis.x()*f_axis.y();
    const double xz_d    = f_axis.x()*f_axis.z();
    const double yz_d    = f_axis.y()*f_axis.z();
    const double x_sin_d = f_axis.x()*sin_d;
    const double y_sin_d = f_axis.y()*sin_d;
    const double z_sin_d = f_axis.z()*sin_d;

    rotation.m_elems_p[ 0 ] = cos_d + f_axis.x()*f_axis.x()*icos_d;
    rotation.m_elems_p[ 1 ] = xy_d*icos_d - z_sin_d;
    rotation.m_elems_p[ 2 ] = xz_d*icos_d + y_sin_d;

    rotation.m_elems_p[ 3 ] = xy_d*icos_d + z_sin_d;
    rotation.m_elems_p[ 4 ] = cos_d + f_axis.y()*f_axis.y()*icos_d;
    rotation.m_elems_p[ 5 ] = yz_d*icos_d - x_sin_d;

    rotation.m_elems_p[ 6 ] = xz_d*icos_d - y_sin_d;
    rotation.m_elems_p[ 7 ] = yz_d*icos_d + x_sin_d;
    rotation.m_elems_p[ 8 ] = cos_d + f_axis.z()*f_axis.z()*icos_d;

    *this *= rotation;

#else
    ////// UNTESTED METHOD!!!!
    C3DMatrix rotation;

    double sin_d = sin(f_angle_d);
    double cos_d = cos(f_angle_d);
    C3DRowVector xyzS  = f_axis * sin_d;
    C3DRowVector xyzC  = f_axis * (1 - cos_d);

    C3DRowVector xyz2C ( f_axis.m_x_d * xyzC.m_y_d,
                         f_axis.m_y_d * xyzC.m_z_d,
                         f_axis.m_z_d * xyzC.m_x_d );


    rotation.m_elems_p[0] = f_axis.m_x_d * xyzC.m_x_d + cos_d;
    rotation.m_elems_p[1] = xyz2C.m_x_d - xyzS.m_z_d;
    rotation.m_elems_p[2] = xyz2C.m_z_d + xyzS.m_y_d;
    rotation.m_elems_p[3] = xyz2C.m_x_d + xyzS.m_z_d;
    rotation.m_elems_p[4] = f_axis.m_y_d * xyzC.m_y_d + cos_d;
    rotation.m_elems_p[5] = xyz2C.m_y_d - xyzS.m_x_d;
    rotation.m_elems_p[6] = xyz2C.m_z_d - xyzS.m_y_d;
    rotation.m_elems_p[7] = xyz2C.m_y_d + xyzS.m_x_d;
    rotation.m_elems_p[8] = f_axis.m_z_d * xyzC.m_z_d + cos_d;
  
    *this *= rotation;
#endif
}

/// Comparison operator.
inline bool
C3DMatrix::operator == ( const C3DMatrix & f_other )
{
    return ( (m_elems_p[0] == f_other.m_elems_p[0]) &&
             ( (m_elems_p[1] == f_other.m_elems_p[1]) && 
               ( (m_elems_p[2] == f_other.m_elems_p[2]) && 
                 ( (m_elems_p[3] == f_other.m_elems_p[3]) && 
                   ( (m_elems_p[4] == f_other.m_elems_p[4]) && 
                     ( (m_elems_p[5] == f_other.m_elems_p[5]) && 
                       ( (m_elems_p[6] == f_other.m_elems_p[6]) && 
                         ( (m_elems_p[7] == f_other.m_elems_p[7]) && 
                           ( (m_elems_p[8] == f_other.m_elems_p[8]) ) ) ) ) ) ) ) ) );
}

/// Comparison operator.
inline bool
C3DMatrix::operator != ( const C3DMatrix & f_other )
{
    return !operator == (f_other);
}

/// Compute trace.
inline double
C3DMatrix::trace ( ) const
{
    return m_elems_p[0] + m_elems_p[4] + m_elems_p[8];
}

/// Compute trace.
inline double
C3DMatrix::determinant ( ) const
{
    double sum1_d = ( m_elems_p[0] * 
                      ( m_elems_p[4] * m_elems_p[8] -
                        m_elems_p[5] * m_elems_p[7] ) );
    
    double sum2_d = ( m_elems_p[1] * 
                      ( m_elems_p[5] * m_elems_p[6] -
                        m_elems_p[3] * m_elems_p[8] ) );
    
    double sum3_d = ( m_elems_p[2] * 
                      ( m_elems_p[3] * m_elems_p[7] -
                        m_elems_p[4] * m_elems_p[6] ) );
    
    return sum1_d + sum2_d + sum3_d;
}


/// Return the outer product of two vectors.
//C3DMatrix    outerProduct ( const C3DRowVector & f_other ) const;

inline C3DMatrix
C3DRowVector::outerProduct ( const C3DRowVector & f_other ) const
{
    C3DMatrix m;
    m.m_elems_p[0*3+0] = m_x_d * f_other.m_x_d;
    m.m_elems_p[0*3+1] = m_x_d * f_other.m_y_d;
    m.m_elems_p[0*3+2] = m_x_d * f_other.m_z_d;
    m.m_elems_p[1*3+1] = m_y_d * f_other.m_y_d;
    m.m_elems_p[1*3+2] = m_y_d * f_other.m_z_d;
    m.m_elems_p[2*3+2] = m_z_d * f_other.m_z_d;
    m.m_elems_p[1*3+0] = m.m_elems_p[0*3+1];
    m.m_elems_p[2*3+0] = m.m_elems_p[0*3+2];
    m.m_elems_p[2*3+1] = m.m_elems_p[1*3+2];
    
    return m;
}





/* ////////////  Version History ///////////////
 *  $Log: 3DMatrix_inline.h,v $
 *  Revision 1.3  2010/01/18 17:12:29  badino
 *  General updates and added new classes.
 *
 *  Revision 1.2  2009/11/18 15:51:01  badino
 *  badino: documentation added. Some other global changes.
 *
 *//////////////////////////////////////////////
