GLSLでクォータニオン(四元数)

GLSLでクォータニオンを実装してみます。

解説

クォータニオンの定義

クォータニオン$\boldsymbol{q}$は以下のように4つの実数$q_x, q_y, q_z, q_w$で表されます。

\boldsymbol{q} = q_w + q_xi + q_yj + q_zk\\
i^2 + j^2 + k ^2 = -1\\
ij = k\\
jk = i\\
ki = j

GLSLでクォータニオンを実装するにあたり、4つの実数を持つQuaternion構造体を定義します。

struct Quaternion {
  float x;
  float y;
  float z;
  float w;
};

組み込み型のvec4を使ってもいいのですが、今回はわかりやすさのために構造体を定義しています。

クォータニオンの和・差

クォータニオンの和・差は単に各要素ごとの和・差になります。

\boldsymbol{p} = p_w + p_xi + p_yj + p_zk\\
\boldsymbol{q} = q_w + q_xi + q_yj + q_zk\\
\boldsymbol{p} + \boldsymbol{q} = (p_w + q_w) + (p_x + q_x)i + (p_y + q_y)j + (p_z + q_z)k\\
\boldsymbol{p} - \boldsymbol{q} = (p_w - q_w) + (p_x - q_x)i + (p_y - q_y)j + (p_z - q_z)k\\

Quaternion add(Quaternion q1, Quaternion q2) {
  return Quaternion(
    q1.x + q2.x,
    q1.y + q2.y,
    q1.z + q2.z,
    q1.w + q2.w
  );
}

Quaternion sub(Quaternion q1, Quaternion q2) {
  return Quaternion(
    q1.x - q2.x,
    q1.y - q2.y,
    q1.z - q2.z,
    q1.w - q2.w
  );
}

クォータニオンの積

実数とクォータニオンの積は実数をクォータニオンの各要素にかけます。

\boldsymbol{p} = p_w + p_xi + p_yj + p_zk\\
a\boldsymbol{p} = ap_w + ap_xi + ap_yj + ap_zk

Quaternion mul(Quaternion q, float f) {
  return Quaternion(f * q.x, f * q.y, f * q.z, f * q.w);
}

クォータニオン同士の積は以下のようになります。

\boldsymbol{p} = p_w + p_xi + p_yj + p_zk\\
\boldsymbol{q} = q_w + q_xi + q_yj + q_zk\\
\boldsymbol{pq} = (-p_xq_x - p_yq_y - p_zq_z + p_wq_w)\\
+ (p_wq_x - p_zq_y + p_yq_z + p_xq_w)i\\
+ (p_zq_x + p_wq_y - p_xq_z + p_yq_w)j\\
+ (-p_yq_x + p_xq_y + p_wq_z + p_zq_w)k\\

GLSLで実装すると以下のようになります。引数q1は$\boldsymbol{p}$に引数q2は$\boldsymbol{q}$に相当します。クォータニオンの積は非可換($\boldsymbol{pq} \neq \boldsymbol{qp}$)なので注意してください。

Quaternion mul(Quaternion q1, Quaternion q2) {
  return Quaternion(
    q2.w * q1.x - q2.z * q1.y + q2.y * q1.z + q2.x * q1.w,
    q2.z * q1.x + q2.w * q1.y - q2.x * q1.z + q2.y * q1.w,
    -q2.y * q1.x + q2.x * q1.y + q2.w * q1.z + q2.z * q1.w,
    -q2.x * q1.x - q2.y * q1.y - q2.z * q1.z + q2.w * q1.w
  );
}

クォータニオンのドット積

クォータニオンのドット積はベクトルのドット積と同じように各要素の積の和になります。

\boldsymbol{p} = p_w + p_xi + p_yj + p_zk\\
\boldsymbol{q} = q_w + q_xi + q_yj + q_zk\\
\boldsymbol{p} \cdot \boldsymbol{q} = p_wq_w + p_xq_x + p_yq_y + p_zq_z

float qdot(Quaternion q1, Quaternion q2) {
  return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w + q2.w;
}

dotという名前はGLSLのビルトイン関数として使われているのでqdotという関数名にしています。

共役クォータニオン

共役クォータニオン$\tilde{\boldsymbol{q}}$は次のようになります。

\tilde{\boldsymbol{q}} = q_w - q_xi - q_yj - q_zk\\

Quaternion conjugate(Quaternion q) {
  return Quaternion(-q.x, -q.y, -q.z, q.w);
}

クォータニオンのノルム

クォータニオンの大きさを表すノルムは以下のように計算できます。

\|\boldsymbol{q}\| = \sqrt{q_wq_w + q_xq_x + q_yq_y + q_zq_z}

float squareNorm(Quaternion q) {
  return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
}

float norm(Quaternion q) {
  return sqrt(squareNorm(q));
}

共役クォータニオンを使って次のように求めることもできます。

\|\boldsymbol{q}\|^2 = \tilde{\boldsymbol{q}}\boldsymbol{q}

ノルムが1のクォータニオンは回転を表すことができます。

恒等クォータニオン

以下のような$\boldsymbol{I}$を恒等クォータニオンと呼びます。

\boldsymbol{q}\boldsymbol{I} = \boldsymbol{q}\boldsymbol{I} = \boldsymbol{q}\\

クォータニオンの積の定義から$\boldsymbol{I}$は以下のようになります。

\boldsymbol{I} = 1

Quaternion identity() {
  return Quaternion(0.0, 0.0, 0.0, 1.0);
}

恒等クォータニオンは無回転を表しています。

逆クォータニオン

以下を満たす$\boldsymbol{q^{-1}}$を$\boldsymbol{q}$の逆クォータニオンと呼びます。

\boldsymbol{q^{-1}}\boldsymbol{q} = \boldsymbol{I}

逆クォータニオンは以下のように計算できます。

\boldsymbol{q^{-1}} = \frac{\tilde{\boldsymbol{q}}}{\|\boldsymbol{q}\|}\\

Quaternion qinverse(Quaternion q) {
  Quaternion c = conjugate(q);
  float s = norm(q);
  return mul(c, 1.0 / s);
}

inverseはGLSLの予約語なのでqinverseという関数名にしています。

任意軸の回転を表すクォータニオン

任意の軸に対する回転を表すクォータニオンは、回転軸を$\vec{v}$、回転角度を$\theta$とすると以下のようになります。

\vec{v} = (v_x, v_y, v_z), \|\vec{v}\| = 1\\
\boldsymbol{q} = cos(\frac{\theta}{2}) + v_xsin(\frac{\theta}{2})i +  v_ysin(\frac{\theta}{2})j + v_zsin(\frac{\theta}{2})k

Quaternion axisAngle(vec3 axis, float radian) {
  vec3 naxis = normalize(axis);
  float h = 0.5 * radian;
  float s = sin(h);
  return Quaternion(naxis.x * s, naxis.y * s, naxis.z * s, cos(h));
}

クォータニオンによるベクトルの回転

クォータニオンによりベクトル$\vec{v}$を回転させます。$\vec{v}$をクォータニオンとみなして演算をおこない、最終的に$\vec{v'}$が回転を適用させたベクトルになります。

\vec{v} = (v_x, v_y, v_z)\\
\boldsymbol{v} = v_xi + v_yj + v_zk\\
\boldsymbol{q} = q_w + q_xi + q_yj + q_zk, \|\boldsymbol{q}\| = 1\\
\boldsymbol{v'} = \boldsymbol{qv\tilde{q}} = v'_xi + v'_yj + v'_zk\\
\vec{v'} = (v'_x, v'_y, v'_z)

vec3 rotate(vec3 v, Quaternion q) {
  // norm of q must be 1.
  Quaternion vq = Quaternion(v.x, v.y, v.z, 0.0);
  Quaternion cq = conjugate(q);
  Quaternion mq = mul(mul(cq, vq), q);
  return vec3(mq.x, mq.y, mq.z);
}

球面線形補間

2つのクォータニオン$\boldsymbol{q_1}$、$\boldsymbol{q_2}$を球面線形補間で滑らかに補間します。$\theta$は2つのクォータニオンがなす角になります。

\boldsymbol{q} = \frac{sin(1 - t)\theta}{sin\theta}\boldsymbol{q_1}  + \frac{sint\theta}{sin\theta}\boldsymbol{q_2} (0 \leq t \leq 1)

Quaternion slerp(Quaternion q1, Quaternion q2, float t) {
  float r = acos(qdot(q1, q2));
  float is = 1.0 / sin(r);
  return add(
    mul(q1, sin((1.0 - t) * r) * is),
    mul(q2, sin(t * r) * is)
  );
}

ソースコード全文

struct Quaternion {
  float x;
  float y;
  float z;
  float w;
};

Quaternion identity() {
  return Quaternion(0.0, 0.0, 0.0, 1.0);
}

Quaternion axisAngle(vec3 axis, float radian) {
  vec3 naxis = normalize(axis);
  float h = 0.5 * radian;
  float s = sin(h);
  return Quaternion(naxis.x * s, naxis.y * s, naxis.z * s, cos(h));
}

Quaternion conjugate(Quaternion q) {
  return Quaternion(-q.x, -q.y, -q.z, q.w);
}

Quaternion add(Quaternion q1, Quaternion q2) {
  return Quaternion(
    q1.x + q2.x,
    q1.y + q2.y,
    q1.z + q2.z,
    q1.w + q2.w
  );
}

Quaternion sub(Quaternion q1, Quaternion q2) {
  return Quaternion(
    q1.x - q2.x,
    q1.y - q2.y,
    q1.z - q2.z,
    q1.w - q2.w
  );
}

Quaternion mul(Quaternion q, float f) {
  return Quaternion(f * q.x, f * q.y, f * q.z, f * q.w);
}

Quaternion mul(Quaternion q1, Quaternion q2) {
  return Quaternion(
    q2.w * q1.x - q2.z * q1.y + q2.y * q1.z + q2.x * q1.w,
    q2.z * q1.x + q2.w * q1.y - q2.x * q1.z + q2.y * q1.w,
    -q2.y * q1.x + q2.x * q1.y + q2.w * q1.z + q2.z * q1.w,
    -q2.x * q1.x - q2.y * q1.y - q2.z * q1.z + q2.w * q1.w
  );
}

float qdot(Quaternion q1, Quaternion q2) {
  return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
}

float squareNorm(Quaternion q) {
  return q.x * q.x + q.y * q.y + q.z * q.z + q.w + q.w;
}

float norm(Quaternion q) {
  return sqrt(squareNorm(q));
}

Quaternion qinverse(Quaternion q) {
  Quaternion c = conjugate(q);
  float s = norm(q);
  return mul(c, 1.0 / s);
}

vec3 rotate(vec3 v, Quaternion q) {
  // norm of q must be 1.
  Quaternion vq = Quaternion(v.x, v.y, v.z, 0.0);
  Quaternion cq = conjugate(q);
  Quaternion mq = mul(mul(cq, vq), q);
  return vec3(mq.x, mq.y, mq.z);
}

Quaternion slerp(Quaternion q1, Quaternion q2, float t) {
  float cosine = qdot(q1, q2);
  if (cosine < 0.0) {
    cosine = qdot(q1, mul(q2, -1.0));
  }
  float r = acos(qdot(q1, q2));
  float is = 1.0 / sin(r);
  return add(
    mul(q1, sin((1.0 - t) * r) * is),
    mul(q2, sin(t * r) * is)
  );
}

サンプル

球面線形補間で2つのクォータニオンの補間しています。マウスのx座標で$t$の値を変更します。
http://glslsandbox.com/e#58040.0

参考

• クォータニオン (Quaternion) を総整理！ ～ 三次元物体の回転と姿勢を鮮やかに扱う ～ - Qiita
• Quaternionによる3次元の回転変換 - Qiita
• クォータニオン（回転）についてメモ - e.blog
• TransformのLookAtをUnity APIを使わずに書く - e.blog
• クォータニオン計算便利ノート