OCCT

// Copyright (c) 1997-1999 Matra Datavision
// Copyright (c) 1999-2014 OPEN CASCADE SAS
//
// This file is part of Open CASCADE Technology software library.
//
// This library is free software; you can redistribute it and/or modify it under
// the terms of the GNU Lesser General Public License version 2.1 as published
// by the Free Software Foundation, with special exception defined in the file
// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
// distribution for complete text of the license and disclaimer of any warranty.
//
// Alternatively, this file may be used under the terms of Open CASCADE
// commercial license or contractual agreement.

#include <math_NotSquare.hxx>
#include <Standard_DimensionError.hxx>
#include <Standard_DivideByZero.hxx>

//=================================================================================================

inline Standard_OStream& operator<<(Standard_OStream& o, const math_Matrix& mat)
{
  mat.Dump(o);
  return o;
}

//=================================================================================================

inline math_Matrix operator*(const double Left, const math_Matrix& Right)
{
  return Right.Multiplied(Left);
}

//=================================================================================================

inline const double& math_Matrix::Value(const int Row, const int Col) const
{
  return Array.Value(Row, Col);
}

//=================================================================================================

inline double& math_Matrix::Value(const int Row, const int Col)
{
  return Array.Value(Row, Col);
}

//=================================================================================================

inline int math_Matrix::RowNumber() const noexcept
{
  return Array.NbRows();
}

//=================================================================================================

inline int math_Matrix::ColNumber() const noexcept
{
  return Array.NbColumns();
}

//=================================================================================================

inline int math_Matrix::LowerRow() const noexcept
{
  return Array.LowerRow();
}

//=================================================================================================

inline int math_Matrix::UpperRow() const noexcept
{
  return Array.UpperRow();
}

//=================================================================================================

inline int math_Matrix::LowerCol() const noexcept
{
  return Array.LowerCol();
}

//=================================================================================================

inline int math_Matrix::UpperCol() const noexcept
{
  return Array.UpperCol();
}

//=================================================================================================

inline void math_Matrix::SetLower(const int LowerRow, const int LowerCol) noexcept
{
  SetLowerRow(LowerRow);
  SetLowerCol(LowerCol);
}

//=================================================================================================

inline math_Matrix::math_Matrix(const size_t theNbRows, const size_t theNbColumns)
    : Array(theNbRows, theNbColumns)
{
}

//=================================================================================================

inline math_Matrix::math_Matrix(const size_t theNbRows,
                                const size_t theNbColumns,
                                const double theInitialValue)
    : Array(theNbRows, theNbColumns)
{
  Array.Init(theInitialValue);
}

//=================================================================================================

inline math_Matrix::math_Matrix(const int LowerRow,
                                const int UpperRow,
                                const int LowerCol,
                                const int UpperCol)
    : Array(LowerRow, UpperRow, LowerCol, UpperCol)
{
}

//=================================================================================================

inline math_Matrix::math_Matrix(const int    LowerRow,
                                const int    UpperRow,
                                const int    LowerCol,
                                const int    UpperCol,
                                const double InitialValue)
    : Array(LowerRow, UpperRow, LowerCol, UpperCol)
{
  Array.Init(InitialValue);
}

//=================================================================================================

inline math_Matrix::math_Matrix(void* const Tab,
                                const int   LowerRow,
                                const int   UpperRow,
                                const int   LowerCol,
                                const int   UpperCol)
    : Array(Tab, LowerRow, UpperRow, LowerCol, UpperCol)
{
}

//=================================================================================================

inline math_Matrix::math_Matrix(const math_Matrix& Other)

  = default;

//=================================================================================================

inline math_Matrix::math_Matrix(math_Matrix&& Other) noexcept
    : Array(std::move(Other.Array))
{
}

//=================================================================================================

inline void math_Matrix::Init(const double InitialValue) noexcept
{
  Array.Init(InitialValue);
}

//=================================================================================================

inline void math_Matrix::SetLowerRow(const int LowerRow) noexcept
{
  Array.SetLowerRow(LowerRow);
}

//=================================================================================================

inline void math_Matrix::SetLowerCol(const int LowerCol) noexcept
{
  Array.SetLowerCol(LowerCol);
}

//=================================================================================================

inline void math_Matrix::Multiply(const double Right) noexcept
{
  const int aLowerRow  = Array.LowerRow();
  const int anUpperRow = Array.UpperRow();
  const int aLowerCol  = Array.LowerCol();
  const int anUpperCol = Array.UpperCol();

  for (int I = aLowerRow; I <= anUpperRow; I++)
  {
    for (int J = aLowerCol; J <= anUpperCol; J++)
    {
      Array(I, J) = Array(I, J) * Right;
    }
  }
}

//=================================================================================================

inline math_Matrix math_Matrix::Multiplied(const double Right) const noexcept
{
  const int aLowerRow  = Array.LowerRow();
  const int anUpperRow = Array.UpperRow();
  const int aLowerCol  = Array.LowerCol();
  const int anUpperCol = Array.UpperCol();

  math_Matrix Result(aLowerRow, anUpperRow, aLowerCol, anUpperCol);
  for (int I = aLowerRow; I <= anUpperRow; I++)
  {
    for (int J = aLowerCol; J <= anUpperCol; J++)
    {
      Result.Array(I, J) = Array(I, J) * Right;
    }
  }
  return Result;
}

//=================================================================================================

inline math_Matrix math_Matrix::TMultiplied(const double Right) const noexcept
{
  const int aLowerRow  = Array.LowerRow();
  const int anUpperRow = Array.UpperRow();
  const int aLowerCol  = Array.LowerCol();
  const int anUpperCol = Array.UpperCol();

  math_Matrix Result(aLowerRow, anUpperRow, aLowerCol, anUpperCol);
  for (int I = aLowerRow; I <= anUpperRow; I++)
  {
    for (int J = aLowerCol; J <= anUpperCol; J++)
    {
      Result.Array(I, J) = Array(I, J) * Right;
    }
  }
  return Result;
}

//=================================================================================================

inline void math_Matrix::Divide(const double Right)
{
  Standard_DivideByZero_Raise_if(std::abs(Right) <= RealEpsilon(),
                                 "math_Matrix::Divide() - zero divisor");

  const int aLowerRow  = Array.LowerRow();
  const int anUpperRow = Array.UpperRow();
  const int aLowerCol  = Array.LowerCol();
  const int anUpperCol = Array.UpperCol();

  for (int I = aLowerRow; I <= anUpperRow; I++)
  {
    for (int J = aLowerCol; J <= anUpperCol; J++)
    {
      Array(I, J) = Array(I, J) / Right;
    }
  }
}

//=================================================================================================

inline math_Matrix math_Matrix::Divided(const double Right) const
{
  Standard_DivideByZero_Raise_if(std::abs(Right) <= RealEpsilon(),
                                 "math_Matrix::Divided() - zero divisor");
  math_Matrix temp = Multiplied(1. / Right);
  return temp;
}

//=================================================================================================

inline void math_Matrix::Add(const math_Matrix& Right)
{
  Standard_DimensionError_Raise_if((RowNumber() != Right.RowNumber())
                                     || (ColNumber() != Right.ColNumber()),
                                   "math_Matrix::Add() - input matrix has different dimensions");

  const int aLowerRow  = Array.LowerRow();
  const int anUpperRow = Array.UpperRow();
  const int aLowerCol  = Array.LowerCol();
  const int anUpperCol = Array.UpperCol();

  int I2 = Right.LowerRow();
  for (int I = aLowerRow; I <= anUpperRow; I++)
  {
    int J2 = Right.LowerCol();
    for (int J = aLowerCol; J <= anUpperCol; J++)
    {
      Array(I, J) = Array(I, J) + Right.Array(I2, J2);
      J2++;
    }
    I2++;
  }
}

//=================================================================================================

inline math_Matrix math_Matrix::Added(const math_Matrix& Right) const
{
  Standard_DimensionError_Raise_if((RowNumber() != Right.RowNumber())
                                     || (ColNumber() != Right.ColNumber()),
                                   "math_Matrix::Added() - input matrix has different dimensions");

  const int aLowerRow  = Array.LowerRow();
  const int anUpperRow = Array.UpperRow();
  const int aLowerCol  = Array.LowerCol();
  const int anUpperCol = Array.UpperCol();

  math_Matrix Result(aLowerRow, anUpperRow, aLowerCol, anUpperCol);

  int I2 = Right.LowerRow();
  for (int I = aLowerRow; I <= anUpperRow; I++)
  {
    int J2 = Right.LowerCol();
    for (int J = aLowerCol; J <= anUpperCol; J++)
    {
      Result.Array(I, J) = Array(I, J) + Right.Array(I2, J2);
      J2++;
    }
    I2++;
  }
  return Result;
}

//=================================================================================================

inline void math_Matrix::Add(const math_Matrix& Left, const math_Matrix& Right)
{
  Standard_DimensionError_Raise_if(
    (RowNumber() != Right.RowNumber()) || (ColNumber() != Right.ColNumber())
      || (Right.RowNumber() != Left.RowNumber()) || (Right.ColNumber() != Left.ColNumber()),
    "math_Matrix::Add() - matrices have incompatible dimensions");

  const int aLowerRow  = Array.LowerRow();
  const int anUpperRow = Array.UpperRow();
  const int aLowerCol  = Array.LowerCol();
  const int anUpperCol = Array.UpperCol();

  int I1 = Left.LowerRow();
  int I2 = Right.LowerRow();
  for (int I = aLowerRow; I <= anUpperRow; I++)
  {
    int J1 = Left.LowerCol();
    int J2 = Right.LowerCol();
    for (int J = aLowerCol; J <= anUpperCol; J++)
    {
      Array(I, J) = Left.Array(I1, J1) + Right.Array(I2, J2);
      J1++;
      J2++;
    }
    I1++;
    I2++;
  }
}

//=================================================================================================

inline void math_Matrix::Subtract(const math_Matrix& Right)
{
  Standard_DimensionError_Raise_if(
    (RowNumber() != Right.RowNumber()) || (ColNumber() != Right.ColNumber()),
    "math_Matrix::Subtract() - input matrix has different dimensions");

  const int aLowerRow  = Array.LowerRow();
  const int anUpperRow = Array.UpperRow();
  const int aLowerCol  = Array.LowerCol();
  const int anUpperCol = Array.UpperCol();

  int I2 = Right.LowerRow();
  for (int I = aLowerRow; I <= anUpperRow; I++)
  {
    int J2 = Right.LowerCol();
    for (int J = aLowerCol; J <= anUpperCol; J++)
    {
      Array(I, J) = Array(I, J) - Right.Array(I2, J2);
      J2++;
    }
    I2++;
  }
}

//=================================================================================================

inline math_Matrix math_Matrix::Subtracted(const math_Matrix& Right) const
{
  Standard_DimensionError_Raise_if(
    (RowNumber() != Right.RowNumber()) || (ColNumber() != Right.ColNumber()),
    "math_Matrix::Subtracted() - input matrix has different dimensions");

  const int aLowerRow  = Array.LowerRow();
  const int anUpperRow = Array.UpperRow();
  const int aLowerCol  = Array.LowerCol();
  const int anUpperCol = Array.UpperCol();

  math_Matrix Result(aLowerRow, anUpperRow, aLowerCol, anUpperCol);

  int I2 = Right.LowerRow();
  for (int I = aLowerRow; I <= anUpperRow; I++)
  {
    int J2 = Right.LowerCol();
    for (int J = aLowerCol; J <= anUpperCol; J++)
    {
      Result.Array(I, J) = Array(I, J) - Right.Array(I2, J2);
      J2++;
    }
    I2++;
  }
  return Result;
}

//=================================================================================================

inline void math_Matrix::Subtract(const math_Matrix& Left, const math_Matrix& Right)
{
  Standard_DimensionError_Raise_if(
    (RowNumber() != Right.RowNumber()) || (ColNumber() != Right.ColNumber())
      || (Right.RowNumber() != Left.RowNumber()) || (Right.ColNumber() != Left.ColNumber()),
    "math_Matrix::Subtract() - matrices have incompatible dimensions");

  const int aLowerRow  = Array.LowerRow();
  const int anUpperRow = Array.UpperRow();
  const int aLowerCol  = Array.LowerCol();
  const int anUpperCol = Array.UpperCol();

  int I1 = Left.LowerRow();
  int I2 = Right.LowerRow();
  for (int I = aLowerRow; I <= anUpperRow; I++)
  {
    int J1 = Left.LowerCol();
    int J2 = Right.LowerCol();
    for (int J = aLowerCol; J <= anUpperCol; J++)
    {
      Array(I, J) = Left.Array(I1, J1) - Right.Array(I2, J2);
      J1++;
      J2++;
    }
    I1++;
    I2++;
  }
}

//=================================================================================================

inline math_Matrix math_Matrix::Opposite() const
{
  const int aLowerRow  = Array.LowerRow();
  const int anUpperRow = Array.UpperRow();
  const int aLowerCol  = Array.LowerCol();
  const int anUpperCol = Array.UpperCol();

  math_Matrix Result(aLowerRow, anUpperRow, aLowerCol, anUpperCol);

  for (int I = aLowerRow; I <= anUpperRow; I++)
  {
    for (int J = aLowerCol; J <= anUpperCol; J++)
    {
      Result.Array(I, J) = -Array(I, J);
    }
  }
  return Result;
}

//=================================================================================================

inline void math_Matrix::Set(const int          I1,
                             const int          I2,
                             const int          J1,
                             const int          J2,
                             const math_Matrix& M)
{
  Standard_DimensionError_Raise_if((I1 > I2) || (J1 > J2) || (I2 - I1 + 1 != M.RowNumber())
                                     || (J2 - J1 + 1 != M.ColNumber()),
                                   "math_Matrix::Set() - invalid indices");

  int II = M.LowerRow();
  for (int I = I1; I <= I2; I++)
  {
    int JJ = M.LowerCol();
    for (int J = J1; J <= J2; J++)
    {
      Array(I, J) = M.Array(II, JJ);
      JJ++;
    }
    II++;
  }
}

//=================================================================================================

inline void math_Matrix::SetDiag(const double Value)
{
  math_NotSquare_Raise_if(RowNumber() != ColNumber(),
                          "math_Matrix::SetDiag() - matrix is not square");

  const int aLowerRow  = Array.LowerRow();
  const int anUpperRow = Array.UpperRow();

  for (int I = aLowerRow; I <= anUpperRow; I++)
  {
    Array(I, I) = Value;
  }
}

//=================================================================================================

inline math_Matrix math_Matrix::Transposed() const
{
  const int aLowerRow  = Array.LowerRow();
  const int anUpperRow = Array.UpperRow();
  const int aLowerCol  = Array.LowerCol();
  const int anUpperCol = Array.UpperCol();

  math_Matrix Result(aLowerCol, anUpperCol, aLowerRow, anUpperRow);

  for (int I = aLowerRow; I <= anUpperRow; I++)
  {
    for (int J = aLowerCol; J <= anUpperCol; J++)
    {
      Result.Array(J, I) = Array(I, J);
    }
  }
  return Result;
}

//=================================================================================================

inline void math_Matrix::Multiply(const math_Matrix& Left, const math_Matrix& Right)
{
  Standard_DimensionError_Raise_if(
    (Left.ColNumber() != Right.RowNumber()) || (RowNumber() != Left.RowNumber())
      || (ColNumber() != Right.ColNumber()),
    "math_Matrix::Multiply() - matrices have incompatible dimensions");

  const int aLowerRow  = Array.LowerRow();
  const int anUpperRow = Array.UpperRow();
  const int aLowerCol  = Array.LowerCol();
  const int anUpperCol = Array.UpperCol();

  Init(0.0);

  // Cache-friendly i-k-j loop order for row-major matrix storage.
  // The inner loop accesses Right row-by-row (sequential memory access),
  // improving cache utilization significantly for large matrices.
  int I1 = Left.LowerRow();
  for (int I = aLowerRow; I <= anUpperRow; I++)
  {
    int J1 = Left.LowerCol();
    int I2 = Right.LowerRow();
    for (int K = Left.LowerCol(); K <= Left.UpperCol(); K++)
    {
      const double aLeftVal = Left.Array(I1, J1);
      int          J2       = Right.LowerCol();
      for (int J = aLowerCol; J <= anUpperCol; J++)
      {
        Array(I, J) += aLeftVal * Right.Array(I2, J2);
        J2++;
      }
      J1++;
      I2++;
    }
    I1++;
  }
}

//=================================================================================================

inline void math_Matrix::Multiply(const math_Matrix& Right)
{
  Standard_DimensionError_Raise_if(
    ColNumber() != Right.RowNumber(),
    "math_Matrix::Multiply() - input matrix has incompatible dimensions");

  const int aLowerRow  = Array.LowerRow();
  const int anUpperRow = Array.UpperRow();
  const int aLowerCol  = Array.LowerCol();
  const int anUpperCol = Array.UpperCol();

  // Create a temporary copy to avoid corrupting our own data during calculation
  math_Matrix aTemp = *this;
  if (this == &Right)
  {
    Multiply(aTemp, aTemp);
    return;
  }

  double Som;
  for (int I = aLowerRow; I <= anUpperRow; I++)
  {
    for (int J2 = Right.LowerCol(); J2 <= Right.UpperCol(); J2++)
    {
      Som    = 0.0;
      int I2 = Right.LowerRow();
      for (int J = aLowerCol; J <= anUpperCol; J++)
      {
        Som += aTemp.Array(I, J) * Right.Array(I2, J2);
        I2++;
      }
      Array(I, J2) = Som;
    }
  }
}

//=================================================================================================

inline math_Matrix math_Matrix::Multiplied(const math_Matrix& Right) const
{
  Standard_DimensionError_Raise_if(
    ColNumber() != Right.RowNumber(),
    "math_Matrix::Multiplied() - matrices have incompatible dimensions");

  const int aLowerRow  = Array.LowerRow();
  const int anUpperRow = Array.UpperRow();
  const int aLowerCol  = Array.LowerCol();
  const int anUpperCol = Array.UpperCol();

  // Initialize result to zero
  math_Matrix Result(aLowerRow, anUpperRow, Right.LowerCol(), Right.UpperCol(), 0.0);

  // Cache-friendly i-k-j loop order for row-major matrix storage.
  // The inner loop accesses Right row-by-row (sequential memory access),
  // improving cache utilization significantly for large matrices.
  for (int I = aLowerRow; I <= anUpperRow; I++)
  {
    int I2 = Right.LowerRow();
    for (int J = aLowerCol; J <= anUpperCol; J++)
    {
      const double aLeftVal = Array(I, J);
      int          J2       = Right.LowerCol();
      for (int JR = Right.LowerCol(); JR <= Right.UpperCol(); JR++)
      {
        Result.Array(I, JR) += aLeftVal * Right.Array(I2, J2);
        J2++;
      }
      I2++;
    }
  }
  return Result;
}

//=================================================================================================

inline math_Matrix math_Matrix::TMultiply(const math_Matrix& Right) const
{
  Standard_DimensionError_Raise_if(
    RowNumber() != Right.RowNumber(),
    "math_Matrix::TMultiply() - matrices have incompatible dimensions");

  const int aLowerRow  = Array.LowerRow();
  const int anUpperRow = Array.UpperRow();
  const int aLowerCol  = Array.LowerCol();
  const int anUpperCol = Array.UpperCol();

  math_Matrix Result(aLowerCol, anUpperCol, Right.LowerCol(), Right.UpperCol());

  double Som;
  for (int I = aLowerCol; I <= anUpperCol; I++)
  {
    for (int J2 = Right.LowerCol(); J2 <= Right.UpperCol(); J2++)
    {
      Som    = 0.0;
      int I2 = Right.LowerRow();
      for (int J = aLowerRow; J <= anUpperRow; J++)
      {
        Som = Som + Array(J, I) * Right.Array(I2, J2);
        I2++;
      }
      Result.Array(I, J2) = Som;
    }
  }
  return Result;
}

//=================================================================================================

inline void math_Matrix::TMultiply(const math_Matrix& TLeft, const math_Matrix& Right)
{
  Standard_DimensionError_Raise_if(
    (TLeft.RowNumber() != Right.RowNumber()) || (RowNumber() != TLeft.ColNumber())
      || (ColNumber() != Right.ColNumber()),
    "math_Matrix::TMultiply() - matrices have incompatible dimensions");

  const int aLowerRow  = Array.LowerRow();
  const int anUpperRow = Array.UpperRow();
  const int aLowerCol  = Array.LowerCol();
  const int anUpperCol = Array.UpperCol();

  double Som;
  int    I1 = TLeft.LowerCol();
  for (int I = aLowerRow; I <= anUpperRow; I++)
  {
    int J2 = Right.LowerCol();
    for (int J = aLowerCol; J <= anUpperCol; J++)
    {
      Som    = 0.0;
      int J1 = TLeft.LowerRow();
      int I2 = Right.LowerRow();
      for (int K = TLeft.LowerRow(); K <= TLeft.UpperRow(); K++)
      {
        Som = Som + TLeft.Array(J1, I1) * Right.Array(I2, J2);
        J1++;
        I2++;
      }
      Array(I, J) = Som;
      J2++;
    }
    I1++;
  }
}

//=================================================================================================

inline math_Matrix& math_Matrix::Initialized(const math_Matrix& Other)
{
  (Other.Array).Copy(Array);
  return *this;
}

//=================================================================================================

inline math_Matrix& math_Matrix::operator=(math_Matrix&& Other)
{
  if (this == &Other)
  {
    return *this;
  }

  if (Array.IsDeletable() && Other.Array.IsDeletable() && RowNumber() == Other.RowNumber()
      && ColNumber() == Other.ColNumber() && LowerRow() == Other.LowerRow()
      && LowerCol() == Other.LowerCol())
  {
    Array = std::move(Other.Array);
  }
  else
  {
    Initialized(Other);
  }
  return *this;
}

//=================================================================================================

inline void math_Matrix::Transpose()
{
  math_NotSquare_Raise_if(RowNumber() != ColNumber(),
                          "math_Matrix::Transpose() - matrix is not square");

  const int aLowerRow  = Array.LowerRow();
  const int anUpperRow = Array.UpperRow();
  const int aLowerCol  = Array.LowerCol();
  const int anUpperCol = Array.UpperCol();

  int Row = aLowerRow;
  int Col = aLowerCol;
  SetLowerCol(aLowerRow);
  double Temp;
  for (int I = aLowerRow; I <= anUpperRow; I++)
  {
    for (int J = I; J <= anUpperCol; J++)
    {
      Temp        = Array(I, J);
      Array(I, J) = Array(J, I);
      Array(J, I) = Temp;
    }
  }
  SetLowerRow(Col);
  SetLowerCol(Row);
}

//=================================================================================================

inline void math_Matrix::Dump(Standard_OStream& o) const
{
  const int aLowerRow  = Array.LowerRow();
  const int anUpperRow = Array.UpperRow();
  const int aLowerCol  = Array.LowerCol();
  const int anUpperCol = Array.UpperCol();

  o << "math_Matrix of RowNumber = " << RowNumber();
  o << " and ColNumber = " << ColNumber() << "\n";

  for (int I = aLowerRow; I <= anUpperRow; I++)
  {
    for (int J = aLowerCol; J <= anUpperCol; J++)
    {
      o << "math_Matrix ( " << I << ", " << J << " ) = ";
      o << Array(I, J) << "\n";
    }
  }
}