Help us understand the problem. What is going on with this article?

回転行列を回転軸と回転角に分離する

回転行列の定義

元になる回転行列の式はglRotateのものを使用する。

$ man glRotate
...
DESCRIPTION
       glRotate  produces a rotation of angle degrees around the vector (x, y, z).  The current matrix (see glMatrixMode) is multiplied by a rotation matrix with the product replacing
       the current matrix, as if glMultMatrix were called with the following matrix as its argument:

         x^2(1-c)+c     xy(1-c)-zs     xz(1-c)+ys     0
         yx(1-c)+zs     y^2(1-c)+c     yz(1-c)-xs     0
         xz(1-c)-ys     yz(1-c)+xs     z^2(1-c)+c     0
              0              0               0        1

       Where c = cos (angle), s = sin (angle), and ||(x, y, z)|| = 1 (if not, the GL will normalize this vector).
...

純粋な回転行列のみを扱うので、回転軸 $(x, y, z)$、回転角$\theta$に対し3x3行列で以下のように定義する。

M = \left(\begin{matrix}
m_{00} & m_{01} & m_{02} \\
m_{10} & m_{11} & m_{12} \\
m_{20} & m_{21} & m_{22}
\end{matrix}\right) 
= \left(\begin{matrix}
x^2(1-\cos(\theta))+\cos(\theta) & xy(1-\cos(\theta))-z\sin(\theta) & xz(1-\cos(\theta))+y\sin(\theta) \\
yx(1-\cos(\theta))+z\sin(\theta) & y^2(1-\cos(\theta))+\cos(\theta) & yz(1-\cos(\theta))-x\sin(\theta) \\
xz(1-\cos(\theta))-y\sin(\theta) & yz(1-\cos(\theta))+x\sin(\theta) & z^2(1-\cos(\theta))+\cos(\theta)
\end{matrix}\right)

ここでは回転行列から回転軸を求めるので、 $||(x, y, z)|| = 1$ は前提となる。

以下はSwiftによる回転軸および回転行列の定義である。

import Foundation

struct Vector3 {
    var x, y, z: Float

    func normalized() -> Vector3 {
        let norm = sqrt(x*x + y*y + z*z)
        guard norm > 0 else {
            return self
        }
        return Vector3(x: x/norm, y: y/norm, z: z/norm)
    }
}

struct RotationMatrix {
    let m00, m01, m02, m10, m11, m12, m20, m21, m22: Float

    // glRotateの定義に従った実装
    init(axis: Vector3, angle: Float) {
        let s = sin(angle)
        let c = cos(angle)
        let oneMinusC = 1 - c

        // 正規化
        let normalized = axis.normalized()
        let (x, y, z) = (normalized.x, normalized.y, normalized.z)

        m00 = x*x*oneMinusC + c
        m01 = x*y*oneMinusC - z*s
        m02 = x*z*oneMinusC + y*s

        m10 = x*y*oneMinusC + z*s
        m11 = y*y*oneMinusC + c
        m12 = y*z*oneMinusC - x*s

        m20 = x*z*oneMinusC - y*s
        m21 = y*z*oneMinusC + x*s
        m22 = z*z*oneMinusC + c
    }
}

クォータニオンへの変換

回転軸を求めるには、一旦クォータニオンに変換すると良い。
回転軸 $(x, y, z)$、回転角$\theta$に対するクォータニオンはQuaternion(w: cos(θ/2), x: x*sin(θ/2), y: y*sin(θ/2), z: z*sin(θ/2))であるが、行列から求めるためには工夫が要る。

https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/

上ページのコードをSwiftで再実装したら以下のようになる。

struct Quaternion {
    var w, x, y, z: Float

    init(w: Float, x: Float, y: Float, z: Float) {
        self.w = w
        self.x = x
        self.y = y
        self.z = z
    }

    init(matrix: RotationMatrix) {
        let (m00, m01, m02) = (matrix.m00, matrix.m01, matrix.m02)
        let (m10, m11, m12) = (matrix.m10, matrix.m11, matrix.m12)
        let (m20, m21, m22) = (matrix.m20, matrix.m21, matrix.m22)
        let tr = m00 + m11 + m22

        let w, x, y, z: Float
        if tr > 0 {
            let S = sqrt(tr + 1) * 2 // S=4*qw
            w = 0.25 * S
            x = (m21 - m12) / S
            y = (m02 - m20) / S
            z = (m10 - m01) / S
        } else if m00 > m11 && m00 > m22 {
            let S = sqrt(1.0 + m00 - m11 - m22) * 2 // S=4*qx
            w = (m21 - m12) / S
            x = 0.25 * S
            y = (m01 + m10) / S
            z = (m02 + m20) / S
        } else if m11 > m22 {
            let S = sqrt(1.0 + m11 - m00 - m22) * 2 // S=4*qy
            w = (m02 - m20) / S
            x = (m01 + m10) / S
            y = 0.25 * S
            z = (m12 + m21) / S
        } else {
            let S = sqrt(1.0 + m22 - m00 - m11) * 2 // S=4*qz
            w = (m10 - m01) / S
            x = (m02 + m20) / S
            y = (m12 + m21) / S
            z = 0.25 * S
        }

        self.init(w: w, x: x, y: y, z: z)
    }
}

上コードでは4つの分岐それぞれで行列成分からクォータニオンの成分を求めているが、それぞれがどのような数式でクォータニオンへの変換になっているのか見ていく。

以下ではクォータニオンの成分を$q_x, q_y, q_z, q_w$と書く。

まずtr > 0の場合を見てみる。
冒頭でS = sqrt(tr + 1) * 2 = 4*qwとなっている。

tr + 1 = m_{00} + m_{11} + m_{22} + 1 = (x^2 + y^2 + z^2)(1-\cos(\theta)) + 3\cos(\theta) + 1

$||(x, y, z)|| = 1$ から $x^2 + y^2 + z^2 = 1$ なので、

(x^2 + y^2 + z^2)(1-\cos(\theta)) + 3\cos(\theta) + 1 = 2\cos(\theta) + 2

倍角の公式$\cos(\theta) = 2\cos^2\left(\frac{\theta}{2}\right) - 1$ を用いて、

2\cos(\theta) + 2 = 4\cos^2\left(\frac{\theta}{2}\right)

よってsqrt(tr + 1) * 2 は $4\cos\left(\frac{\theta}{2}\right)$ であり、$q_w = \cos\left(\frac{\theta}{2}\right)$の4倍である。

$q_x$はm21 - m12 / Sとなっているが、行列成分を代入すると $2x\sin(\theta) / 4\cos\left(\frac{\theta}{2}\right)$である。$\sin(\theta) = 2\sin\left(\frac{\theta}{2}\right)\cos\left(\frac{\theta}{2}\right)$を代入すると$x\sin\left(\frac{\theta}{2}\right) = q_x$が得られる。$q_y, q_z$についても同様に得られる。

残り3つの分岐は対角要素のうち最大のものを基準とし、それをもとに各要素を求めるようになっている。
$m_{00}, m_{11}, m_{22}$はそれぞれ$x^2(1-\cos(\theta)) + \cos(\theta), y^2(1-\cos(\theta)) + \cos(\theta), z^2(1-\cos(\theta)) + \cos(\theta)$なので、これらの比較は$x, y, z$の絶対値の大小関係を見ていることになる。$||(x, y, z)|| = 1$なので絶対値最大の要素を取ればそれは必ず非ゼロであり、その絶対値最大の要素を基準に計算をすることでゼロ除算が出現しなくなるというロジックになっている。

代表して$m_{00}$が最大であるm00 > m11 && m00 > m22の分岐の中を見てみる。
S = sqrt(1.0 + m00 - m11 - m22) * 2 = 4*qx となっているが、こちらも行列成分を代入してみる。

1 + m_{00} - m_{11} - m_{22} = 1 + (x^2 - y^2 - z^2)(1- \cos(\theta)) - \cos(\theta)

ここでも$x^2 + y^2 + z^2 = 1$を用いて、

1 + (x^2 - y^2 - z^2)(1- \cos(\theta)) - \cos(\theta) = 1 + (2x^2 - 1)(1-\cos(\theta)) - \cos(\theta) = 2x^2(1-\cos(\theta))

$\cos(\theta) = 1 - 2\sin^2\left(\frac{\theta}{2}\right)$を代入して

2x^2(1-\cos(\theta)) = 4x^2\sin^2\left(\frac{\theta}{2}\right)

よってsqrt(1.0 + m00 - m11 - m22) * 2 = 4*qxである。

$q_w$は(m21 - m12) / Sとなっているが、これは$2x\sin(\theta) / 4x\sin\left(\frac{\theta}{2}\right)$で、$\sin(\theta) = 2\sin\left(\frac{\theta}{2}\right)\cos\left(\frac{\theta}{2}\right)$から$\cos\left(\frac{\theta}{2}\right)$であることが分かる。

$q_y$は(m01 + m10) / Sだが、これは$2xy(1-\cos(\theta))/ 4x\sin\left(\frac{\theta}{2}\right)$であり、$\cos(\theta) = 1 - 2\sin^2\left(\frac{\theta}{2}\right)$を用いて$y\sin\left(\frac{\theta}{2}\right)$であることが分かる。$q_z$も同様に求められる。

あとの2つの分岐は$m_{00}$が最大のパターンと記号が入れ替わっただけで同じような手順で計算できる。

これら4つの分岐だが、ゼロ除算にならない限り、入力に関わらずどれも常に成り立っている。計算過程でゼロ除算や小さな値での除算による計算誤差を避けるためにこのような分岐になっているようだ。
ちなみに最初の条件であるtr > 0は$tr = 2\cos(\theta)+1$なので、$\cos(\theta) > -\frac{1}{2}$、即ち$0 \le \theta \lt \frac{2}{3}\pi$の場合という条件になっている。この条件はsqrt(tr + 1)で負数のルートをとったり後にゼロ除算が出てきたりしないように設けられているはずだが、実際にはかなり余裕を持った値になっている(計算誤差を考慮しなければ$tr > -1$でも問題ないはずである)。この条件の妥当性についてはいまいち分かっていない。

クォータニオンから回転角/回転軸の取り出し

クォータニオンに変換できたら、そこから回転軸と回転角を取り出せる。
https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/

こちらはSwiftでは以下のようになる。

extension Quaternion {
    func getAxisAndAngle() -> (axis: Vector3, angle: Float) {
        var q = self
        if q.w > 1 {
            // if w>1 acos and sqrt will produce errors, this cant happen if quaternion is normalised
            q.x /= q.w
            q.y /= q.w
            q.z /= q.w
            q.w = 1
        }

        let angle = 2 * acos(q.w)

        let s = sqrt(1 - q.w*q.w)

        let x, y, z: Float
        if s < 0.001 {
            // if s close to zero then direction of axis not important
            x = q.x
            y = q.y
            z = q.z
        } else {
            // normalise axis
            x = q.x / s
            y = q.y / s
            z = q.z / s
        }

        return (axis, angle)
    }
}

回転角は$q_w = \cos\left(\frac{\theta}{2}\right)$から、$2\arccos(q_w)$で求められる。

s = sqrt(1 - q.w*q.w)だが、$q_w = \cos\left(\frac{\theta}{2}\right)$なのでこれは$\sin\left(\frac{\theta}{2}\right)$である。
回転角がある程度大きい場合(ここではs >= 0.001である場合)、$q_x, q_y, q_z$を$\sin\left(\frac{\theta}{2}\right)$で割るだけで回転軸が得られる。
回転角が小さい場合(ここではs < 0.001である場合)、除算は計算誤差が大きく、またゼロ除算になる可能性もある。しかし回転角が十分0に近いならほぼ無回転なので、どのような軸で回しても大差ない。ここではオリジナルのコードにしたがって正規化しない状態で返しているが、$(0, 0, 0)$が出てくることもあるので、固定値$(1, 0, 0)$等を返すようにしても良い。

以上で回転角と回転軸が得られた。

クォータニオンを経由しない方法の検討

クォータニオンやそれによる半角を用いずに計算する方法を考案したので掲載する。
ただしこの方法は計算誤差/パフォーマンス共にクォータニオンを経由する場合と比べて不利であり、実用上ではクォータニオン経由のものを使うのが良い。

以下ではglRotateの定義に従って回転角$angle$、$c = \cos(\theta), s = \sin(\theta)$としている。
また回転角の範囲は$0 \le angle \le \pi$とする。その周期性から回転角は$(-\pi, \pi]$の範囲に収まる。さらに軸中心の回転では回転軸の反転によって反対方向の回転を表すことができる。よって$[0, \pi]$の範囲を考えればすべての回転を考慮できることになる。

まず $c$ を求める。

\frac{m_{00} + m_{11} + m_{22} - 1}{ 2 } = \frac{(x^2 + y^2 + z^2)(1-c) + 3c - 1}{2}

ここで $||(x, y, z)|| = 1$ から $x^2 + y^2 + z^2 = 1$ であることを用いて、

\frac{(x^2 + y^2 + z^2)(1-c) + 3c - 1}{2} = \frac{1 - c + 3c - 1 }{ 2 } = c

$angle \in [0, \pi]$ の範囲で求めるので $angle = \arccos(c)$ となる。

$m_{00} = x^2(1-c) + c$ と $c$ を用いて $x^2$ が求められる。 $y^2, z^2$ も同様である。

x^2 = \frac{m_{00} - c}{1-c} \\
y^2 = \frac{m_{11} - c}{1-c} \\
z^2 = \frac{m_{22} - c}{1-c}

$x, y, z$ の絶対値はこれらの平方根で得られる。 $1-c = 0$ で0除算となる可能性があるが、その場合回転角は0なので、回転軸は任意の単位ベクトルを選んで良い。以降では $angle \gt 0$ の場合のみを扱う。
$x, y, z$ の符号はそれぞれ+か-なので、この時点で回転軸の候補は8通りある。他の要素から符号を求め、ここで得られた絶対値と合わせて回転軸を得る。

行列の $m_{01}$ と $m_{10}$ から $xy$ を求めることができる。 $yz, zx$ も同様に2成分から求められる。

xy = \frac{ m_{01} + m_{10} }{ 2(1-c) } \\
yz = \frac{ m_{12} + m_{21} }{ 2(1-c) } \\
zx = \frac{ m_{20} + m_{02} }{ 2(1-c) }

$xy, yz, zx$ の符号は $x, y, z$ 成分から2つを取り出したとき、それらの符号が一致しているか異なっているかを表している。
$xy$ を例に考えると、 $xy>0$ なら $\mathrm{sgn}(x) * \mathrm{sgn}(y) = 1$ で同じ符号、 $xy<0$なら$\mathrm{sgn}(x) * \mathrm{sgn}(y) = -1$ で異なる符号である。
例外として $xy=0$ の場合があるが、これは $x, y$ の少なくとも片方が0であることを示している。この場合 $x, y$ の絶対値が大きい方の符号を既定として、もう片方の0については便宜的に同じ符号であると考えることにする。
以上をまとめると次のようになる。

xy \ge 0 \longrightarrow xとyの符号は同じ \\
xy \lt 0 \longrightarrow xとyの符号は異なる

$yz, zx$ についても同様の関係がある。

$x, y, z$ それぞれの間の符号の関係がわかったため、一つが決定すると他の二つも決定する。この時点で8通りあった回転軸の候補は2通りまで絞り込まれ、さらにその2つは符号反転の関係にある。

次に $m_{21}$ と $m_{12}$ から $xs$ を求める。 $ys, zs$ についても同様に2成分から求められる。

xs = \frac{ m_{21} - m_{12} }{ 2 } \\
ys = \frac{ m_{02} - m_{20} }{ 2 } \\
zs = \frac{ m_{10} - m_{01} }{ 2 }

$0 \lt angle \lt \pi$ の場合 $s > 0$ であり、 $xs, ys, zs$ の符号はそのまま $x, y, z$ の符号となる。 $||(x, y, z)|| = 1$ より $x, y, z$ のうち少なくとも1つは非ゼロなので、非ゼロ要素を取り出し、その符号を起点に先程求めた成分間の符号の関係から他2つの符号を決定できる。

$angle \in { 0, \pi }$ である場合、 $s = 0$ となるので $xs = ys = zs = 0$ である。この場合 $xs, ys, zs$ どの符号も取得できないが、この回転角の場合に限り $axis$ 周り $angle$ 回転と、 $-axis$ 周り $angle$ 回転は一致する(ただし $angle=0$ の場合は既に任意軸として排除されている)。現在回転軸の候補は符号反転した2通りだけなので、この場合どちらを解としても正しいことになる。

Swiftでの実装

import Foundation

// 型の定義
struct Vector3 {
    var x, y, z: Float

    func normalized() -> Vector3 {
        let norm = sqrt(x*x + y*y + z*z)
        guard norm > 0 else {
            return self
        }
        return Vector3(x: x/norm, y: y/norm, z: z/norm)
    }
}

struct RotationMatrix {
    var m00, m01, m02, m10, m11, m12, m20, m21, m22: Float

    // glRotateの定義に従った実装
    init(axis: Vector3, angle: Float) {
        let s = sin(angle)
        let c = cos(angle)
        let oneMinusC = 1 - c

        // 正規化
        let normalized = axis.normalized()
        let (x, y, z) = (normalized.x, normalized.y, normalized.z)

        m00 = x*x*oneMinusC + c
        m01 = x*y*oneMinusC - z*s
        m02 = x*z*oneMinusC + y*s

        m10 = x*y*oneMinusC + z*s
        m11 = y*y*oneMinusC + c
        m12 = y*z*oneMinusC - x*s

        m20 = x*z*oneMinusC - y*s
        m21 = y*z*oneMinusC + x*s
        m22 = z*z*oneMinusC + c
    }
}

// 分解の実装
extension RotationMatrix {
    func decompose() -> (axis: Vector3, angle: Float) {
        let c = min(max((m00 + m11 + m22 - 1) / 2, -1), 1)
        let angle = acos(c)
        let oneMinusC = 1 - c

        // 後の除算で使うため、極端に小さい場合は無回転として処理する。
        // しきい値は1-cos(Float.pi * 2 / 10000) = 1.7881393e-07から決めた。
        // 回転角が十分0に近いため、回転軸は適当に返す。
        guard oneMinusC > 1e-7 else {
            return (Vector3(x: 1, y: 0, z: 0), 0)
        }

        // 常に正だが計算誤差で負になりうるので0に丸める
        let xx = max((m00 - c) / oneMinusC, 0)
        let yy = max((m11 - c) / oneMinusC, 0)
        let zz = max((m22 - c) / oneMinusC, 0)

        let xy = (m01 + m10) / (2*oneMinusC)
        let yz = (m12 + m21) / (2*oneMinusC)
        let zx = (m20 + m02) / (2*oneMinusC)

        let xs = (m21 - m12) / 2
        let ys = (m02 - m20) / 2
        let zs = (m10 - m01) / 2

        let vs = [xs, ys, zs]
        // 同符号は1、異符号は-1とする
        let relations: [Float] = [
            xy >= 0 ? 1 : -1,
            yz >= 0 ? 1 : -1,
            zx >= 0 ? 1 : -1
        ]

        // 基準として使う成分を決定する。
        // あれば非ゼロ成分が欲しいので、ここでは絶対値最大の成分を選択している。
        var index = 0
        if abs(vs[1]) > abs(vs[index]) {
            index = 1
        }
        if abs(vs[2]) > abs(vs[index]) {
            index = 2
        }

        var signs: [Float] = [0, 0, 0]

        // indexの成分が非ゼロならその符号をそのまま用いる。
        // ゼロならどちらでも良いので正としている。
        signs[index] = vs[index] >= 0 ? 1 : -1

        // 一つの符号が定まったので、relationsと組み合わせて他の符号を決定する。
        signs[(index+1) % 3] = signs[index] * relations[index]
        signs[(index+2) % 3] = signs[index] * relations[(index+2) % 3]

        let x = signs[0] * sqrt(xx)
        let y = signs[1] * sqrt(yy)
        let z = signs[2] * sqrt(zz)

        // 計算誤差で単位ベクトルにならないことがあるので正規化する。
        let axis = Vector3(x: x, y: y, z: z).normalized()

        return (axis, angle)
    }
}

この方法はクォータニオンこそ出現しないが類似した式が多く出てきており、クォータニオン経由のアルゴリズムの変形のように思われる。
一方でクォータニオンの場合回転角が$0$のときを特別扱いすればよかったのに対し、こちらでは$\pi$の場合においても特別に考慮する必要がある。このあたりの対応関係がまだ完全に掴めていないが、それは今後の課題とする。

t-ae
qoncept
リアルタイム画像認識を専門にした会社です。近年はスポーツにおける認識技術の応用に力を入れています。
https://qoncept.co.jp/
Why not register and get more from Qiita?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
Comments
No comments
Sign up for free and join this conversation.
If you already have a Qiita account
Why do not you register as a user and use Qiita more conveniently?
You need to log in to use this function. Qiita can be used more conveniently after logging in.
You seem to be reading articles frequently this month. Qiita can be used more conveniently after logging in.
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
ユーザーは見つかりませんでした