helpdelete

クォータニオンクラスを作る

次に、回転を表すクォータニオンクラスを作りましょう。
オイラー角による回転は、ジンバルロックの問題や、XYZそれぞれの軸の回転のかける順番が様々あることなど、問題も多いため、純粋な回転を表すのにクォータニオンがよく使われます。glTFでも回転の表現にオイラー角ではなくクォータニオンを使っていますので、spinelライブラリでも扱えるようにします。

クォータニオンの回転の動きとオイラー角との関係については、以下のサイトのデモがわかりやすいです。
https://quaternions.online/ 

クォータニオンクラス

src/mathディレクトリ以下にQuaternion.tsファイルを作成し、以下のように記述します。

import { Vector3 } from "./Vector3.js";

export class Quaternion {
  private v: Float32Array;

  constructor(x: number, y: number, z: number, w: number) {
      this.v = new Float32Array([x, y, z, w]);
  }
  get x() {
      return this.v[0];
  }
  set x(value) {
      this.v[0] = value;
  }
  get y() {
      return this.v[1];
  }
  set y(value) {
      this.v[1] = value;
  }
  get z() {
      return this.v[2];
  }
  set z(value) {
      this.v[2] = value;
  }
  get w() {
      return this.v[3];
  }
  set w(value) {
      this.v[3] = value;
  }
  get raw() {
      return this.v;
  }

  isEqual(quat: Quaternion, delta: number = Number.EPSILON) {
    return (
      Math.abs(quat.x - this.x) < delta &&
      Math.abs(quat.y - this.y) < delta &&
      Math.abs(quat.z - this.z) < delta &&
      Math.abs(quat.w - this.w) < delta
    );
  }

  static identity() {
    return new Quaternion(0, 0, 0, 1);
  }

  clone(): Quaternion {
    return new Quaternion(this.x, this.y, this.z, this.w);
  }

  /**
  * Compute spherical linear interpolation
  */
  static qlerp(l_quat: Quaternion, r_quat: Quaternion, ratio: number): Quaternion {
    let dotProduct =
      l_quat.x * r_quat.x +
      l_quat.y * r_quat.y +
      l_quat.z * r_quat.z +
      l_quat.w * r_quat.w;
    const ss = 1.0 - dotProduct * dotProduct;

    if (ss === 0.0) {
      return l_quat.clone();
    } else {
      if (dotProduct > 1) {
        dotProduct = 0.999;
      } else if (dotProduct < -1) {
        dotProduct = -0.999;
      }

      let theta = Math.acos(dotProduct);
      const sinTheta = Math.sin(theta);

      let s2;
      if (dotProduct < 0.0) {
        dotProduct *= -1;
        theta = Math.acos(dotProduct);
        s2 = (-1 * Math.sin(theta * ratio)) / sinTheta;
      } else {
        s2 = Math.sin(theta * ratio) / sinTheta;
      }
      const s1 = Math.sin(theta * (1.0 - ratio)) / sinTheta;

      let x = l_quat.x * s1 + r_quat.x * s2;
      let y = l_quat.y * s1 + r_quat.y * s2;
      let z = l_quat.z * s1 + r_quat.z * s2;
      let w = l_quat.w * s1 + r_quat.w * s2;

      // normalize
      const length = Math.hypot(x, y, z, w);
      x = x / length;
      y = y / length;
      z = z / length;
      w = w / length;

      return new Quaternion(x, y, z, w);
    }
  }

  multiply(q: Quaternion) {
    const x =
      q.w * this.x +
      q.z * this.y -
      q.y * this.z +
      q.x * this.w;
    const y =
      -q.z * this.x +
      q.w * this.y +
      q.x * this.z +
      q.y * this.w;
    const z =
      q.y * this.x -
      q.x * this.y +
      q.w * this.z +
      q.z * this.w;
    const w =
      -q.x * this.x -
      q.y * this.y -
      q.z * this.z +
      q.w * this.w;

    return new Quaternion(x, y, z, w);
  }

  toEulerAngles() {
    const x = this.x, y = this.y, z = this.z, w = this.w;
		const x2 = x + x,	y2 = y + y, z2 = z + z;
		const xx = x * x2, xy = x * y2, xz = x * z2;
		const yy = y * y2, yz = y * z2, zz = z * z2;
		const wx = w * x2, wy = w * y2, wz = w * z2;

		const e0 = ( 1 - ( yy + zz ) );
		const e1 = ( xy + wz );
		const e2 = ( xz - wy );
		const e3 = 0;

		const e4 = ( xy - wz );
		const e5 = ( 1 - ( xx + zz ) );
		const e6 = ( yz + wx );
		const e7 = 0;

		const e8 = ( xz + wy );
		const e9 = ( yz - wx );
		const e10 = ( 1 - ( xx + yy ) );
		const e11 = 0;

		const e12 = 0;
		const e13 = 0;
		const e14 = 0;
		const e15 = 1;

    // order: XYZ
    const _y = Math.asin( - Math.max(-1, Math.min(1, e2) ) );
    if ( Math.abs( e2 ) < 0.99999 ) {
      const _x = Math.atan2( e6, e10 );
      const _z = Math.atan2( e1, e0 );
      return new Vector3(_x, _y, _z);
    } else {
      const _x = 0
      const _z = Math.atan2( -e4, e5 );
      return new Vector3(_x, _y, _z);
    }
  }

  static fromMatrix4(mat: Matrix4) {
    const tr = mat.m00 + mat.m11 + mat.m22;

    if (tr > 0) {
      const S = 0.5 / Math.sqrt(tr + 1.0);
      const x = (mat.m21 - mat.m12) * S;
      const y = (mat.m02 - mat.m20) * S;
      const z = (mat.m10 - mat.m01) * S;
      const w = 0.25 / S;
      return new Quaternion(x, y, z, w);
    } else if (mat.m00 > mat.m11 && mat.m00 > mat.m22) {
      const S = Math.sqrt(1.0 + mat.m00 - mat.m11 - mat.m22) * 2;
      const x = 0.25 * S;
      const y = (mat.m01 + mat.m10) / S;
      const z = (mat.m02 + mat.m20) / S;
      const w = (mat.m21 - mat.m12) / S;
      return new Quaternion(x, y, z, w);
    } else if (mat.m11 > mat.m22) {
      const S = Math.sqrt(1.0 + mat.m11 - mat.m00 - mat.m22) * 2;
      const x = (mat.m01 + mat.m10) / S;
      const y = 0.25 * S;
      const z = (mat.m12 + mat.m21) / S;
      const w = (mat.m02 - mat.m20) / S;
      return new Quaternion(x, y, z, w);
    } else {
      const S = Math.sqrt(1.0 + mat.m22 - mat.m00 - mat.m11) * 2;
      const x = (mat.m02 + mat.m20) / S;
      const y = (mat.m12 + mat.m21) / S;
      const z = 0.25 * S;
      const w = (mat.m10 - mat.m01) / S;
      return new Quaternion(x, y, z, w);
    }
  }
}

また、クォータニオンから行列へ変換できるようにするため、Matrix4クラスに以下の記述を追加します。

...
 static fromQuaternion(q: Quaternion) {
    const sx = q.x * q.x;
    const sy = q.y * q.y;
    const sz = q.z * q.z;
    const cx = q.y * q.z;
    const cy = q.x * q.z;
    const cz = q.x * q.y;
    const wx = q.w * q.x;
    const wy = q.w * q.y;
    const wz = q.w * q.z;

    const v0 = 1.0 - 2.0 * (sy + sz);
    const v4 = 2.0 * (cz - wz);
    const v8 = 2.0 * (cy + wy);
    const v12 = 0;
    const v1 = 2.0 * (cz + wz);
    const v5 = 1.0 - 2.0 * (sx + sz);
    const v9 = 2.0 * (cx - wx);
    const v13 = 0;
    const v2 = 2.0 * (cy - wy);
    const v6 = 2.0 * (cx + wx);
    const v10 = 1.0 - 2.0 * (sx + sy);
    const v14 = 0;
    const v3 = 0;
    const v7 = 0;
    const v11 = 0;
    const v15 = 1;

    return new Matrix4(
      v0, v4, v8, v12,
      v1, v5, v9, v13,
      v2, v6, v10, v14,
      v3, v7, v11, v15
    );
  }
...

クォータニオンクラスの使い方

クォータニオンは回転を表します。

  const q = new Quaternion(0.5, 0.5, 0.5, 0.5);

クォータニオン同士は、掛け算することができます。
以下は、X軸周りに45度回転するクォータニオンと、次にY軸周りに４５度回転するクォータニオンを掛け算して、
X軸周りに45度回転し、次にY軸周りに４５度回転した結果の方向に回転するクォータニオンを生成しています。

  const qx = new Quaternion(0.383, 0, 0, 0.924); // 45 degree rotation around x axis
  const qy = new Quaternion(0, 0.383, 0, 0.924); // 45 degree rotation around y axis
  const qyx = qy.multiply(qx);

クォータニオンからオイラー角に変換することができます。

  const q = new Quaternion(0.5, 0.5, 0.5, 0.5);
  const e = q.toEulerAngles(); // Vector3(Math.PI / 2, 0, Math.PI / 2)

クォータニオン同士を補間することもできます。次は、X軸周りに90度回転するクォータニオンとY軸周りに90度回転するクォータニオンの中間の回転をするクォータニオンを生成しています。

  const x = new Quaterion(0.707, 0, 0, 0.707);　// 90 degree rotation around x axis
  const y = new Quaterion(0, 0.707, 0, 0.707);　// ９０ degree rotation around x axis
  const q = Quaternion.qlerp(x, y, 0.5);

クォータニオンを行列（回転行列）に変換することができます。また、行列からクォータニオンに変換することもできます。

  const q = new Quaternion(0.5, 0.5, 0.5, 0.5);
  const mat = Matrix4.fromQuaternion(q);
  const q2 = Quaternion.fromMatrix4(mat);

クォータニオンクラスのテスト

テストコードも追加しましょう。

import { Matrix4 } from "./Matrix4.js";
import { Quaternion } from "./Quaternion.js";
import { Vector3 } from "./Vector3.js";
import { Vector4 } from "./Vector4.js";

test("Quaternion toEulerAngle", ()=>{
  const q = new Quaternion(0.5, 0.5, 0.5, 0.5);
  const e = q.toEulerAngles();

  expect(e.isEqual(new Vector3(Math.PI / 2, 0, Math.PI / 2))).toBe(true);
});

test("Quaternion.multiply", () => {
  const qx = new Quaternion(0.383, 0, 0, 0.924); // 45 degree rotation around x axis
  const qy = new Quaternion(0, 0.383, 0, 0.924); // 45 degree rotation around y axis
  const qyx = qy.multiply(qx);

  const mx = Matrix4.fromQuaternion(qx);
  const my = Matrix4.fromQuaternion(qy);
  const myx = my.multiply(mx);
  const myx2 = Matrix4.fromQuaternion(qyx);

  expect(myx.isEqual(myx2, 0.001)).toBe(true);
});

test("Quaternion.multiply", () => {
  const qx = new Quaternion(0.383, 0, 0, 0.924); // 45 degree rotation around x axis
  const qy = new Quaternion(0, 0.383, 0, 0.924); // 45 degree rotation around y axis
  const qyx = qy.multiply(qx);
  const myx = Matrix4.fromQuaternion(qyx);
  const myx2 = Matrix4.rotationXYZ(qyx.toEulerAngles());

  expect(myx.isEqual(myx2, 0.001)).toBe(true);
});

test("Quaternion.fromMatrix4", () => {
  const q = new Quaternion(0.5, 0.5, 0.5, 0.5);
  const m = Matrix4.fromQuaternion(q);
  const q2 = Quaternion.fromMatrix4(m);

  expect(q.isEqual(q2)).toBe(true);
});

最後に

クォータニオンクラスを導入したことで、回転を扱えるようになりました。
次は、Vector、Quaternionを使ってTransformクラスを作ります。

ここまでのソースコードはこちらで公開しています 。

１3日目：行列クラスを作る

15日目：トランスフォームクラスを作る