core チュートリアル

このチュートリアルでは、Luna-Flow/quaternion を使って、空のプロジェクトからベクトルの回転、姿勢の合成と補間、オイラー角との変換までを進めます。最後に luna-generic の trait を使ったジェネリックなコードと、陥りやすい誤りを扱います。ここでは数学は控えめにし、導出は core 設計に任せます。

クイックスタート

プロジェクトにモジュールを追加します。

moon add Luna-Flow/quaternion@0.2.0

moon.pkg でパッケージをインポートします。例では luna-generic（ジェネリックなコード用）と moonbitlang/core/math（ $\pi$ 用）も使います。

import {
  "Luna-Flow/quaternion",
  "Luna-Flow/luna-generic" @lf_alg,
  "moonbitlang/core/math",
}

最小限の実用的なプログラムとして、x 軸を z 軸の周りに 4 分の 1 回転させます。

fn main {
  let quarter_turn = @quaternion.from_axis_angle((0.0, 0.0, 1.0), @math.PI / 2.0)
  println(quarter_turn)
  let (x, y, z) = quarter_turn.rotate((1.0, 0.0, 0.0))
  println("(\{x}, \{y}, \{z})")
}

出力は次のとおりです。

0.7071067811865476 + 0i + 0j + 0.7071067811865475k
(2.220446049250313e-16, 1, 0)

x 軸は y 軸に移ります。最初の座標には約 $2 \times 10^{-16}$ の丸め誤差があります。

日常的な作業

四元数を作って掛け合わせる

Quaternion::from_vec((w, x, y, z)) は $w + xi + yj + zk$ を作ります。通常の演算子が使え、* は Hamilton 積です。基底同士は $ij = k$ ですが $ji = -k$ と掛け合わされるので、因子の順序が重要です。

test "units" {
  let i = @quaternion.Quaternion::from_vec((0, 1, 0, 0))
  let j = @quaternion.Quaternion::from_vec((0, 0, 1, 0))
  let k = @quaternion.Quaternion::from_vec((0, 0, 0, 1))
  inspect(i * j, content="0 + 0i + 0j + 1k")
  inspect(j * i, content="0 + 0i + 0j + -1k")
  inspect(i * i, content="-1 + 0i + 0j + 0k")
  inspect(i * j * k, content="-1 + 0i + 0j + 0k")
}

整数の成分は厳密なので、Quaternion[Int] は恒等式の確認に便利です。回転には Double の成分を使ってください。

ベクトルを回転し、回転を合成する

from_axis_angle(axis, angle) は回転の単位四元数を与え、rotate はそれを 3 次元ベクトルに適用します。first の後に second を適用するには、列ベクトルに作用する行列と同じように second * first と掛けます。

test "compose rotations" {
  let first = @quaternion.from_axis_angle((0.0, 0.0, 1.0), @math.PI / 2.0) // z, 90 degrees
  let second = @quaternion.from_axis_angle((1.0, 0.0, 0.0), @math.PI / 2.0) // x, 90 degrees
  let both = second * first
  let (x, y, z) = both.rotate((1.0, 0.0, 0.0))
  // x -> y under `first`, then y -> z under `second`
  assert_true(x.abs() < 1.0e-15 && y.abs() < 1.0e-15 && (z - 1.0).abs() < 1.0e-15)
  let (x2, y2, z2) = second.rotate(first.rotate((1.0, 0.0, 0.0)))
  assert_true((x - x2).abs() < 1.0e-15 && (y - y2).abs() < 1.0e-15 && (z - z2).abs() < 1.0e-15)
}

rotate は単位四元数を前提とします。from_axis_angle で作った四元数は単位四元数です。自分で組み立てたものには normalize() を呼んでください。

回転を元に戻し、2 つの姿勢の間の回転を求める

単位四元数の逆元はその共役です。姿勢 from を姿勢 to に移す回転 delta（つまり delta * from == to）を求めるには、右側で割ります：to / from は $to \cdot from^{-1}$ です。left_div はもう一方の方程式 from * x == to を解き、from の座標系で表した回転を与えます。

test "relative rotation" {
  let from = @quaternion.from_axis_angle((0.0, 1.0, 0.0), 0.3)
  let to = @quaternion.from_axis_angle((1.0, 1.0, 0.0), 1.2)
  let delta = to / from
  assert_true((delta * from - to).square_len() < 1.0e-30)
  let in_from_frame = to.left_div(from)
  assert_true((from * in_from_frame - to).square_len() < 1.0e-30)
  // a unit quaternion's inverse is its conjugate
  assert_true((from.inv() - from.conjugate()).square_len() < 1.0e-30)
}

2 つの回転を比較する

q と -q は同じ回転を表すので、== は「同じ姿勢か」の判定には向きません。単位四元数では $|q \cdot r| = \cos(\theta/2)$ で、 $\theta$ は 2 つの姿勢の間の角です。

///|
fn rotation_angle_between(
  q : @quaternion.Quaternion[Double],
  r : @quaternion.Quaternion[Double],
) -> Double {
  let c = q.normalize().dot(r.normalize()).abs()
  2.0 * @math.acos(if c > 1.0 { 1.0 } else { c })
}

test "compare rotations" {
  let q = @quaternion.from_axis_angle((0.0, 0.0, 1.0), 0.5)
  assert_true(q != -q)
  inspect(rotation_angle_between(q, -q), content="0")
  let r = @quaternion.from_axis_angle((0.0, 0.0, 1.0), 0.75)
  assert_true((rotation_angle_between(q, r) - 0.25).abs() < 1.0e-12)
}

姿勢の間を補間する

slerp(q1, q2, t) は最短の大円の弧に沿って一定の角速度で動きます。

test "slerp steps" {
  let start = @quaternion.id()
  let end = @quaternion.from_axis_angle((0.0, 0.0, 1.0), @math.PI / 2.0)
  for step in 0..<=4 {
    let t = step.to_double() / 4.0
    let (x, y, _) = @quaternion.slerp(start, end, t).rotate((1.0, 0.0, 0.0))
    let degrees = @math.atan2(y, x) * 180.0 / @math.PI
    assert_true((degrees - 22.5 * step.to_double()).abs() < 1.0e-9)
  }
}

一定の角速度のとおり、x 軸は 1 ステップごとに $22.5^\circ$ 回転します。

オイラー角との変換

from_euler(roll, pitch, yaw) は既定で順序 "XYZ" を使います。これは $q_X(\text{roll})\,q_Y(\text{pitch})\,q_Z(\text{yaw})$ 、つまり X、新しい Y、新しい Z の周りの内因的回転です。対応する逆変換は to_euler(order="XYZ", external=false) です。to_euler の既定値は別の規約である外因的 Z-Y-X で、同じ 3 つの角を逆順で返します。

test "euler round trip" {
  let q = @quaternion.from_euler(0.1, 0.2, 0.3)
  let (roll, pitch, yaw) = q.to_euler(order="XYZ", external=false)
  assert_true((roll - 0.1).abs() < 1.0e-12)
  assert_true((pitch - 0.2).abs() < 1.0e-12)
  assert_true((yaw - 0.3).abs() < 1.0e-12)
  let (about_z, _, about_x) = q.to_euler() // extrinsic ZYX
  assert_true((about_z - 0.3).abs() < 1.0e-12 && (about_x - 0.1).abs() < 1.0e-12)
}

to_euler が返す k 番目の角は、常に order の k 番目の軸に対応します。他の順序を使う前に API の説明を読んでください。一部の組み合わせには既知の問題があります。

成分を読み取る

Quaternion は抽象型で、フィールドのアクセサがありません。基底四元数との内積で 1 つの成分を読み取れ、Debug は 4 つすべてを表示します。

///|
fn components(q : @quaternion.Quaternion[Double]) -> (Double, Double, Double, Double) {
  let e = (w, x, y, z) => q.dot(@quaternion.Quaternion::from_vec((w, x, y, z)))
  (e(1.0, 0.0, 0.0, 0.0), e(0.0, 1.0, 0.0, 0.0), e(0.0, 0.0, 1.0, 0.0), e(0.0, 0.0, 0.0, 1.0))
}

test "components" {
  let q = @quaternion.Quaternion::from_vec((0.5, -1.5, 2.0, 4.0))
  debug_inspect(components(q), content="(0.5, -1.5, 2, 4)")
  debug_inspect(q, content="{ r: 0.5, vec: (-1.5, 2, 4) }")
}

成分が有限であれば、基底四元数との内積はその成分を厳密に返します。

さらに進んで

luna-generic の trait を使ったジェネリックなコード

T が luna-generic の Ring を実装していれば Quaternion[T] も実装するので、環に対するジェネリックなアルゴリズムは四元数を受け付けます。次の関数は任意の Ring について繰り返し二乗法で $x^n$ を計算します。

///|
fn[R : @lf_alg.Ring] power(x : R, n : Int) -> R {
  if n == 0 {
    @lf_alg.One::one()
  } else {
    let half = power(x, n / 2)
    if n % 2 == 0 { half * half } else { x * half * half }
  }
}

test "generic power" {
  inspect(power(3, 4), content="81")
  let q = @quaternion.Quaternion::from_vec((1, 1, 1, 1))
  inspect(power(q, 3), content="-8 + 0i + 0j + 0k")
  assert_eq(power(q, 3), q.pow_by_int(3))
}

四元数は Field ではないので、Field を要求する（そして $ab = ba$ を前提としうる）アルゴリズムは四元数を受け付けません。これは意図的なものです。理由は設計ページで説明しています。

`Int` 上の厳密な恒等式

厳密な成分なら、ノルムの恒等式 $|pq|^2 = |p|^2|q|^2$ を丸めなしで確かめられます。Int 上ではこれはオイラーの四平方恒等式です。4 つの平方の和 2 つの積は再び 4 つの平方の和になり、どの和になるかは Hamilton 積が教えてくれます。

test "four squares" {
  let p = @quaternion.Quaternion::from_vec((1, 2, 3, 4)) // 1+4+9+16 = 30
  let q = @quaternion.Quaternion::from_vec((2, 0, 1, 5)) // 4+0+1+25 = 30
  let pq = p * q
  inspect(pq, content="-21 + 15i + -3j + 15k")
  assert_eq(pq.square_len(), p.square_len() * q.square_len()) // 900
}

平方根や三角関数を必要とする関数（magnitude、normalize、inv の除算、slerp、オイラー角変換）は Int では切り捨てを行います。Int は環演算に使ってください。

長い回転の連鎖

Double での Hamilton 積は毎回丸めを伴うため、多数の単位四元数の積のノルムは 1 からずれていき、 $n$ 回の積の後でおよそ $n \cdot 10^{-16}$ になります。すると rotate はベクトルをわずかに拡大縮小します。ときどき正規化し直してください。

test "renormalize" {
  let step = @quaternion.from_axis_angle((1.0, 2.0, 3.0), 0.001)
  let mut q = @quaternion.id()
  for _ in 0..<100000 {
    q = step * q
  }
  let drift = (q.square_len() - 1.0).abs()
  assert_true(drift < 1.0e-10)
  q = q.normalize()
  assert_true((q.square_len() - 1.0).abs() < 1.0e-15)
}

性能

すべての演算は 4 つのスカラーに対して定数時間で動作します。例外は pow_by_int で、 $O(\log n)$ 回の乗算を使います。q.rotate(v) のコストは外積 2 回で、完全な Hamilton 積で q * v * q.inv() を計算するより安価です。すべての関数は新しい値を返し、何も変更しません。

よくある落とし穴

/ は 0.2.0 で変わりました。 q / r は現在 $q\,r^{-1}$ （右除算）です。0.1.x 向けに書かれ、/ が $r^{-1} q$ を計算することに依存していたコードは q.left_div(r) を呼ぶ必要があります。
因子の順序。 a * b は先に b で回転します。両方が同じ軸の周りの回転でない限り、因子を入れ替えると別の回転になります。
rotate での単位でない四元数。 rotate は正規化を行いません。単位でない $q$ ではその式は $q v q^{-1}$ ではなく、結果は一般に入力と同じ長さですらありません。先に正規化してください。
回転に対する ==。 q と -q は同じ回転ですが、値としては等しくありません。浮動小数点の結果はそもそも厳密に一致することがまれなので、許容誤差で比較してください。
ゼロ除算。 inv、/、left_div は $|r|^2$ で割ります。零四元数は Double では NaN 成分を、Int では実行時トラップを生じます。normalize と pow_by_T は零四元数をそのまま返します。
Int の成分と超越関数。 magnitude、normalize、inv などは Int では切り捨てを行い、単位元以外の inv は零になります。
オイラー角の規約。 from_euler の既定は "XYZ"、to_euler の既定は外因的 "ZYX" です。どちらも明示的に指定してください。"ZYX" や "YZX" での from_euler、外因的 "XZY" や内因的 "YZX" での to_euler、ジンバルロック付近（ $|\text{pitch}| \approx 90^\circ$ ）の結果は避けてください。これらには API に記載した既知の問題があります。ジンバルロック付近では、to_euler は標準出力に警告を 1 行出力します。
分数べき。 pow_by_T はスカラー部が負の四元数では誤った結果になります。回転を表している場合は、先に符号を反転してください。
独自の成分型。 DoubleConvert はこのパッケージ外で実装できないため、それを必要とする関数で使えるのは Int と Double だけです。

次のステップ

core API には、各関数の正確なシグネチャと境界条件が記載されています。
core 設計では、Hamilton 積、回転公式、slerp、オイラー角の抽出を導出しています。
luna-generic は、ここで使っている Ring、Inverse、Conjugate の trait を定義しています。