lib/maracasVisuLib/include/vmfuncs.h

   1 ////////////////////////////////////////////////////////////////////////////////
   2 // vmfuncs.h
   3 // Creation : 02/02/2000
   4 // Author   : Leonardo FLOREZ VALENCIA
   5 //               l-florez@uniandes.edu.co
   6 //               lflorez@creatis.insa-lyon.fr
   7 // Copyright (C) 2000-2002 Leonardo FLOREZ VALENCIA
   8 //
   9 // This program is free software; you can redistribute it and/or
  10 // modify it under the terms of the GNU General Public License
  11 // as published by the Free Software Foundation; either version 2
  12 // of the License, or (at your option) any later version.
  13 //
  14 // This program is distributed in the hope that it will be useful,
  15 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  16 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  17 // GNU General Public License for more details.
  18 //
  19 // You should have received a copy of the GNU General Public License
  20 // along with this program; if not, write to the Free Software
  21 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  22 ////////////////////////////////////////////////////////////////////////////////
  23
  24 #ifndef GTMLIB__MATH__VMFUNCS__HXX
  25 #define GTMLIB__MATH__VMFUNCS__HXX
  26
  27 #include <iostream>
  28 #include <math.h>
  29 #include <memory.h>
  30 #include "mathdefs.h"
  31
  32 using namespace std;
  33
  34 namespace gtm
  35 {
  36 // -----------------------------------------------------------------------------
  37 //
  38 /// Functions for vectors.
  39 //@{
  40 // -----------------------------------------------------------------------------
  41     /** Stream out function for vectors.
  42      *
  43      *  @param o Output stream.
  44      *  @param v ANSI array.
  45      *  @param n Cardinality.
  46      *  @param col Should I print it in column format?
  47      */
  48     template< class T >
  49     inline
  50     void VectorPrint( std::ostream& o, T* v, uint n, bool col )
  51     {
  52         uint i;
  53
  54         o << n << ": [";
  55         if( col ) o << endl;
  56         else      o << " ";
  57         for( uint i = 0; i < n; i++ ) {
  58
  59             o << v[ i ];
  60             if( col ) o << endl;
  61             else      o << " ";
  62
  63         } // rof
  64         o << "]" << endl;
  65
  66     }
  67
  68 // -----------------------------------------------------------------------------
  69     /** Allocation function.
  70      *
  71      *  @param n Cardinality.
  72      */
  73     template< class T >
  74     inline
  75     T* VectorAllocateMemory( uint n )
  76     {
  77         T* v = new T[ n ];
  78
  79         memset( v, 0, sizeof( T ) * n );
  80         return( v );
  81
  82     }
  83
  84 // -----------------------------------------------------------------------------
  85     /** Frees vector's memory.
  86      *
  87      *  @param v Vector.
  88      */
  89     template< class T >
  90     inline
  91     void VectorFreeMemory( T* v )
  92     {
  93         if( v ) delete [] v;
  94
  95     }
  96
  97 // -----------------------------------------------------------------------------
  98     /** Copy.
  99      *
 100      *  @param v Target vector.
 101      *  @param v_o Source vector.
 102      *  @param n Cardinality.
 103      */
 104     template< class T >
 105     inline
 106     void VectorAssignMemory( T* v, T* v_o, uint n )
 107     {
 108         memcpy( v, v_o, sizeof( T ) * n );
 109
 110     }
 111
 112 // -----------------------------------------------------------------------------
 113     /** Allocation & copy.
 114      *
 115      *  @param v Vector to be copied.
 116      *  @param n Cardinality.
 117      */
 118     template< class T >
 119     inline
 120     T* VectorCopyMemory( T* v, uint n )
 121     {
 122         T* n_v = VectorAllocateMemory< T >( n );
 123         VectorAssignMemory< T >( n_v, v, n );
 124         return( n_v );
 125
 126     }
 127
 128 // -----------------------------------------------------------------------------
 129     /** Scalar assignation.
 130      *
 131      *  @param v Vector.
 132      *  @param s Scalar value.
 133      *  @param n Cardinality.
 134      */
 135     template< class T >
 136     inline
 137     void VectorAssignScalar( T* v, T s, uint n )
 138     {
 139         uint i;
 140
 141         for( i = 0; i < n; v[ i ] = s, i++ );
 142
 143     }
 144
 145 // -----------------------------------------------------------------------------
 146     /** Convert a vector in a matrix.
 147      *
 148      *  @param ma Matrix.
 149      *  @param v Vector.
 150      *  @param n Columns.
 151      *  @param m Rows.
 152      *  @param col Is it a column vector?
 153      */
 154     template< class T >
 155     inline
 156     void VectorMatrixCast( T** ma, T* v, uint n, uint m, bool col )
 157     {
 158         uint i;
 159
 160         for( i = 0; i < ( ( col )? m: n ); i++ )
 161             ma[ ( col )? n: i ][ ( col )? i: m ] = v[ i ];
 162
 163     }
 164
 165 // -----------------------------------------------------------------------------
 166     /** Performs a vectorial addition.
 167      *
 168      *  @param v Result.
 169      *  @param v_l Left operand.
 170      *  @param v_r Right operand.
 171      *  @param n Cardinality.
 172      */
 173     template< class T >
 174     inline
 175     void VectorAdd( T* v, T* v_l, T* v_r, uint n )
 176     {
 177         uint i;
 178
 179         for( i = 0; i < n; v[ i ] = v_l[ i ] + v_r[ i ], i++ );
 180
 181     }
 182
 183 // -----------------------------------------------------------------------------
 184     /** Performs a vectorial substraction.
 185      *
 186      *  @param v Result.
 187      *  @param v_l Left operand.
 188      *  @param v_r Right operand.
 189      *  @param n Cardinality.
 190      */
 191     template< class T >
 192     inline
 193     void VectorSubtract( T* v, T* v_l, T* v_r, uint n )
 194     {
 195         uint i;
 196
 197         for( i = 0; i < n; v[ i ] = v_l[ i ] - v_r[ i ], i++ );
 198
 199     }
 200
 201 // -----------------------------------------------------------------------------
 202     /** Performs a vectorial cross product.
 203      *
 204      *  @param v Result.
 205      *  @param v_l Left operand.
 206      *  @param v_r Right operand.
 207      */
 208     template< class T >
 209     inline
 210     void VectorCrossProduct( T* v, T* v_l, T* v_r )
 211     {
 212         v[ 0 ] = ( v_l[ 1 ] * v_r[ 2 ] ) - ( v_l[ 2 ] * v_r[ 1 ] );
 213         v[ 1 ] = ( v_l[ 2 ] * v_r[ 0 ] ) - ( v_l[ 0 ] * v_r[ 2 ] );
 214         v[ 2 ] = ( v_l[ 0 ] * v_r[ 1 ] ) - ( v_l[ 1 ] * v_r[ 0 ] );
 215
 216     }
 217
 218 // -----------------------------------------------------------------------------
 219     /** Performs a vectorial dot product.
 220      *
 221      *  @param v_l Left operand.
 222      *  @param v_r Right operand.
 223      *  @param n Cardinality.
 224      *  @return Dot product.
 225      */
 226     template< class T >
 227     inline
 228     T VectorDotProduct( T* v_l, T* v_r, uint n )
 229     {
 230         T ret;
 231         uint i;
 232
 233         for( i = 0, ret = ( T )0; i < n; ret = ret + ( v_l[ i ] * v_r[ i ] ), i++ );
 234         return( ret );
 235
 236     }
 237
 238 // -----------------------------------------------------------------------------
 239     /** Performs a vector-scalar product.
 240      *
 241      *  @param v Result.
 242      *  @param v_l Left operand.
 243      *  @param s Scalar.
 244      *  @param n Cardinality.
 245      */
 246     template< class T >
 247     inline
 248     void VectorScalarProduct( T* v, T* v_l, T s, uint n )
 249     {
 250         uint i;
 251
 252         for( i = 0; i < n; v[ i ] = v_l[ i ] * s, i++ );
 253
 254     }
 255
 256 // -----------------------------------------------------------------------------
 257     /** Calculates vectorial L2 norm.
 258      *
 259      *  @param v Vector.
 260      *  @param n Cardinality.
 261      */
 262     template< class T >
 263     inline
 264     T VectorNorm( T* v, uint n )
 265     {
 266         return( ( T )( sqrt( ( double )( VectorDotProduct< T >( v, v, n ) ) ) ) );
 267
 268     }
 269
 270 // -----------------------------------------------------------------------------
 271     /** Calculates a normal vector, usign the L2 norm.
 272      *
 273      *  @param v Result.
 274      *  @param v_o Original vector.
 275      *  @param n Cardinality.
 276      */
 277     template< class T >
 278     inline
 279     void VectorNormalize( T* v, T* v_o, uint n )
 280     {
 281         uint i;
 282         T norm = VectorNorm< T >( v_o, n );
 283
 284         norm = ( norm == ( T )0 )? ( T )1: norm;
 285         for( i = 0; i < n; v[ i ] = v_o[ i ] / norm, i++ );
 286
 287     }
 288
 289 // -----------------------------------------------------------------------------
 290 //@}
 291 /// Functions for matrices.
 292 //@{
 293 // -----------------------------------------------------------------------------
 294     /** Stream out function for matrices.
 295      *
 296      *  @param o Output stream.
 297      *  @param ma Matrix.
 298      *  @param n Columns.
 299      *  @param m Rows.
 300      */
 301     template< class T >
 302     inline
 303     void MatrixPrint( std::ostream& o, T** ma, uint n, uint m )
 304     {
 305         uint i, j;
 306
 307         o << n << " x " << m << endl;
 308         for( j = 0; j < m; j++ ) {
 309
 310             for( i = 0; i < n; o << ma[ i ][ j ] << " ", i++ );
 311             o << endl;
 312
 313         } // rof
 314
 315     }
 316
 317 // -----------------------------------------------------------------------------
 318     /** Allocates memory for a matrix.
 319      *
 320      *  @param n Columns.
 321      *  @param m Rows.
 322      */
 323     template< class T >
 324     inline
 325     T** MatrixAllocateMemory( uint n, uint m )
 326     {
 327         uint i;
 328         T** ma = new T*[ n ];
 329
 330         for( i = 0; i < n; i++ ) {
 331
 332             ma[ i ] = new T[ m ];
 333             memset( ma[ i ], 0, sizeof( T ) * m );
 334
 335         } // rof
 336         return( ma );
 337
 338     }
 339
 340 // -----------------------------------------------------------------------------
 341     /** Copies.
 342      *
 343      *  @param ma Target matrix.
 344      *  @param ma_o Source matrix.
 345      *  @param n Columns.
 346      *  @param m Rows.
 347      */
 348     template< class T >
 349     inline
 350     void MatrixAssignMemory( T** ma, T** ma_o, uint n, uint m )
 351     {
 352         uint i;
 353
 354         for( i = 0; i < n; i++ )
 355             memcpy( ma[ i ], ma_o[ i ], sizeof( T ) * m );
 356
 357     }
 358
 359 // -----------------------------------------------------------------------------
 360     /** Allocates & copies.
 361      *
 362      *  @param ma Matrix.
 363      *  @param n Columns.
 364      *  @param m Rows.
 365      */
 366     template< class T >
 367     inline
 368     T** MatrixCopyMemory( T** ma, uint n, uint m )
 369     {
 370         T** n_ma = MatrixAllocateMemory< T >( n, m );
 371         MatrixAssignMemory< T >( n_ma, ma, n, m );
 372         return( n_ma );
 373
 374     }
 375
 376 // -----------------------------------------------------------------------------
 377     /** Scalar assignation.
 378      *
 379      *  @param ma Matrix.
 380      *  @param s Scalar.
 381      *  @param n Columns.
 382      *  @param m Rows.
 383      */
 384     template< class T >
 385     inline
 386     void MatrixAssignScalar( T** ma, T s, uint n, uint m )
 387     {
 388         uint i, j;
 389
 390         for( i = 0; i < n; i++ )
 391             for( j = 0; j < m; j++ )
 392                 ma[ i ][ j ] = s;
 393
 394     }
 395
 396 // -----------------------------------------------------------------------------
 397     /** Converts a matrix in a vector.
 398      *
 399      *  @param v Vector.
 400      *  @param ma Matrix.
 401      *  @param n Columns.
 402      *  @param m Rows.
 403      *  @param col Convert to a column vector?
 404      */
 405     template< class T >
 406     inline
 407     void MatrixVectorCast( T* v, T** ma, uint n, uint m, bool col )
 408     {
 409         uint i;
 410
 411         for( i = 0; i < ( ( col )? m: n ); i++ )
 412             v[ i ] = ma[ ( col )? n: i ][ ( col )? i: m ];
 413
 414     }
 415
 416 // -----------------------------------------------------------------------------
 417     /** Frees matrix memory.
 418      *
 419      *  @param ma Matrix.
 420      *  @param n Columns.
 421      */
 422     template< class T >
 423     inline
 424     void MatrixFreeMemory( T** ma, uint n )
 425     {
 426         uint i;
 427
 428         if( ma ) {
 429
 430             for( i = 0; i < n; i++ )
 431                 if( ma[ i ] ) delete [] ma[ i ];
 432             delete [] ma;
 433
 434         } // fi
 435
 436     }
 437
 438 // -----------------------------------------------------------------------------
 439     /** Performs a matricial addition.
 440      *
 441      *  @param ma Result.
 442      *  @param ma_l Left operand.
 443      *  @param ma_r Right operand.
 444      *  @param n Columns.
 445      *  @param m Rows.
 446      */
 447     template< class T >
 448     inline
 449     void MatrixAdd( T** ma, T** ma_l, T** ma_r, uint n, uint m )
 450     {
 451         uint i, j;
 452
 453         for( i = 0; i < n; i++ )
 454             for( j = 0; j < m; ma[ i ][ j ] = ma_l[ i ][ j ] + ma_r[ i ][ j ], j++ );
 455
 456     }
 457
 458 // -----------------------------------------------------------------------------
 459     /** Performs a matricial substraction.
 460      *
 461      *  @param ma Result.
 462      *  @param ma_l Left operand.
 463      *  @param ma_r Right operand.
 464      *  @param n Columns.
 465      *  @param m Rows.
 466      */
 467     template< class T >
 468     inline
 469     void MatrixSubtract( T** ma, T** ma_l, T** ma_r, uint n, uint m )
 470     {
 471         uint i, j;
 472
 473         for( i = 0; i < n; i++ )
 474             for( j = 0; j < m; ma[ i ][ j ] = ma_l[ i ][ j ] - ma_r[ i ][ j ], j++ );
 475
 476     }
 477
 478 // -----------------------------------------------------------------------------
 479     /** Performs a matricial product.
 480      *
 481      *  @param ma Result.
 482      *  @param ma_l Left operand.
 483      *  @param ma_r Right operand.
 484      *  @param n Left columns.
 485      *  @param m Left rows/right columns.
 486      *  @param nr Right columns.
 487      */
 488     template< class T >
 489     inline
 490     void MatrixProduct( T** ma, T** ma_l, T** ma_r, uint n, uint m, uint nr )
 491     {
 492         unsigned i, j, k;
 493
 494         for( i = 0; i < nr; i++ )
 495             for( j = 0; j < m; j++ )
 496                 for(
 497                     k = 0, ma[ i ][ j ] = ( T )0;
 498                     k < n;
 499                     ma[ i ][ j ] = ma[ i ][ j ] + ( ma_l[ k ][ j ] * ma_r[ i ][ k ] ), k++
 500                     );
 501
 502     }
 503
 504 // -----------------------------------------------------------------------------
 505     /** Performs a scalar-matrix product.
 506      *
 507      *  @param ma Result.
 508      *  @param ma_l Left operand.
 509      *  @param s Scalar.
 510      *  @param n Columns.
 511      *  @param m Rows.
 512      */
 513     template< class T >
 514     inline
 515     void MatrixScalarProduct( T** ma, T** ma_l, T s, uint n, uint m )
 516     {
 517         uint i, j;
 518
 519         for( i = 0; i < n; i++ )
 520             for( j = 0; j < m; ma[ i ][ j ] = ma_l[ i ][ j ] * s, j++ );
 521
 522     }
 523
 524 // -----------------------------------------------------------------------------
 525     /** Gets the matrix cofactor.
 526      *
 527      *  @param ma Result.
 528      *  @param ma_o Matrix.
 529      *  @param i Column index.
 530      *  @param j Row index.
 531      *  @param n Columns.
 532      *  @param m Rows.
 533      */
 534     template< class T >
 535     inline
 536     void MatrixCofactor( T** ma, T** ma_o, uint i, uint j, uint n, uint m )
 537     {
 538         uint k, l;
 539
 540         for( k = 0; k < i; k++ ) {
 541
 542             for( l = 0;     l < j; l++ ) ma[ k ][ l ]     = ma_o[ k ][ l ];
 543             for( l = j + 1; l < m; l++ ) ma[ k ][ l - 1 ] = ma_o[ k ][ l ];
 544
 545         } // rof
 546
 547         for( k = i + 1; k < n; k++ ) {
 548
 549             for( l = 0;     l < j; l++ ) ma[ k - 1 ][ l ]     = ma_o[ k ][ l ];
 550             for( l = j + 1; l < m; l++ ) ma[ k - 1 ][ l - 1 ] = ma_o[ k ][ l ];
 551
 552         } // rof
 553
 554     }
 555
 556 // -----------------------------------------------------------------------------
 557     /** Gets the matrix determinant (square matrices only).
 558      *
 559      *  @param ma Matrix.
 560      *  @param n Dimension.
 561      */
 562     template< class T >
 563     inline
 564     T MatrixDeterminant( T** ma, uint n )
 565     {
 566         uint k;
 567         T** tmp = NULL;
 568         T ret = ( T )0, c;
 569
 570         //  All these cases are for speed-up calculations.
 571         if( n == 1 ) {
 572
 573             ret = ma[ 0 ][ 0 ];
 574
 575         } else if( n == 2 ) {
 576
 577             ret =
 578                 ( ma[ 0 ][ 0 ] * ma[ 1 ][ 1 ] ) -
 579                 ( ma[ 1 ][ 0 ] * ma[ 0 ][ 1 ] );
 580
 581         } else if( n == 3 ) {
 582
 583             ret =
 584                 ( ma[ 0 ][ 0 ] * ma[ 1 ][ 1 ] * ma[ 2 ][ 2 ] ) -
 585                 ( ma[ 0 ][ 0 ] * ma[ 2 ][ 1 ] * ma[ 1 ][ 2 ] ) -
 586                 ( ma[ 1 ][ 0 ] * ma[ 0 ][ 1 ] * ma[ 2 ][ 2 ] ) +
 587                 ( ma[ 1 ][ 0 ] * ma[ 2 ][ 1 ] * ma[ 0 ][ 2 ] ) +
 588                 ( ma[ 2 ][ 0 ] * ma[ 0 ][ 1 ] * ma[ 1 ][ 2 ] ) -
 589                 ( ma[ 2 ][ 0 ] * ma[ 1 ][ 1 ] * ma[ 0 ][ 2 ] );
 590
 591         } else {
 592
 593             tmp = MatrixAllocateMemory< T >( n - 1, n - 1 );
 594             for( k = 0, c = ( T )1, ret = ( T )0; k < n; k++ ) {
 595
 596                 MatrixCofactor< T >( tmp, ma, k, 0, n, n );
 597                 ret = ret + ( c * ( ma[ k ][ 0 ] * MatrixDeterminant< T >( tmp, n - 1 ) ) );
 598                 c = c * ( T )( 0 - 1 );
 599
 600             } // rof
 601             MatrixFreeMemory< T >( tmp, n - 1 );
 602
 603         } // fi
 604         return( ret );
 605
 606     }
 607
 608 // -----------------------------------------------------------------------------
 609     /** Calculates the inverse using the adjoint method (only square matrices).
 610      *
 611      *  @param ma Result.
 612      *  @param ma_o Matrix.
 613      *  @param n Dimension.
 614      */
 615     template< class T >
 616     inline
 617     void MatrixInverseAdjoint( T** ma, T** ma_o, uint n )
 618     {
 619         uint i, j;
 620         T** tmp;
 621         T c;
 622
 623         tmp = MatrixAllocateMemory< T >( n - 1, n - 1 );
 624         for( i = 0; i < n; i++ ) {
 625
 626             for( j = 0; j < n; j++ ) {
 627
 628                 c = ( ( i + j ) % 2 == 0 )? ( T )1: ( T )( 0 - 1 );
 629                 MatrixCofactor< T >( tmp, ma_o, i, j, n, n );
 630                 ma[ j ][ i ] = c * MatrixDeterminant< T >( tmp, n - 1 );
 631
 632             } // rof
 633
 634         } // rof
 635         MatrixFreeMemory< T >( tmp, n - 1 );
 636
 637         c = ( T )1 / MatrixDeterminant< T >( ma_o, n );
 638         MatrixScalarProduct< T >( ma, ma, c, n, n );
 639
 640     }
 641
 642 // -----------------------------------------------------------------------------
 643     /** Calculates the inverse using the gauss method (only square matrices).
 644      *
 645      *  @param ma Result.
 646      *  @param ma_o Matrix.
 647      *  @param n Dimension.
 648      *  @warning Adapted from a java-implementation: http://www.nauticom.net/www/jdtaft/JavaMatrix.htm
 649      */
 650     template< class T >
 651     inline
 652     void MatrixInverseGauss( T** ma, T** ma_o, uint n )
 653     {
 654         uint i, j, k, n2 = 2 * n;
 655         T** ma_a = MatrixAllocateMemory< T >( n2, n );
 656         T a, b;
 657
 658         //  Augmented matrix initialization
 659         for( i = 0; i < n; i++ )
 660             for( j = 0; j < n; ma_a[ i ][ j ] = ma_o[ i ][ j ], j++ );
 661         for( i = n; i < n2; i++ )
 662             for( j = 0; j < n; ma_a[ i ][ j ] = ( i - n == j )? ( T )1: ( T )0, j++ );
 663
 664         //  Reduction
 665         for( j = 0; j < n; j++ ) {
 666
 667             a = ma_a[ j ][ j ];
 668             if( a != 0 ) for( i = 0; i < n2; ma_a[ i ][ j ] = ma_a[ i ][ j ] / a, i++ );
 669             for( k = 0; k < n; k++ ) {
 670
 671                 if( ( k - j ) != 0 ) {
 672
 673                     b = ma_a[ j ][ k ];
 674                     for(
 675                         i = 0;
 676                         i < n2;
 677                         ma_a[ i ][ k ] = ma_a[ i ][ k ] - ( b * ma_a[ i ][ j ] ), i++
 678                         );
 679
 680                 } // fi
 681
 682             } // rof
 683
 684         } // rof
 685
 686         //  Result assignation
 687         MatrixAssignMemory< T >( ma, ma_a, n, n );
 688         MatrixFreeMemory< T >( ma_a, n2 );
 689
 690     }
 691
 692 // -----------------------------------------------------------------------------
 693     /** Gets the transpose matrix.
 694      *
 695      *  @param ma Matrix result.
 696      *  @param ma_o Matrix.
 697      *  @param n Columns.
 698      *  @param m Rows.
 699      */
 700     template< class T >
 701     inline
 702     void MatrixTranspose( T** ma, T** ma_o, uint n, uint m )
 703     {
 704         uint i, j;
 705
 706         for( i = 0; i < m; i++ )
 707             for( j = 0; j < n; ma[ i ][ j ] = ma_o[ j ][ i ], j++ );
 708
 709     }
 710
 711 // -----------------------------------------------------------------------------
 712     /** Load a square matrix with the identity.
 713      *
 714      *  @param ma Matrix.
 715      *  @param n Dimension.
 716      */
 717     template< class T >
 718     inline
 719     void MatrixLoadIdentity( T** ma, uint n )
 720     {
 721         uint i, j;
 722
 723         for( i = 0; i < n; i++ )
 724             for( j = 0; j < n; ma[ i ][ j ] = ( i == j )? ( T )1: ( T )0, j++ );
 725
 726     }
 727
 728 }
 729 //@}
 730
 731 #endif // GTMLIB__MATH__VMFUNCS__HXX
 732
 733 // EOF - vmfuncs.h