double Det(Matrix &M)
{
int dim = M[0].size();
if( dim == 1 ) return M[0][0];
double det = 0.0;
for(int i = 0; i < dim; i++ )
{
Matrix Cofactor(dim-1);
for(int l=0;l<dim-1;l++) Cofactor[l].resize(dim-1);
int col=0, row=0;
for(int j = 0; j < dim; j++)
{
if(j != i)
{
row = 0;
for(int k = 1; k < dim; k++)
{
Cofactor[row][col] = M[k][j];
row++;
}
col++;
}
}
det += (i%2==1?-1.0:1.0) * M[0][i] * Det(Cofactor);
}
return det;
}
// ****************************************************************************
// ****************************************************************************
bool Matrix4x4::Invert()
{
// Transposed cofactors
// r[0][0] = r[1][2]*r[2][3]*r[3][1] - r[1][3]*r[2][2]*r[3][1] + r[1][3]*r[2][1]*r[3][2] - r[1][1]*r[2][3]*r[3][2] - r[1][2]*r[2][1]*r[3][3] + r[1][1]*r[2][2]*r[3][3];
// r[0][1] = r[0][3]*r[2][2]*r[3][1] - r[0][2]*r[2][3]*r[3][1] - r[0][3]*r[2][1]*r[3][2] + r[0][1]*r[2][3]*r[3][2] + r[0][2]*r[2][1]*r[3][3] - r[0][1]*r[2][2]*r[3][3];
// r[0][2] = r[0][2]*r[1][3]*r[3][1] - r[0][3]*r[1][2]*r[3][1] + r[0][3]*r[1][1]*r[3][2] - r[0][1]*r[1][3]*r[3][2] - r[0][2]*r[1][1]*r[3][3] + r[0][1]*r[1][2]*r[3][3];
// r[0][3] = r[0][3]*r[1][2]*r[2][1] - r[0][2]*r[1][3]*r[2][1] - r[0][3]*r[1][1]*r[2][2] + r[0][1]*r[1][3]*r[2][2] + r[0][2]*r[1][1]*r[2][3] - r[0][1]*r[1][2]*r[2][3];
// r[1][0] = r[1][3]*r[2][2]*r[3][0] - r[1][2]*r[2][3]*r[3][0] - r[1][3]*r[2][0]*r[3][2] + r[1][0]*r[2][3]*r[3][2] + r[1][2]*r[2][0]*r[3][3] - r[1][0]*r[2][2]*r[3][3];
// r[1][1] = r[0][2]*r[2][3]*r[3][0] - r[0][3]*r[2][2]*r[3][0] + r[0][3]*r[2][0]*r[3][2] - r[0][0]*r[2][3]*r[3][2] - r[0][2]*r[2][0]*r[3][3] + r[0][0]*r[2][2]*r[3][3];
// r[1][2] = r[0][3]*r[1][2]*r[3][0] - r[0][2]*r[1][3]*r[3][0] - r[0][3]*r[1][0]*r[3][2] + r[0][0]*r[1][3]*r[3][2] + r[0][2]*r[1][0]*r[3][3] - r[0][0]*r[1][2]*r[3][3];
// r[1][3] = r[0][2]*r[1][3]*r[2][0] - r[0][3]*r[1][2]*r[2][0] + r[0][3]*r[1][0]*r[2][2] - r[0][0]*r[1][3]*r[2][2] - r[0][2]*r[1][0]*r[2][3] + r[0][0]*r[1][2]*r[2][3];
// r[2][0] = r[1][1]*r[2][3]*r[3][0] - r[1][3]*r[2][1]*r[3][0] + r[1][3]*r[2][0]*r[3][1] - r[1][0]*r[2][3]*r[3][1] - r[1][1]*r[2][0]*r[3][3] + r[1][0]*r[2][1]*r[3][3];
// r[2][1] = r[0][3]*r[2][1]*r[3][0] - r[0][1]*r[2][3]*r[3][0] - r[0][3]*r[2][0]*r[3][1] + r[0][0]*r[2][3]*r[3][1] + r[0][1]*r[2][0]*r[3][3] - r[0][0]*r[2][1]*r[3][3];
// r[2][2] = r[0][1]*r[1][3]*r[3][0] - r[0][3]*r[1][1]*r[3][0] + r[0][3]*r[1][0]*r[3][1] - r[0][0]*r[1][3]*r[3][1] - r[0][1]*r[1][0]*r[3][3] + r[0][0]*r[1][1]*r[3][3];
// r[2][3] = r[0][3]*r[1][1]*r[2][0] - r[0][1]*r[1][3]*r[2][0] - r[0][3]*r[1][0]*r[2][1] + r[0][0]*r[1][3]*r[2][1] + r[0][1]*r[1][0]*r[2][3] - r[0][0]*r[1][1]*r[2][3];
// r[3][0] = r[1][2]*r[2][1]*r[3][0] - r[1][1]*r[2][2]*r[3][0] - r[1][2]*r[2][0]*r[3][1] + r[1][0]*r[2][2]*r[3][1] + r[1][1]*r[2][0]*r[3][2] - r[1][0]*r[2][1]*r[3][2];
// r[3][1] = r[0][1]*r[2][2]*r[3][0] - r[0][2]*r[2][1]*r[3][0] + r[0][2]*r[2][0]*r[3][1] - r[0][0]*r[2][2]*r[3][1] - r[0][1]*r[2][0]*r[3][2] + r[0][0]*r[2][1]*r[3][2];
// r[3][2] = r[0][2]*r[1][1]*r[3][0] - r[0][1]*r[1][2]*r[3][0] - r[0][2]*r[1][0]*r[3][1] + r[0][0]*r[1][2]*r[3][1] + r[0][1]*r[1][0]*r[3][2] - r[0][0]*r[1][1]*r[3][2];
// r[3][3] = r[0][1]*r[1][2]*r[2][0] - r[0][2]*r[1][1]*r[2][0] + r[0][2]*r[1][0]*r[2][1] - r[0][0]*r[1][2]*r[2][1] - r[0][1]*r[1][0]*r[2][2] + r[0][0]*r[1][1]*r[2][2];
float det = Determinant();
if(det == 0.0f)
return false;
Matrix4x4 t = *this;
Cofactor(t);
Transpose();
Scale(1.0f/det);
return true;
}
//------------------------------------------------------------------------------
// Real Determinant() const
//------------------------------------------------------------------------------
Real Rmatrix66::Determinant() const
{
Real D;
if (rowsD == 1)
D = elementD[0];
else if (rowsD == 2)
D = elementD[0]*elementD[3] - elementD[1]*elementD[2];
else if (rowsD == 3)
{
D = elementD[0]*elementD[4]*elementD[8] +
elementD[1]*elementD[5]*elementD[6] +
elementD[2]*elementD[3]*elementD[7] -
elementD[0]*elementD[5]*elementD[7] -
elementD[1]*elementD[3]*elementD[8] -
elementD[2]*elementD[4]*elementD[6];
}
else
{
D = 0.0;
for (int i = 0; i < colsD; i++)
D += elementD[i]*Cofactor(0,i);
}
return D;
}
Matrix Matrix::Cofactor() const {
Matrix m(Row(), Column());
for (int i = 0; i < Row(); i++) {
for (int j = 0; j < Column(); j++) {
m(i, j) = Cofactor(i, j);
}
}
return m;
}
cMatrix cMatrix::Adjoint()
{
cMatrix matRet(Dimension());
for (int i = 0; i < Dimension(); ++i)
{
for (int j = 0; j < Dimension(); ++j)
{
matRet[i][j] = Cofactor(j, i);
}
}
return matRet;
}
Matrix Inverse(Matrix &M)
{
int dim = M.size();
Matrix CofactorMatrix(dim);
Matrix InverseMatrix(dim);
for(int l=0;l<dim;l++) CofactorMatrix[l].resize(dim);
for(int l=0;l<dim;l++) InverseMatrix[l].resize(dim);
for(int i = 0; i < dim; i++)
{
for(int m = 0; m < dim; m++)
{
Matrix Cofactor(dim-1);
for(int l=0;l<dim-1;l++) Cofactor[l].resize(dim-1);
int col=0, row=0;
for(int j = 0; j < dim; j++)
{
if(j != i)
{
row = 0;
for(int k = 0; k < dim; k++)
{
if (k != m)
{
Cofactor[row][col] = M[k][j];
row++;
}
}
col++;
}
}
CofactorMatrix[i][m] = ((i+m)%2==1?-1.0:1.0) * Det(Cofactor);
}
}
for(int i=0; i< dim; i++)
{
InverseMatrix[i] = (1.0/Det(M))*CofactorMatrix[i];
}
return InverseMatrix;
}
float cMatrix::Determinent()
{
if(Dimension() == 2)
{
return (*this)[0][0] * (*this)[1][1] -
(*this)[1][0] * (*this)[0][1];
}
float fDeterminent = 0.0f;
for (int i = 0; i < Dimension(); ++i)
{
fDeterminent += ((*this)[i][0] * Cofactor(i, 0));
}
return fDeterminent;
}
NMatrix<MType> Adjugate(const NMatrix<MType> pMatrix)
{
//Only width cause here is square matrices
unsigned int vWidth = pMatrix.Width();
NMatrix<MType> vMatrix(vWidth);
//We find, for each element, his cofactor to construct the Adjugate Matrix
for(unsigned int j = 0; j < vWidth; ++j)
{
for(unsigned int i = 0; i < vWidth; ++i)
{
vMatrix(i, j) = (((j * vWidth + i) % 2 == 0)?1:-1) * Cofactor(i, j, pMatrix);
}
}
return vMatrix;
}
//------------------------------------------------------------------------------
// virtual Real Determinant() const
//------------------------------------------------------------------------------
Real Rmatrix::Determinant() const
{
#ifdef DEBUG_DETERMINANT
MessageInterface::ShowMessage(wxT("Entering Determinant with rowsD = %d and colsD = %d\n"), rowsD, colsD);
#endif
if (isSizedD == false)
{
throw TableTemplateExceptions::UnsizedTable();
}
if (rowsD != colsD)
throw Rmatrix::NotSquare();
Real D;
if (rowsD == 1)
{
#ifdef DEBUG_DETERMINANT
MessageInterface::ShowMessage(wxT("Entering Determinant rowsD == 1 clause\n"));
#endif
D = elementD[0];
}
else if (rowsD == 2)
{
#ifdef DEBUG_DETERMINANT
MessageInterface::ShowMessage(wxT("Entering Determinant rowsD == 2 clause\n"));
#endif
D = elementD[0]*elementD[3] - elementD[1]*elementD[2];
}
else if (rowsD == 3)
{
#ifdef DEBUG_DETERMINANT
MessageInterface::ShowMessage(wxT("Entering Determinant rowsD == 3 clause\n"));
#endif
D = elementD[0]*elementD[4]*elementD[8] +
elementD[1]*elementD[5]*elementD[6] +
elementD[2]*elementD[3]*elementD[7] -
elementD[0]*elementD[5]*elementD[7] -
elementD[1]*elementD[3]*elementD[8] -
elementD[2]*elementD[4]*elementD[6];
}
else
{
// Currently limited by inefficiencies in the algorithm
if (rowsD > 9)
{
wxString errmsg = wxT("GMAT Determinant method not yet optimized. ");
errmsg += wxT("Currently limited to matrices of size 9x9 or smaller.");
throw UtilityException(errmsg);
}
#ifdef DEBUG_DETERMINANT
MessageInterface::ShowMessage(wxT("Entering Determinant else clause\n"));
#endif
D = 0.0;
int i;
for (i = 0; i < colsD; i++)
{
Real c = Cofactor(0,i);
#ifdef DEBUG_DETERMINANT
MessageInterface::ShowMessage(wxT("Cofactor(0,%d) = %12.10f\n"), (Integer) i, c);
MessageInterface::ShowMessage(wxT(" now multiplying by element[%d] (%12.10f) to get %12.10f\n"),
(Integer) i, elementD[i], (elementD[i] * c));
#endif
D += elementD[i] * c;
// D += elementD[i]*Cofactor(0,i);
}
#ifdef DEBUG_DETERMINANT
MessageInterface::ShowMessage(wxT("... at end of summation, D = %12.10f\n"), D);
#endif
}
return D;
}
