vmmlib  1.7.0
 All Classes Namespaces Functions 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 #ifdef EQFABRIC_API_H
456  const std::ios::fmtflags flags = os.flags();
457  const int prec = os.precision();
458 
459  os.setf( std::ios::right, std::ios::adjustfield );
460  os.precision( 5 );
461 
462  for( size_t row_index = 0; row_index < M; ++row_index )
463  {
464  os << "|";
465  for( size_t col_index = 0; col_index < N; ++col_index )
466  {
467  os << std::setw(10) << matrix.at( row_index, col_index ) << " ";
468  }
469  os << "|" << std::endl;
470  }
471  os.precision( prec );
472  os.setf( flags );
473 #else
474  for( size_t row_index = 0; row_index < M; ++row_index )
475  {
476  os << "(";
477  for( size_t col_index = 0; col_index < N; ++col_index )
478  {
479  if( sizeof(T) ==sizeof(unsigned char)) {
480  os << int(matrix.at( row_index, col_index ));
481  } else {
482  os << matrix.at( row_index, col_index );
483  }
484  if (col_index + 1 < N )
485  os << ", ";
486  }
487  os << ")" << std::endl;
488  }
489 #endif
490  return os;
491  };
492 
493  // protected:
494  // column_by_column
495  VMMLIB_ALIGN( T array[ M * N ] );
496 
497  // static members
498  static const matrix< M, N, T > IDENTITY;
499  static const matrix< M, N, T > ZERO;
500 
501 }; // class matrix
502 
503 
504 #ifndef VMMLIB_NO_TYPEDEFS
509 #endif
510 
511 /*
512 * free functions
513 */
514 
515 template< size_t M, size_t N, typename T >
516 bool equals( const matrix< M, N, T >& m0, const matrix< M, N, T >& m1,
517  const T tolerance = std::numeric_limits< T >::epsilon())
518 {
519  return m0.equals( m1, tolerance );
520 }
521 
522 
523 template< size_t M, size_t N, typename T >
524 inline void multiply( const matrix< M, N, T >& left,
525  const matrix< M, N, T >& right,
526  matrix< M, N, T >& result )
527 {
528  result.multiply( left, right );
529 }
530 
531 
532 
533 template< size_t M, size_t N, typename T >
534 template< size_t U, size_t V >
535 void matrix< M, N, T>::convolve(const matrix< U, V, T >& kernel)
536 {
537  matrix< M, N, T> temp; // do not override original values instantly as old values are needed for calculation
538 
539  for(size_t y_ = 0; y_ < N; ++y_)
540  {
541  for(size_t x_ = 0; x_ < M; ++x_)
542  {
543  double sum = 0.0;
544 
545  for(size_t j = 0; j < V; ++j)
546  {
547  int srcy = y_ - V/2 + j;
548 
549  // Extending border values
550  if(srcy < 0) srcy = 0;
551  if(srcy >= int(N)) srcy = N-1;
552 
553  for(size_t i = 0; i < U; ++i)
554  {
555  int srcx = x_ - U/2 + i;
556 
557  // Extending border values
558  if(srcx < 0) srcx = 0;
559  if(srcx >= int(M)) srcx = M-1;
560 
561  sum += kernel.at(j,i) * at(srcy,srcx);
562  }
563  }
564  temp.at(y_,x_) = sum;
565  }
566  }
567 
568  *this = temp;
569 }
570 
571 
572 
573 template< size_t M, size_t N, size_t P, typename T >
574 inline void
575 multiply( const matrix< M, P, T >& left, const matrix< P, N, T >& right,
576  matrix< M, N, T >& result )
577 {
578  result.multiply( left, right );
579 }
580 
581 
582 template< size_t M, size_t N, typename T >
583 inline typename enable_if< M == N >::type* identity( matrix< M, N, T >& m )
584 {
585  m = static_cast< T >( 0.0 );
586  for( size_t index = 0; index < M; ++index )
587  m( index, index ) = static_cast< T >( 1.0 );
588  return 0; // for sfinae
589 }
590 
591 
592 
593 template< typename T >
594 inline T compute_determinant( const matrix< 1, 1, T >& matrix_ )
595 {
596  return matrix_.array[ 0 ];
597 }
598 
599 
600 
601 template< typename T >
602 inline T compute_determinant( const matrix< 2, 2, T >& matrix_ )
603 {
604  const T& a = matrix_( 0, 0 );
605  const T& b = matrix_( 0, 1 );
606  const T& c = matrix_( 1, 0 );
607  const T& d = matrix_( 1, 1 );
608  return a * d - b * c;
609 }
610 
611 
612 
613 template< typename T >
614 inline T compute_determinant( const matrix< 3, 3, T >& m_ )
615 {
616  return
617  m_( 0,0 ) * ( m_( 1,1 ) * m_( 2,2 ) - m_( 1,2 ) * m_( 2,1 ) )
618  + m_( 0,1 ) * ( m_( 1,2 ) * m_( 2,0 ) - m_( 1,0 ) * m_( 2,2 ) )
619  + m_( 0,2 ) * ( m_( 1,0 ) * m_( 2,1 ) - m_( 1,1 ) * m_( 2,0 ) );
620 }
621 
622 
623 template< typename T >
624 inline T compute_determinant( const matrix< 4, 4, T >& m )
625 {
626  T m00 = m( 0, 0 );
627  T m10 = m( 1, 0 );
628  T m20 = m( 2, 0 );
629  T m30 = m( 3, 0 );
630  T m01 = m( 0, 1 );
631  T m11 = m( 1, 1 );
632  T m21 = m( 2, 1 );
633  T m31 = m( 3, 1 );
634  T m02 = m( 0, 2 );
635  T m12 = m( 1, 2 );
636  T m22 = m( 2, 2 );
637  T m32 = m( 3, 2 );
638  T m03 = m( 0, 3 );
639  T m13 = m( 1, 3 );
640  T m23 = m( 2, 3 );
641  T m33 = m( 3, 3 );
642 
643  return
644  m03 * m12 * m21 * m30
645  - m02 * m13 * m21 * m30
646  - m03 * m11 * m22 * m30
647  + m01 * m13 * m22 * m30
648  + m02 * m11 * m23 * m30
649  - m01 * m12 * m23 * m30
650  - m03 * m12 * m20 * m31
651  + m02 * m13 * m20 * m31
652  + m03 * m10 * m22 * m31
653  - m00 * m13 * m22 * m31
654  - m02 * m10 * m23 * m31
655  + m00 * m12 * m23 * m31
656  + m03 * m11 * m20 * m32
657  - m01 * m13 * m20 * m32
658  - m03 * m10 * m21 * m32
659  + m00 * m13 * m21 * m32
660  + m01 * m10 * m23 * m32
661  - m00 * m11 * m23 * m32
662  - m02 * m11 * m20 * m33
663  + m01 * m12 * m20 * m33
664  + m02 * m10 * m21 * m33
665  - m00 * m12 * m21 * m33
666  - m01 * m10 * m22 * m33
667  + m00 * m11 * m22 * m33;
668 }
669 
670 
671 
672 template< typename T >
673 bool compute_inverse( const matrix< 2, 2, T >& m_, matrix< 2, 2, T >& inverse_,
674  T tolerance_ = std::numeric_limits<T>::epsilon())
675 {
676  const T det_ = compute_determinant( m_ );
677  if( fabs( det_ ) < tolerance_ )
678  return false;
679 
680  const T detinv = static_cast< T >( 1.0 ) / det_;
681 
682  m_.get_adjugate( inverse_ ); // set inverse_ to the adjugate of M
683  inverse_ *= detinv;
684  return true;
685 }
686 
687 
688 
689 template< typename T >
690 bool compute_inverse( const matrix< 3, 3, T >& m_, matrix< 3, 3, T >& inverse_,
691  T tolerance_ = std::numeric_limits<T>::epsilon() )
692 {
693  // Invert a 3x3 using cofactors. This is about 8 times faster than
694  // the Numerical Recipes code which uses Gaussian elimination.
695  inverse_.at( 0, 0 ) = m_.at( 1, 1 ) * m_.at( 2, 2 ) - m_.at( 1, 2 ) * m_.at( 2, 1 );
696  inverse_.at( 0, 1 ) = m_.at( 0, 2 ) * m_.at( 2, 1 ) - m_.at( 0, 1 ) * m_.at( 2, 2 );
697  inverse_.at( 0, 2 ) = m_.at( 0, 1 ) * m_.at( 1, 2 ) - m_.at( 0, 2 ) * m_.at( 1, 1 );
698  inverse_.at( 1, 0 ) = m_.at( 1, 2 ) * m_.at( 2, 0 ) - m_.at( 1, 0 ) * m_.at( 2, 2 );
699  inverse_.at( 1, 1 ) = m_.at( 0, 0 ) * m_.at( 2, 2 ) - m_.at( 0, 2 ) * m_.at( 2, 0 );
700  inverse_.at( 1, 2 ) = m_.at( 0, 2 ) * m_.at( 1, 0 ) - m_.at( 0, 0 ) * m_.at( 1, 2 );
701  inverse_.at( 2, 0 ) = m_.at( 1, 0 ) * m_.at( 2, 1 ) - m_.at( 1, 1 ) * m_.at( 2, 0 );
702  inverse_.at( 2, 1 ) = m_.at( 0, 1 ) * m_.at( 2, 0 ) - m_.at( 0, 0 ) * m_.at( 2, 1 );
703  inverse_.at( 2, 2 ) = m_.at( 0, 0 ) * m_.at( 1, 1 ) - m_.at( 0, 1 ) * m_.at( 1, 0 );
704 
705  const T determinant =
706  m_.at( 0, 0 ) * inverse_.at( 0, 0 )
707  + m_.at( 0, 1 ) * inverse_.at( 1, 0 )
708  + m_.at( 0, 2 ) * inverse_.at( 2, 0 );
709 
710  if ( fabs( determinant ) <= tolerance_ )
711  return false; // matrix is not invertible
712 
713  const T detinv = static_cast< T >( 1.0 ) / determinant;
714 
715  inverse_.at( 0, 0 ) *= detinv;
716  inverse_.at( 0, 1 ) *= detinv;
717  inverse_.at( 0, 2 ) *= detinv;
718  inverse_.at( 1, 0 ) *= detinv;
719  inverse_.at( 1, 1 ) *= detinv;
720  inverse_.at( 1, 2 ) *= detinv;
721  inverse_.at( 2, 0 ) *= detinv;
722  inverse_.at( 2, 1 ) *= detinv;
723  inverse_.at( 2, 2 ) *= detinv;
724 
725  return true;
726 }
727 
728 
729 template< typename T >
730 bool compute_inverse( const matrix< 4, 4, T >& m_,
731  matrix< 4, 4, T >& inv_,
732  T tolerance_ = std::numeric_limits<T>::epsilon() )
733 {
734  const T* array = m_.array;
735 
736  // tuned version from Claude Knaus
737  /* first set of 2x2 determinants: 12 multiplications, 6 additions */
738  const T t1[6] = { array[ 2] * array[ 7] - array[ 6] * array[ 3],
739  array[ 2] * array[11] - array[10] * array[ 3],
740  array[ 2] * array[15] - array[14] * array[ 3],
741  array[ 6] * array[11] - array[10] * array[ 7],
742  array[ 6] * array[15] - array[14] * array[ 7],
743  array[10] * array[15] - array[14] * array[11] };
744 
745  /* first half of comatrix: 24 multiplications, 16 additions */
746  inv_.array[0] = array[ 5] * t1[5] - array[ 9] * t1[4] + array[13] * t1[3];
747  inv_.array[1] = array[ 9] * t1[2] - array[13] * t1[1] - array[ 1] * t1[5];
748  inv_.array[2] = array[13] * t1[0] - array[ 5] * t1[2] + array[ 1] * t1[4];
749  inv_.array[3] = array[ 5] * t1[1] - array[ 1] * t1[3] - array[ 9] * t1[0];
750  inv_.array[4] = array[ 8] * t1[4] - array[ 4] * t1[5] - array[12] * t1[3];
751  inv_.array[5] = array[ 0] * t1[5] - array[ 8] * t1[2] + array[12] * t1[1];
752  inv_.array[6] = array[ 4] * t1[2] - array[12] * t1[0] - array[ 0] * t1[4];
753  inv_.array[7] = array[ 0] * t1[3] - array[ 4] * t1[1] + array[ 8] * t1[0];
754 
755  /* second set of 2x2 determinants: 12 multiplications, 6 additions */
756  const T t2[6] = { array[ 0] * array[ 5] - array[ 4] * array[ 1],
757  array[ 0] * array[ 9] - array[ 8] * array[ 1],
758  array[ 0] * array[13] - array[12] * array[ 1],
759  array[ 4] * array[ 9] - array[ 8] * array[ 5],
760  array[ 4] * array[13] - array[12] * array[ 5],
761  array[ 8] * array[13] - array[12] * array[ 9] };
762 
763  /* second half of comatrix: 24 multiplications, 16 additions */
764  inv_.array[8] = array[ 7] * t2[5] - array[11] * t2[4] + array[15] * t2[3];
765  inv_.array[9] = array[11] * t2[2] - array[15] * t2[1] - array[ 3] * t2[5];
766  inv_.array[10] = array[15] * t2[0] - array[ 7] * t2[2] + array[ 3] * t2[4];
767  inv_.array[11] = array[ 7] * t2[1] - array[ 3] * t2[3] - array[11] * t2[0];
768  inv_.array[12] = array[10] * t2[4] - array[ 6] * t2[5] - array[14] * t2[3];
769  inv_.array[13] = array[ 2] * t2[5] - array[10] * t2[2] + array[14] * t2[1];
770  inv_.array[14] = array[ 6] * t2[2] - array[14] * t2[0] - array[ 2] * t2[4];
771  inv_.array[15] = array[ 2] * t2[3] - array[ 6] * t2[1] + array[10] * t2[0];
772 
773  /* determinant: 4 multiplications, 3 additions */
774  const T determinant = array[0] * inv_.array[0] + array[4] * inv_.array[1] +
775  array[8] * inv_.array[2] + array[12] * inv_.array[3];
776 
777  if( fabs( determinant ) <= tolerance_ )
778  return false; // matrix is not invertible
779 
780  /* division: 16 multiplications, 1 division */
781  const T detinv = static_cast< T >( 1.0 ) / determinant;
782 
783  for( size_t i = 0; i != 16; ++i )
784  inv_.array[i] *= detinv;
785 
786  return true;
787 }
788 
789 
790 // this function returns the transpose of a matrix
791 // however, using matrix::transpose_to( .. ) avoids the copy.
792 template< size_t M, size_t N, typename T >
793 inline matrix< N, M, T >
794 transpose( const matrix< M, N, T >& matrix_ )
795 {
796  matrix< N, M, T > transpose_;
797  matrix_.transpose_to( transpose_ );
798  return transpose_;
799 }
800 
801 
802 template< size_t M, size_t N, typename T >
803 bool is_positive_definite( const matrix< M, N, T >& matrix_,
804  const T limit = -1e12,
805  typename enable_if< M == N && M <= 3 >::type* = 0 )
806 {
807  if ( matrix_.at( 0, 0 ) < limit )
808  return false;
809 
810  // sylvester criterion
811  if ( M > 1 )
812  {
813  matrix< 2, 2, T > m;
814  matrix_.get_sub_matrix( m, 0, 0 );
815  if ( compute_determinant( m ) < limit )
816  return false;
817  }
818 
819  if ( M > 2 )
820  {
821  matrix< 3, 3, T > m;
822  matrix_.get_sub_matrix( m, 0, 0 );
823  if ( compute_determinant( m ) < limit )
824  return false;
825  }
826 
827  return true;
828 }
829 
830 
831 template< size_t M, size_t N, typename T >
832 matrix< M, N, T >::matrix()
833  : array() // http://stackoverflow.com/questions/5602030
834 {
835  if( M == N )
836  for( size_t i = 0; i < M; ++i )
837  at( i, i ) = static_cast< T >( 1.0 );
838 }
839 
840 template< size_t M, size_t N, typename T >
841 template< size_t P, size_t Q, typename U >
842 matrix< M, N, T >::matrix( const matrix< P, Q, U >& source_ )
843 {
844  (*this) = source_;
845 }
846 
847 
848 template< size_t M, size_t N, typename T >
849 inline T& matrix< M, N, T >::at( size_t row_index, size_t col_index )
850 {
851 #ifdef VMMLIB_SAFE_ACCESSORS
852  if ( row_index >= M || col_index >= N )
853  VMMLIB_ERROR( "at( row, col ) - index out of bounds", VMMLIB_HERE );
854 #endif
855  return array[ col_index * M + row_index ];
856 }
857 
858 
859 
860 template< size_t M, size_t N, typename T >
861 const inline T&
862 matrix< M, N, T >::at( size_t row_index, size_t col_index ) const
863 {
864 #ifdef VMMLIB_SAFE_ACCESSORS
865  if ( row_index >= M || col_index >= N )
866  VMMLIB_ERROR( "at( row, col ) - index out of bounds", VMMLIB_HERE );
867 #endif
868  return array[ col_index * M + row_index ];
869 }
870 
871 
872 template< size_t M, size_t N, typename T >
873 inline T&
874 matrix< M, N, T >::operator()( size_t row_index, size_t col_index )
875 {
876  return at( row_index, col_index );
877 }
878 
879 
880 
881 template< size_t M, size_t N, typename T >
882 const inline T&
883 matrix< M, N, T >::operator()( size_t row_index, size_t col_index ) const
884 {
885  return at( row_index, col_index );
886 }
887 
888 #ifndef VMMLIB_NO_CONVERSION_OPERATORS
889 
890 template< size_t M, size_t N, typename T >
891 matrix< M, N, T >::
892 operator T*()
893 {
894  return array;
895 }
896 
897 
898 
899 template< size_t M, size_t N, typename T >
900 matrix< M, N, T >::
901 operator const T*() const
902 {
903  return array;
904 }
905 
906 #endif
907 
908 
909 template< size_t M, size_t N, typename T >
910 bool
911 matrix< M, N, T >::
912 operator==( const matrix< M, N, T >& other ) const
913 {
914  bool is_ok = true;
915  for( size_t i = 0; is_ok && i < M * N; ++i )
916  {
917  is_ok = array[ i ] == other.array[ i ];
918  }
919  return is_ok;
920 }
921 
922 
923 
924 
925 template< size_t M, size_t N, typename T >
926 bool
927 matrix< M, N, T >::
928 operator!=( const matrix< M, N, T >& other ) const
929 {
930  return ! operator==( other );
931 }
932 
933 
934 
935 template< size_t M, size_t N, typename T >
936 bool
937 matrix< M, N, T >::
938 equals( const matrix< M, N, T >& other, T tolerance ) const
939 {
940  bool is_ok = true;
941  for( size_t row_index = 0; is_ok && row_index < M; row_index++)
942  {
943  for( size_t col_index = 0; is_ok && col_index < N; col_index++)
944  {
945  is_ok = fabs( at( row_index, col_index ) - other( row_index, col_index ) ) < tolerance;
946  }
947  }
948  return is_ok;
949 }
950 
951 
952 
953 template< size_t M, size_t N, typename T >
954 template< typename compare_t >
955 bool matrix< M, N, T >::equals( const matrix< M, N, T >& other_matrix,
956  compare_t& cmp ) const
957 {
958  bool is_ok = true;
959  for( size_t row = 0; is_ok && row < M; ++row )
960  {
961  for( size_t col = 0; is_ok && col < N; ++col)
962  {
963  is_ok = cmp( at( row, col ), other_matrix.at( row, col ) );
964  }
965  }
966  return is_ok;
967 }
968 
969 #if (( __GNUC__ > 4 ) || ((__GNUC__ == 4) && (__GNUC_MINOR__ >= 3)) )
970 # pragma GCC diagnostic ignored "-Warray-bounds" // gcc 4.4.7 bug WAR
971 #endif
972 template< size_t M, size_t N, typename T >
973 const matrix< M, N, T >&
974 matrix< M, N, T >::operator=( const matrix< M, N, T >& source_ )
975 {
976  memcpy( array, source_.array, M * N * sizeof( T ));
977  return *this;
978 }
979 #if (( __GNUC__ > 4 ) || ((__GNUC__ == 4) && (__GNUC_MINOR__ >= 3)) )
980 # pragma GCC diagnostic warning "-Warray-bounds"
981 #endif
982 
983 template< size_t M, size_t N, typename T >
984 template< size_t P, size_t Q, typename U >
985 void matrix< M, N, T >::operator=( const matrix< P, Q, U >& source_ )
986 {
987  const size_t minL = P < M ? P : M;
988  const size_t minC = Q < N ? Q : N;
989 
990  if( minL < M || minC < N )
991  zero();
992 
993  for ( size_t i = 0 ; i < minL ; i++ )
994  for ( size_t j = 0 ; j < minC ; j++ )
995  at( i,j ) = static_cast< T >( source_( i, j ));
996 }
997 
998 
999 template< size_t M, size_t N, typename T >
1000 void matrix< M, N, T >::operator=( const T old_fashioned_matrix[ M ][ N ] )
1001 {
1002  for( size_t row = 0; row < M; row++)
1003  {
1004  for( size_t col = 0; col < N; col++)
1005  {
1006  at( row, col ) = old_fashioned_matrix[ row ][ col ];
1007  }
1008  }
1009 
1010 }
1011 
1012 
1013 // WARNING: data_array[] must be at least of size M * N - otherwise CRASH!
1014 // WARNING: assumes row_by_row layout - if this is not the case,
1015 // use matrix::set( data_array, false )
1016 template< size_t M, size_t N, typename T >
1017 void
1018 matrix< M, N, T >::operator=( const T* data_array )
1019 {
1020  set( data_array, data_array + M * N, true );
1021 }
1022 
1023 
1024 
1025 template< size_t M, size_t N, typename T >
1026 void
1027 matrix< M, N, T >::operator=( const std::vector< T >& data )
1028 {
1029  if ( data.size() < M * N )
1030  VMMLIB_ERROR( "index out of bounds.", VMMLIB_HERE );
1031 
1032  set( data.begin(), data.end(), true );
1033 }
1034 
1035 
1036 template< size_t M, size_t N, typename T >
1037 void
1038 matrix< M, N, T >::multiply_piecewise( const matrix& other )
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& value = at( row_index, col_index );
1045  value *= other.at( row_index, col_index );
1046  }
1047  }
1048 }
1049 
1050 
1051 template< size_t M, size_t N, typename T >
1052 template< size_t P >
1053 void
1054 matrix< M, N, T >::multiply(
1055  const matrix< M, P, T >& left,
1056  const matrix< P, N, T >& right
1057  )
1058 {
1059  for( size_t row_index = 0; row_index < M; row_index++)
1060  {
1061  for( size_t col_index = 0; col_index < N; col_index++)
1062  {
1063  T& component = at( row_index, col_index );
1064  component = static_cast< T >( 0.0 );
1065  for( size_t p = 0; p < P; p++)
1066  {
1067  component += left.at( row_index, p ) * right.at( p, col_index );
1068  }
1069  }
1070  }
1071 }
1072 
1073 
1074 
1075 template< size_t M, size_t N, typename T >
1076 template< size_t P >
1077 matrix< M, P, T >
1078 matrix< M, N, T >::operator*( const matrix< N, P, T >& other ) const
1079 {
1080  matrix< M, P, T > result;
1081  result.multiply( *this, other );
1082  return result;
1083 }
1084 
1085 
1086 
1087 template< size_t M, size_t N, typename T >
1088 template< size_t O, size_t P, typename TT >
1089 typename enable_if< M == N && O == P && M == O, TT >::type*
1090 matrix< M, N, T >::operator*=( const matrix< O, P, TT >& right )
1091 {
1092  matrix< M, N, T > copy( *this );
1093  multiply( copy, right );
1094  return 0;
1095 }
1096 
1097 template< size_t M, size_t N, typename T >
1098 matrix< M, N, T >
1099 matrix< M, N, T >::operator/( T scalar )
1100 {
1101  matrix< M, N, T > result;
1102 
1103  for( size_t row_index = 0; row_index < M; ++row_index )
1104  {
1105  for( size_t col_index = 0; col_index < N; ++col_index )
1106  {
1107  result.at( row_index, col_index ) = at( row_index, col_index ) / scalar;
1108  }
1109  }
1110  return result;
1111 }
1112 
1113 
1114 
1115 template< size_t M, size_t N, typename T >
1116 void
1117 matrix< M, N, T >::operator/=( T scalar )
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  at( row_index, col_index ) /= scalar;
1124  }
1125  }
1126 }
1127 
1128 
1129 
1130 
1131 
1132 template< size_t M, size_t N, typename T >
1133 matrix< M, N, T >
1134 matrix< M, N, T >::operator*( T scalar )
1135 {
1136  matrix< M, N, T > result;
1137 
1138  for( size_t row_index = 0; row_index < M; ++row_index )
1139  {
1140  for( size_t col_index = 0; col_index < N; ++col_index )
1141  {
1142  result.at( row_index, col_index ) = at( row_index, col_index ) * scalar;
1143  }
1144  }
1145  return result;
1146 }
1147 
1148 
1149 
1150 template< size_t M, size_t N, typename T >
1151 void
1152 matrix< M, N, T >::operator*=( T scalar )
1153 {
1154  for( size_t row_index = 0; row_index < M; ++row_index )
1155  {
1156  for( size_t col_index = 0; col_index < N; ++col_index )
1157  {
1158  at( row_index, col_index ) *= scalar;
1159  }
1160  }
1161 }
1162 
1163 
1164 
1165 template< size_t M, size_t N, typename T >
1166 vector< M, T > matrix< M, N, T >::operator*( const vector< N, T >& vec ) const
1167 {
1168  vector< M, T > result;
1169 
1170  // this < M, 1 > = < M, P > * < P, 1 >
1171  for( size_t i = 0; i < M; ++i )
1172  {
1173  T tmp( 0 );
1174  for( size_t j = 0; j < N; ++j )
1175  tmp += at( i, j ) * vec.at( j );
1176  result.at( i ) = tmp;
1177  }
1178  return result;
1179 }
1180 
1181 
1182 
1183 // transform vector by matrix ( vec = matrix * vec )
1184 // assume homogenous coords( for vec3 = mat4 * vec3 ), e.g. vec[3] = 1.0
1185 template< size_t M, size_t N, typename T >
1186 template< size_t O > vector< O, T >
1187 matrix< M, N, T >::operator*( const vector< O, T >& vector_ ) const
1188 {
1189  vector< O, T > result;
1190  T tmp;
1191  for( size_t row_index = 0; row_index < M; ++row_index )
1192  {
1193  tmp = 0.0;
1194  for( size_t col_index = 0; col_index < N-1; ++col_index )
1195  {
1196  tmp += vector_( col_index ) * at( row_index, col_index );
1197  }
1198  if ( row_index < N - 1 )
1199  result( row_index ) = tmp + at( row_index, N-1 ); // * 1.0 -> homogeneous vec4
1200  else
1201  {
1202  tmp += at( row_index, N - 1 );
1203  for( size_t col_index = 0; col_index < N - 1; ++col_index )
1204  {
1205  result( col_index ) /= tmp;
1206  }
1207  }
1208  }
1209  return result;
1210 }
1211 
1212 
1213 
1214 template< size_t M, size_t N, typename T >
1215 inline matrix< M, N, T >
1216 matrix< M, N, T >::operator-() const
1217 {
1218  return negate();
1219 }
1220 
1221 
1222 
1223 template< size_t M, size_t N, typename T >
1224 matrix< M, N, T >
1225 matrix< M, N, T >::negate() const
1226 {
1227  matrix< M, N, T > result;
1228  result *= -1.0;
1229  return result;
1230 }
1231 
1232 
1233 
1234 template< size_t M, size_t N, typename T >
1235 void
1236 matrix< M, N, T >::tensor( const vector< M, T >& u, const vector< N, T >& v )
1237 {
1238  for ( size_t col_index = 0; col_index < N; ++col_index )
1239  for ( size_t row_index = 0; row_index < M; ++row_index )
1240  at( row_index, col_index ) = u.array[ col_index ] *
1241  v.array[ row_index ];
1242 }
1243 
1244 
1245 
1246 template< size_t M, size_t N, typename T >
1247 template< size_t uM, size_t vM >
1248 typename enable_if< uM == 3 && vM == 3 && M == N && M == 4 >::type*
1249 matrix< M, N, T >::tensor( const vector< uM, T >& u, const vector< vM, T >& v )
1250 {
1251  for ( size_t col_index = 0; col_index < 3; ++col_index )
1252  {
1253  for ( size_t row_index = 0; row_index < 3; ++row_index )
1254  at( row_index, col_index ) = u.array[ col_index ] *
1255  v.array[ row_index ];
1256 
1257  at( 3, col_index ) = u.array[ col_index ];
1258  }
1259 
1260  for ( size_t row_index = 0; row_index < 3; ++row_index )
1261  at( row_index, 3 ) = v.array[ row_index ];
1262 
1263  at( 3, 3 ) = 1.0;
1264 
1265  return 0;
1266 }
1267 
1268 
1269 
1270 template< size_t M, size_t N, typename T >
1271 void
1272 matrix< M, N, T >::
1273 transpose_to( matrix< N, M, T >& tM ) const
1274 {
1275  for( size_t row = 0; row < M; ++row )
1276  {
1277  for( size_t col = 0; col < N; ++col )
1278  {
1279  tM.at( col, row ) = at( row, col );
1280  }
1281  }
1282 }
1283 
1284 template< size_t M, size_t N, typename T >
1285 void
1286 matrix< M, N, T >::
1287 symmetric_covariance( matrix< M, M, T >& cov_m_ ) const
1288 {
1289  T tmp = 0;
1290  for( size_t row = 0; row < M; ++row )
1291  {
1292  for( size_t col = row; col < M; ++col )
1293  {
1294  for ( size_t k = 0; k < N; ++k )
1295  {
1296  tmp += (at( row, k ) * at( col, k ));
1297  }
1298 
1299  cov_m_.at( row, col ) = tmp;
1300  cov_m_.at( col, row ) = tmp;
1301  tmp = 0;
1302  }
1303  }
1304 }
1305 
1306 
1307 
1308 template< size_t M, size_t N, typename T >
1309 vector< M, T > matrix< M, N, T >::get_column( size_t index ) const
1310 {
1311  vector< M, T > column;
1312  get_column( index, column );
1313  return column;
1314 }
1315 
1316 
1317 
1318 template< size_t M, size_t N, typename T >
1319 void matrix< M, N, T >::get_column( size_t index, vector< M, T >& column ) const
1320 {
1321 #ifdef VMMLIB_SAFE_ACCESSORS
1322  if ( index >= N )
1323  VMMLIB_ERROR( "get_column() - index out of bounds.", VMMLIB_HERE );
1324 #endif
1325 
1326  memcpy( &column.array[0], &array[ M * index ], M * sizeof( T ) );
1327 }
1328 
1329 
1330 
1331 template< size_t M, size_t N, typename T >
1332 void matrix< M, N, T >::set_column( size_t index, const vector< M, T >& column )
1333 {
1334  #ifdef VMMLIB_SAFE_ACCESSORS
1335 
1336  if ( index >= N )
1337  VMMLIB_ERROR( "set_column() - index out of bounds.", VMMLIB_HERE );
1338 
1339  #endif
1340 
1341  memcpy( array + M * index, column.array, M * sizeof( T ) );
1342 }
1343 
1344 template< size_t M, size_t N, typename T >
1345 void matrix< M, N, T >::get_column( size_t index, matrix< M, 1, T >& column )
1346  const
1347 {
1348 #ifdef VMMLIB_SAFE_ACCESSORS
1349  if ( index >= N )
1350  VMMLIB_ERROR( "get_column() - index out of bounds.", VMMLIB_HERE );
1351 #endif
1352 
1353  memcpy( column.array, array + M * index, M * sizeof( T ) );
1354 }
1355 
1356 template< size_t M, size_t N, typename T >
1357 void matrix< M, N, T >::set_column( size_t index,
1358  const matrix< M, 1, T >& column )
1359 {
1360 #ifdef VMMLIB_SAFE_ACCESSORS
1361  if ( index >= N )
1362  VMMLIB_ERROR( "set_column() - index out of bounds.", VMMLIB_HERE );
1363 #endif
1364 
1365  memcpy( &array[ M * index ], column.array, M * sizeof( T ) );
1366 }
1367 
1368 template< size_t M, size_t N, typename T >
1369 vector< N, T > matrix< M, N, T >::get_row( size_t index ) const
1370 {
1371  vector< N, T > row;
1372  get_row( index, row );
1373  return row;
1374 }
1375 
1376 template< size_t M, size_t N, typename T >
1377 void matrix< M, N, T >::get_row( size_t row_index, vector< N, T >& row ) const
1378 {
1379 #ifdef VMMLIB_SAFE_ACCESSORS
1380  if ( row_index >= M )
1381  VMMLIB_ERROR( "get_row() - index out of bounds.", VMMLIB_HERE );
1382 #endif
1383 
1384  for( size_t col_index = 0; col_index < N; ++col_index )
1385  {
1386  row.at( col_index ) = at( row_index, col_index );
1387  }
1388 }
1389 
1390 template< size_t M, size_t N, typename T >
1391 void matrix< M, N, T >::set_row( size_t row_index, const vector< N, T >& row )
1392 {
1393 #ifdef VMMLIB_SAFE_ACCESSORS
1394  if ( row_index >= M )
1395  VMMLIB_ERROR( "set_row() - index out of bounds.", VMMLIB_HERE );
1396 #endif
1397 
1398  for( size_t col_index = 0; col_index < N; ++col_index )
1399  {
1400  at( row_index, col_index ) = row.at( col_index );
1401  }
1402 }
1403 
1404 template< size_t M, size_t N, typename T >
1405 void matrix< M, N, T >::get_row( size_t row_index, matrix< 1, N, T >& row )
1406  const
1407 {
1408 #ifdef VMMLIB_SAFE_ACCESSORS
1409  if ( row_index >= M )
1410  VMMLIB_ERROR( "get_row() - index out of bounds.", VMMLIB_HERE );
1411 #endif
1412 
1413  for( size_t col_index = 0; col_index < N; ++col_index )
1414  {
1415  row.at( 0, col_index ) = at( row_index, col_index );
1416  }
1417 }
1418 
1419 template< size_t M, size_t N, typename T >
1420 void matrix< M, N, T >::set_row( size_t row_index,
1421  const matrix< 1, N, T >& row )
1422 {
1423 #ifdef VMMLIB_SAFE_ACCESSORS
1424  if ( row_index >= M )
1425  VMMLIB_ERROR( "set_row() - index out of bounds.", VMMLIB_HERE );
1426 #endif
1427 
1428  for( size_t col_index = 0; col_index < N; ++col_index )
1429  {
1430  at( row_index, col_index ) = row.at( 0, col_index );
1431  }
1432 }
1433 
1434 template< size_t M, size_t N, typename T >
1435 size_t
1436 matrix< M, N, T >::
1437 get_number_of_rows() const
1438 {
1439  return M;
1440 }
1441 
1442 
1443 
1444 template< size_t M, size_t N, typename T >
1445 size_t
1446 matrix< M, N, T >::
1447 get_number_of_columns() const
1448 {
1449  return N;
1450 }
1451 
1452 
1453 
1454 template< size_t M, size_t N, typename T >
1455 void
1456 matrix< M, N, T >::
1457 fill( T fillValue )
1458 {
1459  for( size_t row_index = 0; row_index < M; ++row_index )
1460  {
1461  for( size_t col_index = 0; col_index < N; ++col_index )
1462  {
1463  at( row_index, col_index ) = fillValue;
1464  }
1465  }
1466 }
1467 
1468 
1469 template< size_t M, size_t N, typename T >
1470 inline T&
1471 matrix< M, N, T >::x()
1472 {
1473  return array[ 12 ];
1474 }
1475 
1476 
1477 
1478 template< size_t M, size_t N, typename T >
1479 inline T&
1480 matrix< M, N, T >::y()
1481 {
1482  return array[ 13 ];
1483 }
1484 
1485 
1486 
1487 template< size_t M, size_t N, typename T >
1488 inline T&
1489 matrix< M, N, T >::z()
1490 {
1491  return array[ 14 ];
1492 }
1493 
1494 template< size_t M, size_t N, typename T >
1495 template< typename input_iterator_t >
1496 void matrix< M, N, T >::set( input_iterator_t begin_,
1497  input_iterator_t end_, bool row_major_layout )
1498 {
1499  input_iterator_t it( begin_ );
1500  if( row_major_layout )
1501  {
1502  for( size_t row = 0; row < M; ++row )
1503  {
1504  for( size_t col = 0; col < N; ++col, ++it )
1505  {
1506  if ( it == end_ )
1507  return;
1508  at( row, col ) = static_cast< T >( *it );
1509  }
1510  }
1511  }
1512  else
1513  {
1514  std::copy( it, it + ( M * N ), begin() );
1515  }
1516 }
1517 
1518 
1519 
1520 template< size_t M, size_t N, typename T >
1521 void matrix< M, N, T >::zero()
1522 {
1523  fill( static_cast< T >( 0.0 ) );
1524 }
1525 
1526 template< size_t M, size_t N, typename T >
1527 void matrix< M, N, T >::operator=( T value_ )
1528 {
1529  std::fill( begin(), end(), value_ );
1530 }
1531 
1532 
1533 
1534 template< size_t M, size_t N, typename T >
1535 inline matrix< M, N, T >
1536 matrix< M, N, T >::operator+( const matrix< M, N, T >& other ) const
1537 {
1538  matrix< M, N, T > result( *this );
1539  result += other;
1540  return result;
1541 }
1542 
1543 
1544 
1545 template< size_t M, size_t N, typename T >
1546 void matrix< M, N, T >::operator+=( const matrix< M, N, T >& other )
1547 {
1548  iterator it = begin(), it_end = end();
1549  const_iterator other_it = other.begin();
1550  for( ; it != it_end; ++it, ++other_it )
1551  {
1552  *it += *other_it;
1553  }
1554 }
1555 
1556 
1557 template< size_t M, size_t N, typename T >
1558 void matrix< M, N, T >::operator+=( T scalar )
1559 {
1560  iterator it = begin(), it_end = end();
1561  for( ; it != it_end; ++it )
1562  {
1563  *it += scalar;
1564  }
1565 }
1566 
1567 template< size_t M, size_t N, typename T >
1568 inline matrix< M, N, T >
1569 matrix< M, N, T >::
1570 operator-( const matrix< M, N, T >& other ) const
1571 {
1572  matrix< M, N, T > result( *this );
1573  result -= other;
1574  return result;
1575 }
1576 
1577 
1578 
1579 template< size_t M, size_t N, typename T >
1580 void
1581 matrix< M, N, T >::
1582 operator-=( const matrix< M, N, T >& other )
1583 {
1584  iterator it = begin(), it_end = end();
1585  const_iterator other_it = other.begin();
1586  for( ; it != it_end; ++it, ++other_it )
1587  {
1588  *it -= *other_it;
1589  }
1590 }
1591 
1592 template< size_t M, size_t N, typename T >
1593 void
1594 matrix< M, N, T >::operator-=( T scalar )
1595 {
1596  iterator it = begin(), it_end = end();
1597  for( ; it != it_end; ++it )
1598  {
1599  *it -= scalar;
1600  }
1601 }
1602 
1603 
1604 template< size_t M, size_t N, typename T >
1605 template< size_t O, size_t P, size_t Q, size_t R >
1606 typename enable_if< M == O + Q && N == P + R >::type*
1607 matrix< M, N, T >::direct_sum( const matrix< O, P, T >& upper_left,
1608  const matrix< Q, R, T >& lower_right )
1609 {
1610  (*this) = static_cast< T >( 0.0 );
1611 
1612  for( size_t row = 0; row < O; ++row )
1613  {
1614  for( size_t col = 0; col < P; ++col )
1615  {
1616  at( row, col ) = upper_left( row, col );
1617  }
1618  }
1619 
1620  for( size_t row = 0; row < Q; ++row )
1621  {
1622  for( size_t col = 0; col < R; ++col )
1623  {
1624  at( O + row, P + col ) = lower_right( row, col );
1625  }
1626  }
1627 
1628  return 0;
1629 }
1630 
1631 
1632 
1633 template< size_t M, size_t N, typename T >
1634 template< size_t O, size_t P >
1635 matrix< O, P, T >
1636 matrix< M, N, T >::get_sub_matrix( size_t row_offset, size_t col_offset,
1637 typename enable_if< O <= M && P <= N >::type* ) const
1638 {
1639  matrix< O, P, T > result;
1640  get_sub_matrix( result, row_offset, col_offset );
1641  return result;
1642 }
1643 
1644 
1645 
1646 template< size_t M, size_t N, typename T >
1647 template< size_t O, size_t P >
1648 typename enable_if< O <= M && P <= N >::type*
1649 matrix< M, N, T >::
1650 get_sub_matrix( matrix< O, P, T >& result,
1651  size_t row_offset, size_t col_offset ) const
1652 {
1653  #ifdef VMMLIB_SAFE_ACCESSORS
1654  if ( O + row_offset > M || P + col_offset > N )
1655  VMMLIB_ERROR( "index out of bounds.", VMMLIB_HERE );
1656  #endif
1657 
1658  for( size_t row = 0; row < O; ++row )
1659  {
1660  for( size_t col = 0; col < P; ++col )
1661  {
1662  result.at( row, col )
1663  = at( row_offset + row, col_offset + col );
1664  }
1665  }
1666  return 0;
1667 }
1668 
1669 
1670 
1671 template< size_t M, size_t N, typename T >
1672 template< size_t O, size_t P >
1673 typename enable_if< O <= M && P <= N >::type*
1674 matrix< M, N, T >::
1675 set_sub_matrix( const matrix< O, P, T >& sub_matrix,
1676  size_t row_offset, size_t col_offset )
1677 {
1678  for( size_t row = 0; row < O; ++row )
1679  {
1680  for( size_t col = 0; col < P; ++col )
1681  {
1682  at( row_offset + row, col_offset + col )
1683  = sub_matrix.at( row, col );
1684  }
1685  }
1686  return 0; // for sfinae
1687 }
1688 
1689 
1690 
1691 template< size_t M, size_t N, typename T >
1692 inline T
1693 matrix< M, N, T >::det() const
1694 {
1695  return compute_determinant( *this );
1696 }
1697 
1698 
1699 
1700 template< size_t M, size_t N, typename T >
1701 template< size_t O, size_t P, typename TT >
1702 inline bool matrix< M, N, T >::inverse( matrix< O, P, TT >& inverse_,
1703  T tolerance, typename
1704  enable_if< M == N && O == P && O == M && M >= 2 && M <= 4, TT >::type* )
1705  const
1706 {
1707  return compute_inverse( *this, inverse_, tolerance );
1708 }
1709 
1710 
1711 
1712 template< size_t M, size_t N, typename T >
1713 template< size_t O, size_t P >
1714 typename enable_if< O == P && M == N && O == M && M >= 2 >::type*
1715 matrix< M, N, T >::
1716 get_adjugate( matrix< O, P, T >& adjugate ) const
1717 {
1718  get_cofactors( adjugate );
1719  adjugate = transpose( adjugate );
1720  return 0;
1721 }
1722 
1723 
1724 
1725 template< size_t M, size_t N, typename T >
1726 template< size_t O, size_t P >
1727 typename enable_if< O == P && M == N && O == M && M >= 2 >::type*
1728 matrix< M, N, T >::
1729 get_cofactors( matrix< O, P, T >& cofactors ) const
1730 {
1731  matrix< M-1, N-1, T > minor_;
1732 
1733  const size_t _negate = 1u;
1734  for( size_t row_index = 0; row_index < M; ++row_index )
1735  {
1736  for( size_t col_index = 0; col_index < N; ++col_index )
1737  {
1738  if ( ( row_index + col_index ) & _negate )
1739  cofactors( row_index, col_index ) = -get_minor( minor_, row_index, col_index );
1740  else
1741  cofactors( row_index, col_index ) = get_minor( minor_, row_index, col_index );
1742  }
1743  }
1744  return 0;
1745 }
1746 
1747 
1748 
1749 template< size_t M, size_t N, typename T >
1750 template< size_t O, size_t P >
1751 T
1752 matrix< M, N, T >::
1753 get_minor( matrix< O, P, T >& minor_, size_t row_to_cut, size_t col_to_cut,
1754 typename enable_if< O == M-1 && P == N-1 && M == N && M >= 2 >::type* ) const
1755 {
1756  size_t row_offset = 0;
1757  size_t col_offset = 0;
1758  for( size_t row_index = 0; row_index < M; ++row_index )
1759  {
1760  if ( row_index == row_to_cut )
1761  row_offset = -1;
1762  else
1763  {
1764  for( size_t col_index = 0; col_index < M; ++col_index )
1765  {
1766  if ( col_index == col_to_cut )
1767  col_offset = -1;
1768  else
1769  minor_.at( row_index + row_offset, col_index + col_offset )
1770  = at( row_index, col_index );
1771  }
1772  col_offset = 0;
1773  }
1774  }
1775  return compute_determinant( minor_ );
1776 }
1777 
1778 template< size_t M, size_t N, typename T >
1779 template< typename TT >
1780 matrix< M, N, T >& matrix< M, N, T >::rotate(
1781  const TT angle_, const vector< M-1, T >& axis,
1782  typename enable_if< M == N && M == 4, TT >::type* )
1783 {
1784  const T angle = static_cast< T >( angle_ );
1785 
1786  const T sine = std::sin( angle );
1787  const T cosine = std::cos( angle );
1788 
1789  // this is necessary since Visual Studio cannot resolve the
1790  // std::pow()-call correctly if we just use 2.0 directly.
1791  // this way, the '2.0' is converted to the same format
1792  // as the axis components
1793 
1794  const T _zero = 0.0;
1795  const T one = 1.0;
1796  const T two = 2.0;
1797 
1798  array[0] = cosine + ( one - cosine ) * std::pow( axis.array[0], two );
1799  array[1] = ( one - cosine ) * axis.array[0] * axis.array[1] + sine * axis.array[2];
1800  array[2] = ( one - cosine ) * axis.array[0] * axis.array[2] - sine * axis.array[1];
1801  array[3] = _zero;
1802 
1803  array[4] = ( one - cosine ) * axis.array[0] * axis.array[1] - sine * axis.array[2];
1804  array[5] = cosine + ( one - cosine ) * std::pow( axis.array[1], two );
1805  array[6] = ( one - cosine ) * axis.array[1] * axis.array[2] + sine * axis.array[0];
1806  array[7] = _zero;
1807 
1808  array[8] = ( one - cosine ) * axis.array[0] * axis.array[2] + sine * axis.array[1];
1809  array[9] = ( one - cosine ) * axis.array[1] * axis.array[2] - sine * axis.array[0];
1810  array[10] = cosine + ( one - cosine ) * std::pow( axis.array[2], two );
1811  array[11] = _zero;
1812 
1813  array[12] = _zero;
1814  array[13] = _zero;
1815  array[14] = _zero;
1816  array[15] = one;
1817 
1818  return *this;
1819 }
1820 
1821 template< size_t M, size_t N, typename T >
1822 template< typename TT >
1823 matrix< M, N, T >& matrix< M, N, T >::rotate_x( const TT angle_,
1824  typename enable_if< M == N && M == 4, TT >::type* )
1825 {
1826  const T angle = static_cast< T >( angle_ );
1827 
1828  const T sine = std::sin( angle );
1829  const T cosine = std::cos( angle );
1830 
1831  T tmp;
1832 
1833  tmp = array[ 4 ] * cosine + array[ 8 ] * sine;
1834  array[ 8 ] = - array[ 4 ] * sine + array[ 8 ] * cosine;
1835  array[ 4 ] = tmp;
1836 
1837  tmp = array[ 5 ] * cosine + array[ 9 ] * sine;
1838  array[ 9 ] = - array[ 5 ] * sine + array[ 9 ] * cosine;
1839  array[ 5 ] = tmp;
1840 
1841  tmp = array[ 6 ] * cosine + array[ 10 ] * sine;
1842  array[ 10 ] = - array[ 6 ] * sine + array[ 10 ] * cosine;
1843  array[ 6 ] = tmp;
1844 
1845  tmp = array[ 7 ] * cosine + array[ 11 ] * sine;
1846  array[ 11 ] = - array[ 7 ] * sine + array[ 11 ] * cosine;
1847  array[ 7 ] = tmp;
1848 
1849  return *this;
1850 }
1851 
1852 template< size_t M, size_t N, typename T >
1853 template< typename TT >
1854 matrix< M, N, T >& matrix< M, N, T >::rotate_y( const TT angle_,
1855  typename enable_if< M == N && M == 4, TT >::type* )
1856 {
1857  const T angle = static_cast< T >( angle_ );
1858 
1859  const T sine = std::sin( angle );
1860  const T cosine = std::cos( angle );
1861 
1862  T tmp;
1863 
1864  tmp = array[ 0 ] * cosine - array[ 8 ] * sine;
1865  array[ 8 ] = array[ 0 ] * sine + array[ 8 ] * cosine;
1866  array[ 0 ] = tmp;
1867 
1868  tmp = array[ 1 ] * cosine - array[ 9 ] * sine;
1869  array[ 9 ] = array[ 1 ] * sine + array[ 9 ] * cosine;
1870  array[ 1 ] = tmp;
1871 
1872  tmp = array[ 2 ] * cosine - array[ 10 ] * sine;
1873  array[ 10 ] = array[ 2 ] * sine + array[ 10 ] * cosine;
1874  array[ 2 ] = tmp;
1875 
1876  tmp = array[ 3 ] * cosine - array[ 11 ] * sine;
1877  array[ 11 ] = array[ 3 ] * sine + array[ 11 ] * cosine;
1878  array[ 3 ] = tmp;
1879 
1880  return *this;
1881 }
1882 
1883 template< size_t M, size_t N, typename T >
1884 template< typename TT >
1885 matrix< M, N, T >& matrix< M, N, T >::rotate_z( const TT angle_,
1886  typename enable_if< M == N && M == 4, TT >::type* )
1887 {
1888  const T angle = static_cast< T >( angle_ );
1889 
1890  const T sine = std::sin( angle );
1891  const T cosine = std::cos( angle );
1892 
1893  T tmp;
1894 
1895  tmp = array[ 0 ] * cosine + array[ 4 ] * sine;
1896  array[ 4 ] = - array[ 0 ] * sine + array[ 4 ] * cosine;
1897  array[ 0 ] = tmp;
1898 
1899  tmp = array[ 1 ] * cosine + array[ 5 ] * sine;
1900  array[ 5 ] = - array[ 1 ] * sine + array[ 5 ] * cosine;
1901  array[ 1 ] = tmp;
1902 
1903  tmp = array[ 2 ] * cosine + array[ 6 ] * sine;
1904  array[ 6 ] = - array[ 2 ] * sine + array[ 6 ] * cosine;
1905  array[ 2 ] = tmp;
1906 
1907  tmp = array[ 3 ] * cosine + array[ 7 ] * sine;
1908  array[ 7 ] = - array[ 3 ] * sine + array[ 7 ] * cosine;
1909  array[ 3 ] = tmp;
1910 
1911  return *this;
1912 }
1913 
1914 template< size_t M, size_t N, typename T >
1915 template< typename TT >
1916 matrix< M, N, T >& matrix< M, N, T >::pre_rotate_x( const TT angle_,
1917  typename enable_if< M == N && M == 4, TT >::type* )
1918 {
1919  const T angle = static_cast< T >( angle_ );
1920 
1921  const T sine = std::sin( angle );
1922  const T cosine = std::cos( angle );
1923 
1924  T tmp;
1925 
1926  tmp = array[ 1 ];
1927  array[ 1 ] = array[ 1 ] * cosine + array[ 2 ] * sine;
1928  array[ 2 ] = tmp * -sine + array[ 2 ] * cosine;
1929 
1930  tmp = array[ 5 ];
1931  array[ 5 ] = array[ 5 ] * cosine + array[ 6 ] * sine;
1932  array[ 6 ] = tmp * -sine + array[ 6 ] * cosine;
1933 
1934  tmp = array[ 9 ];
1935  array[ 9 ] = array[ 9 ] * cosine + array[ 10 ] * sine;
1936  array[ 10 ] = tmp * -sine + array[ 10 ] * cosine;
1937 
1938  tmp = array[ 13 ];
1939  array[ 13 ] = array[ 13 ] * cosine + array[ 14 ] * sine;
1940  array[ 14 ] = tmp * -sine + array[ 14 ] * cosine;
1941 
1942  return *this;
1943 }
1944 
1945 template< size_t M, size_t N, typename T >
1946 template< typename TT >
1947 matrix< M, N, T >& matrix< M, N, T >::pre_rotate_y( const TT angle_,
1948  typename enable_if< M == N && M == 4, TT >::type* )
1949 {
1950  const T angle = static_cast< T >( angle_ );
1951 
1952  const T sine = std::sin( angle );
1953  const T cosine = std::cos( angle );
1954 
1955  T tmp;
1956 
1957  tmp = array[ 0 ];
1958  array[ 0 ] = array[ 0 ] * cosine - array[ 2 ] * sine;
1959  array[ 2 ] = tmp * sine + array[ 2 ] * cosine;
1960 
1961  tmp = array[ 4 ];
1962  array[ 4 ] = array[ 4 ] * cosine - array[ 6 ] * sine;
1963  array[ 6 ] = tmp * sine + array[ 6 ] * cosine;
1964 
1965  tmp = array[ 8 ];
1966  array[ 8 ] = array[ 8 ] * cosine - array[ 10 ] * sine;
1967  array[ 10 ] = tmp * sine + array[ 10 ] * cosine;
1968 
1969  tmp = array[ 12 ];
1970  array[ 12 ] = array[ 12 ] * cosine - array[ 14 ] * sine;
1971  array[ 14 ] = tmp * sine + array[ 14 ] * cosine;
1972 
1973  return *this;
1974 }
1975 
1976 template< size_t M, size_t N, typename T >
1977 template< typename TT >
1978 matrix< M, N, T >& matrix< M, N, T >::pre_rotate_z( const TT angle_,
1979  typename enable_if< M == N && M == 4, TT >::type* )
1980 {
1981  const T angle = static_cast< T >( angle_ );
1982 
1983  const T sine = std::sin( angle );
1984  const T cosine = std::cos( angle );
1985 
1986  T tmp;
1987 
1988  tmp = array[ 0 ];
1989  array[ 0 ] = array[ 0 ] * cosine + array[ 1 ] * sine;
1990  array[ 1 ] = tmp * -sine + array[ 1 ] * cosine;
1991 
1992  tmp = array[ 4 ];
1993  array[ 4 ] = array[ 4 ] * cosine + array[ 5 ] * sine;
1994  array[ 5 ] = tmp * -sine + array[ 5 ] * cosine;
1995 
1996  tmp = array[ 8 ];
1997  array[ 8 ] = array[ 8 ] * cosine + array[ 9 ] * sine;
1998  array[ 9 ] = tmp * -sine + array[ 9 ] * cosine;
1999 
2000  tmp = array[ 12 ];
2001  array[ 12 ] = array[ 12 ] * cosine + array[ 13 ] * sine;
2002  array[ 13 ] = tmp * -sine + array[ 13 ] * cosine;
2003 
2004  return *this;
2005 }
2006 
2007 template< size_t M, size_t N, typename T >
2008 template< typename TT >
2009 matrix< M, N, T >& matrix< M, N, T >::scale( const TT _scale[3],
2010  typename enable_if< M == N && M == 4, TT >::type* )
2011 {
2012  const T scale0 = static_cast< T >( _scale[ 0 ] );
2013  const T scale1 = static_cast< T >( _scale[ 1 ] );
2014  const T scale2 = static_cast< T >( _scale[ 2 ] );
2015 
2016  array[0] *= scale0;
2017  array[1] *= scale0;
2018  array[2] *= scale0;
2019  array[3] *= scale0;
2020  array[4] *= scale1;
2021  array[5] *= scale1;
2022  array[6] *= scale1;
2023  array[7] *= scale1;
2024  array[8] *= scale2;
2025  array[9] *= scale2;
2026  array[10] *= scale2;
2027  array[11] *= scale2;
2028 
2029  return *this;
2030 }
2031 
2032 template< size_t M, size_t N, typename T >
2033 template< typename TT >
2034 matrix< M, N, T >& matrix< M, N, T >::scale( const TT x_, const T y_, const T z_,
2035  typename enable_if< M == N && M == 4, TT >::type* )
2036 {
2037  const T _x = static_cast< T >( x_ );
2038 
2039  array[0] *= _x;
2040  array[1] *= _x;
2041  array[2] *= _x;
2042  array[3] *= _x;
2043  array[4] *= y_;
2044  array[5] *= y_;
2045  array[6] *= y_;
2046  array[7] *= y_;
2047  array[8] *= z_;
2048  array[9] *= z_;
2049  array[10] *= z_;
2050  array[11] *= z_;
2051 
2052  return *this;
2053 }
2054 
2055 template< size_t M, size_t N, typename T >
2056 template< typename TT >
2057 inline matrix< M, N, T >& matrix< M, N, T >::scale(
2058  const vector< 3, TT >& scale_,
2059  typename enable_if< M == N && M == 4, TT >::type* )
2060 {
2061  return scale( scale_.array );
2062 }
2063 
2064 
2065 
2066 template< size_t M, size_t N, typename T >
2067 template< typename TT >
2068 matrix< M, N, T >& matrix< M, N, T >::scale_translation( const TT scale_[3],
2069  typename enable_if< M == N && M == 4, TT >::type* )
2070 {
2071  array[12] *= static_cast< T >( scale_[0] );
2072  array[13] *= static_cast< T >( scale_[1] );
2073  array[14] *= static_cast< T >( scale_[2] );
2074  return *this;
2075 }
2076 
2077 template< size_t M, size_t N, typename T >
2078 template< typename TT >
2079 inline matrix< M, N, T >& matrix< M, N, T >::scale_translation(
2080  const vector< 3, TT >& scale_,
2081  typename enable_if< M == N && M == 4, TT >::type* )
2082 {
2083  return scale_translation( scale_.array );
2084 }
2085 
2086 template< size_t M, size_t N, typename T >
2087 template< typename TT >
2088 inline matrix< M, N, T >& matrix< M, N, T >::set_translation(
2089  const TT x_, const TT y_, const TT z_,
2090  typename enable_if< M == N && M == 4, TT >::type* )
2091 {
2092  array[12] = static_cast< T >( x_ );
2093  array[13] = static_cast< T >( y_ );
2094  array[14] = static_cast< T >( z_ );
2095  return *this;
2096 }
2097 
2098 template< size_t M, size_t N, typename T >
2099 template< typename TT >
2100 inline matrix< M, N, T >& matrix< M, N, T >::set_translation( const TT trans[3],
2101  typename enable_if< M == N && M == 4, TT >::type* )
2102 {
2103  array[12] = static_cast< T >( trans[ 0 ] );
2104  array[13] = static_cast< T >( trans[ 1 ] );
2105  array[14] = static_cast< T >( trans[ 2 ] );
2106  return *this;
2107 }
2108 
2109 template< size_t M, size_t N, typename T >
2110 template< typename TT >
2111 inline matrix< M, N, T >& matrix< M, N, T >::set_translation(
2112  const vector< 3, TT >& translation_,
2113  typename enable_if< M == N && M == 4, TT >::type* )
2114 {
2115  return set_translation( translation_.array );
2116 }
2117 
2118 template< size_t M, size_t N, typename T > template< typename TT > inline
2119 void matrix< M, N, T >::get_translation( vector< N-1, TT >& translation_ )
2120  const
2121 {
2122  for( size_t i = 0; i < N-1; ++i )
2123  translation_.array[ i ] = array[ i + M * (N - 1) ];
2124 }
2125 
2126 
2127 template< size_t M, size_t N, typename T >
2128 inline vector< N-1, T > matrix< M, N, T >::get_translation() const
2129 {
2130  vector< N-1, T > result;
2131  get_translation( result );
2132  return result;
2133 }
2134 
2135 
2136 
2137 template< size_t M, size_t N, typename T >
2138 size_t
2139 matrix< M, N, T >::
2140 size() const
2141 {
2142  return M * N;
2143 }
2144 
2145 
2146 
2147 template< size_t M, size_t N, typename T >
2148 typename matrix< M, N, T >::iterator
2149 matrix< M, N, T >::
2150 begin()
2151 {
2152  return array;
2153 }
2154 
2155 
2156 
2157 template< size_t M, size_t N, typename T >
2158 typename matrix< M, N, T >::iterator
2159 matrix< M, N, T >::
2160 end()
2161 {
2162  return array + size();
2163 }
2164 
2165 
2166 
2167 template< size_t M, size_t N, typename T >
2168 typename matrix< M, N, T >::const_iterator
2169 matrix< M, N, T >::
2170 begin() const
2171 {
2172  return array;
2173 }
2174 
2175 
2176 
2177 template< size_t M, size_t N, typename T >
2178 typename matrix< M, N, T >::const_iterator
2179 matrix< M, N, T >::
2180 end() const
2181 {
2182  return array + size();
2183 }
2184 
2185 
2186 
2187 template< size_t M, size_t N, typename T >
2188 typename matrix< M, N, T >::reverse_iterator
2189 matrix< M, N, T >::
2190 rbegin()
2191 {
2192  return array + size() - 1;
2193 }
2194 
2195 
2196 
2197 template< size_t M, size_t N, typename T >
2198 typename matrix< M, N, T >::reverse_iterator
2199 matrix< M, N, T >::
2200 rend()
2201 {
2202  return array - 1;
2203 }
2204 
2205 
2206 
2207 template< size_t M, size_t N, typename T >
2208 typename matrix< M, N, T >::const_reverse_iterator
2209 matrix< M, N, T >::
2210 rbegin() const
2211 {
2212  return array + size() - 1;
2213 }
2214 
2215 
2216 
2217 template< size_t M, size_t N, typename T >
2218 typename matrix< M, N, T >::const_reverse_iterator
2219 matrix< M, N, T >::
2220 rend() const
2221 {
2222  return array - 1;
2223 }
2224 
2225 
2226 
2227 template< size_t M, size_t N, typename T >
2228 template< typename init_functor_t >
2229 const matrix< M, N, T >
2230 matrix< M, N, T >::get_initialized_matrix()
2231 {
2232  matrix< M, N, T > matrix_;
2233  init_functor_t()( matrix_ );
2234  return matrix_;
2235 }
2236 
2237 
2238 // it's ugly, but it allows having properly initialized static members
2239 // without having to worry about static initialization order.
2240 template< size_t M, size_t N, typename T >
2241 const matrix< M, N, T >
2242 matrix< M, N, T >::IDENTITY(
2243  matrix< M, N, T >::get_initialized_matrix<
2244  set_to_identity_functor< matrix< M, N, T > > >( ));
2245 
2246 
2247 template< size_t M, size_t N, typename T >
2248 const matrix< M, N, T >
2249 matrix< M, N, T >::ZERO(
2250  matrix< M, N, T >::
2251  get_initialized_matrix< set_to_zero_functor< matrix< M, N, T > > >()
2252  );
2253 
2254 
2255 
2256 template< size_t M, size_t N, typename T >
2257 double
2258 matrix< M, N, T >::frobenius_norm( ) const
2259 {
2260  double norm = 0.0;
2261 
2262  const_iterator it = begin(), it_end = end();
2263  for( ; it != it_end; ++it )
2264  {
2265  norm += *it * *it;
2266  }
2267 
2268  return std::sqrt(norm);
2269 }
2270 
2271 template< size_t M, size_t N, typename T >
2272 double
2273 matrix< M, N, T >::p_norm( double p ) const
2274 {
2275  double norm = 0.0;
2276 
2277  const_iterator it = begin(), it_end = end();
2278  for( ; it != it_end; ++it )
2279  {
2280  norm += std::pow(*it, p);
2281  }
2282 
2283  return std::pow(norm,1./p);
2284 }
2285 
2286 
2287 template< size_t M, size_t N, typename T >
2288 template< size_t O >
2289 void
2290 matrix< M, N, T >::khatri_rao_product( const matrix< O, N, T >& right_, matrix< M*O, N, T >& prod_ ) const
2291 {
2292  //build product for every column
2293  for (size_t col = 0; col < N; ++col )
2294  {
2295  for ( size_t m = 0; m < M; ++m )
2296  {
2297  for (size_t o = 0; o < O; ++o )
2298  {
2299  prod_.at(O*m + o, col) = at( m, col ) * right_.at( o, col );
2300  }
2301  }
2302  }
2303 }
2304 
2305 template< size_t M, size_t N, typename T >
2306 template< size_t O, size_t P >
2307 void
2308 matrix< M, N, T >::kronecker_product( const matrix< O, P, T >& right_, matrix< M*O, N*P, T >& result_ ) const
2309 {
2310  //build product for every column
2311  for (size_t m = 0; m < M; ++m )
2312  {
2313  for ( size_t n = 0; n < N; ++n )
2314  {
2315  for (size_t o = 0; o < O; ++o )
2316  {
2317  for (size_t p = 0; p < P; ++p )
2318  {
2319  result_.at(O*m + o, P*n + p) = at( m, n ) * right_.at( o, p );
2320  }
2321  }
2322  }
2323  }
2324 }
2325 
2326 
2327 template< size_t M, size_t N, typename T >
2328 template< typename TT >
2329 void
2330 matrix< M, N, T >::cast_from( const matrix< M, N, TT >& other )
2331 {
2332  typedef vmml::matrix< M, N, TT > matrix_tt_type ;
2333  typedef typename matrix_tt_type::const_iterator tt_const_iterator;
2334 
2335  iterator it = begin(), it_end = end();
2336  tt_const_iterator other_it = other.begin();
2337  for( ; it != it_end; ++it, ++other_it )
2338  {
2339  *it = static_cast< T >( *other_it );
2340  }
2341 }
2342 
2343 
2344 template< size_t M, size_t N, typename T >
2345 T
2346 matrix< M, N, T >::get_min() const
2347 {
2348  T min_value = static_cast<T>(std::numeric_limits<T>::max());
2349 
2350  const_iterator it = begin(),
2351  it_end = end();
2352  for( ; it != it_end; ++it)
2353  {
2354  if ( *it < min_value ) {
2355  min_value = *it;
2356  }
2357  }
2358  return min_value;
2359 }
2360 
2361 template< size_t M, size_t N, typename T >
2362 T
2363 matrix< M, N, T >::get_max() const
2364 {
2365  T max_value = static_cast<T>(0);
2366 
2367  const_iterator it = begin(),
2368  it_end = end();
2369  for( ; it != it_end; ++it)
2370  {
2371  if ( *it > max_value ) {
2372  max_value = *it;
2373  }
2374  }
2375  return max_value;
2376 }
2377 
2378 
2379 template< size_t M, size_t N, typename T >
2380 T
2381 matrix< M, N, T >::get_abs_min() const
2382 {
2383  T min_value = static_cast<T>(std::numeric_limits<T>::max());
2384 
2385  const_iterator it = begin(),
2386  it_end = end();
2387  for( ; it != it_end; ++it)
2388  {
2389  if ( fabs(*it) < fabs(min_value) ) {
2390  min_value = fabs(*it);
2391  }
2392  }
2393  return min_value;
2394 }
2395 
2396 template< size_t M, size_t N, typename T >
2397 T
2398 matrix< M, N, T >::get_abs_max() const
2399 {
2400  T max_value = static_cast<T>(0);
2401 
2402  const_iterator it = begin(),
2403  it_end = end();
2404  for( ; it != it_end; ++it)
2405  {
2406  if ( fabs(*it) > fabs(max_value) ) {
2407  max_value = fabs(*it);
2408  }
2409  }
2410  return max_value;
2411 }
2412 
2413 
2414 
2415 template< size_t M, size_t N, typename T >
2416 size_t
2417 matrix< M, N, T >::nnz() const
2418 {
2419  size_t counter = 0;
2420 
2421  const_iterator it = begin(),
2422  it_end = end();
2423  for( ; it != it_end; ++it)
2424  {
2425  if ( *it != 0 ) {
2426  ++counter;
2427  }
2428  }
2429 
2430  return counter;
2431 }
2432 
2433 template< size_t M, size_t N, typename T >
2434 size_t
2435 matrix< M, N, T >::nnz( const T& threshold_ ) const
2436 {
2437  size_t counter = 0;
2438 
2439  const_iterator it = begin(),
2440  it_end = end();
2441  for( ; it != it_end; ++it)
2442  {
2443  if ( fabs(*it) > threshold_ ) {
2444  ++counter;
2445  }
2446  }
2447 
2448  return counter;
2449 }
2450 
2451 template< size_t M, size_t N, typename T >
2452 void
2453 matrix< M, N, T >::threshold( const T& threshold_value_ )
2454 {
2455  iterator it = begin(),
2456  it_end = end();
2457  for( ; it != it_end; ++it)
2458  {
2459  if ( fabs(*it) <= threshold_value_ ) {
2460  *it = static_cast<T> (0);
2461  }
2462  }
2463 }
2464 
2465 
2466 template< size_t M, size_t N, typename T >
2467 template< typename TT >
2468 void
2469 matrix< M, N, T >::quantize_to( matrix< M, N, TT >& quantized_, const T& min_value, const T& max_value ) const
2470 {
2471  long max_tt_range = long(std::numeric_limits< TT >::max());
2472  long min_tt_range = long(std::numeric_limits< TT >::min());
2473  long tt_range = (max_tt_range - min_tt_range);
2474 
2475  T t_range = max_value - min_value;
2476 
2477  typedef matrix< M, N, TT > m_tt_type ;
2478  typedef typename m_tt_type::iterator tt_iterator;
2479  tt_iterator it_quant = quantized_.begin();
2480  const_iterator it = begin(), it_end = end();
2481 
2482  for( ; it != it_end; ++it, ++it_quant )
2483  {
2484  if (std::numeric_limits<TT>::is_signed ) {
2485  *it_quant = TT( std::min( std::max( min_tt_range, long(( *it * tt_range / t_range ) + 0.5)), max_tt_range ));
2486  } else {
2487  *it_quant = TT( std::min( std::max( min_tt_range, long(((*it - min_value) * tt_range / t_range) + 0.5)), max_tt_range ));
2488  }
2489  }
2490 }
2491 
2492 
2493 template< size_t M, size_t N, typename T >
2494 template< typename TT >
2495 void
2496 matrix< M, N, T >::quantize( matrix< M, N, TT >& quantized_, T& min_value, T& max_value ) const
2497 {
2498  min_value = get_min();
2499  max_value = get_max();
2500  quantize_to( quantized_, min_value, max_value );
2501 }
2502 
2503 
2504 
2505 template< size_t M, size_t N, typename T >
2506 template< typename TT >
2507 void
2508 matrix< M, N, T >::dequantize( matrix< M, N, TT >& dequantized_, const TT& min_value, const TT& max_value ) const
2509 {
2510  long max_t_range = long(std::numeric_limits< T >::max());
2511  long min_t_range = long(std::numeric_limits< T >::min());
2512  long t_range = (max_t_range - min_t_range);
2513 
2514  TT tt_range = max_value - min_value;
2515 
2516  typedef matrix< M, N, TT > m_tt_type ;
2517  typedef typename m_tt_type::iterator tt_iterator;
2518  tt_iterator it_dequant = dequantized_.begin();
2519  const_iterator it = begin(), it_end = end();
2520  for( ; it != it_end; ++it, ++it_dequant )
2521  {
2522  if (std::numeric_limits<T>::is_signed ) {
2523  *it_dequant = std::min( std::max( min_value, TT((TT(*it) / t_range) * tt_range)), max_value );
2524  } else {
2525  *it_dequant = std::min( std::max( min_value, TT((((TT(*it) / t_range)) * tt_range ) + min_value)), max_value );
2526  }
2527  }
2528 }
2529 
2530 template< size_t M, size_t N, typename T >
2531 void
2532 matrix< M, N, T >::columnwise_sum( vector< N, T>& summed_columns_ ) const
2533 {
2534 
2535  for ( size_t n = 0; n < N; ++n )
2536  {
2537  T value = 0;
2538  for ( size_t m = 0; m < M; ++m )
2539  {
2540  value += at( m, n );
2541  }
2542  summed_columns_.at( n ) = value;
2543  }
2544 }
2545 
2546 template< size_t M, size_t N, typename T >
2547 double
2548 matrix< M, N, T >::sum_elements( ) const
2549 {
2550  double sum = 0.0;
2551 
2552  const_iterator it = begin(), it_end = end();
2553  for( ; it != it_end; ++it )
2554  {
2555  sum += *it;
2556  }
2557 
2558  return sum;
2559 }
2560 
2561 template< size_t M, size_t N, typename T >
2562 template< size_t R>
2563 typename enable_if< R == M && R == N>::type*
2564 matrix< M, N, T >::diag( const vector< R, T >& diag_values_ )
2565 {
2566  zero();
2567  for( size_t r = 0; r < R; ++r )
2568  {
2569  at(r, r) = static_cast< T >( diag_values_.at(r) );
2570  }
2571  return 0;
2572 }
2573 
2574 
2575 template< size_t M, size_t N, typename T >
2576 void
2577 matrix< M, N, T >::set_dct()
2578 {
2579  const double num_rows = M;
2580  double fill_value = 0.0f;
2581  for( size_t row = 0; row < M; ++row )
2582  {
2583  // to receive orthonormality
2584  const double weight = ( row == 0.0 ) ? std::sqrt(1/num_rows) :
2585  std::sqrt(2/num_rows);
2586  for( size_t col = 0; col < N; ++col )
2587  {
2588  fill_value = (2 * col + 1) * row * M_PI / (2*M);
2589  fill_value = std::cos( fill_value );
2590  fill_value *= weight;
2591  at( row, col ) = static_cast< T >( fill_value ) ;
2592  }
2593  }
2594 }
2595 
2596 
2597 template< size_t M, size_t N, typename T >
2598 void
2599 matrix< M, N, T >::set_random( int seed )
2600 {
2601  if ( seed >= 0 )
2602  srand( seed );
2603 
2604  double fillValue = 0.0f;
2605  for( size_t row = 0; row < M; ++row )
2606  {
2607  for( size_t col = 0; col < N; ++col )
2608  {
2609  fillValue = rand();
2610  fillValue /= RAND_MAX;
2611  at( row, col ) = -1.0 + 2.0 * static_cast< double >( fillValue ) ;
2612  }
2613  }
2614 }
2615 
2616 template< size_t M, size_t N, typename T >
2617 void
2618 matrix< M, N, T >::write_to_raw( const std::string& dir_, const std::string& filename_ ) const
2619 {
2620  int dir_length = dir_.size() -1;
2621  int last_separator = dir_.find_last_of( "/");
2622  std::string path = dir_;
2623  if (last_separator < dir_length ) {
2624  path.append( "/" );
2625  }
2626  path.append( filename_ );
2627  //check for format
2628  if( filename_.find( "raw", filename_.size() -3) == std::string::npos )
2629  {
2630  path.append( ".");
2631  path.append( "raw" );
2632  }
2633  std::string path_raw = path;
2634 
2635  std::ofstream outfile;
2636  outfile.open( path_raw.c_str() );
2637  if( outfile.is_open() ) {
2638  size_t len_slice = sizeof(T) * M*N;
2639  outfile.write( (char*)&(*this), len_slice );
2640  outfile.close();
2641  } else {
2642  std::cout << "no file open" << std::endl;
2643  }
2644 }
2645 
2646 template< size_t M, size_t N, typename T >
2647 void
2648 matrix< M, N, T >::read_from_raw( const std::string& dir_, const std::string& filename_ )
2649 {
2650  int dir_length = dir_.size() -1;
2651  int last_separator = dir_.find_last_of( "/");
2652  std::string path = dir_;
2653  if (last_separator < dir_length ) {
2654  path.append( "/" );
2655  }
2656  path.append( filename_ );
2657 
2658  size_t max_file_len = 2147483648u - sizeof(T) ;
2659  size_t len_data = sizeof(T) * size();
2660  char* data = new char[ len_data ];
2661  std::ifstream infile;
2662  infile.open( path.c_str(), std::ios::in);
2663 
2664  if( infile.is_open())
2665  {
2666  iterator it = begin(),
2667  it_end = end();
2668 
2669  while ( len_data > 0 )
2670  {
2671  size_t len_read = (len_data % max_file_len ) > 0 ?
2672  len_data % max_file_len : len_data;
2673  len_data -= len_read;
2674  infile.read( data, len_read );
2675 
2676  T* T_ptr = (T*)&(data[0]);
2677  for( ; (it != it_end) && (len_read > 0); ++it, len_read -= sizeof(T) )
2678  {
2679  *it = *T_ptr; ++T_ptr;
2680  }
2681  }
2682 
2683  delete[] data;
2684  infile.close();
2685  } else {
2686  std::cout << "no file open" << std::endl;
2687  }
2688 
2689 }
2690 
2691 
2692 
2693 
2694 template< size_t M, size_t N, typename T >
2695 void
2696 matrix< M, N, T >::write_csv_file( const std::string& dir_, const std::string& filename_ ) const
2697 {
2698  int dir_length = dir_.size() -1;
2699  int last_separator = dir_.find_last_of( "/");
2700  std::string path = dir_;
2701  if (last_separator < dir_length ) {
2702  path.append( "/" );
2703  }
2704  path.append( filename_ );
2705  //check for format
2706  int suffix_pos = filename_.find( "csv", filename_.size() -3);
2707  if( suffix_pos == (-1)) {
2708  path.append( ".");
2709  path.append( "csv" );
2710  }
2711 
2712  std::ofstream outfile;
2713  outfile.open( path.c_str() );
2714  if( outfile.is_open() ) {
2715  outfile << *this << std::endl;
2716  outfile.close();
2717  } else {
2718  std::cout << "no file open" << std::endl;
2719  }
2720 
2721 }
2722 
2723 template< size_t M, size_t N, typename T >
2724 void
2725 matrix< M, N, T >::read_csv_file( const std::string& dir_, const std::string& filename_ )
2726 {
2727  int dir_length = dir_.size() -1;
2728  int last_separator = dir_.find_last_of( "/");
2729  std::string path = dir_;
2730  if (last_separator < dir_length ) {
2731  path.append( "/" );
2732  }
2733  path.append( filename_ );
2734  //check for format
2735  int suffix_pos = filename_.find( "csv", filename_.size() -3);
2736  if( suffix_pos == (-1)) {
2737  path.append( ".");
2738  path.append( "csv" );
2739  }
2740 
2741  std::ifstream infile;
2742  infile.open( path.c_str(), std::ios::in);
2743  if( infile.is_open() ) {
2744  //TODO: not yet implemented
2745  //infile >> *this >> std::endl;
2746  infile.close();
2747  } else {
2748  std::cout << "no file open" << std::endl;
2749  }
2750 
2751 }
2752 
2753 
2754 template< size_t M, size_t N, typename T >
2755 void
2756 matrix< M, N, T >::sum_rows( matrix< M/2, N, T>& other ) const
2757 {
2758  typedef vector< N, T > row_type;
2759 
2760  row_type* row0 = new row_type;
2761  row_type* row1 = new row_type;
2762 
2763  other.zero();
2764 
2765  for ( size_t row = 0; row < M; ++row )
2766  {
2767  get_row( row++, *row0 );
2768  if ( row < M )
2769  {
2770  get_row( row, *row1 );
2771  *row0 += *row1;
2772  other.set_row( row/2 , *row0 );
2773  }
2774  }
2775 
2776  delete row0;
2777  delete row1;
2778 }
2779 
2780 template< size_t M, size_t N, typename T >
2781 void
2782 matrix< M, N, T >::sum_columns( matrix< M, N/2, T>& other ) const
2783 {
2784  typedef vector< M, T > col_type;
2785 
2786  col_type* col0 = new col_type;
2787  col_type* col1 = new col_type;
2788 
2789  other.zero();
2790 
2791  for ( size_t col = 0; col< N; ++col )
2792  {
2793  get_column( col++, *col0 );
2794  if ( col < N )
2795  {
2796  get_column( col, *col1 );
2797  *col0 += *col1;
2798  other.set_column( col/2, *col0 );
2799  }
2800  }
2801 
2802  delete col0;
2803  delete col1;
2804 }
2805 
2806 
2807 
2808 } // namespace vmml
2809 
2810 #endif