1 // This is core/vnl/vnl_matrix_fixed.hxx
2 #ifndef vnl_matrix_fixed_hxx_
3 #define vnl_matrix_fixed_hxx_
4 //:
5 // \file
6 #include <cmath>
7 #include <iostream>
8 #include <cstdlib>
9 #include "vnl_matrix_fixed.h"
10
11 #ifdef _MSC_VER
12 # include <vcl_msvc_warnings.h>
13 #endif
14 #include <cassert>
15
16 #include "vnl_error.h"
17 #include "vnl_math.h"
18 #include "vnl_complex.h" //vnl_math functions for complex variables
19 #include "vnl_vector_fixed.h"
20
21 template<class T, unsigned num_rows, unsigned num_cols>
22 vnl_matrix_fixed<T, num_rows, num_cols>&
operator =(T const & v)23 vnl_matrix_fixed<T, num_rows, num_cols>::operator= (T const&v)
24 {
25 fill(v); return *this;
26 }
27
28 template<class T, unsigned num_rows, unsigned num_cols>
29 vnl_matrix_fixed<T, num_rows, num_cols> &
operator =(const vnl_matrix<T> & rhs)30 vnl_matrix_fixed<T, num_rows, num_cols>::operator=(const vnl_matrix<T>& rhs)
31 {
32 assert(rhs.rows() == num_rows && rhs.columns() == num_cols);
33 std::memcpy(data_[0], rhs.data_block(), num_rows*num_cols * sizeof(T));
34 return *this;
35 }
36
37 template<class T, unsigned nrows, unsigned ncols>
38 T &
39 vnl_matrix_fixed<T, nrows, ncols>::operator() (unsigned r, unsigned c)
40 {
41 #if VNL_CONFIG_CHECK_BOUNDS && (!defined NDEBUG)
42 assert(r < rows()); // Check the row index is valid
43 assert(c < cols()); // Check the column index is valid
44 #endif
45 return this->data_[r][c];
46 }
47
48 template<class T, unsigned nrows, unsigned ncols>
49 T const &
50 vnl_matrix_fixed<T, nrows, ncols>::operator() (unsigned r, unsigned c) const
51 {
52 #if VNL_CONFIG_CHECK_BOUNDS && (!defined NDEBUG)
53 assert(r < rows()); // Check the row index is valid
54 assert(c < cols()); // Check the column index is valid
55 #endif
56 return this->data_[r][c];
57 }
58
59 template<class T, unsigned nrows, unsigned ncols>
60 void
add(const T * a,const T * b,T * r)61 vnl_matrix_fixed<T,nrows,ncols>::add( const T* a, const T* b, T* r )
62 {
63 unsigned int count = nrows*ncols;
64 while ( count-- )
65 *(r++) = *(a++) + *(b++);
66 }
67
68
69 template<class T, unsigned nrows, unsigned ncols>
70 void
add(const T * a,T b,T * r)71 vnl_matrix_fixed<T,nrows,ncols>::add( const T* a, T b, T* r )
72 {
73 unsigned int count = nrows*ncols;
74 while ( count-- )
75 *(r++) = *(a++) + b;
76 }
77
78 template<class T, unsigned nrows, unsigned ncols>
79 void
sub(const T * a,const T * b,T * r)80 vnl_matrix_fixed<T,nrows,ncols>::sub( const T* a, const T* b, T* r )
81 {
82 unsigned int count = nrows*ncols;
83 while ( count-- )
84 *(r++) = *(a++) - *(b++);
85 }
86
87 template<class T, unsigned nrows, unsigned ncols>
88 void
sub(const T * a,T b,T * r)89 vnl_matrix_fixed<T,nrows,ncols>::sub( const T* a, T b, T* r )
90 {
91 unsigned int count = nrows*ncols;
92 while ( count-- )
93 *(r++) = *(a++) - b;
94 }
95
96 template<class T, unsigned nrows, unsigned ncols>
97 void
sub(T a,const T * b,T * r)98 vnl_matrix_fixed<T,nrows,ncols>::sub( T a, const T* b, T* r )
99 {
100 unsigned int count = nrows*ncols;
101 while ( count-- )
102 *(r++) = a - *(b++);
103 }
104
105 template<class T, unsigned nrows, unsigned ncols>
106 void
mul(const T * a,const T * b,T * r)107 vnl_matrix_fixed<T,nrows,ncols>::mul( const T* a, const T* b, T* r )
108 {
109 unsigned int count = nrows*ncols;
110 while ( count-- )
111 *(r++) = *(a++) * *(b++);
112 }
113
114 template<class T, unsigned nrows, unsigned ncols>
115 void
mul(const T * a,T b,T * r)116 vnl_matrix_fixed<T,nrows,ncols>::mul( const T* a, T b, T* r )
117 {
118 unsigned int count = nrows*ncols;
119 while ( count-- )
120 *(r++) = *(a++) * b;
121 }
122
123 template<class T, unsigned nrows, unsigned ncols>
124 void
div(const T * a,const T * b,T * r)125 vnl_matrix_fixed<T,nrows,ncols>::div( const T* a, const T* b, T* r )
126 {
127 unsigned int count = nrows*ncols;
128 while ( count-- )
129 *(r++) = *(a++) / *(b++);
130 }
131
132 template<class T, unsigned nrows, unsigned ncols>
133 void
div(const T * a,T b,T * r)134 vnl_matrix_fixed<T,nrows,ncols>::div( const T* a, T b, T* r )
135 {
136 unsigned int count = nrows*ncols;
137 while ( count-- )
138 *(r++) = *(a++) / b;
139 }
140
141 template<class T, unsigned nrows, unsigned ncols>
142 bool
equal(const T * a,const T * b)143 vnl_matrix_fixed<T,nrows,ncols>::equal( const T* a, const T* b )
144 {
145 unsigned int count = nrows*ncols;
146 while ( count-- )
147 if ( *(a++) != *(b++) ) return false;
148 return true;
149 }
150
151 //------------------------------------------------------------
152
153
154 template<class T, unsigned nrows, unsigned ncols>
155 vnl_matrix_fixed<T,nrows,ncols>&
fill(T value)156 vnl_matrix_fixed<T,nrows,ncols>::fill (T value)
157 {
158 for (unsigned int i = 0; i < nrows; ++i)
159 for (unsigned int j = 0; j < ncols; ++j)
160 this->data_[i][j] = value;
161 return *this;
162 }
163
164
165 template<class T, unsigned nrows, unsigned ncols>
166 vnl_matrix_fixed<T,nrows,ncols>&
fill_diagonal(T value)167 vnl_matrix_fixed<T,nrows,ncols>::fill_diagonal (T value)
168 {
169 for (unsigned int i = 0; i < nrows && i < ncols; ++i)
170 this->data_[i][i] = value;
171 return *this;
172 }
173
174
175 template<class T, unsigned nrows, unsigned ncols>
176 vnl_matrix_fixed<T,nrows,ncols>&
set_diagonal(vnl_vector<T> const & diag)177 vnl_matrix_fixed<T,nrows,ncols>::set_diagonal(vnl_vector<T> const& diag)
178 {
179 assert(diag.size() >= nrows || diag.size() >= ncols);
180 // The length of the diagonal of a non-square matrix is the minimum of
181 // the matrix's width & height; that explains the "||" in the assert,
182 // and the "&&" in the upper bound for the "for".
183 for (unsigned int i = 0; i < nrows && i < ncols; ++i)
184 this->data_[i][i] = diag[i];
185 return *this;
186 }
187
188
189 template<class T, unsigned nrows, unsigned ncols>
190 void
print(std::ostream & os) const191 vnl_matrix_fixed<T,nrows,ncols>::print(std::ostream& os) const
192 {
193 for (unsigned int i = 0; i < nrows; ++i)
194 {
195 os << this->data_[i][0];
196 for (unsigned int j = 1; j < ncols; ++j)
197 os << ' ' << this->data_[i][j];
198 os << '\n';
199 }
200 }
201
202
203 template <class T, unsigned nrows, unsigned ncols>
204 vnl_matrix_fixed<T,nrows,ncols>
205 vnl_matrix_fixed<T,nrows,ncols>
apply(T (* f)(T const &)) const206 ::apply(T (*f)(T const&)) const
207 {
208 vnl_matrix_fixed<T,nrows,ncols> ret;
209 vnl_c_vector<T>::apply(this->data_[0], rows()*cols(), f, ret.data_block());
210 return ret;
211 }
212
213 template <class T, unsigned nrows, unsigned ncols>
214 vnl_matrix_fixed<T,nrows,ncols>
215 vnl_matrix_fixed<T,nrows,ncols>
apply(T (* f)(T)) const216 ::apply(T (*f)(T)) const
217 {
218 vnl_matrix_fixed<T,nrows,ncols> ret;
219 vnl_c_vector<T>::apply(this->data_[0], rows()*cols(), f, ret.data_block());
220 return ret;
221 }
222
223 //: Make a vector by applying a function across rows.
224 template <class T, unsigned nrows, unsigned ncols>
225 vnl_vector_fixed<T,nrows>
226 vnl_matrix_fixed<T,nrows,ncols>
apply_rowwise(T (* f)(vnl_vector_fixed<T,ncols> const &)) const227 ::apply_rowwise(T (*f)(vnl_vector_fixed<T,ncols> const&)) const
228 {
229 vnl_vector_fixed<T,nrows> v;
230 for (unsigned int i = 0; i < nrows; ++i)
231 v.put(i,f(this->get_row(i)));
232 return v;
233 }
234
235 //: Make a vector by applying a function across columns.
236 template <class T, unsigned nrows, unsigned ncols>
237 vnl_vector_fixed<T,ncols>
238 vnl_matrix_fixed<T,nrows,ncols>
apply_columnwise(T (* f)(vnl_vector_fixed<T,nrows> const &)) const239 ::apply_columnwise(T (*f)(vnl_vector_fixed<T,nrows> const&)) const
240 {
241 vnl_vector_fixed<T,ncols> v;
242 for (unsigned int i = 0; i < ncols; ++i)
243 v.put(i,f(this->get_column(i)));
244 return v;
245 }
246
247 ////--------------------------- Additions------------------------------------
248
249
250 template<class T, unsigned nrows, unsigned ncols>
251 vnl_matrix_fixed<T,ncols,nrows>
transpose() const252 vnl_matrix_fixed<T,nrows,ncols>::transpose() const
253 {
254 vnl_matrix_fixed<T,ncols,nrows> result;
255 for (unsigned int i = 0; i < cols(); ++i)
256 for (unsigned int j = 0; j < rows(); ++j)
257 result(i,j) = this->data_[j][i];
258 return result;
259 }
260
261 template<class T, unsigned nrows, unsigned ncols>
262 vnl_matrix_fixed<T,ncols,nrows>
conjugate_transpose() const263 vnl_matrix_fixed<T,nrows,ncols>::conjugate_transpose() const
264 {
265 vnl_matrix_fixed<T,ncols,nrows> result(transpose());
266 vnl_c_vector<T>::conjugate(result.begin(), // src
267 result.begin(), // dst
268 result.size()); // size of block
269 return result;
270 }
271
272 template<class T, unsigned nrows, unsigned ncols>
273 vnl_matrix_fixed<T,nrows,ncols>&
update(vnl_matrix<T> const & m,unsigned top,unsigned left)274 vnl_matrix_fixed<T,nrows,ncols>::update (vnl_matrix<T> const& m,
275 unsigned top, unsigned left)
276 {
277 const unsigned int bottom = top + m.rows();
278 const unsigned int right = left + m.cols();
279 #ifndef NDEBUG
280 if (nrows < bottom || ncols < right)
281 vnl_error_matrix_dimension ("update",
282 bottom, right, m.rows(), m.cols());
283 #endif
284 for (unsigned int i = top; i < bottom; ++i)
285 for (unsigned int j = left; j < right; ++j)
286 this->data_[i][j] = m(i-top,j-left);
287 return *this;
288 }
289
290 template<class T, unsigned nrows, unsigned ncols>
291 vnl_matrix_fixed<T,nrows,ncols>&
update(vnl_matrix_fixed<T,nrows,ncols> const & m,unsigned top,unsigned left)292 vnl_matrix_fixed<T,nrows,ncols>::update (vnl_matrix_fixed<T,nrows,ncols> const& m,
293 unsigned top, unsigned left)
294 {
295 const unsigned int bottom = top + m.rows();
296 const unsigned int right = left + m.cols();
297 #ifndef NDEBUG
298 if (nrows < bottom || ncols < right)
299 vnl_error_matrix_dimension ("update",
300 bottom, right, m.rows(), m.cols());
301 #endif
302 for (unsigned int i = top; i < bottom; ++i)
303 for (unsigned int j = left; j < right; ++j)
304 this->data_[i][j] = m(i-top,j-left);
305 return *this;
306 }
307
308
309 template<class T, unsigned nrows, unsigned ncols>
310 vnl_matrix<T>
extract(unsigned rowz,unsigned colz,unsigned top,unsigned left) const311 vnl_matrix_fixed<T,nrows,ncols>::extract (unsigned rowz, unsigned colz,
312 unsigned top, unsigned left) const
313 {
314 vnl_matrix<T> result(rowz, colz);
315 this->extract( result, top, left );
316 return result;
317 }
318
319
320 template<class T, unsigned nrows, unsigned ncols>
321 void
extract(vnl_matrix<T> & sub_matrix,unsigned top,unsigned left) const322 vnl_matrix_fixed<T,nrows,ncols>::extract (vnl_matrix<T>& sub_matrix,
323 unsigned top, unsigned left) const
324 {
325 unsigned int rowz = sub_matrix.rows();
326 unsigned int colz = sub_matrix.cols();
327 #ifndef NDEBUG
328 unsigned int bottom = top + rowz;
329 unsigned int right = left + colz;
330 if ((nrows < bottom) || (ncols < right))
331 vnl_error_matrix_dimension ("extract",
332 nrows, ncols, bottom, right);
333 #endif
334 for (unsigned int i = 0; i < rowz; ++i) // actual copy of all elements
335 for (unsigned int j = 0; j < colz; ++j) // in submatrix
336 sub_matrix(i,j) = this->data_[top+i][left+j];
337 }
338
339
340 template<class T, unsigned nrows, unsigned ncols>
341 vnl_matrix_fixed<T,nrows,ncols>&
copy_in(T const * p)342 vnl_matrix_fixed<T,nrows,ncols>::copy_in(T const *p)
343 {
344 T* dp = this->data_block();
345 std::copy( p, p + nrows * ncols, dp );
346 return *this;
347 }
348
349 template<class T, unsigned nrows, unsigned ncols>
copy_out(T * p) const350 void vnl_matrix_fixed<T,nrows,ncols>::copy_out(T *p) const
351 {
352 T const* dp = this->data_block();
353 std::copy( dp, dp + nrows * ncols, p );
354 }
355
356 template<class T, unsigned nrows, unsigned ncols>
357 vnl_matrix_fixed<T,nrows,ncols>&
set_identity()358 vnl_matrix_fixed<T,nrows,ncols>::set_identity()
359 {
360 // A reset of this object, followed by a loop over the diagonal, is
361 // probably better than having a branch inside the loop. Probably
362 // worth the O(n) extra writes.
363 *this = vnl_matrix_fixed();
364 const unsigned n = std::min( nrows, ncols );
365 for (unsigned int i = 0; i < n; ++i)
366 this->data_[i][i] = 1;
367 return *this;
368 }
369
370 //: Make each row of the matrix have unit norm.
371 // All-zero rows are ignored.
372 template<class T, unsigned nrows, unsigned ncols>
373 vnl_matrix_fixed<T,nrows,ncols>&
normalize_rows()374 vnl_matrix_fixed<T,nrows,ncols>::normalize_rows()
375 {
376 for (unsigned int i = 0; i < nrows; ++i)
377 {
378 abs_t norm(0); // double will not do for all types.
379 for (unsigned int j = 0; j < ncols; ++j)
380 norm += vnl_math::squared_magnitude( this->data_[i][j] );
381
382 if (norm != 0)
383 {
384 typedef typename vnl_numeric_traits<abs_t>::real_t real_t;
385 real_t scale = real_t(1)/std::sqrt((real_t)norm);
386 for (unsigned int j = 0; j < ncols; ++j)
387 {
388 // FIXME need correct rounding here
389 // There is e.g. no *standard* operator*=(complex<float>, double), hence the T() cast.
390 this->data_[i][j] *= T(scale);
391 }
392 }
393 }
394 return *this;
395 }
396
397 template<class T, unsigned nrows, unsigned ncols>
398 vnl_matrix_fixed<T,nrows,ncols>&
normalize_columns()399 vnl_matrix_fixed<T,nrows,ncols>::normalize_columns()
400 {
401 for (unsigned int j = 0; j < ncols; ++j) { // For each column in the Matrix
402 abs_t norm(0); // double will not do for all types.
403 for (unsigned int i = 0; i < nrows; ++i)
404 norm += vnl_math::squared_magnitude( this->data_[i][j] );
405
406 if (norm != 0)
407 {
408 typedef typename vnl_numeric_traits<abs_t>::real_t real_t;
409 real_t scale = real_t(1)/std::sqrt((real_t)norm);
410 for (unsigned int i = 0; i < nrows; ++i)
411 {
412 // FIXME need correct rounding here
413 // There is e.g. no *standard* operator*=(complex<float>, double), hence the T() cast.
414 this->data_[i][j] *= T(scale);
415 }
416 }
417 }
418 return *this;
419 }
420
421 template<class T, unsigned nrows, unsigned ncols>
422 vnl_matrix_fixed<T,nrows,ncols>&
scale_row(unsigned row_index,T value)423 vnl_matrix_fixed<T,nrows,ncols>::scale_row(unsigned row_index, T value)
424 {
425 #ifndef NDEBUG
426 if (row_index >= nrows)
427 vnl_error_matrix_row_index("scale_row", row_index);
428 #endif
429 for (unsigned int j = 0; j < ncols; ++j)
430 this->data_[row_index][j] *= value;
431 return *this;
432 }
433
434 template<class T, unsigned nrows, unsigned ncols>
435 vnl_matrix_fixed<T,nrows,ncols>&
scale_column(unsigned column_index,T value)436 vnl_matrix_fixed<T,nrows,ncols>::scale_column(unsigned column_index, T value)
437 {
438 #ifndef NDEBUG
439 if (column_index >= ncols)
440 vnl_error_matrix_col_index("scale_column", column_index);
441 #endif
442 for (unsigned int j = 0; j < nrows; ++j)
443 this->data_[j][column_index] *= value;
444 return *this;
445 }
446
447 template <class T, unsigned int nrows, unsigned int ncols>
448 void
449 vnl_matrix_fixed<T,nrows,ncols>
swap(vnl_matrix_fixed<T,nrows,ncols> & that)450 ::swap(vnl_matrix_fixed<T,nrows,ncols> &that)
451 {
452 for (unsigned int r = 0; r < nrows; ++r)
453 {
454 for (unsigned int c = 0; c < ncols; ++c)
455 {
456 std::swap(this->data_[r][c], that.data_[r][c]);
457 }
458 }
459 }
460
461 //: Returns a copy of n rows, starting from "row"
462 template<class T, unsigned nrows, unsigned ncols>
463 vnl_matrix<T>
get_n_rows(unsigned row,unsigned n) const464 vnl_matrix_fixed<T,nrows,ncols>::get_n_rows (unsigned row, unsigned n) const
465 {
466 #ifndef NDEBUG
467 if (row + n > nrows)
468 vnl_error_matrix_row_index ("get_n_rows", row);
469 #endif
470
471 // Extract data rowwise.
472 return vnl_matrix<T>(data_[row], n, ncols);
473 }
474
475 template<class T, unsigned nrows, unsigned ncols>
476 vnl_matrix<T>
get_n_columns(unsigned column,unsigned n) const477 vnl_matrix_fixed<T,nrows,ncols>::get_n_columns (unsigned column, unsigned n) const
478 {
479 #ifndef NDEBUG
480 if (column + n > ncols)
481 vnl_error_matrix_col_index ("get_n_columns", column);
482 #endif
483
484 vnl_matrix<T> result(nrows, n);
485 for (unsigned int c = 0; c < n; ++c)
486 for (unsigned int r = 0; r < nrows; ++r)
487 result(r, c) = this->data_[r][column + c];
488 return result;
489 }
490
491 //: Create a vector out of row[row_index].
492 template<class T, unsigned nrows, unsigned ncols>
get_row(unsigned row_index) const493 vnl_vector_fixed<T,ncols> vnl_matrix_fixed<T,nrows,ncols>::get_row(unsigned row_index) const
494 {
495 #ifdef ERROR_CHECKING
496 if (row_index >= nrows)
497 vnl_error_matrix_row_index ("get_row", row_index);
498 #endif
499
500 vnl_vector_fixed<T,ncols> v;
501 for (unsigned int j = 0; j < ncols; ++j) // For each element in row
502 v[j] = this->data_[row_index][j];
503 return v;
504 }
505
506 //: Create a vector out of column[column_index].
507 template<class T, unsigned nrows, unsigned ncols>
get_column(unsigned column_index) const508 vnl_vector_fixed<T,nrows> vnl_matrix_fixed<T,nrows,ncols>::get_column(unsigned column_index) const
509 {
510 #ifdef ERROR_CHECKING
511 if (column_index >= ncols)
512 vnl_error_matrix_col_index ("get_column", column_index);
513 #endif
514
515 vnl_vector_fixed<T,nrows> v;
516 for (unsigned int j = 0; j < nrows; ++j)
517 v[j] = this->data_[j][column_index];
518 return v;
519 }
520
521 //: Create a vector out of row[row_index].
522 template <class T, unsigned int nrows, unsigned int ncols>
523 vnl_matrix<T>
524 vnl_matrix_fixed<T,nrows,ncols>
get_rows(const vnl_vector<unsigned int> & i) const525 ::get_rows(const vnl_vector<unsigned int> &i) const
526 {
527 vnl_matrix<T> m(i.size(), this->cols());
528 for (unsigned int j = 0; j < i.size(); ++j)
529 m.set_row(j, this->get_row(i.get(j)).as_ref());
530 return m;
531 }
532
533 //: Create a vector out of column[column_index].
534 template <class T, unsigned int nrows, unsigned int ncols>
535 vnl_matrix<T>
536 vnl_matrix_fixed<T,nrows,ncols>
get_columns(const vnl_vector<unsigned int> & i) const537 ::get_columns(const vnl_vector<unsigned int> & i) const
538 {
539 vnl_matrix<T> m(this->rows(), i.size());
540 for (unsigned int j = 0; j < i.size(); ++j)
541 m.set_column(j, this->get_column(i.get(j)).as_ref());
542 return m;
543 }
544
545 //: Return a vector with the content of the (main) diagonal
546 template<class T, unsigned nrows, unsigned ncols>
get_diagonal() const547 vnl_vector<T> vnl_matrix_fixed<T,nrows,ncols>::get_diagonal() const
548 {
549 vnl_vector<T> v(nrows < ncols ? nrows : ncols);
550 for (unsigned int j = 0; j < nrows && j < ncols; ++j)
551 v[j] = this->data_[j][j];
552 return v;
553 }
554
555 //: Flatten row-major (C-style)
556 template <class T, unsigned nrows, unsigned ncols>
flatten_row_major() const557 vnl_vector_fixed<T,nrows*ncols> vnl_matrix_fixed<T,nrows,ncols>::flatten_row_major() const
558 {
559 vnl_vector_fixed<T,nrows*ncols> v;
560 v.copy_in(this->data_block());
561 return v;
562 }
563
564 //: Flatten column-major (Fortran-style)
565 template <class T, unsigned nrows, unsigned ncols>
flatten_column_major() const566 vnl_vector_fixed<T,nrows*ncols> vnl_matrix_fixed<T,nrows,ncols>::flatten_column_major() const
567 {
568 vnl_vector_fixed<T,nrows*ncols> v;
569 for (unsigned int c = 0; c < ncols; ++c)
570 for (unsigned int r = 0; r < nrows; ++r)
571 v[c*nrows+r] = this->data_[r][c];
572 return v;
573 }
574
575 //--------------------------------------------------------------------------------
576
577 template<class T, unsigned nrows, unsigned ncols>
578 vnl_matrix_fixed<T,nrows,ncols>&
set_row(unsigned row_index,T const * v)579 vnl_matrix_fixed<T,nrows,ncols>::set_row(unsigned row_index, T const *v)
580 {
581 for (unsigned int j = 0; j < ncols; ++j)
582 this->data_[row_index][j] = v[j];
583 return *this;
584 }
585
586 template<class T, unsigned nrows, unsigned ncols>
587 vnl_matrix_fixed<T,nrows,ncols>&
set_row(unsigned row_index,vnl_vector<T> const & v)588 vnl_matrix_fixed<T,nrows,ncols>::set_row(unsigned row_index, vnl_vector<T> const &v)
589 {
590 if (v.size() >= ncols)
591 set_row(row_index,v.data_block());
592 else
593 for (unsigned int j = 0; j < v.size(); ++j)
594 this->data_[row_index][j] = v[j];
595 return *this;
596 }
597
598 template<class T, unsigned nrows, unsigned ncols>
599 vnl_matrix_fixed<T,nrows,ncols>&
set_row(unsigned row_index,vnl_vector_fixed<T,ncols> const & v)600 vnl_matrix_fixed<T,nrows,ncols>::set_row(unsigned row_index, vnl_vector_fixed<T,ncols> const &v)
601 {
602 set_row(row_index,v.data_block());
603 return *this;
604 }
605
606 template<class T, unsigned nrows, unsigned ncols>
607 vnl_matrix_fixed<T,nrows,ncols>&
set_row(unsigned row_index,T v)608 vnl_matrix_fixed<T,nrows,ncols>::set_row(unsigned row_index, T v)
609 {
610 for (unsigned int j = 0; j < ncols; ++j)
611 this->data_[row_index][j] = v;
612 return *this;
613 }
614
615 //--------------------------------------------------------------------------------
616
617 template<class T, unsigned nrows, unsigned ncols>
618 vnl_matrix_fixed<T,nrows,ncols>&
set_column(unsigned column_index,T const * v)619 vnl_matrix_fixed<T,nrows,ncols>::set_column(unsigned column_index, T const *v)
620 {
621 for (unsigned int i = 0; i < nrows; ++i)
622 this->data_[i][column_index] = v[i];
623 return *this;
624 }
625
626 template<class T, unsigned nrows, unsigned ncols>
627 vnl_matrix_fixed<T,nrows,ncols>&
set_column(unsigned column_index,vnl_vector<T> const & v)628 vnl_matrix_fixed<T,nrows,ncols>::set_column(unsigned column_index, vnl_vector<T> const &v)
629 {
630 if (v.size() >= nrows)
631 set_column(column_index,v.data_block());
632 else
633 for (unsigned int i = 0; i < v.size(); ++i)
634 this->data_[i][column_index] = v[i];
635 return *this;
636 }
637
638 template<class T, unsigned nrows, unsigned ncols>
639 vnl_matrix_fixed<T,nrows,ncols>&
set_column(unsigned column_index,vnl_vector_fixed<T,nrows> const & v)640 vnl_matrix_fixed<T,nrows,ncols>::set_column(unsigned column_index, vnl_vector_fixed<T,nrows> const &v)
641 {
642 set_column(column_index,v.data_block());
643 return *this;
644 }
645
646 template<class T, unsigned nrows, unsigned ncols>
647 vnl_matrix_fixed<T,nrows,ncols>&
set_column(unsigned column_index,T v)648 vnl_matrix_fixed<T,nrows,ncols>::set_column(unsigned column_index, T v)
649 {
650 for (unsigned int j = 0; j < nrows; ++j)
651 this->data_[j][column_index] = v;
652 return *this;
653 }
654
655
656 template<class T, unsigned nrows, unsigned ncols>
657 vnl_matrix_fixed<T,nrows,ncols>&
set_columns(unsigned starting_column,vnl_matrix<T> const & m)658 vnl_matrix_fixed<T,nrows,ncols>::set_columns(unsigned starting_column, vnl_matrix<T> const& m)
659 {
660 for (unsigned int j = 0; j < m.cols() && starting_column+j < ncols; ++j) // don't go too far right; possibly only use part of m
661 for (unsigned int i = 0; i < nrows && i < m.rows(); ++i) // smallest of the two heights; possibly only use part of m
662 this->data_[i][starting_column + j] = m(i,j);
663 return *this;
664 }
665
666
667 template <class T, unsigned nrows, unsigned ncols>
668 bool
is_identity() const669 vnl_matrix_fixed<T,nrows,ncols>::is_identity() const
670 {
671 T const zero(0);
672 T const one(1);
673 for (unsigned int i = 0; i < nrows; ++i)
674 for (unsigned int j = 0; j < ncols; ++j)
675 {
676 T xm = this->data_[i][j];
677 if ( !((i == j) ? (xm == one) : (xm == zero)) )
678 return false;
679 }
680 return true;
681 }
682
683 //: Return true if maximum absolute deviation of M from identity is <= tol.
684 template <class T, unsigned nrows, unsigned ncols>
685 bool
is_identity(double tol) const686 vnl_matrix_fixed<T,nrows,ncols>::is_identity(double tol) const
687 {
688 T one(1);
689 for (unsigned int i = 0; i < nrows; ++i)
690 for (unsigned int j = 0; j < ncols; ++j)
691 {
692 T xm = this->data_[i][j];
693 abs_t absdev = (i == j) ? vnl_math::abs(xm - one) : vnl_math::abs(xm);
694 if (absdev > tol)
695 return false;
696 }
697 return true;
698 }
699
700 template <class T, unsigned nrows, unsigned ncols>
701 bool
is_zero() const702 vnl_matrix_fixed<T,nrows,ncols>::is_zero() const
703 {
704 T const zero(0);
705 for (unsigned int i = 0; i < nrows; ++i)
706 for (unsigned int j = 0; j < ncols; ++j)
707 if ( !( this->data_[i][ j] == zero) )
708 return false;
709
710 return true;
711 }
712
713 template <class T, unsigned nrows, unsigned ncols>
714 bool vnl_matrix_fixed<T,nrows,ncols>
is_equal(vnl_matrix_fixed<T,nrows,ncols> const & rhs,double tol) const715 ::is_equal(vnl_matrix_fixed<T,nrows,ncols> const& rhs, double tol) const
716 {
717 if (this == &rhs) // same object => equal.
718 return true;
719
720 if (this->rows() != rhs.rows() || this->cols() != rhs.cols())
721 return false; // different sizes => not equal.
722
723 for (unsigned int i = 0; i < nrows; ++i)
724 for (unsigned int j = 0; j < ncols; ++j)
725 if (vnl_math::abs(this->data_[i][j] - rhs.data_[i][j]) > tol)
726 return false; // difference greater than tol
727
728 return true;
729 }
730
731 template <class T, unsigned nrows, unsigned ncols>
732 bool
is_zero(double tol) const733 vnl_matrix_fixed<T,nrows,ncols>::is_zero(double tol) const
734 {
735 for (unsigned int i = 0; i < nrows; ++i)
736 for (unsigned int j = 0; j < ncols; ++j)
737 if (vnl_math::abs(this->data_[i][j]) > tol)
738 return false;
739
740 return true;
741 }
742
743 template <class T, unsigned nrows, unsigned ncols>
744 bool
has_nans() const745 vnl_matrix_fixed<T,nrows,ncols>::has_nans() const
746 {
747 for (unsigned int i = 0; i < nrows; ++i)
748 for (unsigned int j = 0; j < ncols; ++j)
749 if (vnl_math::isnan(this->data_[i][j]))
750 return true;
751
752 return false;
753 }
754
755 template <class T, unsigned nrows, unsigned ncols>
756 bool
is_finite() const757 vnl_matrix_fixed<T,nrows,ncols>::is_finite() const
758 {
759 for (unsigned int i = 0; i < nrows; ++i)
760 for (unsigned int j = 0; j < ncols; ++j)
761 if (!vnl_math::isfinite(this->data_[i][j]))
762 return false;
763
764 return true;
765 }
766
767 //: Abort if any element of M is inf or nan
768 template <class T, unsigned nrows, unsigned ncols>
769 void
assert_finite_internal() const770 vnl_matrix_fixed<T,nrows,ncols>::assert_finite_internal() const
771 {
772 if (is_finite())
773 return;
774
775 std::cerr << "\n\n" __FILE__ ": " << __LINE__ << ": matrix has non-finite elements\n";
776
777 if (rows() <= 20 && cols() <= 20)
778 std::cerr << __FILE__ ": here it is:\n" << *this << '\n';
779 else
780 {
781 std::cerr << __FILE__ ": it is quite big (" << rows() << 'x' << cols() << ")\n"
782 << __FILE__ ": in the following picture '-' means finite and '*' means non-finite:\n";
783
784 for (unsigned int i=0; i<rows(); ++i)
785 {
786 for (unsigned int j=0; j<cols(); ++j)
787 std::cerr << char(vnl_math::isfinite(this->data_[i][ j]) ? '-' : '*');
788 std::cerr << '\n';
789 }
790 }
791 std::cerr << __FILE__ ": calling abort()\n";
792 std::abort();
793 }
794
795 //: Abort unless M has the given size.
796 template <class T, unsigned nrows, unsigned ncols>
797 void
assert_size_internal(unsigned rs,unsigned cs) const798 vnl_matrix_fixed<T,nrows,ncols>::assert_size_internal(unsigned rs,unsigned cs) const
799 {
800 if (nrows!=rs || ncols!=cs)
801 {
802 std::cerr << __FILE__ ": size is " << nrows << 'x' << ncols
803 << ". should be " << rs << 'x' << cs << std::endl;
804 std::abort();
805 }
806 }
807
808 template <class T, unsigned nrows, unsigned ncols>
809 bool
read_ascii(std::istream & s)810 vnl_matrix_fixed<T,nrows,ncols>::read_ascii(std::istream& s)
811 {
812 if (!s.good())
813 {
814 std::cerr << __FILE__ ": vnl_matrix_fixed<T,nrows,ncols>::read_ascii: Called with bad stream\n";
815 return false;
816 }
817
818 for (unsigned int i = 0; i < nrows; ++i)
819 for (unsigned int j = 0; j < ncols; ++j)
820 s >> this->data_[i][j];
821
822 return s.good() || s.eof();
823 }
824
825
826 template <class T, unsigned nrows, unsigned ncols>
827 vnl_matrix_fixed<T,nrows,ncols>&
flipud()828 vnl_matrix_fixed<T,nrows,ncols>::flipud()
829 {
830 for (unsigned int r1 = 0; 2*r1+1 < nrows; ++r1)
831 {
832 const unsigned int r2 = nrows - 1 - r1;
833 for (unsigned int c = 0; c < ncols; ++c)
834 {
835 std::swap(this->data_[r1][c], this->data_[r2][c]);
836 }
837 }
838 return *this;
839 }
840
841
842 template <class T, unsigned nrows, unsigned ncols>
843 vnl_matrix_fixed<T,nrows,ncols>&
fliplr()844 vnl_matrix_fixed<T,nrows,ncols>::fliplr()
845 {
846 for (unsigned int c1 = 0; 2*c1+1 < ncols; ++c1)
847 {
848 const unsigned int c2 = ncols - 1 - c1;
849 for (unsigned int r = 0; r < nrows; ++r)
850 {
851 std::swap(this->data_[r][c1], this->data_[r][c2]);
852 }
853 }
854 return *this;
855 }
856
857 template <class T, unsigned nrows, unsigned ncols>
858 typename vnl_matrix_fixed<T,nrows,ncols>::abs_t
operator_one_norm() const859 vnl_matrix_fixed<T,nrows,ncols>::operator_one_norm() const
860 {
861 abs_t m(0);
862 for (unsigned int j=0; j<ncols; ++j)
863 {
864 abs_t t(0);
865 for (unsigned int i=0; i<nrows; ++i)
866 t += vnl_math::abs( this->data_[i][j] );
867 if (t > m)
868 m = t;
869 }
870 return m;
871 }
872
873 template <class T, unsigned nrows, unsigned ncols>
874 typename vnl_matrix_fixed<T,nrows,ncols>::abs_t
operator_inf_norm() const875 vnl_matrix_fixed<T,nrows,ncols>::operator_inf_norm() const
876 {
877 abs_t m(0);
878 for (unsigned int i=0; i<nrows; ++i)
879 {
880 abs_t t(0);
881 for (unsigned int j=0; j<ncols; ++j)
882 t += vnl_math::abs( this->data_[i][j] );
883 if (t > m)
884 m = t;
885 }
886 return m;
887 }
888
889 //: Transpose square matrix M in place.
890 template <class T, unsigned nrows, unsigned ncols>
891 vnl_matrix_fixed<T,nrows,ncols>&
inplace_transpose()892 vnl_matrix_fixed<T,nrows,ncols>::inplace_transpose()
893 {
894 assert(nrows==ncols); // cannot inplace_transpose non-square fixed size matrix
895 for (unsigned i = 0; i < nrows; ++i)
896 for (unsigned j = i+1; j < ncols; ++j)
897 {
898 T t = this->data_[i][j];
899 this->data_[i][j] = this->data_[j][i];
900 this->data_[j][i] = t;
901 }
902 return *this;
903 }
904
905 template <class T, unsigned m, unsigned n>
906 T const*
907 vnl_matrix_fixed<T, m, n>::data_block() const
908 {
909 return data_[0];
910 }
911
912 template <class T, unsigned m, unsigned n>
913 T *
914 vnl_matrix_fixed<T, m, n>::data_block()
915 {
916 return data_[0];
917 }
918
919 template <class T, unsigned m, unsigned n>
920 vnl_matrix_fixed<T,m,n>
outer_product(vnl_vector_fixed<T,m> const & a,vnl_vector_fixed<T,n> const & b)921 outer_product(vnl_vector_fixed<T,m> const& a, vnl_vector_fixed<T,n> const& b)
922 {
923 vnl_matrix_fixed<T,m,n> out; // = a.column() * b.row()
924 for (unsigned int i = 0; i < m; ++i)
925 for (unsigned int j = 0; j < n; ++j)
926 out[i][j] = a[i] * b[j];
927 return out;
928 }
929
930 #define VNL_OUTER_PRODUCT_FIXED_INSTANTIATE( T, M, N ) \
931 template VNL_EXPORT vnl_matrix_fixed<T,M,N > outer_product(vnl_vector_fixed<T,M > const&,\
932 vnl_vector_fixed<T,N > const& )
933
934 #undef VNL_MATRIX_FIXED_INSTANTIATE
935 #define VNL_MATRIX_FIXED_INSTANTIATE(T, M, N) \
936 template class VNL_EXPORT vnl_matrix_fixed<T,M,N >; \
937 VNL_OUTER_PRODUCT_FIXED_INSTANTIATE( T, M, N )
938
939 #endif // vnl_matrix_fixed_hxx_
940