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

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

## 解説

### クォータニオンの定義

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

```math
\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`構造体を定義します。

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

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

### クォータニオンの和・差

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

```math
\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\\
```

```glsl
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
  );
}
```

### クォータニオンの積

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

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


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

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

```math
\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}$)なので注意してください。

```glsl
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
  );
}
```

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

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

```math
\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
```

```glsl
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}}$は次のようになります。

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

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

### クォータニオンのノルム

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

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

```glsl
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));
}
```

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

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

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

### 恒等クォータニオン

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

```math
\boldsymbol{q}\boldsymbol{I} = \boldsymbol{q}\boldsymbol{I} = \boldsymbol{q}\\
```

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

```math
\boldsymbol{I} = 1
```

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

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

### 逆クォータニオン

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

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

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

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

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

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

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

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

```math
\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
```

```glsl
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'}$が回転を適用させたベクトルになります。

```math
\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)
```

```glsl
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つのクォータニオンがなす角になります。


```math
\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)
```

```glsl
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)
  );
}
```

## ソースコード全文

```glsl
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](https://qiita.com/drken/items/0639cf34cce14e8d58a5)
- [Quaternionによる3次元の回転変換 \- Qiita](https://qiita.com/kenjihiranabe/items/945232fbde58fab45681)
- [クォータニオン（回転）についてメモ \- e\.blog](http://edom18.hateblo.jp/entry/2018/06/25/084023)
- [TransformのLookAtをUnity APIを使わずに書く \- e\.blog](http://edom18.hateblo.jp/entry/2018/04/18/104054)
- [クォータニオン計算便利ノート](chrome-extension://mhjfbmdgcfjbbpaeojofohoefgiehjai/index.html)