1 ////////////////////////////////////////////////////////////////////////////////
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
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.
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.
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 ////////////////////////////////////////////////////////////////////////////////
24 #ifndef GTMLIB__MATH__VMFUNCS__HXX
25 #define GTMLIB__MATH__VMFUNCS__HXX
32 #include <creaSystem.h>
38 // -----------------------------------------------------------------------------
40 /// Functions for vectors.
42 // -----------------------------------------------------------------------------
43 /** Stream out function for vectors.
45 * @param o Output stream.
46 * @param v ANSI array.
47 * @param n Cardinality.
48 * @param col Should I print it in column format?
52 void VectorPrint( std::ostream& o, T* v, uint32_t n, bool col )
59 for( uint32_t i = 0; i < n; i++ ) {
70 // -----------------------------------------------------------------------------
71 /** Allocation function.
73 * @param n Cardinality.
77 T* VectorAllocateMemory( uint32_t n )
81 memset( v, 0, sizeof( T ) * n );
86 // -----------------------------------------------------------------------------
87 /** Frees vector's memory.
93 void VectorFreeMemory( T* v )
99 // -----------------------------------------------------------------------------
102 * @param v Target vector.
103 * @param v_o Source vector.
104 * @param n Cardinality.
108 void VectorAssignMemory( T* v, T* v_o, uint32_t n )
110 memcpy( v, v_o, sizeof( T ) * n );
114 // -----------------------------------------------------------------------------
115 /** Allocation & copy.
117 * @param v Vector to be copied.
118 * @param n Cardinality.
122 T* VectorCopyMemory( T* v, uint32_t n )
124 T* n_v = VectorAllocateMemory< T >( n );
125 VectorAssignMemory< T >( n_v, v, n );
130 // -----------------------------------------------------------------------------
131 /** Scalar assignation.
134 * @param s Scalar value.
135 * @param n Cardinality.
139 void VectorAssignScalar( T* v, T s, uint32_t n )
143 for( i = 0; i < n; v[ i ] = s, i++ );
147 // -----------------------------------------------------------------------------
148 /** Convert a vector in a matrix.
154 * @param col Is it a column vector?
158 void VectorMatrixCast( T** ma, T* v, uint32_t n, uint32_t m, bool col )
162 for( i = 0; i < ( ( col )? m: n ); i++ )
163 ma[ ( col )? n: i ][ ( col )? i: m ] = v[ i ];
167 // -----------------------------------------------------------------------------
168 /** Performs a vectorial addition.
171 * @param v_l Left operand.
172 * @param v_r Right operand.
173 * @param n Cardinality.
177 void VectorAdd( T* v, T* v_l, T* v_r, uint32_t n )
181 for( i = 0; i < n; v[ i ] = v_l[ i ] + v_r[ i ], i++ );
185 // -----------------------------------------------------------------------------
186 /** Performs a vectorial substraction.
189 * @param v_l Left operand.
190 * @param v_r Right operand.
191 * @param n Cardinality.
195 void VectorSubtract( T* v, T* v_l, T* v_r, uint32_t n )
199 for( i = 0; i < n; v[ i ] = v_l[ i ] - v_r[ i ], i++ );
203 // -----------------------------------------------------------------------------
204 /** Performs a vectorial cross product.
207 * @param v_l Left operand.
208 * @param v_r Right operand.
212 void VectorCrossProduct( T* v, T* v_l, T* v_r )
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 ] );
220 // -----------------------------------------------------------------------------
221 /** Performs a vectorial dot product.
223 * @param v_l Left operand.
224 * @param v_r Right operand.
225 * @param n Cardinality.
226 * @return Dot product.
230 T VectorDotProduct( T* v_l, T* v_r, uint32_t n )
235 for( i = 0, ret = ( T )0; i < n; ret = ret + ( v_l[ i ] * v_r[ i ] ), i++ );
240 // -----------------------------------------------------------------------------
241 /** Performs a vector-scalar product.
244 * @param v_l Left operand.
246 * @param n Cardinality.
250 void VectorScalarProduct( T* v, T* v_l, T s, uint32_t n )
254 for( i = 0; i < n; v[ i ] = v_l[ i ] * s, i++ );
258 // -----------------------------------------------------------------------------
259 /** Calculates vectorial L2 norm.
262 * @param n Cardinality.
266 T VectorNorm( T* v, uint32_t n )
268 return( ( T )( sqrt( ( double )( VectorDotProduct< T >( v, v, n ) ) ) ) );
272 // -----------------------------------------------------------------------------
273 /** Calculates a normal vector, usign the L2 norm.
276 * @param v_o Original vector.
277 * @param n Cardinality.
281 void VectorNormalize( T* v, T* v_o, uint32_t n )
284 T norm = VectorNorm< T >( v_o, n );
286 norm = ( norm == ( T )0 )? ( T )1: norm;
287 for( i = 0; i < n; v[ i ] = v_o[ i ] / norm, i++ );
291 // -----------------------------------------------------------------------------
293 /// Functions for matrices.
295 // -----------------------------------------------------------------------------
296 /** Stream out function for matrices.
298 * @param o Output stream.
305 void MatrixPrint( std::ostream& o, T** ma, uint32_t n, uint32_t m )
309 o << n << " x " << m << endl;
310 for( j = 0; j < m; j++ ) {
312 for( i = 0; i < n; o << ma[ i ][ j ] << " ", i++ );
319 // -----------------------------------------------------------------------------
320 /** Allocates memory for a matrix.
327 T** MatrixAllocateMemory( uint32_t n, uint32_t m )
330 T** ma = new T*[ n ];
332 for( i = 0; i < n; i++ ) {
334 ma[ i ] = new T[ m ];
335 memset( ma[ i ], 0, sizeof( T ) * m );
342 // -----------------------------------------------------------------------------
345 * @param ma Target matrix.
346 * @param ma_o Source matrix.
352 void MatrixAssignMemory( T** ma, T** ma_o, uint32_t n, uint32_t m )
356 for( i = 0; i < n; i++ )
357 memcpy( ma[ i ], ma_o[ i ], sizeof( T ) * m );
361 // -----------------------------------------------------------------------------
362 /** Allocates & copies.
370 T** MatrixCopyMemory( T** ma, uint32_t n, uint32_t m )
372 T** n_ma = MatrixAllocateMemory< T >( n, m );
373 MatrixAssignMemory< T >( n_ma, ma, n, m );
378 // -----------------------------------------------------------------------------
379 /** Scalar assignation.
388 void MatrixAssignScalar( T** ma, T s, uint32_t n, uint32_t m )
392 for( i = 0; i < n; i++ )
393 for( j = 0; j < m; j++ )
398 // -----------------------------------------------------------------------------
399 /** Converts a matrix in a vector.
405 * @param col Convert to a column vector?
409 void MatrixVectorCast( T* v, T** ma, uint32_t n, uint32_t m, bool col )
413 for( i = 0; i < ( ( col )? m: n ); i++ )
414 v[ i ] = ma[ ( col )? n: i ][ ( col )? i: m ];
418 // -----------------------------------------------------------------------------
419 /** Frees matrix memory.
426 void MatrixFreeMemory( T** ma, uint32_t n )
432 for( i = 0; i < n; i++ )
433 if( ma[ i ] ) delete [] ma[ i ];
440 // -----------------------------------------------------------------------------
441 /** Performs a matricial addition.
444 * @param ma_l Left operand.
445 * @param ma_r Right operand.
451 void MatrixAdd( T** ma, T** ma_l, T** ma_r, uint32_t n, uint32_t m )
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++ );
460 // -----------------------------------------------------------------------------
461 /** Performs a matricial substraction.
464 * @param ma_l Left operand.
465 * @param ma_r Right operand.
471 void MatrixSubtract( T** ma, T** ma_l, T** ma_r, uint32_t n, uint32_t m )
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++ );
480 // -----------------------------------------------------------------------------
481 /** Performs a matricial product.
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.
492 void MatrixProduct( T** ma, T** ma_l, T** ma_r, uint32_t n, uint32_t m, uint32_t nr )
496 for( i = 0; i < nr; i++ )
497 for( j = 0; j < m; j++ )
499 k = 0, ma[ i ][ j ] = ( T )0;
501 ma[ i ][ j ] = ma[ i ][ j ] + ( ma_l[ k ][ j ] * ma_r[ i ][ k ] ), k++
506 // -----------------------------------------------------------------------------
507 /** Performs a scalar-matrix product.
510 * @param ma_l Left operand.
517 void MatrixScalarProduct( T** ma, T** ma_l, T s, uint32_t n, uint32_t m )
521 for( i = 0; i < n; i++ )
522 for( j = 0; j < m; ma[ i ][ j ] = ma_l[ i ][ j ] * s, j++ );
526 // -----------------------------------------------------------------------------
527 /** Gets the matrix cofactor.
530 * @param ma_o Matrix.
531 * @param i Column index.
532 * @param j Row index.
538 void MatrixCofactor( T** ma, T** ma_o, uint32_t i, uint32_t j, uint32_t n, uint32_t m )
542 for( k = 0; k < i; k++ ) {
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 ];
549 for( k = i + 1; k < n; k++ ) {
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 ];
558 // -----------------------------------------------------------------------------
559 /** Gets the matrix determinant (square matrices only).
562 * @param n Dimension.
566 T MatrixDeterminant( T** ma, uint32_t n )
572 // All these cases are for speed-up calculations.
577 } else if( n == 2 ) {
580 ( ma[ 0 ][ 0 ] * ma[ 1 ][ 1 ] ) -
581 ( ma[ 1 ][ 0 ] * ma[ 0 ][ 1 ] );
583 } else if( n == 3 ) {
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 ] );
595 tmp = MatrixAllocateMemory< T >( n - 1, n - 1 );
596 for( k = 0, c = ( T )1, ret = ( T )0; k < n; k++ ) {
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 );
603 MatrixFreeMemory< T >( tmp, n - 1 );
610 // -----------------------------------------------------------------------------
611 /** Calculates the inverse using the adjoint method (only square matrices).
614 * @param ma_o Matrix.
615 * @param n Dimension.
619 void MatrixInverseAdjoint( T** ma, T** ma_o, uint32_t n )
625 tmp = MatrixAllocateMemory< T >( n - 1, n - 1 );
626 for( i = 0; i < n; i++ ) {
628 for( j = 0; j < n; j++ ) {
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 );
637 MatrixFreeMemory< T >( tmp, n - 1 );
639 c = ( T )1 / MatrixDeterminant< T >( ma_o, n );
640 MatrixScalarProduct< T >( ma, ma, c, n, n );
644 // -----------------------------------------------------------------------------
645 /** Calculates the inverse using the gauss method (only square matrices).
648 * @param ma_o Matrix.
649 * @param n Dimension.
650 * @warning Adapted from a java-implementation: http://www.nauticom.net/www/jdtaft/JavaMatrix.htm
654 void MatrixInverseGauss( T** ma, T** ma_o, uint32_t n )
656 uint32_t i, j, k, n2 = 2 * n;
657 T** ma_a = MatrixAllocateMemory< T >( n2, n );
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++ );
667 for( j = 0; j < n; 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++ ) {
673 if( ( k - j ) != 0 ) {
679 ma_a[ i ][ k ] = ma_a[ i ][ k ] - ( b * ma_a[ i ][ j ] ), i++
688 // Result assignation
689 MatrixAssignMemory< T >( ma, ma_a, n, n );
690 MatrixFreeMemory< T >( ma_a, n2 );
694 // -----------------------------------------------------------------------------
695 /** Gets the transpose matrix.
697 * @param ma Matrix result.
698 * @param ma_o Matrix.
704 void MatrixTranspose( T** ma, T** ma_o, uint32_t n, uint32_t m )
708 for( i = 0; i < m; i++ )
709 for( j = 0; j < n; ma[ i ][ j ] = ma_o[ j ][ i ], j++ );
713 // -----------------------------------------------------------------------------
714 /** Load a square matrix with the identity.
717 * @param n Dimension.
721 void MatrixLoadIdentity( T** ma, uint32_t n )
725 for( i = 0; i < n; i++ )
726 for( j = 0; j < n; ma[ i ][ j ] = ( i == j )? ( T )1: ( T )0, j++ );
733 #endif // GTMLIB__MATH__VMFUNCS__HXX