1 /*# ---------------------------------------------------------------------
3 # Copyright (c) CREATIS (Centre de Recherche en Acquisition et Traitement de l'Image
5 # Authors : Eduardo Davila, Frederic Cervenansky, Claire Mouton
6 # Previous Authors : Laurent Guigues, Jean-Pierre Roux
7 # CreaTools website : www.creatis.insa-lyon.fr/site/fr/creatools_accueil
9 # This software is governed by the CeCILL-B license under French law and
10 # abiding by the rules of distribution of free software. You can use,
11 # modify and/ or redistribute the software under the terms of the CeCILL-B
12 # license as circulated by CEA, CNRS and INRIA at the following URL
13 # http://www.cecill.info/licences/Licence_CeCILL-B_V1-en.html
14 # or in the file LICENSE.txt.
16 # As a counterpart to the access to the source code and rights to copy,
17 # modify and redistribute granted by the license, users are provided only
18 # with a limited warranty and the software's author, the holder of the
19 # economic rights, and the successive licensors have only limited
22 # The fact that you are presently reading this means that you have had
23 # knowledge of the CeCILL-B license and that you accept its terms.
24 # ------------------------------------------------------------------------ */
26 ////////////////////////////////////////////////////////////////////////////////
28 // Creation : 02/02/2000
29 // Author : Leonardo FLOREZ VALENCIA
30 // l-florez@uniandes.edu.co
31 // lflorez@creatis.insa-lyon.fr
32 // Copyright (C) 2000-2002 Leonardo FLOREZ VALENCIA
34 // This program is free software; you can redistribute it and/or
35 // modify it under the terms of the GNU General Public License
36 // as published by the Free Software Foundation; either version 2
37 // of the License, or (at your option) any later version.
39 // This program is distributed in the hope that it will be useful,
40 // but WITHOUT ANY WARRANTY; without even the implied warranty of
41 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
42 // GNU General Public License for more details.
44 // You should have received a copy of the GNU General Public License
45 // along with this program; if not, write to the Free Software
46 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
47 ////////////////////////////////////////////////////////////////////////////////
49 #ifndef GTMLIB__MATH__VMFUNCS__HXX
50 #define GTMLIB__MATH__VMFUNCS__HXX
57 #include <creaSystem.h>
63 // -----------------------------------------------------------------------------
65 /// Functions for vectors.
67 // -----------------------------------------------------------------------------
68 /** Stream out function for vectors.
70 * @param o Output stream.
71 * @param v ANSI array.
72 * @param n Cardinality.
73 * @param col Should I print it in column format?
77 void VectorPrint( std::ostream& o, T* v, uint32_t n, bool col )
84 for( uint32_t i = 0; i < n; i++ ) {
95 // -----------------------------------------------------------------------------
96 /** Allocation function.
98 * @param n Cardinality.
102 T* VectorAllocateMemory( uint32_t n )
106 memset( v, 0, sizeof( T ) * n );
111 // -----------------------------------------------------------------------------
112 /** Frees vector's memory.
118 void VectorFreeMemory( T* v )
124 // -----------------------------------------------------------------------------
127 * @param v Target vector.
128 * @param v_o Source vector.
129 * @param n Cardinality.
133 void VectorAssignMemory( T* v, T* v_o, uint32_t n )
135 memcpy( v, v_o, sizeof( T ) * n );
139 // -----------------------------------------------------------------------------
140 /** Allocation & copy.
142 * @param v Vector to be copied.
143 * @param n Cardinality.
147 T* VectorCopyMemory( T* v, uint32_t n )
149 T* n_v = VectorAllocateMemory< T >( n );
150 VectorAssignMemory< T >( n_v, v, n );
155 // -----------------------------------------------------------------------------
156 /** Scalar assignation.
159 * @param s Scalar value.
160 * @param n Cardinality.
164 void VectorAssignScalar( T* v, T s, uint32_t n )
168 for( i = 0; i < n; v[ i ] = s, i++ );
172 // -----------------------------------------------------------------------------
173 /** Convert a vector in a matrix.
179 * @param col Is it a column vector?
183 void VectorMatrixCast( T** ma, T* v, uint32_t n, uint32_t m, bool col )
187 for( i = 0; i < ( ( col )? m: n ); i++ )
188 ma[ ( col )? n: i ][ ( col )? i: m ] = v[ i ];
192 // -----------------------------------------------------------------------------
193 /** Performs a vectorial addition.
196 * @param v_l Left operand.
197 * @param v_r Right operand.
198 * @param n Cardinality.
202 void VectorAdd( T* v, T* v_l, T* v_r, uint32_t n )
206 for( i = 0; i < n; v[ i ] = v_l[ i ] + v_r[ i ], i++ );
210 // -----------------------------------------------------------------------------
211 /** Performs a vectorial substraction.
214 * @param v_l Left operand.
215 * @param v_r Right operand.
216 * @param n Cardinality.
220 void VectorSubtract( T* v, T* v_l, T* v_r, uint32_t n )
224 for( i = 0; i < n; v[ i ] = v_l[ i ] - v_r[ i ], i++ );
228 // -----------------------------------------------------------------------------
229 /** Performs a vectorial cross product.
232 * @param v_l Left operand.
233 * @param v_r Right operand.
237 void VectorCrossProduct( T* v, T* v_l, T* v_r )
239 v[ 0 ] = ( v_l[ 1 ] * v_r[ 2 ] ) - ( v_l[ 2 ] * v_r[ 1 ] );
240 v[ 1 ] = ( v_l[ 2 ] * v_r[ 0 ] ) - ( v_l[ 0 ] * v_r[ 2 ] );
241 v[ 2 ] = ( v_l[ 0 ] * v_r[ 1 ] ) - ( v_l[ 1 ] * v_r[ 0 ] );
245 // -----------------------------------------------------------------------------
246 /** Performs a vectorial dot product.
248 * @param v_l Left operand.
249 * @param v_r Right operand.
250 * @param n Cardinality.
251 * @return Dot product.
255 T VectorDotProduct( T* v_l, T* v_r, uint32_t n )
260 for( i = 0, ret = ( T )0; i < n; ret = ret + ( v_l[ i ] * v_r[ i ] ), i++ );
265 // -----------------------------------------------------------------------------
266 /** Performs a vector-scalar product.
269 * @param v_l Left operand.
271 * @param n Cardinality.
275 void VectorScalarProduct( T* v, T* v_l, T s, uint32_t n )
279 for( i = 0; i < n; v[ i ] = v_l[ i ] * s, i++ );
283 // -----------------------------------------------------------------------------
284 /** Calculates vectorial L2 norm.
287 * @param n Cardinality.
291 T VectorNorm( T* v, uint32_t n )
293 return( ( T )( sqrt( ( double )( VectorDotProduct< T >( v, v, n ) ) ) ) );
297 // -----------------------------------------------------------------------------
298 /** Calculates a normal vector, usign the L2 norm.
301 * @param v_o Original vector.
302 * @param n Cardinality.
306 void VectorNormalize( T* v, T* v_o, uint32_t n )
309 T norm = VectorNorm< T >( v_o, n );
311 norm = ( norm == ( T )0 )? ( T )1: norm;
312 for( i = 0; i < n; v[ i ] = v_o[ i ] / norm, i++ );
316 // -----------------------------------------------------------------------------
318 /// Functions for matrices.
320 // -----------------------------------------------------------------------------
321 /** Stream out function for matrices.
323 * @param o Output stream.
330 void MatrixPrint( std::ostream& o, T** ma, uint32_t n, uint32_t m )
334 o << n << " x " << m << endl;
335 for( j = 0; j < m; j++ ) {
337 for( i = 0; i < n; o << ma[ i ][ j ] << " ", i++ );
344 // -----------------------------------------------------------------------------
345 /** Allocates memory for a matrix.
352 T** MatrixAllocateMemory( uint32_t n, uint32_t m )
355 T** ma = new T*[ n ];
357 for( i = 0; i < n; i++ ) {
359 ma[ i ] = new T[ m ];
360 memset( ma[ i ], 0, sizeof( T ) * m );
367 // -----------------------------------------------------------------------------
370 * @param ma Target matrix.
371 * @param ma_o Source matrix.
377 void MatrixAssignMemory( T** ma, T** ma_o, uint32_t n, uint32_t m )
381 for( i = 0; i < n; i++ )
382 memcpy( ma[ i ], ma_o[ i ], sizeof( T ) * m );
386 // -----------------------------------------------------------------------------
387 /** Allocates & copies.
395 T** MatrixCopyMemory( T** ma, uint32_t n, uint32_t m )
397 T** n_ma = MatrixAllocateMemory< T >( n, m );
398 MatrixAssignMemory< T >( n_ma, ma, n, m );
403 // -----------------------------------------------------------------------------
404 /** Scalar assignation.
413 void MatrixAssignScalar( T** ma, T s, uint32_t n, uint32_t m )
417 for( i = 0; i < n; i++ )
418 for( j = 0; j < m; j++ )
423 // -----------------------------------------------------------------------------
424 /** Converts a matrix in a vector.
430 * @param col Convert to a column vector?
434 void MatrixVectorCast( T* v, T** ma, uint32_t n, uint32_t m, bool col )
438 for( i = 0; i < ( ( col )? m: n ); i++ )
439 v[ i ] = ma[ ( col )? n: i ][ ( col )? i: m ];
443 // -----------------------------------------------------------------------------
444 /** Frees matrix memory.
451 void MatrixFreeMemory( T** ma, uint32_t n )
457 for( i = 0; i < n; i++ )
458 if( ma[ i ] ) delete [] ma[ i ];
465 // -----------------------------------------------------------------------------
466 /** Performs a matricial addition.
469 * @param ma_l Left operand.
470 * @param ma_r Right operand.
476 void MatrixAdd( T** ma, T** ma_l, T** ma_r, uint32_t n, uint32_t m )
480 for( i = 0; i < n; i++ )
481 for( j = 0; j < m; ma[ i ][ j ] = ma_l[ i ][ j ] + ma_r[ i ][ j ], j++ );
485 // -----------------------------------------------------------------------------
486 /** Performs a matricial substraction.
489 * @param ma_l Left operand.
490 * @param ma_r Right operand.
496 void MatrixSubtract( T** ma, T** ma_l, T** ma_r, uint32_t n, uint32_t m )
500 for( i = 0; i < n; i++ )
501 for( j = 0; j < m; ma[ i ][ j ] = ma_l[ i ][ j ] - ma_r[ i ][ j ], j++ );
505 // -----------------------------------------------------------------------------
506 /** Performs a matricial product.
509 * @param ma_l Left operand.
510 * @param ma_r Right operand.
511 * @param n Left columns.
512 * @param m Left rows/right columns.
513 * @param nr Right columns.
517 void MatrixProduct( T** ma, T** ma_l, T** ma_r, uint32_t n, uint32_t m, uint32_t nr )
521 for( i = 0; i < nr; i++ )
522 for( j = 0; j < m; j++ )
524 k = 0, ma[ i ][ j ] = ( T )0;
526 ma[ i ][ j ] = ma[ i ][ j ] + ( ma_l[ k ][ j ] * ma_r[ i ][ k ] ), k++
531 // -----------------------------------------------------------------------------
532 /** Performs a scalar-matrix product.
535 * @param ma_l Left operand.
542 void MatrixScalarProduct( T** ma, T** ma_l, T s, uint32_t n, uint32_t m )
546 for( i = 0; i < n; i++ )
547 for( j = 0; j < m; ma[ i ][ j ] = ma_l[ i ][ j ] * s, j++ );
551 // -----------------------------------------------------------------------------
552 /** Gets the matrix cofactor.
555 * @param ma_o Matrix.
556 * @param i Column index.
557 * @param j Row index.
563 void MatrixCofactor( T** ma, T** ma_o, uint32_t i, uint32_t j, uint32_t n, uint32_t m )
567 for( k = 0; k < i; k++ ) {
569 for( l = 0; l < j; l++ ) ma[ k ][ l ] = ma_o[ k ][ l ];
570 for( l = j + 1; l < m; l++ ) ma[ k ][ l - 1 ] = ma_o[ k ][ l ];
574 for( k = i + 1; k < n; k++ ) {
576 for( l = 0; l < j; l++ ) ma[ k - 1 ][ l ] = ma_o[ k ][ l ];
577 for( l = j + 1; l < m; l++ ) ma[ k - 1 ][ l - 1 ] = ma_o[ k ][ l ];
583 // -----------------------------------------------------------------------------
584 /** Gets the matrix determinant (square matrices only).
587 * @param n Dimension.
591 T MatrixDeterminant( T** ma, uint32_t n )
597 // All these cases are for speed-up calculations.
602 } else if( n == 2 ) {
605 ( ma[ 0 ][ 0 ] * ma[ 1 ][ 1 ] ) -
606 ( ma[ 1 ][ 0 ] * ma[ 0 ][ 1 ] );
608 } else if( n == 3 ) {
611 ( ma[ 0 ][ 0 ] * ma[ 1 ][ 1 ] * ma[ 2 ][ 2 ] ) -
612 ( ma[ 0 ][ 0 ] * ma[ 2 ][ 1 ] * ma[ 1 ][ 2 ] ) -
613 ( ma[ 1 ][ 0 ] * ma[ 0 ][ 1 ] * ma[ 2 ][ 2 ] ) +
614 ( ma[ 1 ][ 0 ] * ma[ 2 ][ 1 ] * ma[ 0 ][ 2 ] ) +
615 ( ma[ 2 ][ 0 ] * ma[ 0 ][ 1 ] * ma[ 1 ][ 2 ] ) -
616 ( ma[ 2 ][ 0 ] * ma[ 1 ][ 1 ] * ma[ 0 ][ 2 ] );
620 tmp = MatrixAllocateMemory< T >( n - 1, n - 1 );
621 for( k = 0, c = ( T )1, ret = ( T )0; k < n; k++ ) {
623 MatrixCofactor< T >( tmp, ma, k, 0, n, n );
624 ret = ret + ( c * ( ma[ k ][ 0 ] * MatrixDeterminant< T >( tmp, n - 1 ) ) );
625 c = c * ( T )( 0 - 1 );
628 MatrixFreeMemory< T >( tmp, n - 1 );
635 // -----------------------------------------------------------------------------
636 /** Calculates the inverse using the adjoint method (only square matrices).
639 * @param ma_o Matrix.
640 * @param n Dimension.
644 void MatrixInverseAdjoint( T** ma, T** ma_o, uint32_t n )
650 tmp = MatrixAllocateMemory< T >( n - 1, n - 1 );
651 for( i = 0; i < n; i++ ) {
653 for( j = 0; j < n; j++ ) {
655 c = ( ( i + j ) % 2 == 0 )? ( T )1: ( T )( 0 - 1 );
656 MatrixCofactor< T >( tmp, ma_o, i, j, n, n );
657 ma[ j ][ i ] = c * MatrixDeterminant< T >( tmp, n - 1 );
662 MatrixFreeMemory< T >( tmp, n - 1 );
664 c = ( T )1 / MatrixDeterminant< T >( ma_o, n );
665 MatrixScalarProduct< T >( ma, ma, c, n, n );
669 // -----------------------------------------------------------------------------
670 /** Calculates the inverse using the gauss method (only square matrices).
673 * @param ma_o Matrix.
674 * @param n Dimension.
675 * @warning Adapted from a java-implementation: http://www.nauticom.net/www/jdtaft/JavaMatrix.htm
679 void MatrixInverseGauss( T** ma, T** ma_o, uint32_t n )
681 uint32_t i, j, k, n2 = 2 * n;
682 T** ma_a = MatrixAllocateMemory< T >( n2, n );
685 // Augmented matrix initialization
686 for( i = 0; i < n; i++ )
687 for( j = 0; j < n; ma_a[ i ][ j ] = ma_o[ i ][ j ], j++ );
688 for( i = n; i < n2; i++ )
689 for( j = 0; j < n; ma_a[ i ][ j ] = ( i - n == j )? ( T )1: ( T )0, j++ );
692 for( j = 0; j < n; j++ ) {
695 if( a != 0 ) for( i = 0; i < n2; ma_a[ i ][ j ] = ma_a[ i ][ j ] / a, i++ );
696 for( k = 0; k < n; k++ ) {
698 if( ( k - j ) != 0 ) {
704 ma_a[ i ][ k ] = ma_a[ i ][ k ] - ( b * ma_a[ i ][ j ] ), i++
713 // Result assignation
714 MatrixAssignMemory< T >( ma, ma_a, n, n );
715 MatrixFreeMemory< T >( ma_a, n2 );
719 // -----------------------------------------------------------------------------
720 /** Gets the transpose matrix.
722 * @param ma Matrix result.
723 * @param ma_o Matrix.
729 void MatrixTranspose( T** ma, T** ma_o, uint32_t n, uint32_t m )
733 for( i = 0; i < m; i++ )
734 for( j = 0; j < n; ma[ i ][ j ] = ma_o[ j ][ i ], j++ );
738 // -----------------------------------------------------------------------------
739 /** Load a square matrix with the identity.
742 * @param n Dimension.
746 void MatrixLoadIdentity( T** ma, uint32_t n )
750 for( i = 0; i < n; i++ )
751 for( j = 0; j < n; ma[ i ][ j ] = ( i == j )? ( T )1: ( T )0, j++ );
758 #endif // GTMLIB__MATH__VMFUNCS__HXX