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