4x4の逆行列

invertmatrix-4x4.png
(1)
\begin{pmatrix} a&e&i&m\\ b&f&j&n\\ c&g&k&o\\ d&h&l&p\\ \end{pmatrix}

Step1.行列式detを求める

上の行列の行列式はこうなる

(2)
\begin{align} a \begin{vmatrix} f&j&n\\ g&k&o\\ h&l&p\\ \end{vmatrix} - e \begin{vmatrix} b&j&n\\ c&k&o\\ d&l&p\\ \end{vmatrix} +i \begin{vmatrix} b&f&n\\ c&g&o\\ d&h&p\\ \end{vmatrix} -m \begin{vmatrix} b&f&j\\ c&g&k\\ d&h&l\\ \end{vmatrix} \end{align}

サラスの公式をもとに計算するとこうなる

(3)
\begin{equation} a(fkp+joh+glh-nkh-olf-jgp)-e(bkp+jod+cln-nkd-olb-jcp)+i(bgp+fod+chn-ngd-fcp-bho)-m(bgl+fkd+chj-jgd-fcl-khb) \end{equation}

いざ、逆行列の公式

(4)
\begin{align} \frac{1}{det} \begin{pmatrix} \begin{vmatrix} f&j&n\\ g&k&o\\ h&l&p \end{vmatrix} & - \begin{vmatrix} e&i&m\\ g&k&o\\ h&l&p \end{vmatrix} & \begin{vmatrix} e&i&m\\ f&j&n\\ h&l&p \end{vmatrix} & - \begin{vmatrix} e&i&m\\ f&j&n\\ g&k&o \end{vmatrix} \\ - \begin{vmatrix} b&j&n\\ c&k&o\\ d&l&p \end{vmatrix} & \begin{vmatrix} a&i&m\\ c&k&o\\ d&l&p \end{vmatrix} & - \begin{vmatrix} a&i&m\\ b&j&n\\ d&l&p \end{vmatrix} & \begin{vmatrix} a&i&m\\ b&j&n\\ c&k&o \end{vmatrix} \\ \begin{vmatrix} b&f&n\\ c&g&o\\ d&h&p \end{vmatrix} & - \begin{vmatrix} a&e&m\\ c&g&o\\ d&h&p \end{vmatrix} & \begin{vmatrix} a&i&m\\ b&j&n\\ d&l&p \end{vmatrix} & - \begin{vmatrix} a&e&m\\ b&j&n\\ d&l&p \end{vmatrix} \\ - \begin{vmatrix} b&f&j\\ c&g&k\\ d&h&l \end{vmatrix} & \begin{vmatrix} a&e&i\\ c&g&k\\ d&h&l \end{vmatrix} & - \begin{vmatrix} a&e&i\\ b&f&j\\ d&h&l \end{vmatrix} & \begin{vmatrix} a&e&i\\ b&f&j\\ d&h&l \end{vmatrix} \end{pmatrix} \end{align}

ソースコード

Matrix4X4 Matrix4X4::getInverse() const
{
    double det;
    det = getDeterminant();//行列式を得る
 
    if(fabs(det) < _EPSILON){//行列式=0なら逆行列なし
        printf("逆行列はありません\n");
        Matrix4X4 emptyM;
        return emptyM;
    }
 
    return Matrix4X4(
        getSubDeterminant(0,0) / det,
        -getSubDeterminant(1,0) / det,
        getSubDeterminant(2,0) / det,
        -getSubDeterminant(3,0) / det,
 
        -getSubDeterminant(0,1) / det,
        getSubDeterminant(1,1) / det,
        -getSubDeterminant(2,1) / det,
        getSubDeterminant(3,1) / det,
 
        getSubDeterminant(0,2) / det,
        -getSubDeterminant(1,2) / det,
        getSubDeterminant(2,2) / det,
        -getSubDeterminant(3,2) / det,
 
        -getSubDeterminant(0,3) / det,
        getSubDeterminant(1,3) / det,
        -getSubDeterminant(2,3) / det,
        getSubDeterminant(3,3) / det
        );
}
 
double Matrix4X4::getDeterminant() const//行列式を得る
{
    return(
        value_[0][0] * getSubDeterminant(0,0)
        - value_[1][0] * getSubDeterminant(1,0)
        + value_[2][0] * getSubDeterminant(2,0)
        - value_[3][0] * getSubDeterminant(3,0)
        );
}
//小行列の行列式を得る
double Matrix4X4::getSubDeterminant(const unsigned short rowIndex_in,const unsigned short colIndex_in) const
{
    if( (rowIndex_in > 3) || (colIndex_in > 3) )
        throw;
 
    double subMatrix[3][3];
    int srcRow;
    int srcCol;
 
    srcRow = 0;
    for(int dstRow=0; dstRow<3; dstRow++)
    {
        if(dstRow == rowIndex_in)
            srcRow++;
 
        srcCol = 0;
        for(int dstCol=0; dstCol<3; dstCol++)
        {
            if(dstCol == colIndex_in)
                srcCol++;
 
            subMatrix[dstRow][dstCol] = value_[srcRow][srcCol];
 
            srcCol++;
        }
 
        srcRow++;
    }
 
    double dSubDet = 
        subMatrix[0][0] * subMatrix[1][1] * subMatrix[2][2]
      + subMatrix[1][0] * subMatrix[2][1] * subMatrix[0][2]
      + subMatrix[0][1] * subMatrix[1][2] * subMatrix[2][0]
      -    subMatrix[2][0] * subMatrix[1][1] * subMatrix[0][2]
      -    subMatrix[2][1] * subMatrix[1][2] * subMatrix[0][0]
      -    subMatrix[1][0] * subMatrix[0][1] * subMatrix[2][2];
 
    return dSubDet;
}

サポートサイト Wikidot.com