Visual Servoing Platform  version 3.6.1 under development (2025-02-08)
vpMatrix_operators.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Stack matrix.
32  */
33 
34 #include <visp3/core/vpConfig.h>
35 #include <visp3/core/vpMatrix.h>
36 
37 #if defined(VISP_HAVE_SIMDLIB)
38 #include <Simd/SimdLib.h>
39 #endif
40 
41 BEGIN_VISP_NAMESPACE
42 
60 {
61  resize(A.getRows(), A.getCols(), false, false);
62 
63  if ((data != nullptr) && (A.data != nullptr) && (data != A.data)) {
64  memcpy(data, A.data, dsize * sizeof(double));
65  }
66 
67  return *this;
68 }
69 
83 {
84  resize(M.getRows(), M.getCols(), false, false);
85 
86  if ((data != nullptr) && (M.data != nullptr) && (data != M.data)) {
87  memcpy(data, M.data, dsize * sizeof(double));
88  }
89 
90  return *this;
91 }
92 
106 {
107  resize(R.getRows(), R.getCols(), false, false);
108 
109  if ((data != nullptr) && (R.data != nullptr) && (data != R.data)) {
110  memcpy(data, R.data, dsize * sizeof(double));
111  }
112 
113  return *this;
114 }
115 
129 {
130  resize(V.getRows(), V.getCols(), false, false);
131 
132  if ((data != nullptr) && (V.data != nullptr) && (data != V.data)) {
133  memcpy(data, V.data, dsize * sizeof(double));
134  }
135 
136  return *this;
137 }
138 
152 {
153  resize(F.getRows(), F.getCols(), false, false);
154 
155  if ((data != nullptr) && (F.data != nullptr) && (data != F.data)) {
156  memcpy(data, F.data, dsize * sizeof(double));
157  }
158 
159  return *this;
160 }
161 
175 {
176  resize(v.getRows(), v.getCols(), false, false);
177 
178  if ((data != nullptr) && (v.data != nullptr) && (data != v.data)) {
179  memcpy(data, v.data, dsize * sizeof(double));
180  }
181 
182  return *this;
183 }
184 
198 {
199  resize(v.getRows(), v.getCols(), false, false);
200 
201  if ((data != nullptr) && (v.data != nullptr) && (data != v.data)) {
202  memcpy(data, v.data, dsize * sizeof(double));
203  }
204 
205  return *this;
206 }
207 
221 {
222  resize(t.getRows(), t.getCols(), false, false);
223 
224  if ((data != nullptr) && (t.data != nullptr) && (data != t.data)) {
225  memcpy(data, t.data, dsize * sizeof(double));
226  }
227 
228  return *this;
229 }
230 
232 {
233  resize(A.getRows(), A.getCols(), false, false);
234 
235  if ((data != nullptr) && (A.data != nullptr) && (data != A.data)) {
236  memcpy(data, A.data, dsize * sizeof(double));
237  }
238 
239  return *this;
240 }
241 
242 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
244 {
245  vpArray2D<double>::operator=(std::move(other));
246  return *this;
247 }
248 
279 vpMatrix &vpMatrix::operator=(const std::initializer_list<double> &list)
280 {
281  if (dsize != static_cast<unsigned int>(list.size())) {
282  resize(1, static_cast<unsigned int>(list.size()), false, false);
283  }
284 
285  std::copy(list.begin(), list.end(), data);
286 
287  return *this;
288 }
289 
317 vpMatrix &vpMatrix::operator=(const std::initializer_list<std::initializer_list<double> > &lists)
318 {
319  unsigned int nrows = static_cast<unsigned int>(lists.size()), ncols = 0;
320  for (auto &l : lists) {
321  if (static_cast<unsigned int>(l.size()) > ncols) {
322  ncols = static_cast<unsigned int>(l.size());
323  }
324  }
325 
326  resize(nrows, ncols, false, false);
327  auto it = lists.begin();
328  for (unsigned int i = 0; i < rowNum; ++i, ++it) {
329  std::copy(it->begin(), it->end(), rowPtrs[i]);
330  }
331 
332  return *this;
333 }
334 #endif
335 
338 {
339  std::fill(data, data + (rowNum * colNum), x);
340  return *this;
341 }
342 
349 {
350  for (unsigned int i = 0; i < rowNum; ++i) {
351  for (unsigned int j = 0; j < colNum; ++j) {
352  rowPtrs[i][j] = *x++;
353  }
354  }
355  return *this;
356 }
357 
359 {
360  resize(1, 1, false, false);
361  rowPtrs[0][0] = val;
362  return *this;
363 }
364 
366 {
367  resize(1, colNum + 1, false, false);
368  rowPtrs[0][colNum - 1] = val;
369  return *this;
370 }
371 
377 {
378  vpTranslationVector t_out;
379 
380  const unsigned int val_3 = 3;
381  if ((rowNum != val_3) || (colNum != val_3)) {
382  throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%dx%d) translation vector",
383  rowNum, colNum, tv.getRows(), tv.getCols()));
384  }
385 
386  for (unsigned int j = 0; j < val_3; ++j) {
387  t_out[j] = 0;
388  }
389 
390  for (unsigned int j = 0; j < val_3; ++j) {
391  double tj = tv[j]; // optimization em 5/12/2006
392  for (unsigned int i = 0; i < val_3; ++i) {
393  t_out[i] += rowPtrs[i][j] * tj;
394  }
395  }
396  return t_out;
397 }
398 
404 {
405  vpColVector v_out;
406  vpMatrix::multMatrixVector(*this, v, v_out);
407  return v_out;
408 }
409 
415 {
416  vpMatrix C;
417 
418  vpMatrix::mult2Matrices(*this, B, C);
419 
420  return C;
421 }
422 
428 {
429  if (colNum != R.getRows()) {
430  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
431  colNum));
432  }
433  vpMatrix C;
434  const unsigned int val_3 = 3;
435  C.resize(rowNum, val_3, false, false);
436 
437  unsigned int RcolNum = R.getCols();
438  unsigned int RrowNum = R.getRows();
439  for (unsigned int i = 0; i < rowNum; ++i) {
440  double *rowptri = rowPtrs[i];
441  double *ci = C[i];
442  for (unsigned int j = 0; j < RcolNum; ++j) {
443  double s = 0;
444  for (unsigned int k = 0; k < RrowNum; ++k) {
445  s += rowptri[k] * R[k][j];
446  }
447  ci[j] = s;
448  }
449  }
450 
451  return C;
452 }
453 
459 {
460  if (colNum != M.getRows()) {
461  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
462  colNum));
463  }
464  vpMatrix C;
465  const unsigned int val_4 = 4;
466  C.resize(rowNum, val_4, false, false);
467 
468  const unsigned int McolNum = M.getCols();
469  const unsigned int MrowNum = M.getRows();
470  for (unsigned int i = 0; i < rowNum; ++i) {
471  const double *rowptri = rowPtrs[i];
472  double *ci = C[i];
473  for (unsigned int j = 0; j < McolNum; ++j) {
474  double s = 0;
475  for (unsigned int k = 0; k < MrowNum; ++k) {
476  s += rowptri[k] * M[k][j];
477  }
478  ci[j] = s;
479  }
480  }
481 
482  return C;
483 }
484 
485 
491 {
492  if (colNum != V.getRows()) {
493  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) velocity twist matrix",
494  rowNum, colNum));
495  }
496  vpMatrix M;
497  const unsigned int val_6 = 6;
498  M.resize(rowNum, val_6, false, false);
499 
500  // Considering perfMatrixMultiplication.cpp benchmark,
501  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
502  // Lapack usage needs to be validated again.
503  // If available use Lapack only for large matrices.
504  // Speed up obtained using SSE2.
505  bool useLapack = ((rowNum > vpMatrix::m_lapack_min_size) || (colNum > vpMatrix::m_lapack_min_size) ||
506  (V.colNum > vpMatrix::m_lapack_min_size));
507 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
508  useLapack = false;
509 #endif
510 
511  if (useLapack) {
512 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
513  const double alpha = 1.0;
514  const double beta = 0.0;
515  const char trans = 'n';
516  vpMatrix::blas_dgemm(trans, trans, V.colNum, rowNum, colNum, alpha, V.data, V.colNum, data, colNum, beta, M.data,
517  M.colNum);
518 #endif
519  }
520  else {
521 #if defined(VISP_HAVE_SIMDLIB)
522  SimdMatMulTwist(data, rowNum, V.data, M.data);
523 #else
524  unsigned int VcolNum = V.getCols();
525  unsigned int VrowNum = V.getRows();
526  for (unsigned int i = 0; i < rowNum; ++i) {
527  double *rowptri = rowPtrs[i];
528  double *ci = M[i];
529  for (unsigned int j = 0; j < VcolNum; ++j) {
530  double s = 0;
531  for (unsigned int k = 0; k < VrowNum; ++k) {
532  s += rowptri[k] * V[k][j];
533  }
534  ci[j] = s;
535  }
536  }
537 #endif
538  }
539 
540  return M;
541 }
542 
548 {
549  if (colNum != V.getRows()) {
550  throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) force/torque twist matrix",
551  rowNum, colNum));
552  }
553  vpMatrix M;
554  const unsigned int val_6 = 6;
555  M.resize(rowNum, val_6, false, false);
556 
557  // Considering perfMatrixMultiplication.cpp benchmark,
558  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
559  // Lapack usage needs to be validated again.
560  // If available use Lapack only for large matrices.
561  // Speed up obtained using SSE2.
562  bool useLapack = ((rowNum > vpMatrix::m_lapack_min_size) || (colNum > vpMatrix::m_lapack_min_size) ||
563  (V.getCols() > vpMatrix::m_lapack_min_size));
564 #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
565  useLapack = false;
566 #endif
567 
568  if (useLapack) {
569 #if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
570  const double alpha = 1.0;
571  const double beta = 0.0;
572  const char trans = 'n';
573  vpMatrix::blas_dgemm(trans, trans, V.getCols(), rowNum, colNum, alpha, V.data, V.getCols(), data, colNum, beta,
574  M.data, M.colNum);
575 #endif
576  }
577  else {
578 #if defined(VISP_HAVE_SIMDLIB)
579  SimdMatMulTwist(data, rowNum, V.data, M.data);
580 #else
581  unsigned int VcolNum = V.getCols();
582  unsigned int VrowNum = V.getRows();
583  for (unsigned int i = 0; i < rowNum; ++i) {
584  double *rowptri = rowPtrs[i];
585  double *ci = M[i];
586  for (unsigned int j = 0; j < VcolNum; ++j) {
587  double s = 0;
588  for (unsigned int k = 0; k < VrowNum; ++k) {
589  s += rowptri[k] * V[k][j];
590  }
591  ci[j] = s;
592  }
593  }
594 #endif
595  }
596 
597  return M;
598 }
599 
605 {
606  vpMatrix C;
607  vpMatrix::add2Matrices(*this, B, C);
608  return C;
609 }
610 
616 {
617  vpMatrix C;
618  vpMatrix::sub2Matrices(*this, B, C);
619  return C;
620 }
621 
623 
625 {
626  if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
627  throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
628  B.getRows(), B.getCols()));
629  }
630 
631  double **BrowPtrs = B.rowPtrs;
632 
633  for (unsigned int i = 0; i < rowNum; ++i) {
634  for (unsigned int j = 0; j < colNum; ++j) {
635  rowPtrs[i][j] += BrowPtrs[i][j];
636  }
637  }
638 
639  return *this;
640 }
641 
644 {
645  if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
646  throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
647  B.getRows(), B.getCols()));
648  }
649 
650  double **BrowPtrs = B.rowPtrs;
651  for (unsigned int i = 0; i < rowNum; ++i) {
652  for (unsigned int j = 0; j < colNum; ++j) {
653  rowPtrs[i][j] -= BrowPtrs[i][j];
654  }
655  }
656 
657  return *this;
658 }
659 
664 vpMatrix vpMatrix::operator-() const // negate
665 {
666  vpMatrix C;
667  vpMatrix::negateMatrix(*this, C);
668  return C;
669 }
670 
671 double vpMatrix::sum() const
672 {
673  double s = 0.0;
674  for (unsigned int i = 0; i < rowNum; ++i) {
675  for (unsigned int j = 0; j < colNum; ++j) {
676  s += rowPtrs[i][j];
677  }
678  }
679 
680  return s;
681 }
682 
688 {
689  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
690  return *this;
691  }
692 
693  vpMatrix M;
694  M.resize(rowNum, colNum, false, false);
695 
696  for (unsigned int i = 0; i < rowNum; ++i) {
697  for (unsigned int j = 0; j < colNum; ++j) {
698  M[i][j] = rowPtrs[i][j] * x;
699  }
700  }
701 
702  return M;
703 }
704 
705 
708 {
709  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
710  return *this;
711  }
712 
713  if (std::fabs(x) < std::numeric_limits<double>::epsilon()) {
714  throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
715  }
716 
717  vpMatrix C;
718  C.resize(rowNum, colNum, false, false);
719 
720  double xinv = 1 / x;
721 
722  for (unsigned int i = 0; i < rowNum; ++i) {
723  for (unsigned int j = 0; j < colNum; ++j) {
724  C[i][j] = rowPtrs[i][j] * xinv;
725  }
726  }
727 
728  return C;
729 }
730 
733 {
734  for (unsigned int i = 0; i < rowNum; ++i) {
735  for (unsigned int j = 0; j < colNum; ++j) {
736  rowPtrs[i][j] += x;
737  }
738  }
739 
740  return *this;
741 }
742 
745 {
746  for (unsigned int i = 0; i < rowNum; ++i) {
747  for (unsigned int j = 0; j < colNum; ++j) {
748  rowPtrs[i][j] -= x;
749  }
750  }
751 
752  return *this;
753 }
754 
760 {
761  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
762  return *this;
763  }
764 
765  for (unsigned int i = 0; i < rowNum; ++i) {
766  for (unsigned int j = 0; j < colNum; ++j) {
767  rowPtrs[i][j] *= x;
768  }
769  }
770 
771  return *this;
772 }
773 
776 {
777  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
778  return *this;
779  }
780 
781  if (std::fabs(x) < std::numeric_limits<double>::epsilon()) {
782  throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
783  }
784 
785  double xinv = 1 / x;
786 
787  for (unsigned int i = 0; i < rowNum; ++i) {
788  for (unsigned int j = 0; j < colNum; ++j) {
789  rowPtrs[i][j] *= xinv;
790  }
791  }
792 
793  return *this;
794 }
795 
800 vpMatrix operator*(const double &x, const vpMatrix &B)
801 {
802  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
803  return B;
804  }
805 
806  unsigned int Brow = B.getRows();
807  unsigned int Bcol = B.getCols();
808 
809  VISP_NAMESPACE_ADDRESSING vpMatrix C;
810  C.resize(Brow, Bcol, false, false);
811 
812  for (unsigned int i = 0; i < Brow; ++i) {
813  for (unsigned int j = 0; j < Bcol; ++j) {
814  C[i][j] = B[i][j] * x;
815  }
816  }
817 
818  return C;
819 }
820 
821 END_VISP_NAMESPACE
unsigned int getCols() const
Definition: vpArray2D.h:417
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:148
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:1190
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:442
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:1186
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:1192
unsigned int getRows() const
Definition: vpArray2D.h:427
vpArray2D< Type > & operator=(Type x)
Set all the elements of the array to x.
Definition: vpArray2D.h:609
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:1188
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ dimensionError
Bad dimension.
Definition: vpException.h:71
@ divideByZeroError
Division by zero.
Definition: vpException.h:70
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
vpMatrix operator-() const
vpMatrix & operator/=(double x)
Divide all the element of the matrix by x : Aij = Aij / x.
vpMatrix operator*(const vpMatrix &B) const
vpMatrix operator/(double x) const
Cij = Aij / x (A is unchanged)
vpMatrix & operator-=(const vpMatrix &B)
Operation A = A - B.
vpMatrix & operator*=(double x)
Multiply all the element of the matrix by x : Aij = Aij * x.
vpMatrix operator*(const double &x, const vpMatrix &B)
vpMatrix & operator<<(double *p)
static void sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
vpMatrix operator+(const vpMatrix &B) const
double sum() const
vpMatrix & operator+=(const vpMatrix &B)
Operation A = A + B.
vpMatrix & operator,(double val)
static void multMatrixVector(const vpMatrix &A, const vpColVector &v, vpColVector &w)
vpMatrix & operator=(const vpArray2D< double > &A)
static void add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
static void mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
static void negateMatrix(const vpMatrix &A, vpMatrix &C)
vpMatrix t() const
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:124
Class that consider the case of a translation vector.