vmfuncs.h

Go to the documentation of this file.

// vmfuncs.h
// Creation : 02/02/2000
// Author   : Leonardo FLOREZ VALENCIA
//               l-florez@uniandes.edu.co
//               lflorez@creatis.insa-lyon.fr
// Copyright (C) 2000-2002 Leonardo FLOREZ VALENCIA
//
// 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; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

#ifndef GTMLIB__MATH__VMFUNCS__HXX
#define GTMLIB__MATH__VMFUNCS__HXX

#include <iostream>
#include <math.h>
#include <memory.h>
#include "mathdefs.h"

#include <creaSystem.h>

using namespace std;

namespace gtm
{
// -----------------------------------------------------------------------------
//

// -----------------------------------------------------------------------------
    template< class T >
    inline
    void VectorPrint( std::ostream& o, T* v, uint32_t n, bool col )
    {
        uint32_t i;
        
        o << n << ": [";
        if( col ) o << endl;
        else      o << " ";
        for( uint32_t i = 0; i < n; i++ ) {
            
            o << v[ i ];
            if( col ) o << endl;
            else      o << " ";
            
        } // rof
        o << "]" << endl;
        
    }
    
// -----------------------------------------------------------------------------
    template< class T >
    inline
    T* VectorAllocateMemory( uint32_t n )
    {
        T* v = new T[ n ];

        memset( v, 0, sizeof( T ) * n );
        return( v );

    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    void VectorFreeMemory( T* v )
    {
        if( v ) delete [] v;

    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    void VectorAssignMemory( T* v, T* v_o, uint32_t n )
    {
        memcpy( v, v_o, sizeof( T ) * n );

    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    T* VectorCopyMemory( T* v, uint32_t n )
    {
        T* n_v = VectorAllocateMemory< T >( n );
        VectorAssignMemory< T >( n_v, v, n );
        return( n_v );

    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    void VectorAssignScalar( T* v, T s, uint32_t n )
    {
        uint32_t i;

        for( i = 0; i < n; v[ i ] = s, i++ );

    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    void VectorMatrixCast( T** ma, T* v, uint32_t n, uint32_t m, bool col )
    {
        uint32_t i;

        for( i = 0; i < ( ( col )? m: n ); i++ )
            ma[ ( col )? n: i ][ ( col )? i: m ] = v[ i ];

    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    void VectorAdd( T* v, T* v_l, T* v_r, uint32_t n )
    {
        uint32_t i;

        for( i = 0; i < n; v[ i ] = v_l[ i ] + v_r[ i ], i++ );

    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    void VectorSubtract( T* v, T* v_l, T* v_r, uint32_t n )
    {
        uint32_t i;

        for( i = 0; i < n; v[ i ] = v_l[ i ] - v_r[ i ], i++ );

    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    void VectorCrossProduct( T* v, T* v_l, T* v_r )
    {
        v[ 0 ] = ( v_l[ 1 ] * v_r[ 2 ] ) - ( v_l[ 2 ] * v_r[ 1 ] );
        v[ 1 ] = ( v_l[ 2 ] * v_r[ 0 ] ) - ( v_l[ 0 ] * v_r[ 2 ] );
        v[ 2 ] = ( v_l[ 0 ] * v_r[ 1 ] ) - ( v_l[ 1 ] * v_r[ 0 ] );

    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    T VectorDotProduct( T* v_l, T* v_r, uint32_t n )
    {
        T ret;
        uint32_t i;

        for( i = 0, ret = ( T )0; i < n; ret = ret + ( v_l[ i ] * v_r[ i ] ), i++ );
        return( ret );

    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    void VectorScalarProduct( T* v, T* v_l, T s, uint32_t n )
    {
        uint32_t i;

        for( i = 0; i < n; v[ i ] = v_l[ i ] * s, i++ );

    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    T VectorNorm( T* v, uint32_t n )
    {
        return( ( T )( sqrt( ( double )( VectorDotProduct< T >( v, v, n ) ) ) ) );

    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    void VectorNormalize( T* v, T* v_o, uint32_t n )
    {
        uint32_t i;
        T norm = VectorNorm< T >( v_o, n );

        norm = ( norm == ( T )0 )? ( T )1: norm;
        for( i = 0; i < n; v[ i ] = v_o[ i ] / norm, i++ );

    }

// -----------------------------------------------------------------------------


// -----------------------------------------------------------------------------
    template< class T >
    inline
    void MatrixPrint( std::ostream& o, T** ma, uint32_t n, uint32_t m )
    {
        uint32_t i, j;

        o << n << " x " << m << endl;
        for( j = 0; j < m; j++ ) {

            for( i = 0; i < n; o << ma[ i ][ j ] << " ", i++ );
            o << endl;

        } // rof

    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    T** MatrixAllocateMemory( uint32_t n, uint32_t m )
    {
        uint32_t i;
        T** ma = new T*[ n ];

        for( i = 0; i < n; i++ ) {

            ma[ i ] = new T[ m ];
            memset( ma[ i ], 0, sizeof( T ) * m );

        } // rof
        return( ma );

    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    void MatrixAssignMemory( T** ma, T** ma_o, uint32_t n, uint32_t m )
    {
        uint32_t i;

        for( i = 0; i < n; i++ )
            memcpy( ma[ i ], ma_o[ i ], sizeof( T ) * m );

    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    T** MatrixCopyMemory( T** ma, uint32_t n, uint32_t m )
    {
        T** n_ma = MatrixAllocateMemory< T >( n, m );
        MatrixAssignMemory< T >( n_ma, ma, n, m );
        return( n_ma );
        
    }
    
// -----------------------------------------------------------------------------
    template< class T >
    inline
    void MatrixAssignScalar( T** ma, T s, uint32_t n, uint32_t m )
    {
        uint32_t i, j;

        for( i = 0; i < n; i++ )
            for( j = 0; j < m; j++ )
                ma[ i ][ j ] = s;

    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    void MatrixVectorCast( T* v, T** ma, uint32_t n, uint32_t m, bool col )
    {
        uint32_t i;

        for( i = 0; i < ( ( col )? m: n ); i++ )
            v[ i ] = ma[ ( col )? n: i ][ ( col )? i: m ];

    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    void MatrixFreeMemory( T** ma, uint32_t n )
    {
        uint32_t i;

        if( ma ) {

            for( i = 0; i < n; i++ )
                if( ma[ i ] ) delete [] ma[ i ];
            delete [] ma;

        } // fi

    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    void MatrixAdd( T** ma, T** ma_l, T** ma_r, uint32_t n, uint32_t m )
    {
        uint32_t i, j;

        for( i = 0; i < n; i++ )
            for( j = 0; j < m; ma[ i ][ j ] = ma_l[ i ][ j ] + ma_r[ i ][ j ], j++ );
        
    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    void MatrixSubtract( T** ma, T** ma_l, T** ma_r, uint32_t n, uint32_t m )
    {
        uint32_t i, j;

        for( i = 0; i < n; i++ )
            for( j = 0; j < m; ma[ i ][ j ] = ma_l[ i ][ j ] - ma_r[ i ][ j ], j++ );

    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    void MatrixProduct( T** ma, T** ma_l, T** ma_r, uint32_t n, uint32_t m, uint32_t nr )
    {
        unsigned i, j, k;

        for( i = 0; i < nr; i++ )
            for( j = 0; j < m; j++ )
                for(
                    k = 0, ma[ i ][ j ] = ( T )0;
                    k < n;
                    ma[ i ][ j ] = ma[ i ][ j ] + ( ma_l[ k ][ j ] * ma_r[ i ][ k ] ), k++
                    );

    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    void MatrixScalarProduct( T** ma, T** ma_l, T s, uint32_t n, uint32_t m )
    {
        uint32_t i, j;

        for( i = 0; i < n; i++ )
            for( j = 0; j < m; ma[ i ][ j ] = ma_l[ i ][ j ] * s, j++ );

    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    void MatrixCofactor( T** ma, T** ma_o, uint32_t i, uint32_t j, uint32_t n, uint32_t m )
    {
        uint32_t k, l;
        
        for( k = 0; k < i; k++ ) {
            
            for( l = 0;     l < j; l++ ) ma[ k ][ l ]     = ma_o[ k ][ l ];
            for( l = j + 1; l < m; l++ ) ma[ k ][ l - 1 ] = ma_o[ k ][ l ];
            
        } // rof
        
        for( k = i + 1; k < n; k++ ) {
            
            for( l = 0;     l < j; l++ ) ma[ k - 1 ][ l ]     = ma_o[ k ][ l ];
            for( l = j + 1; l < m; l++ ) ma[ k - 1 ][ l - 1 ] = ma_o[ k ][ l ];
            
        } // rof
        
    }
    
// -----------------------------------------------------------------------------
    template< class T >
    inline
    T MatrixDeterminant( T** ma, uint32_t n )
    {
        uint32_t k;
        T** tmp = NULL;
        T ret = ( T )0, c;
        
        //  All these cases are for speed-up calculations.
        if( n == 1 ) {

            ret = ma[ 0 ][ 0 ];

        } else if( n == 2 ) {

            ret =
                ( ma[ 0 ][ 0 ] * ma[ 1 ][ 1 ] ) -
                ( ma[ 1 ][ 0 ] * ma[ 0 ][ 1 ] );

        } else if( n == 3 ) {

            ret =
                ( ma[ 0 ][ 0 ] * ma[ 1 ][ 1 ] * ma[ 2 ][ 2 ] ) -
                ( ma[ 0 ][ 0 ] * ma[ 2 ][ 1 ] * ma[ 1 ][ 2 ] ) -
                ( ma[ 1 ][ 0 ] * ma[ 0 ][ 1 ] * ma[ 2 ][ 2 ] ) +
                ( ma[ 1 ][ 0 ] * ma[ 2 ][ 1 ] * ma[ 0 ][ 2 ] ) +
                ( ma[ 2 ][ 0 ] * ma[ 0 ][ 1 ] * ma[ 1 ][ 2 ] ) -
                ( ma[ 2 ][ 0 ] * ma[ 1 ][ 1 ] * ma[ 0 ][ 2 ] );

        } else {
            
            tmp = MatrixAllocateMemory< T >( n - 1, n - 1 );
            for( k = 0, c = ( T )1, ret = ( T )0; k < n; k++ ) {

                MatrixCofactor< T >( tmp, ma, k, 0, n, n );
                ret = ret + ( c * ( ma[ k ][ 0 ] * MatrixDeterminant< T >( tmp, n - 1 ) ) );
                c = c * ( T )( 0 - 1 );

            } // rof
            MatrixFreeMemory< T >( tmp, n - 1 );

        } // fi
        return( ret );

    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    void MatrixInverseAdjoint( T** ma, T** ma_o, uint32_t n )
    {
        uint32_t i, j;
        T** tmp;
        T c;

        tmp = MatrixAllocateMemory< T >( n - 1, n - 1 );
        for( i = 0; i < n; i++ ) {

            for( j = 0; j < n; j++ ) {

                c = ( ( i + j ) % 2 == 0 )? ( T )1: ( T )( 0 - 1 );
                MatrixCofactor< T >( tmp, ma_o, i, j, n, n );
                ma[ j ][ i ] = c * MatrixDeterminant< T >( tmp, n - 1 );

            } // rof

        } // rof
        MatrixFreeMemory< T >( tmp, n - 1 );

        c = ( T )1 / MatrixDeterminant< T >( ma_o, n );
        MatrixScalarProduct< T >( ma, ma, c, n, n );

    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    void MatrixInverseGauss( T** ma, T** ma_o, uint32_t n )
    {
        uint32_t i, j, k, n2 = 2 * n;
        T** ma_a = MatrixAllocateMemory< T >( n2, n );
        T a, b;

        //  Augmented matrix initialization
        for( i = 0; i < n; i++ )
            for( j = 0; j < n; ma_a[ i ][ j ] = ma_o[ i ][ j ], j++ );
        for( i = n; i < n2; i++ )
            for( j = 0; j < n; ma_a[ i ][ j ] = ( i - n == j )? ( T )1: ( T )0, j++ );

        //  Reduction
        for( j = 0; j < n; j++ ) {

            a = ma_a[ j ][ j ];
            if( a != 0 ) for( i = 0; i < n2; ma_a[ i ][ j ] = ma_a[ i ][ j ] / a, i++ );
            for( k = 0; k < n; k++ ) {

                if( ( k - j ) != 0 ) {

                    b = ma_a[ j ][ k ];
                    for(
                        i = 0;
                        i < n2;
                        ma_a[ i ][ k ] = ma_a[ i ][ k ] - ( b * ma_a[ i ][ j ] ), i++
                        );

                } // fi

            } // rof

        } // rof

        //  Result assignation
        MatrixAssignMemory< T >( ma, ma_a, n, n );
        MatrixFreeMemory< T >( ma_a, n2 );

    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    void MatrixTranspose( T** ma, T** ma_o, uint32_t n, uint32_t m )
    {
        uint32_t i, j;

        for( i = 0; i < m; i++ )
            for( j = 0; j < n; ma[ i ][ j ] = ma_o[ j ][ i ], j++ );
        
    }

// -----------------------------------------------------------------------------
    template< class T >
    inline
    void MatrixLoadIdentity( T** ma, uint32_t n )
    {
        uint32_t i, j;

        for( i = 0; i < n; i++ )
            for( j = 0; j < n; ma[ i ][ j ] = ( i == j )? ( T )1: ( T )0, j++ );

    }

}

#endif // GTMLIB__MATH__VMFUNCS__HXX

// EOF - vmfuncs.h