LoginSignup
1
0

More than 1 year has passed since last update.

@nomaton

【Rust】4次行列の逆行列計算の色々な実装

Posted at

大きい行列の逆行列計算はLU分解などの手法が一般的だが、3DCGで使用する4次程度の行列であれば行列式と余因子を力技で計算する方が単純で早いと思っている。

最近Rustで4次の逆行列計算を懲りずにまた実装したので、将来のコピペのために久しぶりの投稿。

行列の表現

この記事では、行列成分を長さ16の配列に列優先（Column-major）で保持する実装を想定する。
（逆行列の計算に限れば行優先（Row-major）でも大して変わらないはず）

単純化のため型パラメータは使用せず、数値の型はf32で固定する。

// Column-major:
// [a11, a21, a31, a41, a12, a22, a32, a42, a13, a23, a33, a43, a14, a24, a34, a44]
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Matrix4([f32; 16]);

\begin{bmatrix}
a_{11} & a_{12} & a_{13} & a_{14} \\
a_{21} & a_{22} & a_{23} & a_{24} \\
a_{31} & a_{32} & a_{33} & a_{34} \\
a_{41} & a_{42} & a_{43} & a_{44}
\end{bmatrix} \longleftrightarrow \begin{bmatrix}
\_[0] & \_[4] & \_[8] & \_[12] \\
\_[1] & \_[5] & \_[9] & \_[13] \\
\_[2] & \_[6] & \_[10] & \_[14] \\
\_[3] & \_[7] & \_[11] & \_[15]
\end{bmatrix}

実装例

以下の３パターンを掲載（名前は適当）

  1. 直接計算（invert_direct）
  2. 間接計算（invert_indirect）
  3. アフィン変換特化（invert_affine）

いずれも自分自身を逆行列に変換する関数で、逆行列が存在しない（正則でない）場合はすべての要素をNaNにする。

実装１：直接計算

行列式と余因子を普通に計算する実装（なので特に言うことはない）。
分かりやすさ重視で変数は多め。

普通に書くと絶対に間違えるので、ややこしい箇所はスクリプトで生成した。

impl Matrix4 {
    #[rustfmt::skip]
    pub fn invert_direct(&mut self) {
        let [a11, a21, a31, a41, a12, a22, a32, a42, a13, a23, a33, a43, a14, a24, a34, a44] = self.0;

        // 行列式の計算
        let det =
              a11 * a22 * a33 * a44 - a11 * a22 * a34 * a43 - a12 * a21 * a33 * a44 + a12 * a21 * a34 * a43
            + a12 * a23 * a31 * a44 - a12 * a23 * a34 * a41 - a13 * a22 * a31 * a44 + a13 * a22 * a34 * a41
            + a13 * a24 * a31 * a42 - a13 * a24 * a32 * a41 - a14 * a23 * a31 * a42 + a14 * a23 * a32 * a41
            + a11 * a24 * a32 * a43 - a11 * a24 * a33 * a42 - a14 * a21 * a32 * a43 + a14 * a21 * a33 * a42
            - a11 * a23 * a32 * a44 + a11 * a23 * a34 * a42 + a13 * a21 * a32 * a44 - a13 * a21 * a34 * a42
            - a12 * a24 * a31 * a43 + a12 * a24 * a33 * a41 + a14 * a22 * a31 * a43 - a14 * a22 * a33 * a41
            ;

        if det == 0.0 {
            // 正則でない場合
            self.0.fill(f32::NAN);
            return;
        }
    
        // 逆行列の成分の計算（余因子 / 行列式）
        let b11 = (a22 * a33 * a44 + a23 * a34 * a42 + a24 * a32 * a43 
                 - a22 * a34 * a43 - a23 * a32 * a44 - a24 * a33 * a42) / det;
        let b21 = (a21 * a34 * a43 + a23 * a31 * a44 + a24 * a33 * a41
                 - a21 * a33 * a44 - a23 * a34 * a41 - a24 * a31 * a43) / det;
        let b31 = (a21 * a32 * a44 + a22 * a34 * a41 + a24 * a31 * a42
                 - a21 * a34 * a42 - a22 * a31 * a44 - a24 * a32 * a41) / det;
        let b41 = (a21 * a33 * a42 + a22 * a31 * a43 + a23 * a32 * a41
                 - a21 * a32 * a43 - a22 * a33 * a41 - a23 * a31 * a42) / det;
        let b12 = (a12 * a34 * a43 + a13 * a32 * a44 + a14 * a33 * a42
                 - a12 * a33 * a44 - a13 * a34 * a42 - a14 * a32 * a43) / det;
        let b22 = (a11 * a33 * a44 + a13 * a34 * a41 + a14 * a31 * a43
                 - a11 * a34 * a43 - a13 * a31 * a44 - a14 * a33 * a41) / det;
        let b32 = (a11 * a34 * a42 + a12 * a31 * a44 + a14 * a32 * a41
                 - a11 * a32 * a44 - a12 * a34 * a41 - a14 * a31 * a42) / det;
        let b42 = (a11 * a32 * a43 + a12 * a33 * a41 + a13 * a31 * a42
                 - a11 * a33 * a42 - a12 * a31 * a43 - a13 * a32 * a41) / det;
        let b13 = (a13 * a24 * a42 + a14 * a22 * a43 + a12 * a23 * a44
                 - a14 * a23 * a42 - a12 * a24 * a43 - a13 * a22 * a44) / det;
        let b23 = (a14 * a23 * a41 + a11 * a24 * a43 + a13 * a21 * a44 
                 - a13 * a24 * a41 - a14 * a21 * a43 - a11 * a23 * a44) / det;
        let b33 = (a12 * a24 * a41 + a14 * a21 * a42 + a11 * a22 * a44
                 - a14 * a22 * a41 - a11 * a24 * a42 - a12 * a21 * a44) / det;
        let b43 = (a13 * a22 * a41 + a11 * a23 * a42 + a12 * a21 * a43 
                 - a12 * a23 * a41 - a13 * a21 * a42 - a11 * a22 * a43) / det;
        let b14 = (a14 * a23 * a32 + a12 * a24 * a33 + a13 * a22 * a34 
                 - a13 * a24 * a32 - a14 * a22 * a33 - a12 * a23 * a34) / det;
        let b24 = (a13 * a24 * a31 + a14 * a21 * a33 + a11 * a23 * a34
                 - a14 * a23 * a31 - a11 * a24 * a33 - a13 * a21 * a34) / det;
        let b34 = (a14 * a22 * a31 + a11 * a24 * a32 + a12 * a21 * a34
                 - a12 * a24 * a31 - a14 * a21 * a32 - a11 * a22 * a34) / det;
        let b44 = (a12 * a23 * a31 + a13 * a21 * a32 + a11 * a22 * a33
                 - a13 * a22 * a31 - a11 * a23 * a32 - a12 * a21 * a33) / det;
        
        self.0 = [b11, b21, b31, b41, b12, b22, b32, b42, b13, b23, b33, b43, b14, b24, b34, b44];
    }
}

実装２：間接計算

4×4行列を上下2行ずつに分割し、それぞれで作成可能な2×2行列の行列式をすべて計算した後に、その結果を使って全体の行列式と余因子を計算する方法。
何かのOSSライブラリで見つけて感心した記憶がある。

途中式を代入して展開すると数学的には実装１と同じ計算になる。

impl Matrix4 {
    #[rustfmt::skip]
    pub fn invert_indirect(&mut self) {
        let [a11, a21, a31, a41, a12, a22, a32, a42, a13, a23, a33, a43, a14, a24, a34, a44] = self.0;

        // 上２行の任意の２列（６通り）で作る２次行列の行列式の計算
        // （uの添字は２つの列を表す）
        let u12 = a11 * a22 - a12 * a21;
        let u13 = a11 * a23 - a13 * a21;
        let u14 = a11 * a24 - a14 * a21;
        let u23 = a12 * a23 - a13 * a22;
        let u24 = a12 * a24 - a14 * a22;
        let u34 = a13 * a24 - a14 * a23;
        // 下２行の任意の２列（６通り）で作る２次行列の行列式の計算
        // （vの添字は２つの列を表す）
        let v12 = a31 * a42 - a32 * a41;
        let v13 = a31 * a43 - a33 * a41;
        let v14 = a31 * a44 - a34 * a41;
        let v23 = a32 * a43 - a33 * a42;
        let v24 = a32 * a44 - a34 * a42;
        let v34 = a33 * a44 - a34 * a43;

        // 行列式の計算
        // （列を共有しないuとvの積の和、ただし、1-3列・2-4列の積は符号を反転）
        let det = u12 * v34 + u23 * v14 + u34 * v12 + u14 * v23 - u13 * v24 - u24 * v13;

        if det == 0.0 {
            // 正則でない場合
            self.0.fill(f32::NAN);
            return;
        }

        // 逆行列の成分の計算（余因子 / 行列式）
        // （左２列（転置前の上２行）はvを使用）
        let b11 = (0.0 + a22 * v34 - a23 * v24 + a24 * v23) / det;
        let b21 = (0.0 - a21 * v34 + a23 * v14 - a24 * v13) / det;
        let b31 = (0.0 + a21 * v24 - a22 * v14 + a24 * v12) / det;
        let b41 = (0.0 - a21 * v23 + a22 * v13 - a23 * v12) / det;
        let b12 = (0.0 - a12 * v34 + a13 * v24 - a14 * v23) / det;
        let b22 = (0.0 + a11 * v34 - a13 * v14 + a14 * v13) / det;
        let b32 = (0.0 - a11 * v24 + a12 * v14 - a14 * v12) / det;
        let b42 = (0.0 + a11 * v23 - a12 * v13 + a13 * v12) / det;
        // （右２列（転置前の下２行）はuを使用）
        let b13 = (0.0 + a42 * u34 - a43 * u24 + a44 * u23) / det;
        let b23 = (0.0 - a41 * u34 + a43 * u14 - a44 * u13) / det;
        let b33 = (0.0 + a41 * u24 - a42 * u14 + a44 * u12) / det;
        let b43 = (0.0 - a41 * u23 + a42 * u13 - a43 * u12) / det;
        let b14 = (0.0 - a32 * u34 + a33 * u24 - a34 * u23) / det;
        let b24 = (0.0 + a31 * u34 - a33 * u14 + a34 * u13) / det;
        let b34 = (0.0 - a31 * u24 + a32 * u14 - a34 * u12) / det;
        let b44 = (0.0 + a31 * u23 - a32 * u13 + a33 * u12) / det;

        self.0 = [b11, b21, b31, b41, b12, b22, b32, b42, b13, b23, b33, b43, b14, b24, b34, b44];
    }
}

実装３：アフィン変換特化

3DCGで使用される変換行列の多くは最後の行（または列）が[0, 0, 0, 1]となるため、ブロック行列に対する逆行列の簡易形を利用できる。

\begin{bmatrix}
A & B \\
O & E
\end{bmatrix} ^{-1} = \begin{bmatrix}
A^{-1} & -A^{-1} \times B \\
O & E
\end{bmatrix}

この実装は最後の行が[0, 0, 0, 1]である行列にしか使えないが、

\begin{bmatrix}
* & * & * & * \\
* & * & * & * \\
* & * & * & * \\
0 & 0 & 0 & 1
\end{bmatrix} \times \begin{bmatrix}
* & * & * & * \\
* & * & * & * \\
* & * & * & * \\
0 & 0 & 0 & 1
\end{bmatrix} \rightarrow \begin{bmatrix}
* & * & * & * \\
* & * & * & * \\
* & * & * & * \\
0 & 0 & 0 & 1
\end{bmatrix}

\begin{bmatrix}
* & * & * & * \\
* & * & * & * \\
* & * & * & * \\
0 & 0 & 0 & 1
\end{bmatrix} ^{-1} \rightarrow \begin{bmatrix}
* & * & * & * \\
* & * & * & * \\
* & * & * & * \\
0 & 0 & 0 & 1
\end{bmatrix} （正則の場合）

によっていわゆる群（アフィン変換群）が形成されるため、アフィン変換で閉じた系であれば常に有効。場合によっては最後の行を定数として最初から配列に保持しない実装も考えられる。

3DCGにおいては、平行移動、回転、拡大・縮小の変換行列とそれらの積がすべて対象となる（投影行列の一部はパターンから外れるが、投影変換の逆を適用する機会は多分ないと思う）。

計算式は実装１の変数の一部を定数（0または1）にして整理した形になる。

impl Matrix4 {
    #[rustfmt::skip]
    pub fn invert_affine(&mut self) {
        let [a11, a21, a31, a41, a12, a22, a32, a42, a13, a23, a33, a43, a14, a24, a34, a44] = self.0;

        // アフィン変換行列のみ有効
        assert!(a41 == 0.0 && a42 == 0.0 && a43 == 0.0 && a44 == 1.0);

        // 行列式の計算
        let det =
              a11 * a22 * a33 + a12 * a23 * a31 + a13 * a21 * a32
            - a11 * a23 * a32 - a12 * a21 * a33 - a13 * a22 * a31;

        if det == 0.0 {
            // 正則でない場合
            self.0.fill(f32::NAN);
            return;
        }
    
        // 逆行列の成分の計算
        let b11 = (a22 * a33 - a23 * a32) / det;
        let b21 = (a23 * a31 - a21 * a33) / det;
        let b31 = (a21 * a32 - a22 * a31) / det;
        let b41 = 0.0;
        let b12 = (a32 * a13 - a33 * a12) / det;
        let b22 = (a33 * a11 - a31 * a13) / det;
        let b32 = (a31 * a12 - a32 * a11) / det;
        let b42 = 0.0;
        let b13 = (a12 * a23 - a13 * a22) / det;
        let b23 = (a13 * a21 - a11 * a23) / det;
        let b33 = (a11 * a22 - a12 * a21) / det;
        let b43 = 0.0;
        let b14 = -(b11 * a14 + b12 * a24 + b13 * a34);
        let b24 = -(b21 * a14 + b22 * a24 + b23 * a34);
        let b34 = -(b31 * a14 + b32 * a24 + b33 * a34);
        let b44 = 1.0;
        
        self.0 = [b11, b21, b31, b41, b12, b22, b32, b42, b13, b23, b33, b43, b14, b24, b34, b44];
    }
}

比較

実装１は冗長なコードだが、加算・減算の前に必要な乗算を終わらせるため浮動小数点演算に起因する誤差は小さくなる（はず）。実装したくないことを除けばおそらく最も無難な実装。

実装２は途中の計算結果を再利用するため乗算の回数を半分程度に減らすことができる。計算量を抑えたい場合に有効。

実装３は用途が限定されるが、4次行列の本場である3DCGと相性がよく処理もコンパクトなので知っておいて損はない。

おまけ（テスト用コード）

浮動小数点数の比較は誤差を考慮する必要があるが、今回はきりのいい数値を使ってごまかした。
まじめにテストしてないからどこかにバグがあるかもしれない。

impl Matrix4 {
    #[rustfmt::skip]
    fn identity() -> Self {
        Self ([
                1.0, 0.0, 0.0, 0.0,
                0.0, 1.0, 0.0, 0.0,
                0.0, 0.0, 1.0, 0.0,
                0.0, 0.0, 0.0, 1.0,
            ])
    }
}

impl From<[f32; 16]> for Matrix4 {
    fn from(value: [f32; 16]) -> Self {
        Self(value)
    }
}

impl std::ops::Mul for Matrix4 {
    type Output = Self;

    fn mul(self, rhs: Self) -> Self::Output {
        const N: usize = 4;

        let lhs = self.0;
        let rhs = rhs.0;
        let mut a = [0.0; N * N];

        for i in 0..N {
            for j in 0..N {
                a[i + j * N] = (0..N).map(|k| lhs[i + k * N] * rhs[k + j * N]).sum();
            }
        }
        Self(a)
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    
    #[test]
    fn test_direct() {
        let m = Matrix4::from([
            2.0, 1.0, 0.0, 0.0,
            0.0, 1.0, 1.0, 0.0,
            2.0, 0.0, 1.0, 2.0,
            2.0, 1.0, 0.0, 2.0
        ]);
    
        let m_inv = Matrix4::from([
            0.5, -0.25, 0.25, -0.25,
            0.0, 0.5, -0.5, 0.5,
            0.0, 0.5, 0.5, -0.5,
            -0.5, 0.0, 0.0, 0.5
        ]);
    
        let mut n = m;
        n.invert_direct();
    
        assert_eq!(n, m_inv);
        assert_eq!(m * n, Matrix4::identity());
    }
    
    #[test]
    fn test_indirect() {
        let m = Matrix4::from([
            2.0, 1.0, 0.0, 0.0,
            0.0, 1.0, 1.0, 0.0,
            2.0, 0.0, 1.0, 2.0,
            2.0, 1.0, 0.0, 2.0
        ]);
    
        let m_inv = Matrix4::from([
            0.5, -0.25, 0.25, -0.25,
            0.0, 0.5, -0.5, 0.5,
            0.0, 0.5, 0.5, -0.5,
            -0.5, 0.0, 0.0, 0.5
        ]);
    
        let mut n = m;
        n.invert_indirect();
    
        assert_eq!(n, m_inv);
        assert_eq!(m * n, Matrix4::identity());
    }
    
    #[test]
    fn test_affine() {
        let m = Matrix4::from([
            2.0, 0.0, 2.0, 0.0,
            1.0, 1.0, 0.0, 0.0,
            0.0, 1.0, 1.0, 0.0,
            2.0, 0.0, 2.0, 1.0
        ]);
    
        let m_inv = Matrix4::from([
            0.25, 0.5, -0.5, 0.0,
            -0.25, 0.5, 0.5, 0.0,
            0.25, -0.5, 0.5, 0.0,
            -1.0, 0.0, 0.0, 1.0
        ]);
    
        let mut n = m;
        n.invert_affine();
    
        assert_eq!(n, m_inv);
        assert_eq!(m * n, Matrix4::identity());
    }
}
1
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
1
0