はじめに
クォータニオンのクラスを作ってみようと思いました。回転の取り扱いが便利なので、作っておくといいです。なお単位クォータニオンにはこだわらず、一般クォータニオンとします。つまり実質的にはvec4ですね。乗算の定義などでクォータニオンらしさを出す形です。そもそもいちいち正規化するのめんどくさい...
コード全文
コンストラクタはベクトルと一緒で列挙のみです。シンプル。
ベクトルはVectaを使っていますが、p5.Vectorでも同じようにできるかと思います。
class Quarternion{
constructor(w = 1, x = 0, y = 0, z = 0){
this.w = w;
this.x = x;
this.y = y;
this.z = z;
}
set(q){
if(arguments.length === 4){
this.w = arguments[0];
this.x = arguments[1];
this.y = arguments[2];
this.z = arguments[3];
return this;
}
this.w = q.w;
this.x = q.x;
this.y = q.y;
this.z = q.z;
return this;
}
copy(){
return new Quarternion(this.w, this.x, this.y, this.z);
}
mult(s = 1, immutable = false){
if(immutable){
return this.copy().mult(s, false);
}
this.w *= s;
this.x *= s;
this.y *= s;
this.z *= s;
return this;
}
multQ(q, immutable = false){
if(immutable){
return this.copy().multQ(q, false);
}
const {w:d, x:a, y:b, z:c} = this;
this.w = d * q.w - a * q.x - b * q.y - c * q.z;
this.x = d * q.x + a * q.w + b * q.z - c * q.y;
this.y = d * q.y + b * q.w + c * q.x - a * q.z;
this.z = d * q.z + c * q.w + a * q.y - b * q.x;
return this;
}
conj(immutable = false){
if(immutable){
return this.copy().conj(false);
}
this.x *= -1;
this.y *= -1;
this.z *= -1;
return this;
}
applyV(v){
// vに適用する。軸と角度。
// 具体的にはq * v * \bar{q} を計算してx,y,zを取るだけ。
const q = this.copy();
const vq = Quarternion.fromV(v);
const qConj = q.conj(true); // ここのqは変えちゃまずいのでtrueです。
// qは変えちゃってOK
q.multQ(vq).multQ(qConj);
return new Vecta(q.x, q.y, q.z);
}
mag(){
return Math.sqrt(this.magSq());
}
magSq(){
return this.x*this.x + this.y*this.y + this.z*this.z + this.w*this.w;
}
normalize(){
const m = this.mag();
if(m < Number.EPSILON){
// 0の正規化はゼロとする
return new Quarternion(0,0,0,0);
}
return this.mult(1/m);
}
pow(a){
// この関数自体は補助関数なのでimmutableは不要かと思う
const m = this.magSq();
if(m < Number.EPSILON){
// 0のべき乗は0とする
return Quarternion(0,0,0,0);
}
// ここで排除してしまう。
if(this.w < 0){
this.mult(-1);
}
const n = Math.sqrt(m);
const c = this.w/n;
const s = Math.sqrt(m - this.w*this.w)/n;
const t = Math.atan2(s, c); // 0～PI/2
const multiplier = Math.pow(n, a);
if(Math.abs(t) < Number.EPSILON){
this.w = (this.w/n)*multiplier;
this.x = (this.x/n)*multiplier;
this.y = (this.y/n)*multiplier;
this.z = (this.z/n)*multiplier;
return this;
}
const ax = (this.x/n)/s;
const ay = (this.y/n)/s;
const az = (this.z/n)/s;
const phi = a*t;
this.w = Math.cos(phi)*multiplier;
this.x = Math.sin(phi)*ax*multiplier;
this.y = Math.sin(phi)*ay*multiplier;
this.z = Math.sin(phi)*az*multiplier; // zがyになってたよ。
return this;
}
slerp(q1, ratio, immutable = false){
if(immutable){
return this.copy.slerp(q1, ratio, false);
}
// qは要するにq1*(thisの逆元). これを0～1乗して補間するだけ。
const m = this.magSq();
if(m < Number.EPSILON){
// 0との補間は考えられないのでオールゼロでいいと思う. 線形補間とは違うので。
// 実数では0の0でない値でのベキはすべてゼロなので妥当な判断。
return new Quarternion(0,0,0,0);
}
const q = q1.multQ(this.conj(true), true).mult(1/m);
return q.pow(ratio).multQ(this);
// 右乗算の場合。どっちがいいのかは知らない。
//const q = this.conj(true).multQ(q1).mult(1/m);
//return this.copy().multQ(q.pow(ratio));
// これでいいかどうかは知らんです。
// 参考：クォータニオンのべき乗、これは単位限定だけどね。なお右乗算。
// https://zenn.dev/mebiusbox/books/132b654aa02124/viewer/2966c7
}
getAxes(){
// 単位クォータニオンの場合は3本の軸ベクトルを順繰りに用意する関数になる。
// 行列的にはこれらは列ベクトルで、配置的には転置となっている。
// クォータニオンに3本の列ベクトルという別の姿があるイメージ。1to1ではないが。
const {w,x,y,z} = this;
return {
x:new Vecta(2*w*w-1 + 2*x*x, 2*(x*y + z*w), 2*(x*z - y*w)),
y:new Vecta(2*(x*y - z*w), 2*w*w-1 + 2*y*y, 2*(y*z + x*w)),
z:new Vecta(2*(x*z + y*w), 2*(y*z - x*w), 2*w*w-1 + 2*z*z)
}
}
static fromAA(axis, angle){
// 軸の指定方法は3種類
if(Array.isArray(axis)){
axis = new Vecta(axis[0], axis[1], axis[2]);
}else if(arguments.length === 4){
axis = new Vecta(arguments[0], arguments[1], arguments[2]);
angle = arguments[3];
}
axis.normalize();
const q = new Quarternion();
q.w = Math.cos(angle/2);
const s = Math.sin(angle/2);
q.x = axis.x * s;
q.y = axis.y * s;
q.z = axis.z * s;
return q;
}
static fromV(v){
const q = new Quarternion();
q.w = 0;
q.x = v.x;
q.y = v.y;
q.z = v.z;
return q;
}
static fromAxes(x, y, z){
// 正規直交基底から出す。正規直交基底でないと失敗する。
// 3つの引数はすべてベクトル限定とする。列ベクトル。
// 参考：https://github.com/mrdoob/three.js/blob/r172/src/math/Quaternion.js#L294
const q = new Quarternion();
const {x:a, y:d, z:g} = x;
const {x:b, y:e, z:h} = y;
const {x:c, y:f, z:i} = z;
// a b c
// d e f
// g h i
const trace = a + e + i;
// 角度がPIに近いと割り算ができないが、
// traceが正ならそれは起きえない。
if(trace > 0){
// ここだけあっちと違う計算だが、意味的に分かりやすいので。
q.w = Math.sqrt((trace + 1) / 4);
const factor = 0.25/q.w;
q.x = (h - f)*factor;
q.y = (c - g)*factor;
q.z = (d - b)*factor;
}else{
if(a > e && a > i){
// aが最大の場合
const s = 2 * Math.sqrt(1 + a - e - i);
q.w = (h - f) / s;
q.x = 0.25 * s;
q.y = (b + d) / s;
q.z = (c + g) / s;
}else if(e > i){
// eが最大の場合
const s = 2 * Math.sqrt(1 + e - i - a);
q.w = (c - g) / s;
q.x = (b + d) / s;
q.y = 0.25 * s;
q.z = (f + h) / s;
}else{
// iが最大の場合
const s = 2 * Math.sqrt(1 + i - a - e);
q.w = (d - b) / s;
q.x = (c + g) / s;
q.y = (f + h) / s;
q.z = 0.25 * s;
}
}
return q;
}
}
関数一覧
set
クォータニオンもしくは列挙引数を取ります。
copy
自分のコピーを取る関数です。
mult
定数倍するだけです。immutable指定が付きます。自分と異なるものを返したい場合にこれをtrueにします。Vectaと同じく、immutable系はすべてfalseをデフォルトとしています。これは行列クラスでも同じ規約を徹底しています。
multQ
クォータニオンの右乗算です。immutable指定が付きます。右から掛けます。Threeなんかは左から掛けるのも用意しています。クォータニオンには左乗算と右乗算それぞれに意味がありどっちも重要なんですが、式を書く都合上、左乗算はどうしても不自然さが気になってしまうので、用意しないことにしました。
conj
共役を取る関数です。immutable指定が付きます。実部以外の成分をマイナス1倍するだけです。これを取ることで、両者を掛けてノルムの二乗を得たり、逆元を取ったりできます。複素数に似ていますね。
applyV
クォータニオンをベクトルに作用させて回転させる関数です。ベクトルから実部を0としてクォータニオンを作り、
const q = this.copy();
const vq = Quarternion.fromV(v);
const qConj = q.conj(true);
q.multQ(vq).multQ(qConj);
として乗算したうえで虚部を取ればいいですね。
mag, magSq
いわゆるノルム関数です。magSqだけ作ってmagはその平方根で定めています。
normalize
正規化用です。ゼロの場合はゼロとします。ゼロ判定は通常magSqで行なうのですが、ベクトルでもそうですが、この手の関数はゼロでない場合に実行される場合がほとんどなので、それをやっても大して負荷軽減にならないです。
pow
べき乗です。ゼロの場合はゼロです。実部が負の場合に-1倍しています。クォータニオンは基本的に直交行列のレシピとして使うので、qと-qは同じ役割を果たします。ゆえに-1倍しても問題ないです。足し算や引き算をしない、ベクトルでは無いので。また角度が小さい場合は実質実数なのでああいった処理をしています。一般の場合ですが、ノルムをべき乗したうえで、角度はそのまま掛け算し、最終的に仕上げています。
slerp
いわゆる球面線形補間です。immutable指定が付きます。自身がゼロの場合はゼロとします。対象に自身の逆元を掛けて全体をratio乗し、自身を掛けて仕上げます。左乗算でやっていますが、右乗算とどっちがいいのかは分かんないです。mebiusboxさんは右乗算でやっています。Threeのやり方でもいいんですが、不必要に計算を避けて結果的に分かりづらくなっているので採用しませんでした。
getAxes
単位クォータニオン限定で、3本の正規直交基底を取り出す関数です。3本の列ベクトルですね。列ベクトルとみなしています。列ベクトルですが、成分は横に列挙しないといけないので配置的には転置になっています。列でみなさないといけない理由は、自分の流儀ではCPUサイドにおいては行列を列ベクトルに掛けることとなっているからです。縦と横は全体で統一しないと確実にバグの原因になるので重要です。
なおこれを見ると分かりますがqと-qは同じベクトルの組を与えます。これらは同じ役割を果たすものです。
役割としては、クォータニオンサイドで変換を実行し、その内容を正規直交基底にフィードバックするため使います。
static: fromAA
軸と角度からクォータニオンを作る関数です。基本的にはベクトルとスカラーで作るんですが、ベクトルのところは配列でもいいし、成分列挙も許しています。自由さ、大事。
static: fromV
ベクトルからクォータニオンを作る関数です。実部は0で、虚部にベクトルの成分を並べるだけ。上の方でapplyVを紹介していますがここで使っています。あんま役に立たないかもしれません。
static: fromAxes
正規直交基底からクォータニオンを作る関数です。実質的にはgetAxesの逆ですね。内容的には、
setFromRotationMatrix
Threeのこれと同じものです。違うのは3本のベクトルを引数に取っていることと（Mat3を作る予定はない）、トレースが正の場合の計算方法です。意味的に分かりやすいので。式を見ると分かりますが実部が正になることは保証されていません。
おわりに
クォータニオンはカメラでも使うし、モデル行列の回転パートでも使うので重要らしいです。BlenderでもGLTFなんかで回転を出力すると4つの数が出てきます。回転をクォータニオンに落としているわけです。補間もおそらくslerpです。なのでこの辺の理解があると便利だと思います。mebiusboxさんの記事も参考になると思います。
クォータニオン
自分は数式を使った説明が苦手なので、文章メインで書かせていただきました。
ここまでお読みいただいてありがとうございました。
余談
p5がクォータニオンに手を出したいようなので、興味のある人は開発に協力してみてください。
Quat
追記：クォータニオンの左乗算と右乗算
単位クォータニオン$q$（ノルムが1）に対して、それが対応する正規直交基底のベクトルの組は、四元数の基底を$1,i,j,k$として、
qi\bar{q},~~~qj\bar{q},~~~qk\bar{q}
で与えられます。ゆえに$q$と$-q$は同じ基底に対応します。で、左乗算というのは、軸$e$と角度$\theta$に対して、
s=\cos\frac{\theta}{2} + e\sin\frac{\theta}{2}
（ただし$e$をクォータニオン$e_xi+e_yj+e_zk$とみなしている）と置いた場合に、これを左から掛けたとき、$q$と対応する正規直交基底のベクトルが、それぞれグローバル空間で軸$e$の周りに角度$\theta$だけ回転するということです。
一方、これを右から掛ける場合、ローカル空間で軸$e$の周りに角度$\theta$だけ回転します。どういう意味かというと、
e' = e_x(qi\bar{q}) + e_y(qj\bar{q}) + e_z(qk\bar{q})
の周りに回転するということです。たとえば$i$を右乗算する場合、正規直交基底のx軸に対応する$qi\bar{q}$の周りに回転します。これはこうすると見やすいです。
qs = (qs\bar{q})q.
つまり$s$を$q$で「解釈」してできるベクトルの周りのグローバル回転ということです。このように、グローバルとローカルの回転を容易に扱えるのが、クォータニオンの魅力です。
なお、通常の（vRoidHubのような）orbitControlのカメラの操作では、定められた縦方向の軸の周りのグローバル回転と、横方向の軸周りのローカル回転を両方実行しています。今見たようにこれらは乗算の方向が違うので可換です。ゆえに、マウスポインターの移動方向を縦横に分解して、それぞれ処理に用いても矛盾が生じないわけです。同じ乗算の方向だった場合、どっちを先にするかで結果が変わってしまうからです。
Threeはベクトルにしてもクォータニオンにしても無数にメソッドを用意しています。数えきれないほどです。しかしすべてをあらゆるスケッチで日常的に用いるとは考えにくいので、必要に応じて自分が欲しい処理だけを備えたライブラリがあると便利だと思います。それでもこのくらいの量にはなってしまいますが...（多分かなり少ない方）。