// SPDX-License-Identifier: LGPL-2.1-or-later // Wild Magic Source Code // David Eberly // http://www.geometrictools.com // Copyright (c) 1998-2007 // // This library is free software; you can redistribute it and/or modify it // under the terms of the GNU Lesser General Public License as published by // the Free Software Foundation; either version 2.1 of the License, or (at // your option) any later version. The license is available for reading at // either of the locations: // http://www.gnu.org/copyleft/lgpl.html // http://www.geometrictools.com/License/WildMagicLicense.pdf // The license applies to versions 0 through 4 of Wild Magic. // // Version: 4.0.0 (2006/06/28) namespace Wm4 { //---------------------------------------------------------------------------- template GMatrix::GMatrix (int iRows, int iCols) { m_afData = nullptr; m_aafEntry = nullptr; SetSize(iRows,iCols); } //---------------------------------------------------------------------------- template GMatrix::GMatrix (int iRows, int iCols, const Real* afEntry) { m_afData = nullptr; m_aafEntry = nullptr; SetMatrix(iRows,iCols,afEntry); } //---------------------------------------------------------------------------- template GMatrix::GMatrix (int iRows, int iCols, const Real** aafMatrix) { m_afData = 0; m_aafEntry = 0; SetMatrix(iRows,iCols,aafMatrix); } //---------------------------------------------------------------------------- template GMatrix::GMatrix (const GMatrix& rkM) { m_iRows = 0; m_iCols = 0; m_iQuantity = 0; m_afData = nullptr; m_aafEntry = nullptr; *this = rkM; } //---------------------------------------------------------------------------- template GMatrix::~GMatrix () { Deallocate(); } //---------------------------------------------------------------------------- template void GMatrix::Allocate (bool bSetToZero) { // assert: m_iRows, m_iCols, and m_iQuantity already initialized m_afData = WM4_NEW Real[m_iQuantity]; if (bSetToZero) { memset(m_afData,0,m_iQuantity*sizeof(Real)); } m_aafEntry = WM4_NEW Real*[m_iRows]; for (int iRow = 0; iRow < m_iRows; iRow++) { m_aafEntry[iRow] = &m_afData[iRow*m_iCols]; } } //---------------------------------------------------------------------------- template void GMatrix::Deallocate () { WM4_DELETE[] m_afData; WM4_DELETE[] m_aafEntry; } //---------------------------------------------------------------------------- template void GMatrix::SetSize (int iRows, int iCols) { Deallocate(); if (iRows > 0 && iCols > 0) { m_iRows = iRows; m_iCols = iCols; m_iQuantity = m_iRows*m_iCols; Allocate(true); } else { m_iRows = 0; m_iCols = 0; m_iQuantity = 0; m_afData = nullptr; m_aafEntry = nullptr; } } //---------------------------------------------------------------------------- template void GMatrix::GetSize (int& riRows, int& riCols) const { riRows = m_iRows; riCols = m_iCols; } //---------------------------------------------------------------------------- template int GMatrix::GetRows () const { return m_iRows; } //---------------------------------------------------------------------------- template int GMatrix::GetColumns () const { return m_iCols; } //---------------------------------------------------------------------------- template int GMatrix::GetQuantity () const { return m_iQuantity; } //---------------------------------------------------------------------------- template GMatrix::operator const Real* () const { return m_afData; } //---------------------------------------------------------------------------- template GMatrix::operator Real* () { return m_afData; } //---------------------------------------------------------------------------- template const Real* GMatrix::operator[] (int iRow) const { assert(0 <= iRow && iRow < m_iRows); return m_aafEntry[iRow]; } //---------------------------------------------------------------------------- template Real* GMatrix::operator[] (int iRow) { assert(0 <= iRow && iRow < m_iRows); return m_aafEntry[iRow]; } //---------------------------------------------------------------------------- template Real GMatrix::operator() (int iRow, int iCol) const { return m_aafEntry[iRow][iCol]; } //---------------------------------------------------------------------------- template Real& GMatrix::operator() (int iRow, int iCol) { assert(0 <= iRow && iRow < m_iRows && 0 <= iCol && iCol <= m_iCols); return m_aafEntry[iRow][iCol]; } //---------------------------------------------------------------------------- template void GMatrix::SwapRows (int iRow0, int iRow1) { assert(0 <= iRow0 && iRow0 < m_iRows && 0 <= iRow1 && iRow1 < m_iRows); Real* afSave = m_aafEntry[iRow0]; m_aafEntry[iRow0] = m_aafEntry[iRow1]; m_aafEntry[iRow1] = afSave; } //---------------------------------------------------------------------------- template void GMatrix::SetRow (int iRow, const GVector& rkV) { assert((0 <= iRow && iRow < m_iRows) && (rkV.GetSize() == m_iCols)); for (int iCol = 0; iCol < m_iCols; iCol++) { m_aafEntry[iRow][iCol] = rkV[iCol]; } } //---------------------------------------------------------------------------- template GVector GMatrix::GetRow (int iRow) const { assert(0 <= iRow && iRow < m_iRows); GVector kV(m_iCols); for (int iCol = 0; iCol < m_iCols; iCol++) { kV[iCol] = m_aafEntry[iRow][iCol]; } return kV; } //---------------------------------------------------------------------------- template void GMatrix::SetColumn (int iCol, const GVector& rkV) { assert((0 <= iCol && iCol < m_iCols) && (rkV.GetSize() == m_iRows)); for (int iRow = 0; iRow < m_iRows; iRow++) { m_aafEntry[iRow][iCol] = rkV[iRow]; } } //---------------------------------------------------------------------------- template GVector GMatrix::GetColumn (int iCol) const { assert(0 <= iCol && iCol < m_iCols); GVector kV(m_iRows); for (int iRow = 0; iRow < m_iRows; iRow++) { kV[iRow] = m_aafEntry[iRow][iCol]; } return kV; } //---------------------------------------------------------------------------- template void GMatrix::SetMatrix (int iRows, int iCols, const Real* afData) { Deallocate(); if (iRows > 0 && iCols > 0) { m_iRows = iRows; m_iCols = iCols; m_iQuantity = m_iRows*m_iCols; Allocate(false); size_t uiSize = m_iQuantity*sizeof(Real); System::Memcpy(m_afData,uiSize,afData,uiSize); } else { m_iRows = 0; m_iCols = 0; m_iQuantity = 0; m_afData = nullptr; m_aafEntry = nullptr; } } //---------------------------------------------------------------------------- template void GMatrix::SetMatrix (int iRows, int iCols, const Real** aafEntry) { Deallocate(); if (iRows > 0 && iCols > 0) { m_iRows = iRows; m_iCols = iCols; m_iQuantity = m_iRows*m_iCols; Allocate(false); for (int iRow = 0; iRow < m_iRows; iRow++) { for (int iCol = 0; iCol < m_iCols; iCol++) { m_aafEntry[iRow][iCol] = aafEntry[iRow][iCol]; } } } else { m_iRows = 0; m_iCols = 0; m_iQuantity = 0; m_afData = 0; m_aafEntry = 0; } } //---------------------------------------------------------------------------- template void GMatrix::GetColumnMajor (Real* afCMajor) const { for (int iRow = 0, i = 0; iRow < m_iRows; iRow++) { for (int iCol = 0; iCol < m_iCols; iCol++) { afCMajor[i++] = m_aafEntry[iCol][iRow]; } } } //---------------------------------------------------------------------------- template GMatrix& GMatrix::operator= (const GMatrix& rkM) { if (rkM.m_iQuantity > 0) { if (m_iRows != rkM.m_iRows || m_iCols != rkM.m_iCols) { Deallocate(); m_iRows = rkM.m_iRows; m_iCols = rkM.m_iCols; m_iQuantity = rkM.m_iQuantity; Allocate(false); } for (int iRow = 0; iRow < m_iRows; iRow++) { for (int iCol = 0; iCol < m_iCols; iCol++) { m_aafEntry[iRow][iCol] = rkM.m_aafEntry[iRow][iCol]; } } } else { Deallocate(); m_iRows = 0; m_iCols = 0; m_iQuantity = 0; m_afData = nullptr; m_aafEntry = nullptr; } return *this; } //---------------------------------------------------------------------------- template int GMatrix::CompareArrays (const GMatrix& rkM) const { return memcmp(m_afData,rkM.m_afData,m_iQuantity*sizeof(Real)); } //---------------------------------------------------------------------------- template bool GMatrix::operator== (const GMatrix& rkM) const { return CompareArrays(rkM) == 0; } //---------------------------------------------------------------------------- template bool GMatrix::operator!= (const GMatrix& rkM) const { return CompareArrays(rkM) != 0; } //---------------------------------------------------------------------------- template bool GMatrix::operator< (const GMatrix& rkM) const { return CompareArrays(rkM) < 0; } //---------------------------------------------------------------------------- template bool GMatrix::operator<= (const GMatrix& rkM) const { return CompareArrays(rkM) <= 0; } //---------------------------------------------------------------------------- template bool GMatrix::operator> (const GMatrix& rkM) const { return CompareArrays(rkM) > 0; } //---------------------------------------------------------------------------- template bool GMatrix::operator>= (const GMatrix& rkM) const { return CompareArrays(rkM) >= 0; } //---------------------------------------------------------------------------- template GMatrix GMatrix::operator+ (const GMatrix& rkM) const { GMatrix kSum(rkM.m_iRows,rkM.m_iCols); for (int i = 0; i < m_iQuantity; i++) { kSum.m_afData[i] = m_afData[i] + rkM.m_afData[i]; } return kSum; } //---------------------------------------------------------------------------- template GMatrix GMatrix::operator- (const GMatrix& rkM) const { GMatrix kDiff(rkM.m_iRows,rkM.m_iCols); for (int i = 0; i < m_iQuantity; i++) { kDiff.m_afData[i] = m_afData[i] - rkM.m_afData[i]; } return kDiff; } //---------------------------------------------------------------------------- template GMatrix GMatrix::operator* (const GMatrix& rkM) const { // 'this' is RxN, 'M' is NxC, 'product = this*M' is RxC assert(m_iCols == rkM.m_iRows); GMatrix kProd(m_iRows,rkM.m_iCols); for (int iRow = 0; iRow < kProd.m_iRows; iRow++) { for (int iCol = 0; iCol < kProd.m_iCols; iCol++) { for (int iMid = 0; iMid < m_iCols; iMid++) { kProd.m_aafEntry[iRow][iCol] += m_aafEntry[iRow][iMid] * rkM.m_aafEntry[iMid][iCol]; } } } return kProd; } //---------------------------------------------------------------------------- template GMatrix GMatrix::operator* (Real fScalar) const { GMatrix kProd(m_iRows,m_iCols); for (int i = 0; i < m_iQuantity; i++) { kProd.m_afData[i] = fScalar*m_afData[i]; } return kProd; } //---------------------------------------------------------------------------- template GMatrix GMatrix::operator/ (Real fScalar) const { GMatrix kQuot(m_iRows,m_iCols); int i; if (fScalar != (Real)0.0) { Real fInvScalar = ((Real)1.0)/fScalar; for (i = 0; i < m_iQuantity; i++) { kQuot.m_afData[i] = fInvScalar*m_afData[i]; } } else { for (i = 0; i < m_iQuantity; i++) { kQuot.m_afData[i] = Math::MAX_REAL; } } return kQuot; } //---------------------------------------------------------------------------- template GMatrix GMatrix::operator- () const { GMatrix kNeg(m_iRows,m_iCols); for (int i = 0; i < m_iQuantity; i++) { kNeg.m_afData[i] = -m_afData[i]; } return kNeg; } //---------------------------------------------------------------------------- template GMatrix operator* (Real fScalar, const GMatrix& rkM) { GMatrix kProd(rkM.GetRows(),rkM.GetColumns()); const Real* afMEntry = rkM; Real* afPEntry = kProd; for (int i = 0; i < rkM.GetQuantity(); i++) { afPEntry[i] = fScalar*afMEntry[i]; } return kProd; } //---------------------------------------------------------------------------- template GMatrix& GMatrix::operator+= (const GMatrix& rkM) { for (int i = 0; i < m_iQuantity; i++) { m_afData[i] += rkM.m_afData[i]; } return *this; } //---------------------------------------------------------------------------- template GMatrix& GMatrix::operator-= (const GMatrix& rkM) { for (int i = 0; i < m_iQuantity; i++) { m_afData[i] -= rkM.m_afData[i]; } return *this; } //---------------------------------------------------------------------------- template GMatrix& GMatrix::operator*= (Real fScalar) { for (int i = 0; i < m_iQuantity; i++) { m_afData[i] *= fScalar; } return *this; } //---------------------------------------------------------------------------- template GMatrix& GMatrix::operator/= (Real fScalar) { int i; if (fScalar != (Real)0.0) { Real fInvScalar = ((Real)1.0)/fScalar; for (i = 0; i < m_iQuantity; i++) { m_afData[i] *= fInvScalar; } } else { for (i = 0; i < m_iQuantity; i++) { m_afData[i] = Math::MAX_REAL; } } return *this; } //---------------------------------------------------------------------------- template GMatrix GMatrix::Transpose () const { GMatrix kTranspose(m_iCols,m_iRows); for (int iRow = 0; iRow < m_iRows; iRow++) { for (int iCol = 0; iCol < m_iCols; iCol++) { kTranspose.m_aafEntry[iCol][iRow] = m_aafEntry[iRow][iCol]; } } return kTranspose; } //---------------------------------------------------------------------------- template GMatrix GMatrix::TransposeTimes (const GMatrix& rkM) const { // P = A^T*B, P[r][c] = sum_m A[m][r]*B[m][c] assert(m_iRows == rkM.m_iRows); GMatrix kProd(m_iCols,rkM.m_iCols); for (int iRow = 0; iRow < kProd.m_iRows; iRow++) { for (int iCol = 0; iCol < kProd.m_iCols; iCol++) { for (int iMid = 0; iMid < m_iRows; iMid++) { kProd.m_aafEntry[iRow][iCol] += m_aafEntry[iMid][iRow] * rkM.m_aafEntry[iMid][iCol]; } } } return kProd; } //---------------------------------------------------------------------------- template GMatrix GMatrix::TimesTranspose (const GMatrix& rkM) const { // P = A*B^T, P[r][c] = sum_m A[r][m]*B[c][m] assert(m_iCols == rkM.m_iCols); GMatrix kProd(m_iRows,rkM.m_iRows); for (int iRow = 0; iRow < kProd.m_iRows; iRow++) { for (int iCol = 0; iCol < kProd.m_iCols; iCol++) { for (int iMid = 0; iMid < m_iCols; iMid++) { kProd.m_aafEntry[iRow][iCol] += m_aafEntry[iRow][iMid] * rkM.m_aafEntry[iCol][iMid]; } } } return kProd; } //---------------------------------------------------------------------------- template GVector GMatrix::operator* (const GVector& rkV) const { assert(rkV.GetSize() == m_iCols); GVector kProd(m_iRows); for (int iRow = 0; iRow < m_iRows; iRow++) { for (int iCol = 0; iCol < m_iCols; iCol++) { kProd[iRow] += m_aafEntry[iRow][iCol]*rkV[iCol]; } } return kProd; } //---------------------------------------------------------------------------- template GVector operator* (const GVector& rkV, const GMatrix& rkM) { assert(rkV.GetSize() == rkM.GetRows()); GVector kProd(rkM.GetColumns()); Real* afPEntry = kProd; for (int iCol = 0; iCol < rkM.GetColumns(); iCol++) { for (int iRow = 0; iRow < rkM.GetRows(); iRow++) { afPEntry[iCol] += rkV[iRow]*rkM[iRow][iCol]; } } return kProd; } //---------------------------------------------------------------------------- template Real GMatrix::QForm (const GVector& rkU, const GVector& rkV) const { assert(rkU.GetSize() == m_iRows && rkV.GetSize() == m_iCols); return rkU.Dot((*this)*rkV); } //---------------------------------------------------------------------------- template bool GMatrix::GetInverse (GMatrix& rkInverse) const { // computations are performed in-place if (GetRows() > 0 && GetRows() != GetColumns()) { return false; } int iSize = GetRows(); rkInverse = *this; int* aiColIndex = WM4_NEW int[iSize]; int* aiRowIndex = WM4_NEW int[iSize]; bool* abPivoted = WM4_NEW bool[iSize]; memset(abPivoted,0,iSize*sizeof(bool)); int i1, i2, iRow = 0, iCol = 0; Real fSave; // elimination by full pivoting for (int i0 = 0; i0 < iSize; i0++) { // search matrix (excluding pivoted rows) for maximum absolute entry Real fMax = (Real)0.0; for (i1 = 0; i1 < iSize; i1++) { if (!abPivoted[i1]) { for (i2 = 0; i2 < iSize; i2++) { if (!abPivoted[i2]) { Real fAbs = Math::FAbs(rkInverse[i1][i2]); if (fAbs > fMax) { fMax = fAbs; iRow = i1; iCol = i2; } } } } } if (fMax == (Real)0.0) { // matrix is not invertible WM4_DELETE[] aiColIndex; WM4_DELETE[] aiRowIndex; WM4_DELETE[] abPivoted; return false; } abPivoted[iCol] = true; // swap rows so that A[iCol][iCol] contains the pivot entry if (iRow != iCol) { rkInverse.SwapRows(iRow,iCol); } // keep track of the permutations of the rows aiRowIndex[i0] = iRow; aiColIndex[i0] = iCol; // scale the row so that the pivot entry is 1 Real fInv = ((Real)1.0)/rkInverse[iCol][iCol]; rkInverse[iCol][iCol] = (Real)1.0; for (i2 = 0; i2 < iSize; i2++) { rkInverse[iCol][i2] *= fInv; } // zero out the pivot column locations in the other rows for (i1 = 0; i1 < iSize; i1++) { if (i1 != iCol) { fSave = rkInverse[i1][iCol]; rkInverse[i1][iCol] = (Real)0.0; for (i2 = 0; i2 < iSize; i2++) { rkInverse[i1][i2] -= rkInverse[iCol][i2]*fSave; } } } } // reorder rows so that A[][] stores the inverse of the original matrix for (i1 = iSize-1; i1 >= 0; i1--) { if (aiRowIndex[i1] != aiColIndex[i1]) { for (i2 = 0; i2 < iSize; i2++) { fSave = rkInverse[i2][aiRowIndex[i1]]; rkInverse[i2][aiRowIndex[i1]] = rkInverse[i2][aiColIndex[i1]]; rkInverse[i2][aiColIndex[i1]] = fSave; } } } WM4_DELETE[] aiColIndex; WM4_DELETE[] aiRowIndex; WM4_DELETE[] abPivoted; return true; } //---------------------------------------------------------------------------- } //namespace Wm4