JAVAでQuaternionを使う

はじめに

JAVAではjavax.vecmath.Quat4dでQuaternionを使える
C++のboost::math::quaternionと同じ感覚で使ってちょっとハマったのでメモ

インスタンス化

Quat4d quaternion = new Quat4d(x, y, z, w);

で行える
この場合は(x, y, z)がそのままの値ではなく、正規化された値になるので、そのままの値でQuat4dを作成したい場合は関数を作成すると便利

public static Quat4d genQuat4d(final double x, final double y, final double z, final double w) {
    Quat4d tmp = new Quat4d();
    tmp.setX(x);
    tmp.setY(y);
    tmp.setZ(z);
    tmp.setW(w);

    return tmp;
}

回転

任意の点の回転は上の関数を用いて以下のようにする

Quat4d point = genQuat4d(x, y, z, 0);       // 回転させたい座標
Quat4d rot = new Quat4d(xr, yr, zr, wr);    // 回転クォータニオンを作成
Quat4d conj = new Quat4d(rot);

conj.conjugate();
conj.mul(point);
conj.mul(rot);

point.setX(conj.x);
point.setY(conj.y);
point.setZ(conj.z);

---

サンプルコード

import javax.vecmath.Vector3d;
import javax.vecmath.Quat4d;

public class Quaternion {
    public static Quat4d gen(final double x, final double y, final double z, final double w) {
        Quat4d tmp = new Quat4d();
        tmp.setX(x);
        tmp.setY(y);
        tmp.setZ(z);
        tmp.setW(w);

        return tmp;
    }
    public static Quat4d gen(final double angle, final Vector3d axis) {
        double sin2 = Math.sin(angle / 2.0);
        axis.normalize();
        return gen(axis.x*sin2, axis.y*sin2, axis.z*sin2, Math.cos(angle / 2.0));
    }
    public static Quat4d gen(final Vector3d axis) {
        return gen(axis.x, axis.y, axis.z, 0);
    }


    public static Quat4d rotate(final Quat4d src, final Quat4d rot) {
        Quat4d tmp = new Quat4d(rot);
        tmp.conjugate();
        tmp.mul(src);
        tmp.mul(rot);

        return tmp;
    }
    public static Quat4d rotate(final Vector3d src, final Quat4d rot) {
        return rotate(gen(src), rot);
    }
    public static Quat4d rotate(final double x, final double y, final double z, final Quat4d rot) {
        return rotate(new Vector3d(x, y, z), rot);
    }
}