vpMatrix_operators.cpp

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See https://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Stack matrix.
 */
 
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMatrix.h>
 
#if defined(VISP_HAVE_SIMDLIB)
#include <Simd/SimdLib.h>
#endif
 
BEGIN_VISP_NAMESPACE
 
vpMatrix &vpMatrix::operator=(const vpArray2D<double> &A)
{
  resize(A.getRows(), A.getCols(), false, false);
 
  if ((data != nullptr) && (A.data != nullptr) && (data != A.data)) {
    memcpy(data, A.data, dsize * sizeof(double));
  }
 
  return *this;
}
 
vpMatrix &vpMatrix::operator=(const vpHomogeneousMatrix &M)
{
  resize(M.getRows(), M.getCols(), false, false);
 
  if ((data != nullptr) && (M.data != nullptr) && (data != M.data)) {
    memcpy(data, M.data, dsize * sizeof(double));
  }
 
  return *this;
}
 
vpMatrix &vpMatrix::operator=(const vpRotationMatrix &R)
{
  resize(R.getRows(), R.getCols(), false, false);
 
  if ((data != nullptr) && (R.data != nullptr) && (data != R.data)) {
    memcpy(data, R.data, dsize * sizeof(double));
  }
 
  return *this;
}
 
vpMatrix &vpMatrix::operator=(const vpVelocityTwistMatrix &V)
{
  resize(V.getRows(), V.getCols(), false, false);
 
  if ((data != nullptr) && (V.data != nullptr) && (data != V.data)) {
    memcpy(data, V.data, dsize * sizeof(double));
  }
 
  return *this;
}
 
vpMatrix &vpMatrix::operator=(const vpForceTwistMatrix &F)
{
  resize(F.getRows(), F.getCols(), false, false);
 
  if ((data != nullptr) && (F.data != nullptr) && (data != F.data)) {
    memcpy(data, F.data, dsize * sizeof(double));
  }
 
  return *this;
}
 
vpMatrix &vpMatrix::operator=(const vpColVector &v)
{
  resize(v.getRows(), v.getCols(), false, false);
 
  if ((data != nullptr) && (v.data != nullptr) && (data != v.data)) {
    memcpy(data, v.data, dsize * sizeof(double));
  }
 
  return *this;
}
 
vpMatrix &vpMatrix::operator=(const vpRowVector &v)
{
  resize(v.getRows(), v.getCols(), false, false);
 
  if ((data != nullptr) && (v.data != nullptr) && (data != v.data)) {
    memcpy(data, v.data, dsize * sizeof(double));
  }
 
  return *this;
}
 
vpMatrix &vpMatrix::operator=(const vpTranslationVector &t)
{
  resize(t.getRows(), t.getCols(), false, false);
 
  if ((data != nullptr) && (t.data != nullptr) && (data != t.data)) {
    memcpy(data, t.data, dsize * sizeof(double));
  }
 
  return *this;
}
 
vpMatrix &vpMatrix::operator=(const vpMatrix &A)
{
  resize(A.getRows(), A.getCols(), false, false);
 
  if ((data != nullptr) && (A.data != nullptr) && (data != A.data)) {
    memcpy(data, A.data, dsize * sizeof(double));
  }
 
  return *this;
}
 
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
vpMatrix &vpMatrix::operator=(vpMatrix &&other)
{
  if (this != &other) {
    if (data) {
      free(data);
    }
    if (rowPtrs) {
      free(rowPtrs);
    }
 
    rowNum = other.rowNum;
    colNum = other.colNum;
    rowPtrs = other.rowPtrs;
    dsize = other.dsize;
    data = other.data;
 
    other.rowNum = 0;
    other.colNum = 0;
    other.rowPtrs = nullptr;
    other.dsize = 0;
    other.data = nullptr;
  }
 
  return *this;
}
 
vpMatrix &vpMatrix::operator=(const std::initializer_list<double> &list)
{
  if (dsize != static_cast<unsigned int>(list.size())) {
    resize(1, static_cast<unsigned int>(list.size()), false, false);
  }
 
  std::copy(list.begin(), list.end(), data);
 
  return *this;
}
 
vpMatrix &vpMatrix::operator=(const std::initializer_list<std::initializer_list<double> > &lists)
{
  unsigned int nrows = static_cast<unsigned int>(lists.size()), ncols = 0;
  for (auto &l : lists) {
    if (static_cast<unsigned int>(l.size()) > ncols) {
      ncols = static_cast<unsigned int>(l.size());
    }
  }
 
  resize(nrows, ncols, false, false);
  auto it = lists.begin();
  for (unsigned int i = 0; i < rowNum; ++i, ++it) {
    std::copy(it->begin(), it->end(), rowPtrs[i]);
  }
 
  return *this;
}
#endif
 
vpMatrix &vpMatrix::operator=(double x)
{
  std::fill(data, data + (rowNum * colNum), x);
  return *this;
}
 
vpMatrix &vpMatrix::operator<<(double *x)
{
  for (unsigned int i = 0; i < rowNum; ++i) {
    for (unsigned int j = 0; j < colNum; ++j) {
      rowPtrs[i][j] = *x++;
    }
  }
  return *this;
}
 
vpMatrix &vpMatrix::operator<<(double val)
{
  resize(1, 1, false, false);
  rowPtrs[0][0] = val;
  return *this;
}
 
vpMatrix &vpMatrix::operator,(double val)
{
  resize(1, colNum + 1, false, false);
  rowPtrs[0][colNum - 1] = val;
  return *this;
}
 
vpTranslationVector vpMatrix::operator*(const vpTranslationVector &tv) const
{
  vpTranslationVector t_out;
 
  const unsigned int val_3 = 3;
  if ((rowNum != val_3) || (colNum != val_3)) {
    throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%dx%d) translation vector",
                      rowNum, colNum, tv.getRows(), tv.getCols()));
  }
 
  for (unsigned int j = 0; j < val_3; ++j) {
    t_out[j] = 0;
  }
 
  for (unsigned int j = 0; j < val_3; ++j) {
    double tj = tv[j]; // optimization em 5/12/2006
    for (unsigned int i = 0; i < val_3; ++i) {
      t_out[i] += rowPtrs[i][j] * tj;
    }
  }
  return t_out;
}
 
vpColVector vpMatrix::operator*(const vpColVector &v) const
{
  vpColVector v_out;
  vpMatrix::multMatrixVector(*this, v, v_out);
  return v_out;
}
 
vpMatrix vpMatrix::operator*(const vpMatrix &B) const
{
  vpMatrix C;
 
  vpMatrix::mult2Matrices(*this, B, C);
 
  return C;
}
 
vpMatrix vpMatrix::operator*(const vpRotationMatrix &R) const
{
  if (colNum != R.getRows()) {
    throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
                      colNum));
  }
  vpMatrix C;
  const unsigned int val_3 = 3;
  C.resize(rowNum, val_3, false, false);
 
  unsigned int RcolNum = R.getCols();
  unsigned int RrowNum = R.getRows();
  for (unsigned int i = 0; i < rowNum; ++i) {
    double *rowptri = rowPtrs[i];
    double *ci = C[i];
    for (unsigned int j = 0; j < RcolNum; ++j) {
      double s = 0;
      for (unsigned int k = 0; k < RrowNum; ++k) {
        s += rowptri[k] * R[k][j];
      }
      ci[j] = s;
    }
  }
 
  return C;
}
 
vpMatrix vpMatrix::operator*(const vpHomogeneousMatrix &M) const
{
  if (colNum != M.getRows()) {
    throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
                      colNum));
  }
  vpMatrix C;
  const unsigned int val_4 = 4;
  C.resize(rowNum, val_4, false, false);
 
  const unsigned int McolNum = M.getCols();
  const unsigned int MrowNum = M.getRows();
  for (unsigned int i = 0; i < rowNum; ++i) {
    const double *rowptri = rowPtrs[i];
    double *ci = C[i];
    for (unsigned int j = 0; j < McolNum; ++j) {
      double s = 0;
      for (unsigned int k = 0; k < MrowNum; ++k) {
        s += rowptri[k] * M[k][j];
      }
      ci[j] = s;
    }
  }
 
  return C;
}
 
 
vpMatrix vpMatrix::operator*(const vpVelocityTwistMatrix &V) const
{
  if (colNum != V.getRows()) {
    throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) velocity twist matrix",
                      rowNum, colNum));
  }
  vpMatrix M;
  const unsigned int val_6 = 6;
  M.resize(rowNum, val_6, false, false);
 
  // Considering perfMatrixMultiplication.cpp benchmark,
  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
  // Lapack usage needs to be validated again.
  // If available use Lapack only for large matrices.
  // Speed up obtained using SSE2.
  bool useLapack = ((rowNum > vpMatrix::m_lapack_min_size) || (colNum > vpMatrix::m_lapack_min_size) ||
                    (V.colNum > vpMatrix::m_lapack_min_size));
#if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
  useLapack = false;
#endif
 
  if (useLapack) {
#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
    const double alpha = 1.0;
    const double beta = 0.0;
    const char trans = 'n';
    vpMatrix::blas_dgemm(trans, trans, V.colNum, rowNum, colNum, alpha, V.data, V.colNum, data, colNum, beta, M.data,
                         M.colNum);
#endif
  }
  else {
#if defined(VISP_HAVE_SIMDLIB)
    SimdMatMulTwist(data, rowNum, V.data, M.data);
#else
    unsigned int VcolNum = V.getCols();
    unsigned int VrowNum = V.getRows();
    for (unsigned int i = 0; i < rowNum; ++i) {
      double *rowptri = rowPtrs[i];
      double *ci = M[i];
      for (unsigned int j = 0; j < VcolNum; ++j) {
        double s = 0;
        for (unsigned int k = 0; k < VrowNum; ++k) {
          s += rowptri[k] * V[k][j];
        }
        ci[j] = s;
      }
    }
#endif
  }
 
  return M;
}
 
vpMatrix vpMatrix::operator*(const vpForceTwistMatrix &V) const
{
  if (colNum != V.getRows()) {
    throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) force/torque twist matrix",
                      rowNum, colNum));
  }
  vpMatrix M;
  const unsigned int val_6 = 6;
  M.resize(rowNum, val_6, false, false);
 
  // Considering perfMatrixMultiplication.cpp benchmark,
  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
  // Lapack usage needs to be validated again.
  // If available use Lapack only for large matrices.
  // Speed up obtained using SSE2.
  bool useLapack = ((rowNum > vpMatrix::m_lapack_min_size) || (colNum > vpMatrix::m_lapack_min_size) ||
                    (V.getCols() > vpMatrix::m_lapack_min_size));
#if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
  useLapack = false;
#endif
 
  if (useLapack) {
#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
    const double alpha = 1.0;
    const double beta = 0.0;
    const char trans = 'n';
    vpMatrix::blas_dgemm(trans, trans, V.getCols(), rowNum, colNum, alpha, V.data, V.getCols(), data, colNum, beta,
                         M.data, M.colNum);
#endif
  }
  else {
#if defined(VISP_HAVE_SIMDLIB)
    SimdMatMulTwist(data, rowNum, V.data, M.data);
#else
    unsigned int VcolNum = V.getCols();
    unsigned int VrowNum = V.getRows();
    for (unsigned int i = 0; i < rowNum; ++i) {
      double *rowptri = rowPtrs[i];
      double *ci = M[i];
      for (unsigned int j = 0; j < VcolNum; ++j) {
        double s = 0;
        for (unsigned int k = 0; k < VrowNum; ++k) {
          s += rowptri[k] * V[k][j];
        }
        ci[j] = s;
      }
    }
#endif
  }
 
  return M;
}
 
vpMatrix vpMatrix::operator+(const vpMatrix &B) const
{
  vpMatrix C;
  vpMatrix::add2Matrices(*this, B, C);
  return C;
}
 
vpMatrix vpMatrix::operator-(const vpMatrix &B) const
{
  vpMatrix C;
  vpMatrix::sub2Matrices(*this, B, C);
  return C;
}
 
 
vpMatrix &vpMatrix::operator+=(const vpMatrix &B)
{
  if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
    throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
                      B.getRows(), B.getCols()));
  }
 
  double **BrowPtrs = B.rowPtrs;
 
  for (unsigned int i = 0; i < rowNum; ++i) {
    for (unsigned int j = 0; j < colNum; ++j) {
      rowPtrs[i][j] += BrowPtrs[i][j];
    }
  }
 
  return *this;
}
 
vpMatrix &vpMatrix::operator-=(const vpMatrix &B)
{
  if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
    throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
                      B.getRows(), B.getCols()));
  }
 
  double **BrowPtrs = B.rowPtrs;
  for (unsigned int i = 0; i < rowNum; ++i) {
    for (unsigned int j = 0; j < colNum; ++j) {
      rowPtrs[i][j] -= BrowPtrs[i][j];
    }
  }
 
  return *this;
}
 
vpMatrix vpMatrix::operator-() const // negate
{
  vpMatrix C;
  vpMatrix::negateMatrix(*this, C);
  return C;
}
 
double vpMatrix::sum() const
{
  double s = 0.0;
  for (unsigned int i = 0; i < rowNum; ++i) {
    for (unsigned int j = 0; j < colNum; ++j) {
      s += rowPtrs[i][j];
    }
  }
 
  return s;
}
 
vpMatrix vpMatrix::operator*(double x) const
{
  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
    return *this;
  }
 
  vpMatrix M;
  M.resize(rowNum, colNum, false, false);
 
  for (unsigned int i = 0; i < rowNum; ++i) {
    for (unsigned int j = 0; j < colNum; ++j) {
      M[i][j] = rowPtrs[i][j] * x;
    }
  }
 
  return M;
}
 
 
vpMatrix vpMatrix::operator/(double x) const
{
  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
    return *this;
  }
 
  if (std::fabs(x) < std::numeric_limits<double>::epsilon()) {
    throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
  }
 
  vpMatrix C;
  C.resize(rowNum, colNum, false, false);
 
  double xinv = 1 / x;
 
  for (unsigned int i = 0; i < rowNum; ++i) {
    for (unsigned int j = 0; j < colNum; ++j) {
      C[i][j] = rowPtrs[i][j] * xinv;
    }
  }
 
  return C;
}
 
vpMatrix &vpMatrix::operator+=(double x)
{
  for (unsigned int i = 0; i < rowNum; ++i) {
    for (unsigned int j = 0; j < colNum; ++j) {
      rowPtrs[i][j] += x;
    }
  }
 
  return *this;
}
 
vpMatrix &vpMatrix::operator-=(double x)
{
  for (unsigned int i = 0; i < rowNum; ++i) {
    for (unsigned int j = 0; j < colNum; ++j) {
      rowPtrs[i][j] -= x;
    }
  }
 
  return *this;
}
 
vpMatrix &vpMatrix::operator*=(double x)
{
  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
    return *this;
  }
 
  for (unsigned int i = 0; i < rowNum; ++i) {
    for (unsigned int j = 0; j < colNum; ++j) {
      rowPtrs[i][j] *= x;
    }
  }
 
  return *this;
}
 
vpMatrix &vpMatrix::operator/=(double x)
{
  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
    return *this;
  }
 
  if (std::fabs(x) < std::numeric_limits<double>::epsilon()) {
    throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
  }
 
  double xinv = 1 / x;
 
  for (unsigned int i = 0; i < rowNum; ++i) {
    for (unsigned int j = 0; j < colNum; ++j) {
      rowPtrs[i][j] *= xinv;
    }
  }
 
  return *this;
}
 
vpMatrix operator*(const double &x, const vpMatrix &B)
{
  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
    return B;
  }
 
  unsigned int Brow = B.getRows();
  unsigned int Bcol = B.getCols();
 
  VISP_NAMESPACE_ADDRESSING vpMatrix C;
  C.resize(Brow, Bcol, false, false);
 
  for (unsigned int i = 0; i < Brow; ++i) {
    for (unsigned int j = 0; j < Bcol; ++j) {
      C[i][j] = B[i][j] * x;
    }
  }
 
  return C;
}
 
END_VISP_NAMESPACE