Geogram  Version 1.9.1-rc
A programming library of geometric algorithms
matrix.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2000-2022 Inria
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright notice,
9  * this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright notice,
11  * this list of conditions and the following disclaimer in the documentation
12  * and/or other materials provided with the distribution.
13  * * Neither the name of the ALICE Project-Team nor the names of its
14  * contributors may be used to endorse or promote products derived from this
15  * software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *
29  * Contact: Bruno Levy
30  *
31  * https://www.inria.fr/fr/bruno-levy
32  *
33  * Inria,
34  * Domaine de Voluceau,
35  * 78150 Le Chesnay - Rocquencourt
36  * FRANCE
37  *
38  */
39 
40 #ifndef GEOGRAM_BASIC_MATRIX
41 #define GEOGRAM_BASIC_MATRIX
42 
43 #include <geogram/basic/common.h>
44 #include <geogram/basic/vecg.h>
45 #include <initializer_list>
46 
52 namespace GEO {
53 
54  /************************************************************************/
55 
56 
65  template <index_t DIM, class FT>
66  class Matrix {
67  public:
70 
72  typedef FT value_type;
73 
75  static const index_t dim = DIM;
76 
82  inline Matrix() {
83  load_identity();
84  }
85 
92  explicit Matrix(const FT* vals) {
93  for(index_t i = 0; i < DIM; i++) {
94  for(index_t j = 0; j < DIM; j++) {
95  coeff_[i][j] = *vals;
96  ++vals;
97  }
98  }
99  }
100 
105  Matrix(const std::initializer_list< std::initializer_list<FT> >& Mi) {
106  index_t i = 0;
107  for(auto& it: Mi) {
108  index_t j = 0;
109  for(auto& jt: it) {
110  geo_debug_assert(i < DIM);
111  geo_debug_assert(j < DIM);
112  coeff_[i][j] = jt;
113  ++j;
114  }
115  ++i;
116  }
117  }
118 
119 
124  inline index_t dimension() const {
125  return DIM;
126  }
127 
132  inline void load_zero() {
133  for(index_t i = 0; i < DIM; i++) {
134  for(index_t j = 0; j < DIM; j++) {
135  coeff_[i][j] = FT(0);
136  }
137  }
138  }
139 
145  inline void load_identity() {
146  for(index_t i = 0; i < DIM; i++) {
147  for(index_t j = 0; j < DIM; j++) {
148  coeff_[i][j] = (i == j) ? FT(1) : FT(0);
149  }
150  }
151  }
152 
158  inline bool is_identity() const {
159  for(index_t i = 0; i < DIM; i++) {
160  for(index_t j = 0; j < DIM; j++) {
161  FT rhs = ((i == j) ? FT(1) : FT(0));
162  if(coeff_[i][j] != rhs) {
163  return false;
164  }
165  }
166  }
167  return true;
168  }
169 
178  inline FT& operator() (index_t i, index_t j) {
179  geo_debug_assert(i < DIM);
180  geo_debug_assert(j < DIM);
181  return coeff_[i][j];
182  }
183 
192  inline const FT& operator() (index_t i, index_t j) const {
193  geo_debug_assert(i < DIM);
194  geo_debug_assert(j < DIM);
195  return coeff_[i][j];
196  }
197 
204  inline matrix_type& operator+= (const matrix_type& m) {
205  for(index_t i = 0; i < DIM; i++) {
206  for(index_t j = 0; j < DIM; j++) {
207  coeff_[i][j] += m.coeff_[i][j];
208  }
209  }
210  return *this;
211  }
212 
219  inline matrix_type& operator-= (const matrix_type& m) {
220  for(index_t i = 0; i < DIM; i++) {
221  for(index_t j = 0; j < DIM; j++) {
222  coeff_[i][j] -= m.coeff_[i][j];
223  }
224  }
225  return *this;
226  }
227 
235  inline matrix_type& operator*= (FT val) {
236  for(index_t i = 0; i < DIM; i++) {
237  for(index_t j = 0; j < DIM; j++) {
238  coeff_[i][j] *= val;
239  }
240  }
241  return *this;
242  }
243 
251  inline matrix_type& operator/= (FT val) {
252  for(index_t i = 0; i < DIM; i++) {
253  for(index_t j = 0; j < DIM; j++) {
254  coeff_[i][j] /= val;
255  }
256  }
257  return *this;
258  }
259 
266  inline matrix_type operator+ (const matrix_type& m) const {
267  matrix_type result = *this;
268  result += m;
269  return result;
270  }
271 
278  inline matrix_type operator- (const matrix_type& m) const {
279  matrix_type result = *this;
280  result -= m;
281  return result;
282  }
283 
291  inline matrix_type operator* (FT val) const {
292  matrix_type result = *this;
293  result *= val;
294  return result;
295  }
296 
304  inline matrix_type operator/ (FT val) const {
305  matrix_type result = *this;
306  result /= val;
307  return result;
308  }
309 
318  matrix_type result;
319  for(index_t i = 0; i < DIM; i++) {
320  for(index_t j = 0; j < DIM; j++) {
321  result.coeff_[i][j] = FT(0);
322  for(index_t k = 0; k < DIM; k++) {
323  result.coeff_[i][j] += coeff_[i][k] * m.coeff_[k][j];
324  }
325  }
326  }
327  return result;
328  }
329 
336  matrix_type result;
337  bool invertible = compute_inverse(result);
338  geo_assert(invertible);
339  return result;
340  }
341 
342 
350  bool compute_inverse(matrix_type& result) const {
351  FT val=FT(0.0), val2=FT(0.0);
352  matrix_type tmp = (*this);
353 
354  result.load_identity();
355 
356  for(index_t i = 0; i != DIM; i++) {
357  val = tmp(i, i); /* find pivot */
358  index_t ind = i;
359  for(index_t j = i + 1; j != DIM; j++) {
360  if(fabs(tmp(j, i)) > fabs(val)) {
361  ind = j;
362  val = tmp(j, i);
363  }
364  }
365 
366  if(ind != i) {
367  for(index_t j = 0; j != DIM; j++) {
368  val2 = result(i, j);
369  result(i, j) = result(ind, j);
370  result(ind, j) = val2; /* swap columns */
371  val2 = tmp(i, j);
372  tmp(i, j) = tmp(ind, j);
373  tmp(ind, j) = val2;
374  }
375  }
376 
377  if(val == 0.0) {
378  return false;
379  }
380 
381  for(index_t j = 0; j != DIM; j++) {
382  tmp(i, j) /= val;
383  result(i, j) /= val;
384  }
385 
386  for(index_t j = 0; j != DIM; j++) {
387  if(j == i) {
388  continue; /* eliminate column */
389  }
390  val = tmp(j, i);
391  for(index_t k = 0; k != DIM; k++) {
392  tmp(j, k) -= tmp(i, k) * val;
393  result(j, k) -= result(i, k) * val;
394  }
395  }
396  }
397 
398  return true;
399  }
400 
406  matrix_type result;
407  for(index_t i = 0; i < DIM; i++) {
408  for(index_t j = 0; j < DIM; j++) {
409  result(i, j) = (* this)(j, i);
410  }
411  }
412  return result;
413  }
414 
421  inline const FT* data() const {
422  return &(coeff_[0][0]);
423  }
424 
431  inline FT* data() {
432  return &(coeff_[0][0]);
433  }
434 
442  void get_lower_triangle(FT* store) const {
443  for(index_t i = 0; i < DIM; i++) {
444  for(index_t j = 0; j <= i; j++) {
445  *store++ = coeff_[i][j];
446  }
447  }
448  }
449 
450  private:
451  FT coeff_[DIM][DIM];
452  };
453 
454  /************************************************************************/
455 
465  template <index_t DIM, class FT>
466  inline std::ostream& operator<< (
467  std::ostream& output, const Matrix<DIM, FT>& m
468  ) {
469  const char* sep = "";
470  for(index_t i = 0; i < DIM; i++) {
471  for(index_t j = 0; j < DIM; j++) {
472  output << sep << m(i, j);
473  sep = " ";
474  }
475  }
476  return output;
477  }
478 
488  template <index_t DIM, class FT>
489  inline std::istream& operator>> (
490  std::istream& input, Matrix<DIM, FT>& m
491  ) {
492  for(index_t i = 0; i < DIM; i++) {
493  for(index_t j = 0; j < DIM; j++) {
494  input >> m(i, j);
495  }
496  }
497  return input;
498  }
499 
500  /************************************************************************/
501 
515  template <index_t DIM, class FT> inline
516  void mult(const Matrix<DIM, FT>& M, const FT* x, FT* y) {
517  for(index_t i = 0; i < DIM; i++) {
518  y[i] = 0;
519  for(index_t j = 0; j < DIM; j++) {
520  y[i] += M(i, j) * x[j];
521  }
522  }
523  }
524 
525  /************************************************************************/
526 
535  template <index_t DIM, class FT> inline
536  vecng<DIM,FT> operator*(
537  const Matrix<DIM, FT>& M, const vecng<DIM,FT>& x
538  ) {
539  vecng<DIM,FT> y;
540  for(index_t i = 0; i < DIM; i++) {
541  y[i] = 0;
542  for(index_t j = 0; j < DIM; j++) {
543  y[i] += M(i, j) * x[j];
544  }
545  }
546  return y;
547  }
548 
557  template <index_t DIM, class FT> inline
559  const Matrix<DIM, FT>& M, const vecng<DIM,FT>& x
560  ) {
561  vecng<DIM,FT> y;
562  for(index_t i = 0; i < DIM; i++) {
563  y[i] = 0;
564  for(index_t j = 0; j < DIM; j++) {
565  y[i] += M(i, j) * x[j];
566  }
567  }
568  return y;
569  }
570 
571  /************************************************************************/
572 
573 }
574 
575 #endif
#define geo_assert(x)
Verifies that a condition is met.
Definition: assert.h:149
#define geo_debug_assert(x)
Verifies that a condition is met.
Definition: assert.h:196
A matrix type.
Definition: matrix.h:66
matrix_type transpose() const
Computes the transposed matrix.
Definition: matrix.h:405
void get_lower_triangle(FT *store) const
Gets the lower triangle of the matrix.
Definition: matrix.h:442
Matrix(const FT *vals)
Constructs a matrix from an array of values.
Definition: matrix.h:92
matrix_type & operator-=(const matrix_type &m)
Subtracts a matrix in place.
Definition: matrix.h:219
FT value_type
Definition: matrix.h:72
index_t dimension() const
Gets the matrix dimension.
Definition: matrix.h:124
Matrix< DIM, FT > matrix_type
Definition: matrix.h:69
matrix_type operator+(const matrix_type &m) const
Adds 2 matrices.
Definition: matrix.h:266
static const index_t dim
Definition: matrix.h:75
matrix_type operator*(FT val) const
Multiplies a matrix by a scalar.
Definition: matrix.h:291
void load_zero()
Clears the matrix.
Definition: matrix.h:132
Matrix(const std::initializer_list< std::initializer_list< FT > > &Mi)
Constructs a matrix from 2d array of initializers.
Definition: matrix.h:105
bool is_identity() const
Tests whether a matrix is the identity matrix.
Definition: matrix.h:158
FT * data()
Gets modifiable matrix data.
Definition: matrix.h:431
matrix_type & operator*=(FT val)
Multiplies by a scalar in place.
Definition: matrix.h:235
matrix_type operator-(const matrix_type &m) const
Subtracts 2 matrices.
Definition: matrix.h:278
void mult(const Matrix< DIM, FT > &M, const FT *x, FT *y)
Multiplies a matrix by a vector.
Definition: matrix.h:516
matrix_type inverse() const
Computes the inverse matrix.
Definition: matrix.h:335
FT & operator()(index_t i, index_t j)
Gets a modifiable element.
Definition: matrix.h:178
void load_identity()
Sets the matrix to identity.
Definition: matrix.h:145
Matrix()
Default constructor.
Definition: matrix.h:82
bool compute_inverse(matrix_type &result) const
Computes the inverse matrix.
Definition: matrix.h:350
matrix_type & operator/=(FT val)
Divides by a scalar in place.
Definition: matrix.h:251
const FT * data() const
Gets non-modifiable matrix data.
Definition: matrix.h:421
matrix_type & operator+=(const matrix_type &m)
Adds a matrix in place.
Definition: matrix.h:204
matrix_type operator/(FT val) const
Divides a matrix by a scalar.
Definition: matrix.h:304
Generic maths vector.
Definition: vecg.h:70
Common include file, providing basic definitions. Should be included before anything else by all head...
Global Vorpaline namespace.
Definition: basic.h:55
geo_index_t index_t
The type for storing and manipulating indices.
Definition: numeric.h:329
Generic implementation of geometric vectors.