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