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
36 // -----------------------------------------------------------------------------
38 /// Functions for vectors.
40 // -----------------------------------------------------------------------------
41 /** Stream out function for vectors.
43 * @param o Output stream.
44 * @param v ANSI array.
45 * @param n Cardinality.
46 * @param col Should I print it in column format?
50 void VectorPrint( std::ostream& o, T* v, uint n, bool col )
57 for( uint i = 0; i < n; i++ ) {
68 // -----------------------------------------------------------------------------
69 /** Allocation function.
71 * @param n Cardinality.
75 T* VectorAllocateMemory( uint n )
79 memset( v, 0, sizeof( T ) * n );
84 // -----------------------------------------------------------------------------
85 /** Frees vector's memory.
91 void VectorFreeMemory( T* v )
97 // -----------------------------------------------------------------------------
100 * @param v Target vector.
101 * @param v_o Source vector.
102 * @param n Cardinality.
106 void VectorAssignMemory( T* v, T* v_o, uint n )
108 memcpy( v, v_o, sizeof( T ) * n );
112 // -----------------------------------------------------------------------------
113 /** Allocation & copy.
115 * @param v Vector to be copied.
116 * @param n Cardinality.
120 T* VectorCopyMemory( T* v, uint n )
122 T* n_v = VectorAllocateMemory< T >( n );
123 VectorAssignMemory< T >( n_v, v, n );
128 // -----------------------------------------------------------------------------
129 /** Scalar assignation.
132 * @param s Scalar value.
133 * @param n Cardinality.
137 void VectorAssignScalar( T* v, T s, uint n )
141 for( i = 0; i < n; v[ i ] = s, i++ );
145 // -----------------------------------------------------------------------------
146 /** Convert a vector in a matrix.
152 * @param col Is it a column vector?
156 void VectorMatrixCast( T** ma, T* v, uint n, uint m, bool col )
160 for( i = 0; i < ( ( col )? m: n ); i++ )
161 ma[ ( col )? n: i ][ ( col )? i: m ] = v[ i ];
165 // -----------------------------------------------------------------------------
166 /** Performs a vectorial addition.
169 * @param v_l Left operand.
170 * @param v_r Right operand.
171 * @param n Cardinality.
175 void VectorAdd( T* v, T* v_l, T* v_r, uint n )
179 for( i = 0; i < n; v[ i ] = v_l[ i ] + v_r[ i ], i++ );
183 // -----------------------------------------------------------------------------
184 /** Performs a vectorial substraction.
187 * @param v_l Left operand.
188 * @param v_r Right operand.
189 * @param n Cardinality.
193 void VectorSubtract( T* v, T* v_l, T* v_r, uint n )
197 for( i = 0; i < n; v[ i ] = v_l[ i ] - v_r[ i ], i++ );
201 // -----------------------------------------------------------------------------
202 /** Performs a vectorial cross product.
205 * @param v_l Left operand.
206 * @param v_r Right operand.
210 void VectorCrossProduct( T* v, T* v_l, T* v_r )
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 ] );
218 // -----------------------------------------------------------------------------
219 /** Performs a vectorial dot product.
221 * @param v_l Left operand.
222 * @param v_r Right operand.
223 * @param n Cardinality.
224 * @return Dot product.
228 T VectorDotProduct( T* v_l, T* v_r, uint n )
233 for( i = 0, ret = ( T )0; i < n; ret = ret + ( v_l[ i ] * v_r[ i ] ), i++ );
238 // -----------------------------------------------------------------------------
239 /** Performs a vector-scalar product.
242 * @param v_l Left operand.
244 * @param n Cardinality.
248 void VectorScalarProduct( T* v, T* v_l, T s, uint n )
252 for( i = 0; i < n; v[ i ] = v_l[ i ] * s, i++ );
256 // -----------------------------------------------------------------------------
257 /** Calculates vectorial L2 norm.
260 * @param n Cardinality.
264 T VectorNorm( T* v, uint n )
266 return( ( T )( sqrt( ( double )( VectorDotProduct< T >( v, v, n ) ) ) ) );
270 // -----------------------------------------------------------------------------
271 /** Calculates a normal vector, usign the L2 norm.
274 * @param v_o Original vector.
275 * @param n Cardinality.
279 void VectorNormalize( T* v, T* v_o, uint n )
282 T norm = VectorNorm< T >( v_o, n );
284 norm = ( norm == ( T )0 )? ( T )1: norm;
285 for( i = 0; i < n; v[ i ] = v_o[ i ] / norm, i++ );
289 // -----------------------------------------------------------------------------
291 /// Functions for matrices.
293 // -----------------------------------------------------------------------------
294 /** Stream out function for matrices.
296 * @param o Output stream.
303 void MatrixPrint( std::ostream& o, T** ma, uint n, uint m )
307 o << n << " x " << m << endl;
308 for( j = 0; j < m; j++ ) {
310 for( i = 0; i < n; o << ma[ i ][ j ] << " ", i++ );
317 // -----------------------------------------------------------------------------
318 /** Allocates memory for a matrix.
325 T** MatrixAllocateMemory( uint n, uint m )
328 T** ma = new T*[ n ];
330 for( i = 0; i < n; i++ ) {
332 ma[ i ] = new T[ m ];
333 memset( ma[ i ], 0, sizeof( T ) * m );
340 // -----------------------------------------------------------------------------
343 * @param ma Target matrix.
344 * @param ma_o Source matrix.
350 void MatrixAssignMemory( T** ma, T** ma_o, uint n, uint m )
354 for( i = 0; i < n; i++ )
355 memcpy( ma[ i ], ma_o[ i ], sizeof( T ) * m );
359 // -----------------------------------------------------------------------------
360 /** Allocates & copies.
368 T** MatrixCopyMemory( T** ma, uint n, uint m )
370 T** n_ma = MatrixAllocateMemory< T >( n, m );
371 MatrixAssignMemory< T >( n_ma, ma, n, m );
376 // -----------------------------------------------------------------------------
377 /** Scalar assignation.
386 void MatrixAssignScalar( T** ma, T s, uint n, uint m )
390 for( i = 0; i < n; i++ )
391 for( j = 0; j < m; j++ )
396 // -----------------------------------------------------------------------------
397 /** Converts a matrix in a vector.
403 * @param col Convert to a column vector?
407 void MatrixVectorCast( T* v, T** ma, uint n, uint m, bool col )
411 for( i = 0; i < ( ( col )? m: n ); i++ )
412 v[ i ] = ma[ ( col )? n: i ][ ( col )? i: m ];
416 // -----------------------------------------------------------------------------
417 /** Frees matrix memory.
424 void MatrixFreeMemory( T** ma, uint n )
430 for( i = 0; i < n; i++ )
431 if( ma[ i ] ) delete [] ma[ i ];
438 // -----------------------------------------------------------------------------
439 /** Performs a matricial addition.
442 * @param ma_l Left operand.
443 * @param ma_r Right operand.
449 void MatrixAdd( T** ma, T** ma_l, T** ma_r, uint n, uint m )
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++ );
458 // -----------------------------------------------------------------------------
459 /** Performs a matricial substraction.
462 * @param ma_l Left operand.
463 * @param ma_r Right operand.
469 void MatrixSubtract( T** ma, T** ma_l, T** ma_r, uint n, uint m )
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++ );
478 // -----------------------------------------------------------------------------
479 /** Performs a matricial product.
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.
490 void MatrixProduct( T** ma, T** ma_l, T** ma_r, uint n, uint m, uint nr )
494 for( i = 0; i < nr; i++ )
495 for( j = 0; j < m; j++ )
497 k = 0, ma[ i ][ j ] = ( T )0;
499 ma[ i ][ j ] = ma[ i ][ j ] + ( ma_l[ k ][ j ] * ma_r[ i ][ k ] ), k++
504 // -----------------------------------------------------------------------------
505 /** Performs a scalar-matrix product.
508 * @param ma_l Left operand.
515 void MatrixScalarProduct( T** ma, T** ma_l, T s, uint n, uint m )
519 for( i = 0; i < n; i++ )
520 for( j = 0; j < m; ma[ i ][ j ] = ma_l[ i ][ j ] * s, j++ );
524 // -----------------------------------------------------------------------------
525 /** Gets the matrix cofactor.
528 * @param ma_o Matrix.
529 * @param i Column index.
530 * @param j Row index.
536 void MatrixCofactor( T** ma, T** ma_o, uint i, uint j, uint n, uint m )
540 for( k = 0; k < i; k++ ) {
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 ];
547 for( k = i + 1; k < n; k++ ) {
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 ];
556 // -----------------------------------------------------------------------------
557 /** Gets the matrix determinant (square matrices only).
560 * @param n Dimension.
564 T MatrixDeterminant( T** ma, uint n )
570 // All these cases are for speed-up calculations.
575 } else if( n == 2 ) {
578 ( ma[ 0 ][ 0 ] * ma[ 1 ][ 1 ] ) -
579 ( ma[ 1 ][ 0 ] * ma[ 0 ][ 1 ] );
581 } else if( n == 3 ) {
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 ] );
593 tmp = MatrixAllocateMemory< T >( n - 1, n - 1 );
594 for( k = 0, c = ( T )1, ret = ( T )0; k < n; k++ ) {
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 );
601 MatrixFreeMemory< T >( tmp, n - 1 );
608 // -----------------------------------------------------------------------------
609 /** Calculates the inverse using the adjoint method (only square matrices).
612 * @param ma_o Matrix.
613 * @param n Dimension.
617 void MatrixInverseAdjoint( T** ma, T** ma_o, uint n )
623 tmp = MatrixAllocateMemory< T >( n - 1, n - 1 );
624 for( i = 0; i < n; i++ ) {
626 for( j = 0; j < n; j++ ) {
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 );
635 MatrixFreeMemory< T >( tmp, n - 1 );
637 c = ( T )1 / MatrixDeterminant< T >( ma_o, n );
638 MatrixScalarProduct< T >( ma, ma, c, n, n );
642 // -----------------------------------------------------------------------------
643 /** Calculates the inverse using the gauss method (only square matrices).
646 * @param ma_o Matrix.
647 * @param n Dimension.
648 * @warning Adapted from a java-implementation: http://www.nauticom.net/www/jdtaft/JavaMatrix.htm
652 void MatrixInverseGauss( T** ma, T** ma_o, uint n )
654 uint i, j, k, n2 = 2 * n;
655 T** ma_a = MatrixAllocateMemory< T >( n2, n );
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++ );
665 for( j = 0; j < n; 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++ ) {
671 if( ( k - j ) != 0 ) {
677 ma_a[ i ][ k ] = ma_a[ i ][ k ] - ( b * ma_a[ i ][ j ] ), i++
686 // Result assignation
687 MatrixAssignMemory< T >( ma, ma_a, n, n );
688 MatrixFreeMemory< T >( ma_a, n2 );
692 // -----------------------------------------------------------------------------
693 /** Gets the transpose matrix.
695 * @param ma Matrix result.
696 * @param ma_o Matrix.
702 void MatrixTranspose( T** ma, T** ma_o, uint n, uint m )
706 for( i = 0; i < m; i++ )
707 for( j = 0; j < n; ma[ i ][ j ] = ma_o[ j ][ i ], j++ );
711 // -----------------------------------------------------------------------------
712 /** Load a square matrix with the identity.
715 * @param n Dimension.
719 void MatrixLoadIdentity( T** ma, uint n )
723 for( i = 0; i < n; i++ )
724 for( j = 0; j < n; ma[ i ][ j ] = ( i == j )? ( T )1: ( T )0, j++ );
731 #endif // GTMLIB__MATH__VMFUNCS__HXX