vmmlib  1.8.0
Templatized C++ vector and matrix math library
 All Classes Namespaces Files Functions Variables Typedefs Pages
matrix.hpp
1 /*
2  * Copyright (c) 2006-2014, Visualization and Multimedia Lab,
3  * University of Zurich <http://vmml.ifi.uzh.ch>,
4  * Eyescale Software GmbH,
5  * Blue Brain Project, EPFL
6  *
7  * This file is part of VMMLib <https://github.com/VMML/vmmlib/>
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions are met:
11  *
12  * Redistributions of source code must retain the above copyright notice, this
13  * list of conditions and the following disclaimer. Redistributions in binary
14  * form must reproduce the above copyright notice, this list of conditions and
15  * the following disclaimer in the documentation and/or other materials provided
16  * with the distribution. Neither the name of the Visualization and Multimedia
17  * Lab, University of Zurich nor the names of its contributors may be used to
18  * endorse or promote products derived from this software without specific prior
19  * written permission.
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
23  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
24  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
25  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
26  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
27  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
28  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
29  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 #ifndef __VMML__MATRIX__HPP__
33 #define __VMML__MATRIX__HPP__
34 
35 #include <vmmlib/vmmlib_config.hpp>
36 
37 #include <vmmlib/matrix_functors.hpp>
38 #include <vmmlib/vector.hpp>
39 #include <vmmlib/math.hpp>
40 #include <vmmlib/exception.hpp>
41 #include <vmmlib/enable_if.hpp>
42 
43 #include <iostream>
44 #include <iomanip>
45 #include <vector>
46 #include <cstddef>
47 #include <limits>
48 #include <algorithm>
49 #include <string>
50 #include <cstring>
51 #include <fstream> // file I/O
52 
53 namespace vmml
54 {
55 
56 // matrix of type T with m rows and n columns
57 template< size_t M, size_t N, typename T = float >
58 class matrix
59 {
60 public:
61  typedef T value_type;
62  typedef T* iterator;
63  typedef const T* const_iterator;
64  typedef std::reverse_iterator< iterator > reverse_iterator;
65  typedef std::reverse_iterator< const_iterator > const_reverse_iterator;
66 
67  static const size_t ROWS = M;
68  static const size_t COLS = N;
69 
70  // ctors
71  matrix();
72 
73  template< size_t P, size_t Q, typename U >
74  matrix( const matrix< P, Q, U >& source_ );
75 
76  // accessors
77  inline T& operator()( size_t row_index, size_t col_index );
78  inline const T& operator()( size_t row_index, size_t col_index ) const;
79 
80  inline T& at( size_t row_index, size_t col_index );
81  inline const T& at( size_t row_index, size_t col_index ) const;
82 
83  // element iterators - NOTE: column-major order
84  iterator begin();
85  iterator end();
86  const_iterator begin() const;
87  const_iterator end() const;
88 
89  reverse_iterator rbegin();
90  reverse_iterator rend();
91  const_reverse_iterator rbegin() const;
92  const_reverse_iterator rend() const;
93 
94 #ifndef VMMLIB_NO_CONVERSION_OPERATORS
95  // auto conversion operator
96  operator T*();
97  operator const T*() const;
98 #endif
99 
100  bool operator==( const matrix& other ) const;
101  bool operator!=( const matrix& other ) const;
102 
103  // due to limited precision, two 'identical' matrices might seem different.
104  // this function allows to specify a tolerance when comparing matrices.
105  bool equals( const matrix& other, T tolerance ) const;
106  // this version takes a comparison functor to compare the components of
107  // the two matrices
108  template< typename compare_t >
109  bool equals( const matrix& other, compare_t& cmp ) const;
110 
111  void multiply_piecewise( const matrix& other );
112 
113  // (this) matrix = left matrix_mxp * right matrix_pxn
114  template< size_t P > void multiply( const matrix< M, P, T >& left,
115  const matrix< P, N, T >& right );
116 
117  // convolution operation (extending borders) of (this) matrix and the given kernel
118  template< size_t U, size_t V >
119  void convolve(const matrix< U, V, T >& kernel);
120 
121  // returned matrix_mxp = (this) matrix * other matrix_nxp;
122  // note: using multiply(...) it avoids a copy of the resulting matrix
123  template< size_t P >
124  matrix< M, P, T > operator*( const matrix< N, P, T >& other ) const;
125 
126  // operator *= is only enabled for square matrices
127  template< size_t O, size_t P, typename TT >
128  typename enable_if< M == N && O == P && M == O, TT >::type*
129  operator*=( const matrix< O, P, TT >& right );
130 
131  inline matrix operator+( const matrix& other ) const;
132  inline matrix operator-( const matrix& other ) const;
133 
134  void operator+=( const matrix& other );
135  void operator-=( const matrix& other );
136 
137  void operator+=( T scalar );
138  void operator-=( T scalar );
139 
140  template< size_t O, size_t P, size_t Q, size_t R >
141  typename enable_if< M == O + Q && N == P + R >::type*
142  direct_sum( const matrix< O, P, T >& m0, const matrix< Q, R, T >& m1 );
143 
144  //
145  // matrix-scalar operations / scaling
146  //
147  matrix operator*( T scalar );
148  void operator*=( T scalar );
149 
150  matrix operator/( T scalar );
151  void operator/=( T scalar );
152 
153  //
154  // matrix-vector operations
155  //
156  // transform column vector by matrix ( vec = matrix * vec )
157  vector< M, T > operator*( const vector< N, T >& other ) const;
158 
159  // transform column vector by matrix ( vec = matrix * vec )
160  // assume homogenous coords, e.g. vec3 = mat4x4 * vec3, with w = 1.0
161  template< size_t O >
162  vector< O, T > operator*( const vector< O, T >& vector_ ) const;
163 
164  inline matrix< M, N, T > operator-() const;
165  matrix< M, N, T > negate() const;
166 
167  // compute tensor product: (this) = vector (X) vector
168  void tensor( const vector< M, T >& u, const vector< N, T >& v );
169  // tensor, for api compatibility with old vmmlib version.
170  // WARNING: for M = N = 4 only.
171  template< size_t uM, size_t vM >
172  typename enable_if< uM == 3 && vM == 3 && M == N && M == 4 >::type*
173  tensor( const vector< uM, T >& u, const vector< vM, T >& v );
174 
175  // row_offset and col_offset define the starting indices for the sub_matrix
176  // the sub_matrix is extracted according to the size of the target matrix, i.e. ( OXP )
177  template< size_t O, size_t P >
179  get_sub_matrix( size_t row_offset, size_t col_offset,
180  typename enable_if< O <= M && P <= N >::type* = 0 ) const;
181 
182  template< size_t O, size_t P >
184  get_sub_matrix( matrix< O, P, T >& result,
185  size_t row_offset = 0, size_t col_offset = 0 ) const;
186 
187  template< size_t O, size_t P >
189  set_sub_matrix( const matrix< O, P, T >& sub_matrix,
190  size_t row_offset = 0, size_t col_offset = 0 );
191 
192  // copies a transposed version of *this into transposedMatrix
193  void transpose_to( matrix< N, M, T >& transpose_ ) const;
194 
195  //symmetric covariance matrix of a right matrix multiplication: MxN x NxM = MxM
196  void symmetric_covariance( matrix< M, M, T >& cov_m_ ) const;
197 
198 
199  const matrix& operator=( const matrix< M, N, T >& source_ );
200 
201  // these assignment operators return nothing to avoid silent loss of
202  // precision
203  template< size_t P, size_t Q, typename U >
204  void operator=( const matrix< P, Q, U >& source_ );
205  void operator=( const T old_fashioned_matrix[ M ][ N ] );
206 
207  // WARNING: data_array[] must be at least of size M * N - otherwise CRASH!
208  // WARNING: assumes row_by_row layout - if this is not the case,
209  // use matrix::set( data_array, false )
210  void operator=( const T* data_array );
211  void operator=( const std::vector< T >& data );
212 
213  // sets all elements to fill_value
214  void operator=( T fill_value );
215  void fill( T fill_value );
216 
217  // note: this function copies elements until either the matrix is full or
218  // the iterator equals end_.
219  template< typename input_iterator_t >
220  void set( input_iterator_t begin_, input_iterator_t end_,
221  bool row_major_layout = true );
222 
223  //sets all matrix values with random values
224  //remember to set srand( seed );
225  //if seed is set to -1, srand( seed ) was set outside set_random
226  //otherwise srand( seed ) will be called with the given seed
227  void set_random( int seed = -1 );
228 
229  //sets all matrix values with discrete cosine transform coefficients (receive orthonormal coefficients)
230  void set_dct();
231 
232  void zero();
233 
234  double frobenius_norm() const;
235  double p_norm( double p ) const;
236 
237  template< typename TT >
238  void cast_from( const matrix< M, N, TT >& other );
239 
240  void read_csv_file( const std::string& dir_, const std::string& filename_ );
241  void write_csv_file( const std::string& dir_, const std::string& filename_ ) const;
242  void write_to_raw( const std::string& dir_, const std::string& filename_ ) const;
243  void read_from_raw( const std::string& dir_, const std::string& filename_ ) ;
244 
245  template< typename TT >
246  void quantize_to( matrix< M, N, TT >& quantized_, const T& min_value, const T& max_value ) const;
247  template< typename TT >
248  void quantize( matrix< M, N, TT >& quantized_, T& min_value, T& max_value ) const;
249  template< typename TT >
250  void dequantize( matrix< M, N, TT >& quantized_, const TT& min_value, const TT& max_value ) const;
251 
252  void columnwise_sum( vector< N, T>& summed_columns_ ) const;
253  double sum_elements() const;
254 
255  void sum_rows( matrix< M/2, N, T>& other ) const;
256  void sum_columns( matrix< M, N/2, T>& other ) const;
257 
258  template< size_t R >
259  typename enable_if< R == M && R == N >::type*
260  diag( const vector< R, T >& diag_values_ );
261 
262 
263  //Khatri-Rao Product: columns must be of same size
264  template< size_t O >
265  void khatri_rao_product( const matrix< O, N, T >& right_, matrix< M*O, N, T >& prod_ ) const;
266  //Kronecker Product: MxN x_kronecker OxP = M*OxN*P
267  template< size_t O, size_t P >
268  void kronecker_product( const matrix< O, P, T >& right_, matrix< M*O, N*P, T >& result_) const;
269 
270  T get_min() const;
271  T get_max() const;
272  T get_abs_min() const;
273  T get_abs_max() const;
274 
275  //returns number of non-zeros
276  size_t nnz() const;
277  size_t nnz( const T& threshold_ ) const;
278  void threshold( const T& threshold_value_ );
279 
280  vector< M, T > get_column( size_t column_index ) const;
281  void get_column( size_t column_index, vector< M, T>& column ) const;
282 
283  void set_column( size_t index, const vector< M, T >& column );
284 
285  void get_column( size_t index, matrix< M, 1, T >& column ) const;
286  void set_column( size_t index, const matrix< M, 1, T >& column );
287 
288  vector< N, T > get_row( size_t index ) const;
289  void get_row( size_t index, vector< N, T >& row ) const;
290  void set_row( size_t index, const vector< N, T >& row );
291 
292  void get_row( size_t index, matrix< 1, N, T >& row ) const;
293  void set_row( size_t index, const matrix< 1, N, T >& row );
294 
295  size_t size() const; // return M * N;
296 
297  size_t get_number_of_rows() const;
298  size_t get_number_of_columns() const;
299 
300  T det() const;
301 
302  // the return value indicates if the matrix is invertible.
303  // we need a tolerance term since the computation of the determinant is
304  // subject to precision errors.
305  template< size_t O, size_t P, typename TT >
306  bool inverse( matrix< O, P, TT >& inverse_,
307  T tolerance = std::numeric_limits<T>::epsilon(),
308  typename enable_if< M == N && O == P && O == M && M >= 2 && M <= 4, TT >::type* = 0 ) const;
309 
310  template< size_t O, size_t P >
312  get_adjugate( matrix< O, P, T >& adjugate ) const;
313 
314  template< size_t O, size_t P >
316  get_cofactors( matrix< O, P, T >& cofactors ) const;
317 
318 
319  // returns the determinant of a square matrix M-1, N-1
320  template< size_t O, size_t P >
321  T get_minor( matrix< O, P, T >& minor_, size_t row_to_cut, size_t col_to_cut,
322  typename enable_if< O == M-1 && P == N-1 && M == N && M >= 2 >::type* = 0 ) const;
323 
324  //
325  // 4*4 matrices only
326  //
331  template< typename TT >
332  matrix< M, N, T >& rotate( const TT angle, const vector< M-1, T >& axis,
333  typename enable_if< M == N && M == 4, TT >::type* = 0 );
334 
335  template< typename TT >
336  matrix< M, N, T >& rotate_x( const TT angle,
337  typename enable_if< M == N && M == 4, TT >::type* = 0 );
338 
339  template< typename TT >
340  matrix< M, N, T >& rotate_y( const TT angle,
341  typename enable_if< M == N && M == 4, TT >::type* = 0 );
342 
343  template< typename TT >
344  matrix< M, N, T >& rotate_z( const TT angle,
345  typename enable_if< M == N && M == 4, TT >::type* = 0 );
346 
347  template< typename TT >
348  matrix< M, N, T >& pre_rotate_x( const TT angle,
349  typename enable_if< M == N && M == 4, TT >::type* = 0 );
350 
351  template< typename TT >
352  matrix< M, N, T >& pre_rotate_y( const TT angle,
353  typename enable_if< M == N && M == 4, TT >::type* = 0 );
354 
355  template< typename TT >
356  matrix< M, N, T >& pre_rotate_z( const TT angle,
357  typename enable_if< M == N && M == 4, TT >::type* = 0 );
358 
359  template< typename TT >
360  matrix< M, N, T >& scale( const TT scale[3],
361  typename enable_if< M == N && M == 4, TT >::type* = 0 );
362 
363  template< typename TT >
364  matrix< M, N, T >& scale( const TT x_, const T y, const T z,
365  typename enable_if< M == N && M == 4, TT >::type* = 0 );
366 
367  template< typename TT >
368  matrix< M, N, T >& scale( const vector< 3, TT >& scale_,
369  typename enable_if< M == N && M == 4, TT >::type* = 0 );
370 
371  template< typename TT >
372  matrix< M, N, T >& scale_translation( const TT scale_[3],
373  typename enable_if< M == N && M == 4, TT >::type* = 0 );
374 
375  template< typename TT >
376  matrix< M, N, T >& scale_translation( const vector< 3, TT >& scale_,
377  typename enable_if< M == N && M == 4, TT >::type* = 0 );
378 
379  template< typename TT >
380  matrix< M, N, T >& set_translation( const TT x_, const TT y_, const TT z_,
381  typename enable_if< M == N && M == 4, TT >::type* = 0 );
382 
383  template< typename TT >
384  matrix< M, N, T >& set_translation( const TT trans[3],
385  typename enable_if< M == N && M == 4, TT >::type* = 0 );
386 
387  template< typename TT >
388  matrix< M, N, T >& set_translation( const vector< 3, TT >& t,
389  typename enable_if< M == N && M == 4, TT >::type* = 0 );
390 
391  template< typename TT >
392  void get_translation( vector< N-1, TT >& translation_ ) const;
393 
394  vector< N-1, T > get_translation() const;
395 
396  // hack for static-member-init
397  template< typename init_functor_t >
398  static const matrix get_initialized_matrix();
399 
400  inline T& x();
401  inline T& y();
402  inline T& z();
403 
404 
405  // tests every component for isnan && isinf
406  // inline bool is_valid() const; -> moved to class validator
407 
408  // legacy/compatibility accessor
409  struct row_accessor
410  {
411  row_accessor( T* array_ ) : array( array_ ) {}
412  T&
413  operator[]( size_t col_index )
414  {
415  #ifdef VMMLIB_SAFE_ACCESSORS
416  if ( col_index >= N )
417  VMMLIB_ERROR( "column index out of bounds", VMMLIB_HERE );
418  #endif
419  return array[ col_index * M ];
420  }
421 
422  const T&
423  operator[]( size_t col_index ) const
424  {
425  #ifdef VMMLIB_SAFE_ACCESSORS
426  if ( col_index >= N )
427  VMMLIB_ERROR( "column index out of bounds", VMMLIB_HERE );
428  #endif
429  return array[ col_index * M ];
430  }
431 
432  T* array;
433  private: row_accessor() {} // disallow std ctor
434  };
435  // this is a hack to allow array-style access to matrix elements
436  // usage: matrix< 2, 2, float > m; m[ 1 ][ 0 ] = 37.0f;
437  inline row_accessor operator[]( size_t row_index )
438  {
439  #ifdef VMMLIB_SAFE_ACCESSORS
440  if ( row_index > M )
441  VMMLIB_ERROR( "row index out of bounds", VMMLIB_HERE );
442  #endif
443  return row_accessor( array + row_index );
444  }
445 
446  // this is a hack to remove a warning about implicit conversions
447  inline row_accessor operator[]( int row_index )
448  {
449  return ( *this )[ size_t ( row_index ) ];
450  }
451 
452  friend std::ostream& operator << ( std::ostream& os,
453  const matrix< M, N, T >& matrix )
454  {
455  const std::ios::fmtflags flags = os.flags();
456  const int prec = os.precision();
457 
458  os.setf( std::ios::right, std::ios::adjustfield );
459  os.precision( 5 );
460 
461  for( size_t row_index = 0; row_index < M; ++row_index )
462  {
463  os << "|";
464  for( size_t col_index = 0; col_index < N; ++col_index )
465  {
466  os << std::setw(10) << matrix.at( row_index, col_index ) << " ";
467  }
468  os << "|" << std::endl;
469  }
470  os.precision( prec );
471  os.setf( flags );
472  return os;
473  };
474 
475  // protected:
476  T array[ M * N ];
477 
478  // static members
479  static const matrix< M, N, T > IDENTITY;
480  static const matrix< M, N, T > ZERO;
481 
482 }; // class matrix
483 
484 
485 #ifndef VMMLIB_NO_TYPEDEFS
490 #endif
491 
492 /*
493 * free functions
494 */
495 
496 template< size_t M, size_t N, typename T >
497 bool equals( const matrix< M, N, T >& m0, const matrix< M, N, T >& m1,
498  const T tolerance = std::numeric_limits< T >::epsilon())
499 {
500  return m0.equals( m1, tolerance );
501 }
502 
503 
504 template< size_t M, size_t N, typename T >
505 inline void multiply( const matrix< M, N, T >& left,
506  const matrix< M, N, T >& right,
507  matrix< M, N, T >& result )
508 {
509  result.multiply( left, right );
510 }
511 
512 
513 
514 template< size_t M, size_t N, typename T >
515 template< size_t U, size_t V >
516 void matrix< M, N, T>::convolve(const matrix< U, V, T >& kernel)
517 {
518  matrix< M, N, T> temp; // do not override original values instantly as old values are needed for calculation
519 
520  for(size_t y_ = 0; y_ < N; ++y_)
521  {
522  for(size_t x_ = 0; x_ < M; ++x_)
523  {
524  double sum = 0.0;
525 
526  for(size_t j = 0; j < V; ++j)
527  {
528  int srcy = y_ - V/2 + j;
529 
530  // Extending border values
531  if(srcy < 0) srcy = 0;
532  if(srcy >= int(N)) srcy = N-1;
533 
534  for(size_t i = 0; i < U; ++i)
535  {
536  int srcx = x_ - U/2 + i;
537 
538  // Extending border values
539  if(srcx < 0) srcx = 0;
540  if(srcx >= int(M)) srcx = M-1;
541 
542  sum += kernel.at(j,i) * at(srcy,srcx);
543  }
544  }
545  temp.at(y_,x_) = sum;
546  }
547  }
548 
549  *this = temp;
550 }
551 
552 
553 
554 template< size_t M, size_t N, size_t P, typename T >
555 inline void
556 multiply( const matrix< M, P, T >& left, const matrix< P, N, T >& right,
557  matrix< M, N, T >& result )
558 {
559  result.multiply( left, right );
560 }
561 
562 
563 template< size_t M, size_t N, typename T >
564 inline typename enable_if< M == N >::type* identity( matrix< M, N, T >& m )
565 {
566  m = static_cast< T >( 0.0 );
567  for( size_t index = 0; index < M; ++index )
568  m( index, index ) = static_cast< T >( 1.0 );
569  return 0; // for sfinae
570 }
571 
572 
573 
574 template< typename T >
575 inline T compute_determinant( const matrix< 1, 1, T >& matrix_ )
576 {
577  return matrix_.array[ 0 ];
578 }
579 
580 
581 
582 template< typename T >
583 inline T compute_determinant( const matrix< 2, 2, T >& matrix_ )
584 {
585  const T& a = matrix_( 0, 0 );
586  const T& b = matrix_( 0, 1 );
587  const T& c = matrix_( 1, 0 );
588  const T& d = matrix_( 1, 1 );
589  return a * d - b * c;
590 }
591 
592 
593 
594 template< typename T >
595 inline T compute_determinant( const matrix< 3, 3, T >& m_ )
596 {
597  return
598  m_( 0,0 ) * ( m_( 1,1 ) * m_( 2,2 ) - m_( 1,2 ) * m_( 2,1 ) )
599  + m_( 0,1 ) * ( m_( 1,2 ) * m_( 2,0 ) - m_( 1,0 ) * m_( 2,2 ) )
600  + m_( 0,2 ) * ( m_( 1,0 ) * m_( 2,1 ) - m_( 1,1 ) * m_( 2,0 ) );
601 }
602 
603 
604 template< typename T >
605 inline T compute_determinant( const matrix< 4, 4, T >& m )
606 {
607  T m00 = m( 0, 0 );
608  T m10 = m( 1, 0 );
609  T m20 = m( 2, 0 );
610  T m30 = m( 3, 0 );
611  T m01 = m( 0, 1 );
612  T m11 = m( 1, 1 );
613  T m21 = m( 2, 1 );
614  T m31 = m( 3, 1 );
615  T m02 = m( 0, 2 );
616  T m12 = m( 1, 2 );
617  T m22 = m( 2, 2 );
618  T m32 = m( 3, 2 );
619  T m03 = m( 0, 3 );
620  T m13 = m( 1, 3 );
621  T m23 = m( 2, 3 );
622  T m33 = m( 3, 3 );
623 
624  return
625  m03 * m12 * m21 * m30
626  - m02 * m13 * m21 * m30
627  - m03 * m11 * m22 * m30
628  + m01 * m13 * m22 * m30
629  + m02 * m11 * m23 * m30
630  - m01 * m12 * m23 * m30
631  - m03 * m12 * m20 * m31
632  + m02 * m13 * m20 * m31
633  + m03 * m10 * m22 * m31
634  - m00 * m13 * m22 * m31
635  - m02 * m10 * m23 * m31
636  + m00 * m12 * m23 * m31
637  + m03 * m11 * m20 * m32
638  - m01 * m13 * m20 * m32
639  - m03 * m10 * m21 * m32
640  + m00 * m13 * m21 * m32
641  + m01 * m10 * m23 * m32
642  - m00 * m11 * m23 * m32
643  - m02 * m11 * m20 * m33
644  + m01 * m12 * m20 * m33
645  + m02 * m10 * m21 * m33
646  - m00 * m12 * m21 * m33
647  - m01 * m10 * m22 * m33
648  + m00 * m11 * m22 * m33;
649 }
650 
651 
652 
653 template< typename T >
654 bool compute_inverse( const matrix< 2, 2, T >& m_, matrix< 2, 2, T >& inverse_,
655  T tolerance_ = std::numeric_limits<T>::epsilon())
656 {
657  const T det_ = compute_determinant( m_ );
658  if( fabs( det_ ) < tolerance_ )
659  return false;
660 
661  const T detinv = static_cast< T >( 1.0 ) / det_;
662 
663  m_.get_adjugate( inverse_ ); // set inverse_ to the adjugate of M
664  inverse_ *= detinv;
665  return true;
666 }
667 
668 
669 
670 template< typename T >
671 bool compute_inverse( const matrix< 3, 3, T >& m_, matrix< 3, 3, T >& inverse_,
672  T tolerance_ = std::numeric_limits<T>::epsilon() )
673 {
674  // Invert a 3x3 using cofactors. This is about 8 times faster than
675  // the Numerical Recipes code which uses Gaussian elimination.
676  inverse_.at( 0, 0 ) = m_.at( 1, 1 ) * m_.at( 2, 2 ) - m_.at( 1, 2 ) * m_.at( 2, 1 );
677  inverse_.at( 0, 1 ) = m_.at( 0, 2 ) * m_.at( 2, 1 ) - m_.at( 0, 1 ) * m_.at( 2, 2 );
678  inverse_.at( 0, 2 ) = m_.at( 0, 1 ) * m_.at( 1, 2 ) - m_.at( 0, 2 ) * m_.at( 1, 1 );
679  inverse_.at( 1, 0 ) = m_.at( 1, 2 ) * m_.at( 2, 0 ) - m_.at( 1, 0 ) * m_.at( 2, 2 );
680  inverse_.at( 1, 1 ) = m_.at( 0, 0 ) * m_.at( 2, 2 ) - m_.at( 0, 2 ) * m_.at( 2, 0 );
681  inverse_.at( 1, 2 ) = m_.at( 0, 2 ) * m_.at( 1, 0 ) - m_.at( 0, 0 ) * m_.at( 1, 2 );
682  inverse_.at( 2, 0 ) = m_.at( 1, 0 ) * m_.at( 2, 1 ) - m_.at( 1, 1 ) * m_.at( 2, 0 );
683  inverse_.at( 2, 1 ) = m_.at( 0, 1 ) * m_.at( 2, 0 ) - m_.at( 0, 0 ) * m_.at( 2, 1 );
684  inverse_.at( 2, 2 ) = m_.at( 0, 0 ) * m_.at( 1, 1 ) - m_.at( 0, 1 ) * m_.at( 1, 0 );
685 
686  const T determinant =
687  m_.at( 0, 0 ) * inverse_.at( 0, 0 )
688  + m_.at( 0, 1 ) * inverse_.at( 1, 0 )
689  + m_.at( 0, 2 ) * inverse_.at( 2, 0 );
690 
691  if ( fabs( determinant ) <= tolerance_ )
692  return false; // matrix is not invertible
693 
694  const T detinv = static_cast< T >( 1.0 ) / determinant;
695 
696  inverse_.at( 0, 0 ) *= detinv;
697  inverse_.at( 0, 1 ) *= detinv;
698  inverse_.at( 0, 2 ) *= detinv;
699  inverse_.at( 1, 0 ) *= detinv;
700  inverse_.at( 1, 1 ) *= detinv;
701  inverse_.at( 1, 2 ) *= detinv;
702  inverse_.at( 2, 0 ) *= detinv;
703  inverse_.at( 2, 1 ) *= detinv;
704  inverse_.at( 2, 2 ) *= detinv;
705 
706  return true;
707 }
708 
709 
710 template< typename T >
711 bool compute_inverse( const matrix< 4, 4, T >& m_,
712  matrix< 4, 4, T >& inv_,
713  T tolerance_ = std::numeric_limits<T>::epsilon() )
714 {
715  const T* array = m_.array;
716 
717  // tuned version from Claude Knaus
718  /* first set of 2x2 determinants: 12 multiplications, 6 additions */
719  const T t1[6] = { array[ 2] * array[ 7] - array[ 6] * array[ 3],
720  array[ 2] * array[11] - array[10] * array[ 3],
721  array[ 2] * array[15] - array[14] * array[ 3],
722  array[ 6] * array[11] - array[10] * array[ 7],
723  array[ 6] * array[15] - array[14] * array[ 7],
724  array[10] * array[15] - array[14] * array[11] };
725 
726  /* first half of comatrix: 24 multiplications, 16 additions */
727  inv_.array[0] = array[ 5] * t1[5] - array[ 9] * t1[4] + array[13] * t1[3];
728  inv_.array[1] = array[ 9] * t1[2] - array[13] * t1[1] - array[ 1] * t1[5];
729  inv_.array[2] = array[13] * t1[0] - array[ 5] * t1[2] + array[ 1] * t1[4];
730  inv_.array[3] = array[ 5] * t1[1] - array[ 1] * t1[3] - array[ 9] * t1[0];
731  inv_.array[4] = array[ 8] * t1[4] - array[ 4] * t1[5] - array[12] * t1[3];
732  inv_.array[5] = array[ 0] * t1[5] - array[ 8] * t1[2] + array[12] * t1[1];
733  inv_.array[6] = array[ 4] * t1[2] - array[12] * t1[0] - array[ 0] * t1[4];
734  inv_.array[7] = array[ 0] * t1[3] - array[ 4] * t1[1] + array[ 8] * t1[0];
735 
736  /* second set of 2x2 determinants: 12 multiplications, 6 additions */
737  const T t2[6] = { array[ 0] * array[ 5] - array[ 4] * array[ 1],
738  array[ 0] * array[ 9] - array[ 8] * array[ 1],
739  array[ 0] * array[13] - array[12] * array[ 1],
740  array[ 4] * array[ 9] - array[ 8] * array[ 5],
741  array[ 4] * array[13] - array[12] * array[ 5],
742  array[ 8] * array[13] - array[12] * array[ 9] };
743 
744  /* second half of comatrix: 24 multiplications, 16 additions */
745  inv_.array[8] = array[ 7] * t2[5] - array[11] * t2[4] + array[15] * t2[3];
746  inv_.array[9] = array[11] * t2[2] - array[15] * t2[1] - array[ 3] * t2[5];
747  inv_.array[10] = array[15] * t2[0] - array[ 7] * t2[2] + array[ 3] * t2[4];
748  inv_.array[11] = array[ 7] * t2[1] - array[ 3] * t2[3] - array[11] * t2[0];
749  inv_.array[12] = array[10] * t2[4] - array[ 6] * t2[5] - array[14] * t2[3];
750  inv_.array[13] = array[ 2] * t2[5] - array[10] * t2[2] + array[14] * t2[1];
751  inv_.array[14] = array[ 6] * t2[2] - array[14] * t2[0] - array[ 2] * t2[4];
752  inv_.array[15] = array[ 2] * t2[3] - array[ 6] * t2[1] + array[10] * t2[0];
753 
754  /* determinant: 4 multiplications, 3 additions */
755  const T determinant = array[0] * inv_.array[0] + array[4] * inv_.array[1] +
756  array[8] * inv_.array[2] + array[12] * inv_.array[3];
757 
758  if( fabs( determinant ) <= tolerance_ )
759  return false; // matrix is not invertible
760 
761  /* division: 16 multiplications, 1 division */
762  const T detinv = static_cast< T >( 1.0 ) / determinant;
763 
764  for( size_t i = 0; i != 16; ++i )
765  inv_.array[i] *= detinv;
766 
767  return true;
768 }
769 
770 
771 // this function returns the transpose of a matrix
772 // however, using matrix::transpose_to( .. ) avoids the copy.
773 template< size_t M, size_t N, typename T >
774 inline matrix< N, M, T >
775 transpose( const matrix< M, N, T >& matrix_ )
776 {
777  matrix< N, M, T > transpose_;
778  matrix_.transpose_to( transpose_ );
779  return transpose_;
780 }
781 
782 
783 template< size_t M, size_t N, typename T >
784 bool is_positive_definite( const matrix< M, N, T >& matrix_,
785  const T limit = -1e12,
786  typename enable_if< M == N && M <= 3 >::type* = 0 )
787 {
788  if ( matrix_.at( 0, 0 ) < limit )
789  return false;
790 
791  // sylvester criterion
792  if ( M > 1 )
793  {
794  matrix< 2, 2, T > m;
795  matrix_.get_sub_matrix( m, 0, 0 );
796  if ( compute_determinant( m ) < limit )
797  return false;
798  }
799 
800  if ( M > 2 )
801  {
802  matrix< 3, 3, T > m;
803  matrix_.get_sub_matrix( m, 0, 0 );
804  if ( compute_determinant( m ) < limit )
805  return false;
806  }
807 
808  return true;
809 }
810 
811 
812 template< size_t M, size_t N, typename T >
813 matrix< M, N, T >::matrix()
814  : array() // http://stackoverflow.com/questions/5602030
815 {
816  if( M == N )
817  for( size_t i = 0; i < M; ++i )
818  at( i, i ) = static_cast< T >( 1.0 );
819 }
820 
821 template< size_t M, size_t N, typename T >
822 template< size_t P, size_t Q, typename U >
823 matrix< M, N, T >::matrix( const matrix< P, Q, U >& source_ )
824 {
825  (*this) = source_;
826 }
827 
828 
829 template< size_t M, size_t N, typename T >
830 inline T& matrix< M, N, T >::at( size_t row_index, size_t col_index )
831 {
832 #ifdef VMMLIB_SAFE_ACCESSORS
833  if ( row_index >= M || col_index >= N )
834  VMMLIB_ERROR( "at( row, col ) - index out of bounds", VMMLIB_HERE );
835 #endif
836  return array[ col_index * M + row_index ];
837 }
838 
839 
840 
841 template< size_t M, size_t N, typename T >
842 const inline T&
843 matrix< M, N, T >::at( size_t row_index, size_t col_index ) const
844 {
845 #ifdef VMMLIB_SAFE_ACCESSORS
846  if ( row_index >= M || col_index >= N )
847  VMMLIB_ERROR( "at( row, col ) - index out of bounds", VMMLIB_HERE );
848 #endif
849  return array[ col_index * M + row_index ];
850 }
851 
852 
853 template< size_t M, size_t N, typename T >
854 inline T&
855 matrix< M, N, T >::operator()( size_t row_index, size_t col_index )
856 {
857  return at( row_index, col_index );
858 }
859 
860 
861 
862 template< size_t M, size_t N, typename T >
863 const inline T&
864 matrix< M, N, T >::operator()( size_t row_index, size_t col_index ) const
865 {
866  return at( row_index, col_index );
867 }
868 
869 #ifndef VMMLIB_NO_CONVERSION_OPERATORS
870 
871 template< size_t M, size_t N, typename T >
872 matrix< M, N, T >::
873 operator T*()
874 {
875  return array;
876 }
877 
878 
879 
880 template< size_t M, size_t N, typename T >
881 matrix< M, N, T >::
882 operator const T*() const
883 {
884  return array;
885 }
886 
887 #endif
888 
889 
890 template< size_t M, size_t N, typename T >
891 bool
892 matrix< M, N, T >::
893 operator==( const matrix< M, N, T >& other ) const
894 {
895  bool is_ok = true;
896  for( size_t i = 0; is_ok && i < M * N; ++i )
897  {
898  is_ok = array[ i ] == other.array[ i ];
899  }
900  return is_ok;
901 }
902 
903 
904 
905 
906 template< size_t M, size_t N, typename T >
907 bool
908 matrix< M, N, T >::
909 operator!=( const matrix< M, N, T >& other ) const
910 {
911  return ! operator==( other );
912 }
913 
914 
915 
916 template< size_t M, size_t N, typename T >
917 bool
918 matrix< M, N, T >::
919 equals( const matrix< M, N, T >& other, T tolerance ) const
920 {
921  bool is_ok = true;
922  for( size_t row_index = 0; is_ok && row_index < M; row_index++)
923  {
924  for( size_t col_index = 0; is_ok && col_index < N; col_index++)
925  {
926  is_ok = fabs( at( row_index, col_index ) - other( row_index, col_index ) ) < tolerance;
927  }
928  }
929  return is_ok;
930 }
931 
932 
933 
934 template< size_t M, size_t N, typename T >
935 template< typename compare_t >
936 bool matrix< M, N, T >::equals( const matrix< M, N, T >& other_matrix,
937  compare_t& cmp ) const
938 {
939  bool is_ok = true;
940  for( size_t row = 0; is_ok && row < M; ++row )
941  {
942  for( size_t col = 0; is_ok && col < N; ++col)
943  {
944  is_ok = cmp( at( row, col ), other_matrix.at( row, col ) );
945  }
946  }
947  return is_ok;
948 }
949 
950 #if (( __GNUC__ > 4 ) || ((__GNUC__ == 4) && (__GNUC_MINOR__ >= 3)) )
951 # pragma GCC diagnostic ignored "-Warray-bounds" // gcc 4.4.7 bug WAR
952 #endif
953 template< size_t M, size_t N, typename T >
954 const matrix< M, N, T >&
955 matrix< M, N, T >::operator=( const matrix< M, N, T >& source_ )
956 {
957  memcpy( array, source_.array, M * N * sizeof( T ));
958  return *this;
959 }
960 #if (( __GNUC__ > 4 ) || ((__GNUC__ == 4) && (__GNUC_MINOR__ >= 3)) )
961 # pragma GCC diagnostic warning "-Warray-bounds"
962 #endif
963 
964 template< size_t M, size_t N, typename T >
965 template< size_t P, size_t Q, typename U >
966 void matrix< M, N, T >::operator=( const matrix< P, Q, U >& source_ )
967 {
968  const size_t minL = P < M ? P : M;
969  const size_t minC = Q < N ? Q : N;
970 
971  if( minL < M || minC < N )
972  zero();
973 
974  for ( size_t i = 0 ; i < minL ; i++ )
975  for ( size_t j = 0 ; j < minC ; j++ )
976  at( i,j ) = static_cast< T >( source_( i, j ));
977 }
978 
979 
980 template< size_t M, size_t N, typename T >
981 void matrix< M, N, T >::operator=( const T old_fashioned_matrix[ M ][ N ] )
982 {
983  for( size_t row = 0; row < M; row++)
984  {
985  for( size_t col = 0; col < N; col++)
986  {
987  at( row, col ) = old_fashioned_matrix[ row ][ col ];
988  }
989  }
990 
991 }
992 
993 
994 // WARNING: data_array[] must be at least of size M * N - otherwise CRASH!
995 // WARNING: assumes row_by_row layout - if this is not the case,
996 // use matrix::set( data_array, false )
997 template< size_t M, size_t N, typename T >
998 void
999 matrix< M, N, T >::operator=( const T* data_array )
1000 {
1001  set( data_array, data_array + M * N, true );
1002 }
1003 
1004 
1005 
1006 template< size_t M, size_t N, typename T >
1007 void
1008 matrix< M, N, T >::operator=( const std::vector< T >& data )
1009 {
1010  if ( data.size() < M * N )
1011  VMMLIB_ERROR( "index out of bounds.", VMMLIB_HERE );
1012 
1013  set( data.begin(), data.end(), true );
1014 }
1015 
1016 
1017 template< size_t M, size_t N, typename T >
1018 void
1019 matrix< M, N, T >::multiply_piecewise( const matrix& other )
1020 {
1021  for( size_t row_index = 0; row_index < M; row_index++)
1022  {
1023  for( size_t col_index = 0; col_index < N; col_index++ )
1024  {
1025  T& value = at( row_index, col_index );
1026  value *= other.at( row_index, col_index );
1027  }
1028  }
1029 }
1030 
1031 
1032 template< size_t M, size_t N, typename T >
1033 template< size_t P >
1034 void
1035 matrix< M, N, T >::multiply(
1036  const matrix< M, P, T >& left,
1037  const matrix< P, N, T >& right
1038  )
1039 {
1040  for( size_t row_index = 0; row_index < M; row_index++)
1041  {
1042  for( size_t col_index = 0; col_index < N; col_index++)
1043  {
1044  T& component = at( row_index, col_index );
1045  component = static_cast< T >( 0.0 );
1046  for( size_t p = 0; p < P; p++)
1047  {
1048  component += left.at( row_index, p ) * right.at( p, col_index );
1049  }
1050  }
1051  }
1052 }
1053 
1054 
1055 
1056 template< size_t M, size_t N, typename T >
1057 template< size_t P >
1058 matrix< M, P, T >
1059 matrix< M, N, T >::operator*( const matrix< N, P, T >& other ) const
1060 {
1061  matrix< M, P, T > result;
1062  result.multiply( *this, other );
1063  return result;
1064 }
1065 
1066 
1067 
1068 template< size_t M, size_t N, typename T >
1069 template< size_t O, size_t P, typename TT >
1070 typename enable_if< M == N && O == P && M == O, TT >::type*
1071 matrix< M, N, T >::operator*=( const matrix< O, P, TT >& right )
1072 {
1073  matrix< M, N, T > copy( *this );
1074  multiply( copy, right );
1075  return 0;
1076 }
1077 
1078 template< size_t M, size_t N, typename T >
1079 matrix< M, N, T >
1080 matrix< M, N, T >::operator/( T scalar )
1081 {
1082  matrix< M, N, T > result;
1083 
1084  for( size_t row_index = 0; row_index < M; ++row_index )
1085  {
1086  for( size_t col_index = 0; col_index < N; ++col_index )
1087  {
1088  result.at( row_index, col_index ) = at( row_index, col_index ) / scalar;
1089  }
1090  }
1091  return result;
1092 }
1093 
1094 
1095 
1096 template< size_t M, size_t N, typename T >
1097 void
1098 matrix< M, N, T >::operator/=( T scalar )
1099 {
1100  for( size_t row_index = 0; row_index < M; ++row_index )
1101  {
1102  for( size_t col_index = 0; col_index < N; ++col_index )
1103  {
1104  at( row_index, col_index ) /= scalar;
1105  }
1106  }
1107 }
1108 
1109 
1110 
1111 
1112 
1113 template< size_t M, size_t N, typename T >
1114 matrix< M, N, T >
1115 matrix< M, N, T >::operator*( T scalar )
1116 {
1117  matrix< M, N, T > result;
1118 
1119  for( size_t row_index = 0; row_index < M; ++row_index )
1120  {
1121  for( size_t col_index = 0; col_index < N; ++col_index )
1122  {
1123  result.at( row_index, col_index ) = at( row_index, col_index ) * scalar;
1124  }
1125  }
1126  return result;
1127 }
1128 
1129 
1130 
1131 template< size_t M, size_t N, typename T >
1132 void
1133 matrix< M, N, T >::operator*=( T scalar )
1134 {
1135  for( size_t row_index = 0; row_index < M; ++row_index )
1136  {
1137  for( size_t col_index = 0; col_index < N; ++col_index )
1138  {
1139  at( row_index, col_index ) *= scalar;
1140  }
1141  }
1142 }
1143 
1144 
1145 
1146 template< size_t M, size_t N, typename T >
1147 vector< M, T > matrix< M, N, T >::operator*( const vector< N, T >& vec ) const
1148 {
1149  vector< M, T > result;
1150 
1151  // this < M, 1 > = < M, P > * < P, 1 >
1152  for( size_t i = 0; i < M; ++i )
1153  {
1154  T tmp( 0 );
1155  for( size_t j = 0; j < N; ++j )
1156  tmp += at( i, j ) * vec.at( j );
1157  result.at( i ) = tmp;
1158  }
1159  return result;
1160 }
1161 
1162 
1163 
1164 // transform vector by matrix ( vec = matrix * vec )
1165 // assume homogenous coords( for vec3 = mat4 * vec3 ), e.g. vec[3] = 1.0
1166 template< size_t M, size_t N, typename T >
1167 template< size_t O > vector< O, T >
1168 matrix< M, N, T >::operator*( const vector< O, T >& vector_ ) const
1169 {
1170  vector< O, T > result;
1171  T tmp;
1172  for( size_t row_index = 0; row_index < M; ++row_index )
1173  {
1174  tmp = 0.0;
1175  for( size_t col_index = 0; col_index < N-1; ++col_index )
1176  {
1177  tmp += vector_( col_index ) * at( row_index, col_index );
1178  }
1179  if ( row_index < N - 1 )
1180  result( row_index ) = tmp + at( row_index, N-1 ); // * 1.0 -> homogeneous vec4
1181  else
1182  {
1183  tmp += at( row_index, N - 1 );
1184  for( size_t col_index = 0; col_index < N - 1; ++col_index )
1185  {
1186  result( col_index ) /= tmp;
1187  }
1188  }
1189  }
1190  return result;
1191 }
1192 
1193 
1194 
1195 template< size_t M, size_t N, typename T >
1196 inline matrix< M, N, T >
1197 matrix< M, N, T >::operator-() const
1198 {
1199  return negate();
1200 }
1201 
1202 
1203 
1204 template< size_t M, size_t N, typename T >
1205 matrix< M, N, T >
1206 matrix< M, N, T >::negate() const
1207 {
1208  matrix< M, N, T > result;
1209  result *= -1.0;
1210  return result;
1211 }
1212 
1213 
1214 
1215 template< size_t M, size_t N, typename T >
1216 void
1217 matrix< M, N, T >::tensor( const vector< M, T >& u, const vector< N, T >& v )
1218 {
1219  for ( size_t col_index = 0; col_index < N; ++col_index )
1220  for ( size_t row_index = 0; row_index < M; ++row_index )
1221  at( row_index, col_index ) = u.array[ col_index ] *
1222  v.array[ row_index ];
1223 }
1224 
1225 
1226 
1227 template< size_t M, size_t N, typename T >
1228 template< size_t uM, size_t vM >
1229 typename enable_if< uM == 3 && vM == 3 && M == N && M == 4 >::type*
1230 matrix< M, N, T >::tensor( const vector< uM, T >& u, const vector< vM, T >& v )
1231 {
1232  for ( size_t col_index = 0; col_index < 3; ++col_index )
1233  {
1234  for ( size_t row_index = 0; row_index < 3; ++row_index )
1235  at( row_index, col_index ) = u.array[ col_index ] *
1236  v.array[ row_index ];
1237 
1238  at( 3, col_index ) = u.array[ col_index ];
1239  }
1240 
1241  for ( size_t row_index = 0; row_index < 3; ++row_index )
1242  at( row_index, 3 ) = v.array[ row_index ];
1243 
1244  at( 3, 3 ) = 1.0;
1245 
1246  return 0;
1247 }
1248 
1249 
1250 
1251 template< size_t M, size_t N, typename T >
1252 void
1253 matrix< M, N, T >::
1254 transpose_to( matrix< N, M, T >& tM ) const
1255 {
1256  for( size_t row = 0; row < M; ++row )
1257  {
1258  for( size_t col = 0; col < N; ++col )
1259  {
1260  tM.at( col, row ) = at( row, col );
1261  }
1262  }
1263 }
1264 
1265 template< size_t M, size_t N, typename T >
1266 void
1267 matrix< M, N, T >::
1268 symmetric_covariance( matrix< M, M, T >& cov_m_ ) const
1269 {
1270  T tmp = 0;
1271  for( size_t row = 0; row < M; ++row )
1272  {
1273  for( size_t col = row; col < M; ++col )
1274  {
1275  for ( size_t k = 0; k < N; ++k )
1276  {
1277  tmp += (at( row, k ) * at( col, k ));
1278  }
1279 
1280  cov_m_.at( row, col ) = tmp;
1281  cov_m_.at( col, row ) = tmp;
1282  tmp = 0;
1283  }
1284  }
1285 }
1286 
1287 
1288 
1289 template< size_t M, size_t N, typename T >
1290 vector< M, T > matrix< M, N, T >::get_column( size_t index ) const
1291 {
1292  vector< M, T > column;
1293  get_column( index, column );
1294  return column;
1295 }
1296 
1297 
1298 
1299 template< size_t M, size_t N, typename T >
1300 void matrix< M, N, T >::get_column( size_t index, vector< M, T >& column ) const
1301 {
1302 #ifdef VMMLIB_SAFE_ACCESSORS
1303  if ( index >= N )
1304  VMMLIB_ERROR( "get_column() - index out of bounds.", VMMLIB_HERE );
1305 #endif
1306 
1307  memcpy( &column.array[0], &array[ M * index ], M * sizeof( T ) );
1308 }
1309 
1310 
1311 
1312 template< size_t M, size_t N, typename T >
1313 void matrix< M, N, T >::set_column( size_t index, const vector< M, T >& column )
1314 {
1315  #ifdef VMMLIB_SAFE_ACCESSORS
1316 
1317  if ( index >= N )
1318  VMMLIB_ERROR( "set_column() - index out of bounds.", VMMLIB_HERE );
1319 
1320  #endif
1321 
1322  memcpy( array + M * index, column.array, M * sizeof( T ) );
1323 }
1324 
1325 template< size_t M, size_t N, typename T >
1326 void matrix< M, N, T >::get_column( size_t index, matrix< M, 1, T >& column )
1327  const
1328 {
1329 #ifdef VMMLIB_SAFE_ACCESSORS
1330  if ( index >= N )
1331  VMMLIB_ERROR( "get_column() - index out of bounds.", VMMLIB_HERE );
1332 #endif
1333 
1334  memcpy( column.array, array + M * index, M * sizeof( T ) );
1335 }
1336 
1337 template< size_t M, size_t N, typename T >
1338 void matrix< M, N, T >::set_column( size_t index,
1339  const matrix< M, 1, T >& column )
1340 {
1341 #ifdef VMMLIB_SAFE_ACCESSORS
1342  if ( index >= N )
1343  VMMLIB_ERROR( "set_column() - index out of bounds.", VMMLIB_HERE );
1344 #endif
1345 
1346  memcpy( &array[ M * index ], column.array, M * sizeof( T ) );
1347 }
1348 
1349 template< size_t M, size_t N, typename T >
1350 vector< N, T > matrix< M, N, T >::get_row( size_t index ) const
1351 {
1352  vector< N, T > row;
1353  get_row( index, row );
1354  return row;
1355 }
1356 
1357 template< size_t M, size_t N, typename T >
1358 void matrix< M, N, T >::get_row( size_t row_index, vector< N, T >& row ) const
1359 {
1360 #ifdef VMMLIB_SAFE_ACCESSORS
1361  if ( row_index >= M )
1362  VMMLIB_ERROR( "get_row() - index out of bounds.", VMMLIB_HERE );
1363 #endif
1364 
1365  for( size_t col_index = 0; col_index < N; ++col_index )
1366  {
1367  row.at( col_index ) = at( row_index, col_index );
1368  }
1369 }
1370 
1371 template< size_t M, size_t N, typename T >
1372 void matrix< M, N, T >::set_row( size_t row_index, const vector< N, T >& row )
1373 {
1374 #ifdef VMMLIB_SAFE_ACCESSORS
1375  if ( row_index >= M )
1376  VMMLIB_ERROR( "set_row() - index out of bounds.", VMMLIB_HERE );
1377 #endif
1378 
1379  for( size_t col_index = 0; col_index < N; ++col_index )
1380  {
1381  at( row_index, col_index ) = row.at( col_index );
1382  }
1383 }
1384 
1385 template< size_t M, size_t N, typename T >
1386 void matrix< M, N, T >::get_row( size_t row_index, matrix< 1, N, T >& row )
1387  const
1388 {
1389 #ifdef VMMLIB_SAFE_ACCESSORS
1390  if ( row_index >= M )
1391  VMMLIB_ERROR( "get_row() - index out of bounds.", VMMLIB_HERE );
1392 #endif
1393 
1394  for( size_t col_index = 0; col_index < N; ++col_index )
1395  {
1396  row.at( 0, col_index ) = at( row_index, col_index );
1397  }
1398 }
1399 
1400 template< size_t M, size_t N, typename T >
1401 void matrix< M, N, T >::set_row( size_t row_index,
1402  const matrix< 1, N, T >& row )
1403 {
1404 #ifdef VMMLIB_SAFE_ACCESSORS
1405  if ( row_index >= M )
1406  VMMLIB_ERROR( "set_row() - index out of bounds.", VMMLIB_HERE );
1407 #endif
1408 
1409  for( size_t col_index = 0; col_index < N; ++col_index )
1410  {
1411  at( row_index, col_index ) = row.at( 0, col_index );
1412  }
1413 }
1414 
1415 template< size_t M, size_t N, typename T >
1416 size_t
1417 matrix< M, N, T >::
1418 get_number_of_rows() const
1419 {
1420  return M;
1421 }
1422 
1423 
1424 
1425 template< size_t M, size_t N, typename T >
1426 size_t
1427 matrix< M, N, T >::
1428 get_number_of_columns() const
1429 {
1430  return N;
1431 }
1432 
1433 
1434 
1435 template< size_t M, size_t N, typename T >
1436 void
1437 matrix< M, N, T >::
1438 fill( T fillValue )
1439 {
1440  for( size_t row_index = 0; row_index < M; ++row_index )
1441  {
1442  for( size_t col_index = 0; col_index < N; ++col_index )
1443  {
1444  at( row_index, col_index ) = fillValue;
1445  }
1446  }
1447 }
1448 
1449 
1450 template< size_t M, size_t N, typename T >
1451 inline T&
1452 matrix< M, N, T >::x()
1453 {
1454  return array[ 12 ];
1455 }
1456 
1457 
1458 
1459 template< size_t M, size_t N, typename T >
1460 inline T&
1461 matrix< M, N, T >::y()
1462 {
1463  return array[ 13 ];
1464 }
1465 
1466 
1467 
1468 template< size_t M, size_t N, typename T >
1469 inline T&
1470 matrix< M, N, T >::z()
1471 {
1472  return array[ 14 ];
1473 }
1474 
1475 template< size_t M, size_t N, typename T >
1476 template< typename input_iterator_t >
1477 void matrix< M, N, T >::set( input_iterator_t begin_,
1478  input_iterator_t end_, bool row_major_layout )
1479 {
1480  input_iterator_t it( begin_ );
1481  if( row_major_layout )
1482  {
1483  for( size_t row = 0; row < M; ++row )
1484  {
1485  for( size_t col = 0; col < N; ++col, ++it )
1486  {
1487  if ( it == end_ )
1488  return;
1489  at( row, col ) = static_cast< T >( *it );
1490  }
1491  }
1492  }
1493  else
1494  {
1495  std::copy( it, it + ( M * N ), begin() );
1496  }
1497 }
1498 
1499 
1500 
1501 template< size_t M, size_t N, typename T >
1502 void matrix< M, N, T >::zero()
1503 {
1504  fill( static_cast< T >( 0.0 ) );
1505 }
1506 
1507 template< size_t M, size_t N, typename T >
1508 void matrix< M, N, T >::operator=( T value_ )
1509 {
1510  std::fill( begin(), end(), value_ );
1511 }
1512 
1513 
1514 
1515 template< size_t M, size_t N, typename T >
1516 inline matrix< M, N, T >
1517 matrix< M, N, T >::operator+( const matrix< M, N, T >& other ) const
1518 {
1519  matrix< M, N, T > result( *this );
1520  result += other;
1521  return result;
1522 }
1523 
1524 
1525 
1526 template< size_t M, size_t N, typename T >
1527 void matrix< M, N, T >::operator+=( const matrix< M, N, T >& other )
1528 {
1529  iterator it = begin(), it_end = end();
1530  const_iterator other_it = other.begin();
1531  for( ; it != it_end; ++it, ++other_it )
1532  {
1533  *it += *other_it;
1534  }
1535 }
1536 
1537 
1538 template< size_t M, size_t N, typename T >
1539 void matrix< M, N, T >::operator+=( T scalar )
1540 {
1541  iterator it = begin(), it_end = end();
1542  for( ; it != it_end; ++it )
1543  {
1544  *it += scalar;
1545  }
1546 }
1547 
1548 template< size_t M, size_t N, typename T >
1549 inline matrix< M, N, T >
1550 matrix< M, N, T >::
1551 operator-( const matrix< M, N, T >& other ) const
1552 {
1553  matrix< M, N, T > result( *this );
1554  result -= other;
1555  return result;
1556 }
1557 
1558 
1559 
1560 template< size_t M, size_t N, typename T >
1561 void
1562 matrix< M, N, T >::
1563 operator-=( const matrix< M, N, T >& other )
1564 {
1565  iterator it = begin(), it_end = end();
1566  const_iterator other_it = other.begin();
1567  for( ; it != it_end; ++it, ++other_it )
1568  {
1569  *it -= *other_it;
1570  }
1571 }
1572 
1573 template< size_t M, size_t N, typename T >
1574 void
1575 matrix< M, N, T >::operator-=( T scalar )
1576 {
1577  iterator it = begin(), it_end = end();
1578  for( ; it != it_end; ++it )
1579  {
1580  *it -= scalar;
1581  }
1582 }
1583 
1584 
1585 template< size_t M, size_t N, typename T >
1586 template< size_t O, size_t P, size_t Q, size_t R >
1587 typename enable_if< M == O + Q && N == P + R >::type*
1588 matrix< M, N, T >::direct_sum( const matrix< O, P, T >& upper_left,
1589  const matrix< Q, R, T >& lower_right )
1590 {
1591  (*this) = static_cast< T >( 0.0 );
1592 
1593  for( size_t row = 0; row < O; ++row )
1594  {
1595  for( size_t col = 0; col < P; ++col )
1596  {
1597  at( row, col ) = upper_left( row, col );
1598  }
1599  }
1600 
1601  for( size_t row = 0; row < Q; ++row )
1602  {
1603  for( size_t col = 0; col < R; ++col )
1604  {
1605  at( O + row, P + col ) = lower_right( row, col );
1606  }
1607  }
1608 
1609  return 0;
1610 }
1611 
1612 
1613 
1614 template< size_t M, size_t N, typename T >
1615 template< size_t O, size_t P >
1616 matrix< O, P, T >
1617 matrix< M, N, T >::get_sub_matrix( size_t row_offset, size_t col_offset,
1618 typename enable_if< O <= M && P <= N >::type* ) const
1619 {
1620  matrix< O, P, T > result;
1621  get_sub_matrix( result, row_offset, col_offset );
1622  return result;
1623 }
1624 
1625 
1626 
1627 template< size_t M, size_t N, typename T >
1628 template< size_t O, size_t P >
1629 typename enable_if< O <= M && P <= N >::type*
1630 matrix< M, N, T >::
1631 get_sub_matrix( matrix< O, P, T >& result,
1632  size_t row_offset, size_t col_offset ) const
1633 {
1634  #ifdef VMMLIB_SAFE_ACCESSORS
1635  if ( O + row_offset > M || P + col_offset > N )
1636  VMMLIB_ERROR( "index out of bounds.", VMMLIB_HERE );
1637  #endif
1638 
1639  for( size_t row = 0; row < O; ++row )
1640  {
1641  for( size_t col = 0; col < P; ++col )
1642  {
1643  result.at( row, col )
1644  = at( row_offset + row, col_offset + col );
1645  }
1646  }
1647  return 0;
1648 }
1649 
1650 
1651 
1652 template< size_t M, size_t N, typename T >
1653 template< size_t O, size_t P >
1654 typename enable_if< O <= M && P <= N >::type*
1655 matrix< M, N, T >::
1656 set_sub_matrix( const matrix< O, P, T >& sub_matrix,
1657  size_t row_offset, size_t col_offset )
1658 {
1659  for( size_t row = 0; row < O; ++row )
1660  {
1661  for( size_t col = 0; col < P; ++col )
1662  {
1663  at( row_offset + row, col_offset + col )
1664  = sub_matrix.at( row, col );
1665  }
1666  }
1667  return 0; // for sfinae
1668 }
1669 
1670 
1671 
1672 template< size_t M, size_t N, typename T >
1673 inline T
1674 matrix< M, N, T >::det() const
1675 {
1676  return compute_determinant( *this );
1677 }
1678 
1679 
1680 
1681 template< size_t M, size_t N, typename T >
1682 template< size_t O, size_t P, typename TT >
1683 inline bool matrix< M, N, T >::inverse( matrix< O, P, TT >& inverse_,
1684  T tolerance, typename
1685  enable_if< M == N && O == P && O == M && M >= 2 && M <= 4, TT >::type* )
1686  const
1687 {
1688  return compute_inverse( *this, inverse_, tolerance );
1689 }
1690 
1691 
1692 
1693 template< size_t M, size_t N, typename T >
1694 template< size_t O, size_t P >
1695 typename enable_if< O == P && M == N && O == M && M >= 2 >::type*
1696 matrix< M, N, T >::
1697 get_adjugate( matrix< O, P, T >& adjugate ) const
1698 {
1699  get_cofactors( adjugate );
1700  adjugate = transpose( adjugate );
1701  return 0;
1702 }
1703 
1704 
1705 
1706 template< size_t M, size_t N, typename T >
1707 template< size_t O, size_t P >
1708 typename enable_if< O == P && M == N && O == M && M >= 2 >::type*
1709 matrix< M, N, T >::
1710 get_cofactors( matrix< O, P, T >& cofactors ) const
1711 {
1712  matrix< M-1, N-1, T > minor_;
1713 
1714  const size_t _negate = 1u;
1715  for( size_t row_index = 0; row_index < M; ++row_index )
1716  {
1717  for( size_t col_index = 0; col_index < N; ++col_index )
1718  {
1719  if ( ( row_index + col_index ) & _negate )
1720  cofactors( row_index, col_index ) = -get_minor( minor_, row_index, col_index );
1721  else
1722  cofactors( row_index, col_index ) = get_minor( minor_, row_index, col_index );
1723  }
1724  }
1725  return 0;
1726 }
1727 
1728 
1729 
1730 template< size_t M, size_t N, typename T >
1731 template< size_t O, size_t P >
1732 T
1733 matrix< M, N, T >::
1734 get_minor( matrix< O, P, T >& minor_, size_t row_to_cut, size_t col_to_cut,
1735 typename enable_if< O == M-1 && P == N-1 && M == N && M >= 2 >::type* ) const
1736 {
1737  size_t row_offset = 0;
1738  size_t col_offset = 0;
1739  for( size_t row_index = 0; row_index < M; ++row_index )
1740  {
1741  if ( row_index == row_to_cut )
1742  row_offset = -1;
1743  else
1744  {
1745  for( size_t col_index = 0; col_index < M; ++col_index )
1746  {
1747  if ( col_index == col_to_cut )
1748  col_offset = -1;
1749  else
1750  minor_.at( row_index + row_offset, col_index + col_offset )
1751  = at( row_index, col_index );
1752  }
1753  col_offset = 0;
1754  }
1755  }
1756  return compute_determinant( minor_ );
1757 }
1758 
1759 template< size_t M, size_t N, typename T >
1760 template< typename TT >
1761 matrix< M, N, T >& matrix< M, N, T >::rotate(
1762  const TT angle_, const vector< M-1, T >& axis,
1763  typename enable_if< M == N && M == 4, TT >::type* )
1764 {
1765  const T angle = static_cast< T >( angle_ );
1766 
1767  const T sine = std::sin( angle );
1768  const T cosine = std::cos( angle );
1769 
1770  // this is necessary since Visual Studio cannot resolve the
1771  // std::pow()-call correctly if we just use 2.0 directly.
1772  // this way, the '2.0' is converted to the same format
1773  // as the axis components
1774 
1775  const T _zero = 0.0;
1776  const T one = 1.0;
1777  const T two = 2.0;
1778 
1779  array[0] = cosine + ( one - cosine ) * std::pow( axis.array[0], two );
1780  array[1] = ( one - cosine ) * axis.array[0] * axis.array[1] + sine * axis.array[2];
1781  array[2] = ( one - cosine ) * axis.array[0] * axis.array[2] - sine * axis.array[1];
1782  array[3] = _zero;
1783 
1784  array[4] = ( one - cosine ) * axis.array[0] * axis.array[1] - sine * axis.array[2];
1785  array[5] = cosine + ( one - cosine ) * std::pow( axis.array[1], two );
1786  array[6] = ( one - cosine ) * axis.array[1] * axis.array[2] + sine * axis.array[0];
1787  array[7] = _zero;
1788 
1789  array[8] = ( one - cosine ) * axis.array[0] * axis.array[2] + sine * axis.array[1];
1790  array[9] = ( one - cosine ) * axis.array[1] * axis.array[2] - sine * axis.array[0];
1791  array[10] = cosine + ( one - cosine ) * std::pow( axis.array[2], two );
1792  array[11] = _zero;
1793 
1794  array[12] = _zero;
1795  array[13] = _zero;
1796  array[14] = _zero;
1797  array[15] = one;
1798 
1799  return *this;
1800 }
1801 
1802 template< size_t M, size_t N, typename T >
1803 template< typename TT >
1804 matrix< M, N, T >& matrix< M, N, T >::rotate_x( const TT angle_,
1805  typename enable_if< M == N && M == 4, TT >::type* )
1806 {
1807  const T angle = static_cast< T >( angle_ );
1808 
1809  const T sine = std::sin( angle );
1810  const T cosine = std::cos( angle );
1811 
1812  T tmp;
1813 
1814  tmp = array[ 4 ] * cosine + array[ 8 ] * sine;
1815  array[ 8 ] = - array[ 4 ] * sine + array[ 8 ] * cosine;
1816  array[ 4 ] = tmp;
1817 
1818  tmp = array[ 5 ] * cosine + array[ 9 ] * sine;
1819  array[ 9 ] = - array[ 5 ] * sine + array[ 9 ] * cosine;
1820  array[ 5 ] = tmp;
1821 
1822  tmp = array[ 6 ] * cosine + array[ 10 ] * sine;
1823  array[ 10 ] = - array[ 6 ] * sine + array[ 10 ] * cosine;
1824  array[ 6 ] = tmp;
1825 
1826  tmp = array[ 7 ] * cosine + array[ 11 ] * sine;
1827  array[ 11 ] = - array[ 7 ] * sine + array[ 11 ] * cosine;
1828  array[ 7 ] = tmp;
1829 
1830  return *this;
1831 }
1832 
1833 template< size_t M, size_t N, typename T >
1834 template< typename TT >
1835 matrix< M, N, T >& matrix< M, N, T >::rotate_y( const TT angle_,
1836  typename enable_if< M == N && M == 4, TT >::type* )
1837 {
1838  const T angle = static_cast< T >( angle_ );
1839 
1840  const T sine = std::sin( angle );
1841  const T cosine = std::cos( angle );
1842 
1843  T tmp;
1844 
1845  tmp = array[ 0 ] * cosine - array[ 8 ] * sine;
1846  array[ 8 ] = array[ 0 ] * sine + array[ 8 ] * cosine;
1847  array[ 0 ] = tmp;
1848 
1849  tmp = array[ 1 ] * cosine - array[ 9 ] * sine;
1850  array[ 9 ] = array[ 1 ] * sine + array[ 9 ] * cosine;
1851  array[ 1 ] = tmp;
1852 
1853  tmp = array[ 2 ] * cosine - array[ 10 ] * sine;
1854  array[ 10 ] = array[ 2 ] * sine + array[ 10 ] * cosine;
1855  array[ 2 ] = tmp;
1856 
1857  tmp = array[ 3 ] * cosine - array[ 11 ] * sine;
1858  array[ 11 ] = array[ 3 ] * sine + array[ 11 ] * cosine;
1859  array[ 3 ] = tmp;
1860 
1861  return *this;
1862 }
1863 
1864 template< size_t M, size_t N, typename T >
1865 template< typename TT >
1866 matrix< M, N, T >& matrix< M, N, T >::rotate_z( const TT angle_,
1867  typename enable_if< M == N && M == 4, TT >::type* )
1868 {
1869  const T angle = static_cast< T >( angle_ );
1870 
1871  const T sine = std::sin( angle );
1872  const T cosine = std::cos( angle );
1873 
1874  T tmp;
1875 
1876  tmp = array[ 0 ] * cosine + array[ 4 ] * sine;
1877  array[ 4 ] = - array[ 0 ] * sine + array[ 4 ] * cosine;
1878  array[ 0 ] = tmp;
1879 
1880  tmp = array[ 1 ] * cosine + array[ 5 ] * sine;
1881  array[ 5 ] = - array[ 1 ] * sine + array[ 5 ] * cosine;
1882  array[ 1 ] = tmp;
1883 
1884  tmp = array[ 2 ] * cosine + array[ 6 ] * sine;
1885  array[ 6 ] = - array[ 2 ] * sine + array[ 6 ] * cosine;
1886  array[ 2 ] = tmp;
1887 
1888  tmp = array[ 3 ] * cosine + array[ 7 ] * sine;
1889  array[ 7 ] = - array[ 3 ] * sine + array[ 7 ] * cosine;
1890  array[ 3 ] = tmp;
1891 
1892  return *this;
1893 }
1894 
1895 template< size_t M, size_t N, typename T >
1896 template< typename TT >
1897 matrix< M, N, T >& matrix< M, N, T >::pre_rotate_x( const TT angle_,
1898  typename enable_if< M == N && M == 4, TT >::type* )
1899 {
1900  const T angle = static_cast< T >( angle_ );
1901 
1902  const T sine = std::sin( angle );
1903  const T cosine = std::cos( angle );
1904 
1905  T tmp;
1906 
1907  tmp = array[ 1 ];
1908  array[ 1 ] = array[ 1 ] * cosine + array[ 2 ] * sine;
1909  array[ 2 ] = tmp * -sine + array[ 2 ] * cosine;
1910 
1911  tmp = array[ 5 ];
1912  array[ 5 ] = array[ 5 ] * cosine + array[ 6 ] * sine;
1913  array[ 6 ] = tmp * -sine + array[ 6 ] * cosine;
1914 
1915  tmp = array[ 9 ];
1916  array[ 9 ] = array[ 9 ] * cosine + array[ 10 ] * sine;
1917  array[ 10 ] = tmp * -sine + array[ 10 ] * cosine;
1918 
1919  tmp = array[ 13 ];
1920  array[ 13 ] = array[ 13 ] * cosine + array[ 14 ] * sine;
1921  array[ 14 ] = tmp * -sine + array[ 14 ] * cosine;
1922 
1923  return *this;
1924 }
1925 
1926 template< size_t M, size_t N, typename T >
1927 template< typename TT >
1928 matrix< M, N, T >& matrix< M, N, T >::pre_rotate_y( const TT angle_,
1929  typename enable_if< M == N && M == 4, TT >::type* )
1930 {
1931  const T angle = static_cast< T >( angle_ );
1932 
1933  const T sine = std::sin( angle );
1934  const T cosine = std::cos( angle );
1935 
1936  T tmp;
1937 
1938  tmp = array[ 0 ];
1939  array[ 0 ] = array[ 0 ] * cosine - array[ 2 ] * sine;
1940  array[ 2 ] = tmp * sine + array[ 2 ] * cosine;
1941 
1942  tmp = array[ 4 ];
1943  array[ 4 ] = array[ 4 ] * cosine - array[ 6 ] * sine;
1944  array[ 6 ] = tmp * sine + array[ 6 ] * cosine;
1945 
1946  tmp = array[ 8 ];
1947  array[ 8 ] = array[ 8 ] * cosine - array[ 10 ] * sine;
1948  array[ 10 ] = tmp * sine + array[ 10 ] * cosine;
1949 
1950  tmp = array[ 12 ];
1951  array[ 12 ] = array[ 12 ] * cosine - array[ 14 ] * sine;
1952  array[ 14 ] = tmp * sine + array[ 14 ] * cosine;
1953 
1954  return *this;
1955 }
1956 
1957 template< size_t M, size_t N, typename T >
1958 template< typename TT >
1959 matrix< M, N, T >& matrix< M, N, T >::pre_rotate_z( const TT angle_,
1960  typename enable_if< M == N && M == 4, TT >::type* )
1961 {
1962  const T angle = static_cast< T >( angle_ );
1963 
1964  const T sine = std::sin( angle );
1965  const T cosine = std::cos( angle );
1966 
1967  T tmp;
1968 
1969  tmp = array[ 0 ];
1970  array[ 0 ] = array[ 0 ] * cosine + array[ 1 ] * sine;
1971  array[ 1 ] = tmp * -sine + array[ 1 ] * cosine;
1972 
1973  tmp = array[ 4 ];
1974  array[ 4 ] = array[ 4 ] * cosine + array[ 5 ] * sine;
1975  array[ 5 ] = tmp * -sine + array[ 5 ] * cosine;
1976 
1977  tmp = array[ 8 ];
1978  array[ 8 ] = array[ 8 ] * cosine + array[ 9 ] * sine;
1979  array[ 9 ] = tmp * -sine + array[ 9 ] * cosine;
1980 
1981  tmp = array[ 12 ];
1982  array[ 12 ] = array[ 12 ] * cosine + array[ 13 ] * sine;
1983  array[ 13 ] = tmp * -sine + array[ 13 ] * cosine;
1984 
1985  return *this;
1986 }
1987 
1988 template< size_t M, size_t N, typename T >
1989 template< typename TT >
1990 matrix< M, N, T >& matrix< M, N, T >::scale( const TT _scale[3],
1991  typename enable_if< M == N && M == 4, TT >::type* )
1992 {
1993  const T scale0 = static_cast< T >( _scale[ 0 ] );
1994  const T scale1 = static_cast< T >( _scale[ 1 ] );
1995  const T scale2 = static_cast< T >( _scale[ 2 ] );
1996 
1997  array[0] *= scale0;
1998  array[1] *= scale0;
1999  array[2] *= scale0;
2000  array[3] *= scale0;
2001  array[4] *= scale1;
2002  array[5] *= scale1;
2003  array[6] *= scale1;
2004  array[7] *= scale1;
2005  array[8] *= scale2;
2006  array[9] *= scale2;
2007  array[10] *= scale2;
2008  array[11] *= scale2;
2009 
2010  return *this;
2011 }
2012 
2013 template< size_t M, size_t N, typename T >
2014 template< typename TT >
2015 matrix< M, N, T >& matrix< M, N, T >::scale( const TT x_, const T y_, const T z_,
2016  typename enable_if< M == N && M == 4, TT >::type* )
2017 {
2018  const T _x = static_cast< T >( x_ );
2019 
2020  array[0] *= _x;
2021  array[1] *= _x;
2022  array[2] *= _x;
2023  array[3] *= _x;
2024  array[4] *= y_;
2025  array[5] *= y_;
2026  array[6] *= y_;
2027  array[7] *= y_;
2028  array[8] *= z_;
2029  array[9] *= z_;
2030  array[10] *= z_;
2031  array[11] *= z_;
2032 
2033  return *this;
2034 }
2035 
2036 template< size_t M, size_t N, typename T >
2037 template< typename TT >
2038 inline matrix< M, N, T >& matrix< M, N, T >::scale(
2039  const vector< 3, TT >& scale_,
2040  typename enable_if< M == N && M == 4, TT >::type* )
2041 {
2042  return scale( scale_.array );
2043 }
2044 
2045 
2046 
2047 template< size_t M, size_t N, typename T >
2048 template< typename TT >
2049 matrix< M, N, T >& matrix< M, N, T >::scale_translation( const TT scale_[3],
2050  typename enable_if< M == N && M == 4, TT >::type* )
2051 {
2052  array[12] *= static_cast< T >( scale_[0] );
2053  array[13] *= static_cast< T >( scale_[1] );
2054  array[14] *= static_cast< T >( scale_[2] );
2055  return *this;
2056 }
2057 
2058 template< size_t M, size_t N, typename T >
2059 template< typename TT >
2060 inline matrix< M, N, T >& matrix< M, N, T >::scale_translation(
2061  const vector< 3, TT >& scale_,
2062  typename enable_if< M == N && M == 4, TT >::type* )
2063 {
2064  return scale_translation( scale_.array );
2065 }
2066 
2067 template< size_t M, size_t N, typename T >
2068 template< typename TT >
2069 inline matrix< M, N, T >& matrix< M, N, T >::set_translation(
2070  const TT x_, const TT y_, const TT z_,
2071  typename enable_if< M == N && M == 4, TT >::type* )
2072 {
2073  array[12] = static_cast< T >( x_ );
2074  array[13] = static_cast< T >( y_ );
2075  array[14] = static_cast< T >( z_ );
2076  return *this;
2077 }
2078 
2079 template< size_t M, size_t N, typename T >
2080 template< typename TT >
2081 inline matrix< M, N, T >& matrix< M, N, T >::set_translation( const TT trans[3],
2082  typename enable_if< M == N && M == 4, TT >::type* )
2083 {
2084  array[12] = static_cast< T >( trans[ 0 ] );
2085  array[13] = static_cast< T >( trans[ 1 ] );
2086  array[14] = static_cast< T >( trans[ 2 ] );
2087  return *this;
2088 }
2089 
2090 template< size_t M, size_t N, typename T >
2091 template< typename TT >
2092 inline matrix< M, N, T >& matrix< M, N, T >::set_translation(
2093  const vector< 3, TT >& translation_,
2094  typename enable_if< M == N && M == 4, TT >::type* )
2095 {
2096  return set_translation( translation_.array );
2097 }
2098 
2099 template< size_t M, size_t N, typename T > template< typename TT > inline
2100 void matrix< M, N, T >::get_translation( vector< N-1, TT >& translation_ )
2101  const
2102 {
2103  for( size_t i = 0; i < N-1; ++i )
2104  translation_.array[ i ] = array[ i + M * (N - 1) ];
2105 }
2106 
2107 
2108 template< size_t M, size_t N, typename T >
2109 inline vector< N-1, T > matrix< M, N, T >::get_translation() const
2110 {
2111  vector< N-1, T > result;
2112  get_translation( result );
2113  return result;
2114 }
2115 
2116 
2117 
2118 template< size_t M, size_t N, typename T >
2119 size_t
2120 matrix< M, N, T >::
2121 size() const
2122 {
2123  return M * N;
2124 }
2125 
2126 
2127 
2128 template< size_t M, size_t N, typename T >
2129 typename matrix< M, N, T >::iterator
2130 matrix< M, N, T >::
2131 begin()
2132 {
2133  return array;
2134 }
2135 
2136 
2137 
2138 template< size_t M, size_t N, typename T >
2139 typename matrix< M, N, T >::iterator
2140 matrix< M, N, T >::
2141 end()
2142 {
2143  return array + size();
2144 }
2145 
2146 
2147 
2148 template< size_t M, size_t N, typename T >
2149 typename matrix< M, N, T >::const_iterator
2150 matrix< M, N, T >::
2151 begin() const
2152 {
2153  return array;
2154 }
2155 
2156 
2157 
2158 template< size_t M, size_t N, typename T >
2159 typename matrix< M, N, T >::const_iterator
2160 matrix< M, N, T >::
2161 end() const
2162 {
2163  return array + size();
2164 }
2165 
2166 
2167 
2168 template< size_t M, size_t N, typename T >
2169 typename matrix< M, N, T >::reverse_iterator
2170 matrix< M, N, T >::
2171 rbegin()
2172 {
2173  return array + size() - 1;
2174 }
2175 
2176 
2177 
2178 template< size_t M, size_t N, typename T >
2179 typename matrix< M, N, T >::reverse_iterator
2180 matrix< M, N, T >::
2181 rend()
2182 {
2183  return array - 1;
2184 }
2185 
2186 
2187 
2188 template< size_t M, size_t N, typename T >
2189 typename matrix< M, N, T >::const_reverse_iterator
2190 matrix< M, N, T >::
2191 rbegin() const
2192 {
2193  return array + size() - 1;
2194 }
2195 
2196 
2197 
2198 template< size_t M, size_t N, typename T >
2199 typename matrix< M, N, T >::const_reverse_iterator
2200 matrix< M, N, T >::
2201 rend() const
2202 {
2203  return array - 1;
2204 }
2205 
2206 
2207 
2208 template< size_t M, size_t N, typename T > template< typename init_functor_t >
2209 const matrix< M, N, T > matrix< M, N, T >::get_initialized_matrix()
2210 {
2211  matrix< M, N, T > matrix_;
2212  init_functor_t()( matrix_ );
2213  return matrix_;
2214 }
2215 
2216 
2217 // it's ugly, but it allows having properly initialized static members
2218 // without having to worry about static initialization order.
2219 template< size_t M, size_t N, typename T >
2220 const matrix< M, N, T >
2221 matrix< M, N, T >::IDENTITY(
2222  matrix< M, N, T >::get_initialized_matrix<
2223  set_to_identity_functor< matrix< M, N, T > > >( ));
2224 
2225 
2226 template< size_t M, size_t N, typename T >
2227 const matrix< M, N, T >
2228 matrix< M, N, T >::ZERO(
2229  matrix< M, N, T >::
2230  get_initialized_matrix< set_to_zero_functor< matrix< M, N, T > > >()
2231  );
2232 
2233 
2234 
2235 template< size_t M, size_t N, typename T >
2236 double
2237 matrix< M, N, T >::frobenius_norm( ) const
2238 {
2239  double norm = 0.0;
2240 
2241  const_iterator it = begin(), it_end = end();
2242  for( ; it != it_end; ++it )
2243  {
2244  norm += *it * *it;
2245  }
2246 
2247  return std::sqrt(norm);
2248 }
2249 
2250 template< size_t M, size_t N, typename T >
2251 double
2252 matrix< M, N, T >::p_norm( double p ) const
2253 {
2254  double norm = 0.0;
2255 
2256  const_iterator it = begin(), it_end = end();
2257  for( ; it != it_end; ++it )
2258  {
2259  norm += std::pow(*it, p);
2260  }
2261 
2262  return std::pow(norm,1./p);
2263 }
2264 
2265 
2266 template< size_t M, size_t N, typename T >
2267 template< size_t O >
2268 void
2269 matrix< M, N, T >::khatri_rao_product( const matrix< O, N, T >& right_, matrix< M*O, N, T >& prod_ ) const
2270 {
2271  //build product for every column
2272  for (size_t col = 0; col < N; ++col )
2273  {
2274  for ( size_t m = 0; m < M; ++m )
2275  {
2276  for (size_t o = 0; o < O; ++o )
2277  {
2278  prod_.at(O*m + o, col) = at( m, col ) * right_.at( o, col );
2279  }
2280  }
2281  }
2282 }
2283 
2284 template< size_t M, size_t N, typename T >
2285 template< size_t O, size_t P >
2286 void
2287 matrix< M, N, T >::kronecker_product( const matrix< O, P, T >& right_, matrix< M*O, N*P, T >& result_ ) const
2288 {
2289  //build product for every column
2290  for (size_t m = 0; m < M; ++m )
2291  {
2292  for ( size_t n = 0; n < N; ++n )
2293  {
2294  for (size_t o = 0; o < O; ++o )
2295  {
2296  for (size_t p = 0; p < P; ++p )
2297  {
2298  result_.at(O*m + o, P*n + p) = at( m, n ) * right_.at( o, p );
2299  }
2300  }
2301  }
2302  }
2303 }
2304 
2305 
2306 template< size_t M, size_t N, typename T >
2307 template< typename TT >
2308 void
2309 matrix< M, N, T >::cast_from( const matrix< M, N, TT >& other )
2310 {
2311  typedef vmml::matrix< M, N, TT > matrix_tt_type ;
2312  typedef typename matrix_tt_type::const_iterator tt_const_iterator;
2313 
2314  iterator it = begin(), it_end = end();
2315  tt_const_iterator other_it = other.begin();
2316  for( ; it != it_end; ++it, ++other_it )
2317  {
2318  *it = static_cast< T >( *other_it );
2319  }
2320 }
2321 
2322 
2323 template< size_t M, size_t N, typename T >
2324 T
2325 matrix< M, N, T >::get_min() const
2326 {
2327  T min_value = static_cast<T>(std::numeric_limits<T>::max());
2328 
2329  const_iterator it = begin(),
2330  it_end = end();
2331  for( ; it != it_end; ++it)
2332  {
2333  if ( *it < min_value ) {
2334  min_value = *it;
2335  }
2336  }
2337  return min_value;
2338 }
2339 
2340 template< size_t M, size_t N, typename T >
2341 T
2342 matrix< M, N, T >::get_max() const
2343 {
2344  T max_value = static_cast<T>(0);
2345 
2346  const_iterator it = begin(),
2347  it_end = end();
2348  for( ; it != it_end; ++it)
2349  {
2350  if ( *it > max_value ) {
2351  max_value = *it;
2352  }
2353  }
2354  return max_value;
2355 }
2356 
2357 
2358 template< size_t M, size_t N, typename T >
2359 T
2360 matrix< M, N, T >::get_abs_min() const
2361 {
2362  T min_value = static_cast<T>(std::numeric_limits<T>::max());
2363 
2364  const_iterator it = begin(),
2365  it_end = end();
2366  for( ; it != it_end; ++it)
2367  {
2368  if ( fabs(*it) < fabs(min_value) ) {
2369  min_value = fabs(*it);
2370  }
2371  }
2372  return min_value;
2373 }
2374 
2375 template< size_t M, size_t N, typename T >
2376 T
2377 matrix< M, N, T >::get_abs_max() const
2378 {
2379  T max_value = static_cast<T>(0);
2380 
2381  const_iterator it = begin(),
2382  it_end = end();
2383  for( ; it != it_end; ++it)
2384  {
2385  if ( fabs(*it) > fabs(max_value) ) {
2386  max_value = fabs(*it);
2387  }
2388  }
2389  return max_value;
2390 }
2391 
2392 
2393 
2394 template< size_t M, size_t N, typename T >
2395 size_t
2396 matrix< M, N, T >::nnz() const
2397 {
2398  size_t counter = 0;
2399 
2400  const_iterator it = begin(),
2401  it_end = end();
2402  for( ; it != it_end; ++it)
2403  {
2404  if ( *it != 0 ) {
2405  ++counter;
2406  }
2407  }
2408 
2409  return counter;
2410 }
2411 
2412 template< size_t M, size_t N, typename T >
2413 size_t
2414 matrix< M, N, T >::nnz( const T& threshold_ ) const
2415 {
2416  size_t counter = 0;
2417 
2418  const_iterator it = begin(),
2419  it_end = end();
2420  for( ; it != it_end; ++it)
2421  {
2422  if ( fabs(*it) > threshold_ ) {
2423  ++counter;
2424  }
2425  }
2426 
2427  return counter;
2428 }
2429 
2430 template< size_t M, size_t N, typename T >
2431 void
2432 matrix< M, N, T >::threshold( const T& threshold_value_ )
2433 {
2434  iterator it = begin(),
2435  it_end = end();
2436  for( ; it != it_end; ++it)
2437  {
2438  if ( fabs(*it) <= threshold_value_ ) {
2439  *it = static_cast<T> (0);
2440  }
2441  }
2442 }
2443 
2444 
2445 template< size_t M, size_t N, typename T >
2446 template< typename TT >
2447 void
2448 matrix< M, N, T >::quantize_to( matrix< M, N, TT >& quantized_, const T& min_value, const T& max_value ) const
2449 {
2450  long max_tt_range = long(std::numeric_limits< TT >::max());
2451  long min_tt_range = long(std::numeric_limits< TT >::min());
2452  long tt_range = (max_tt_range - min_tt_range);
2453 
2454  T t_range = max_value - min_value;
2455 
2456  typedef matrix< M, N, TT > m_tt_type ;
2457  typedef typename m_tt_type::iterator tt_iterator;
2458  tt_iterator it_quant = quantized_.begin();
2459  const_iterator it = begin(), it_end = end();
2460 
2461  for( ; it != it_end; ++it, ++it_quant )
2462  {
2463  if (std::numeric_limits<TT>::is_signed ) {
2464  *it_quant = TT( std::min( std::max( min_tt_range, long(( *it * tt_range / t_range ) + 0.5)), max_tt_range ));
2465  } else {
2466  *it_quant = TT( std::min( std::max( min_tt_range, long(((*it - min_value) * tt_range / t_range) + 0.5)), max_tt_range ));
2467  }
2468  }
2469 }
2470 
2471 
2472 template< size_t M, size_t N, typename T >
2473 template< typename TT >
2474 void
2475 matrix< M, N, T >::quantize( matrix< M, N, TT >& quantized_, T& min_value, T& max_value ) const
2476 {
2477  min_value = get_min();
2478  max_value = get_max();
2479  quantize_to( quantized_, min_value, max_value );
2480 }
2481 
2482 
2483 
2484 template< size_t M, size_t N, typename T >
2485 template< typename TT >
2486 void
2487 matrix< M, N, T >::dequantize( matrix< M, N, TT >& dequantized_, const TT& min_value, const TT& max_value ) const
2488 {
2489  long max_t_range = long(std::numeric_limits< T >::max());
2490  long min_t_range = long(std::numeric_limits< T >::min());
2491  long t_range = (max_t_range - min_t_range);
2492 
2493  TT tt_range = max_value - min_value;
2494 
2495  typedef matrix< M, N, TT > m_tt_type ;
2496  typedef typename m_tt_type::iterator tt_iterator;
2497  tt_iterator it_dequant = dequantized_.begin();
2498  const_iterator it = begin(), it_end = end();
2499  for( ; it != it_end; ++it, ++it_dequant )
2500  {
2501  if (std::numeric_limits<T>::is_signed ) {
2502  *it_dequant = std::min( std::max( min_value, TT((TT(*it) / t_range) * tt_range)), max_value );
2503  } else {
2504  *it_dequant = std::min( std::max( min_value, TT((((TT(*it) / t_range)) * tt_range ) + min_value)), max_value );
2505  }
2506  }
2507 }
2508 
2509 template< size_t M, size_t N, typename T >
2510 void
2511 matrix< M, N, T >::columnwise_sum( vector< N, T>& summed_columns_ ) const
2512 {
2513 
2514  for ( size_t n = 0; n < N; ++n )
2515  {
2516  T value = 0;
2517  for ( size_t m = 0; m < M; ++m )
2518  {
2519  value += at( m, n );
2520  }
2521  summed_columns_.at( n ) = value;
2522  }
2523 }
2524 
2525 template< size_t M, size_t N, typename T >
2526 double
2527 matrix< M, N, T >::sum_elements( ) const
2528 {
2529  double sum = 0.0;
2530 
2531  const_iterator it = begin(), it_end = end();
2532  for( ; it != it_end; ++it )
2533  {
2534  sum += *it;
2535  }
2536 
2537  return sum;
2538 }
2539 
2540 template< size_t M, size_t N, typename T >
2541 template< size_t R>
2542 typename enable_if< R == M && R == N>::type*
2543 matrix< M, N, T >::diag( const vector< R, T >& diag_values_ )
2544 {
2545  zero();
2546  for( size_t r = 0; r < R; ++r )
2547  {
2548  at(r, r) = static_cast< T >( diag_values_.at(r) );
2549  }
2550  return 0;
2551 }
2552 
2553 
2554 template< size_t M, size_t N, typename T >
2555 void
2556 matrix< M, N, T >::set_dct()
2557 {
2558  const double num_rows = M;
2559  double fill_value = 0.0f;
2560  for( size_t row = 0; row < M; ++row )
2561  {
2562  // to receive orthonormality
2563  const double weight = ( row == 0.0 ) ? std::sqrt(1/num_rows) :
2564  std::sqrt(2/num_rows);
2565  for( size_t col = 0; col < N; ++col )
2566  {
2567  fill_value = (2 * col + 1) * row * M_PI / (2*M);
2568  fill_value = std::cos( fill_value );
2569  fill_value *= weight;
2570  at( row, col ) = static_cast< T >( fill_value ) ;
2571  }
2572  }
2573 }
2574 
2575 
2576 template< size_t M, size_t N, typename T >
2577 void
2578 matrix< M, N, T >::set_random( int seed )
2579 {
2580  if ( seed >= 0 )
2581  srand( seed );
2582 
2583  double fillValue = 0.0f;
2584  for( size_t row = 0; row < M; ++row )
2585  {
2586  for( size_t col = 0; col < N; ++col )
2587  {
2588  fillValue = rand();
2589  fillValue /= RAND_MAX;
2590  at( row, col ) = -1.0 + 2.0 * static_cast< double >( fillValue ) ;
2591  }
2592  }
2593 }
2594 
2595 template< size_t M, size_t N, typename T >
2596 void
2597 matrix< M, N, T >::write_to_raw( const std::string& dir_, const std::string& filename_ ) const
2598 {
2599  int dir_length = dir_.size() -1;
2600  int last_separator = dir_.find_last_of( "/");
2601  std::string path = dir_;
2602  if (last_separator < dir_length ) {
2603  path.append( "/" );
2604  }
2605  path.append( filename_ );
2606  //check for format
2607  if( filename_.find( "raw", filename_.size() -3) == std::string::npos )
2608  {
2609  path.append( ".");
2610  path.append( "raw" );
2611  }
2612  std::string path_raw = path;
2613 
2614  std::ofstream outfile;
2615  outfile.open( path_raw.c_str() );
2616  if( outfile.is_open() ) {
2617  size_t len_slice = sizeof(T) * M*N;
2618  outfile.write( (char*)&(*this), len_slice );
2619  outfile.close();
2620  } else {
2621  std::cout << "no file open" << std::endl;
2622  }
2623 }
2624 
2625 template< size_t M, size_t N, typename T >
2626 void
2627 matrix< M, N, T >::read_from_raw( const std::string& dir_, const std::string& filename_ )
2628 {
2629  int dir_length = dir_.size() -1;
2630  int last_separator = dir_.find_last_of( "/");
2631  std::string path = dir_;
2632  if (last_separator < dir_length ) {
2633  path.append( "/" );
2634  }
2635  path.append( filename_ );
2636 
2637  size_t max_file_len = 2147483648u - sizeof(T) ;
2638  size_t len_data = sizeof(T) * size();
2639  char* data = new char[ len_data ];
2640  std::ifstream infile;
2641  infile.open( path.c_str(), std::ios::in);
2642 
2643  if( infile.is_open())
2644  {
2645  iterator it = begin(),
2646  it_end = end();
2647 
2648  while ( len_data > 0 )
2649  {
2650  size_t len_read = (len_data % max_file_len ) > 0 ?
2651  len_data % max_file_len : len_data;
2652  len_data -= len_read;
2653  infile.read( data, len_read );
2654 
2655  T* T_ptr = (T*)&(data[0]);
2656  for( ; (it != it_end) && (len_read > 0); ++it, len_read -= sizeof(T) )
2657  {
2658  *it = *T_ptr; ++T_ptr;
2659  }
2660  }
2661 
2662  delete[] data;
2663  infile.close();
2664  } else {
2665  std::cout << "no file open" << std::endl;
2666  }
2667 
2668 }
2669 
2670 
2671 
2672 
2673 template< size_t M, size_t N, typename T >
2674 void
2675 matrix< M, N, T >::write_csv_file( const std::string& dir_, const std::string& filename_ ) const
2676 {
2677  int dir_length = dir_.size() -1;
2678  int last_separator = dir_.find_last_of( "/");
2679  std::string path = dir_;
2680  if (last_separator < dir_length ) {
2681  path.append( "/" );
2682  }
2683  path.append( filename_ );
2684  //check for format
2685  int suffix_pos = filename_.find( "csv", filename_.size() -3);
2686  if( suffix_pos == (-1)) {
2687  path.append( ".");
2688  path.append( "csv" );
2689  }
2690 
2691  std::ofstream outfile;
2692  outfile.open( path.c_str() );
2693  if( outfile.is_open() ) {
2694  outfile << *this << std::endl;
2695  outfile.close();
2696  } else {
2697  std::cout << "no file open" << std::endl;
2698  }
2699 
2700 }
2701 
2702 template< size_t M, size_t N, typename T >
2703 void
2704 matrix< M, N, T >::read_csv_file( const std::string& dir_, const std::string& filename_ )
2705 {
2706  int dir_length = dir_.size() -1;
2707  int last_separator = dir_.find_last_of( "/");
2708  std::string path = dir_;
2709  if (last_separator < dir_length ) {
2710  path.append( "/" );
2711  }
2712  path.append( filename_ );
2713  //check for format
2714  int suffix_pos = filename_.find( "csv", filename_.size() -3);
2715  if( suffix_pos == (-1)) {
2716  path.append( ".");
2717  path.append( "csv" );
2718  }
2719 
2720  std::ifstream infile;
2721  infile.open( path.c_str(), std::ios::in);
2722  if( infile.is_open() ) {
2723  //TODO: not yet implemented
2724  //infile >> *this >> std::endl;
2725  infile.close();
2726  } else {
2727  std::cout << "no file open" << std::endl;
2728  }
2729 
2730 }
2731 
2732 
2733 template< size_t M, size_t N, typename T >
2734 void
2735 matrix< M, N, T >::sum_rows( matrix< M/2, N, T>& other ) const
2736 {
2737  typedef vector< N, T > row_type;
2738 
2739  row_type* row0 = new row_type;
2740  row_type* row1 = new row_type;
2741 
2742  other.zero();
2743 
2744  for ( size_t row = 0; row < M; ++row )
2745  {
2746  get_row( row++, *row0 );
2747  if ( row < M )
2748  {
2749  get_row( row, *row1 );
2750  *row0 += *row1;
2751  other.set_row( row/2 , *row0 );
2752  }
2753  }
2754 
2755  delete row0;
2756  delete row1;
2757 }
2758 
2759 template< size_t M, size_t N, typename T >
2760 void
2761 matrix< M, N, T >::sum_columns( matrix< M, N/2, T>& other ) const
2762 {
2763  typedef vector< M, T > col_type;
2764 
2765  col_type* col0 = new col_type;
2766  col_type* col1 = new col_type;
2767 
2768  other.zero();
2769 
2770  for ( size_t col = 0; col< N; ++col )
2771  {
2772  get_column( col++, *col0 );
2773  if ( col < N )
2774  {
2775  get_column( col, *col1 );
2776  *col0 += *col1;
2777  other.set_column( col/2, *col0 );
2778  }
2779  }
2780 
2781  delete col0;
2782  delete col1;
2783 }
2784 
2785 
2786 
2787 } // namespace vmml
2788 
2789 #endif
vmml::matrix< 3, 3, double > Matrix3d
A 3x3 double matrix.
Definition: matrix.hpp:486
vmml::matrix< 4, 4, float > Matrix4f
A 4x4 float matrix.
Definition: matrix.hpp:489
vmml::matrix< 4, 4, double > Matrix4d
A 4x4 double matrix.
Definition: matrix.hpp:487
vmml::matrix< 3, 3, float > Matrix3f
A 3x3 float matrix.
Definition: matrix.hpp:488