Examples with Quat4f javax.vecmath.Quat4f used on opensource projects

Example 11 with Quat4f

use of javax.vecmath.Quat4f in project MinecraftForge by MinecraftForge.

the class TRSRTransformation method slerp.

public static Quat4f slerp(Quat4f from, Quat4f to, float progress) {
    Quat4f res = new Quat4f();
    res.interpolate(from, to, progress);
    return res;
}

Example 12 with Quat4f

use of javax.vecmath.Quat4f in project MinecraftForge by MinecraftForge.

the class TRSRTransformation method stepJacobi.

private static Quat4f stepJacobi(Matrix3f m) {
    Matrix3f t = new Matrix3f();
    Quat4f qt = new Quat4f(), ret = new Quat4f(0, 0, 0, 1);
    Pair<Float, Float> p;
    // 01
    if (m.m01 * m.m01 + m.m10 * m.m10 > eps) {
        p = approxGivensQuat(m.m00, .5f * (m.m01 + m.m10), m.m11);
        qt.set(0, 0, p.getLeft(), p.getRight());
        //qt.normalize();
        ret.mul(qt);
        //t.set(qt);
        t.setIdentity();
        t.m00 = qt.w * qt.w - qt.z * qt.z;
        t.m11 = t.m00;
        t.m10 = 2 * qt.z * qt.w;
        t.m01 = -t.m10;
        t.m22 = qt.w * qt.w + qt.z * qt.z;
        m.mul(m, t);
        t.transpose();
        m.mul(t, m);
    }
    // 02
    if (m.m02 * m.m02 + m.m20 * m.m20 > eps) {
        p = approxGivensQuat(m.m00, .5f * (m.m02 + m.m20), m.m22);
        qt.set(0, -p.getLeft(), 0, p.getRight());
        //qt.normalize();
        ret.mul(qt);
        //t.set(qt);
        t.setIdentity();
        t.m00 = qt.w * qt.w - qt.y * qt.y;
        t.m22 = t.m00;
        t.m20 = -2 * qt.y * qt.w;
        t.m02 = -t.m20;
        t.m11 = qt.w * qt.w + qt.y * qt.y;
        m.mul(m, t);
        t.transpose();
        m.mul(t, m);
    }
    // 12
    if (m.m12 * m.m12 + m.m21 * m.m21 > eps) {
        p = approxGivensQuat(m.m11, .5f * (m.m12 + m.m21), m.m22);
        qt.set(p.getLeft(), 0, 0, p.getRight());
        //qt.normalize();
        ret.mul(qt);
        //t.set(qt);
        t.setIdentity();
        t.m11 = qt.w * qt.w - qt.x * qt.x;
        t.m22 = t.m11;
        t.m21 = 2 * qt.x * qt.w;
        t.m12 = -t.m21;
        t.m00 = qt.w * qt.w + qt.x * qt.x;
        m.mul(m, t);
        t.transpose();
        m.mul(t, m);
    }
    return ret;
}

Example 13 with Quat4f

use of javax.vecmath.Quat4f in project MinecraftForge by MinecraftForge.

the class TRSRTransformation method svdDecompose.

/*
     * Performs SVD decomposition of m, accumulating reflection in the scale (U and V are pure rotations).
     */
public static Triple<Quat4f, Vector3f, Quat4f> svdDecompose(Matrix3f m) {
    // determine V by doing 5 steps of Jacobi iteration on MT * M
    Quat4f u = new Quat4f(0, 0, 0, 1), v = new Quat4f(0, 0, 0, 1), qt = new Quat4f();
    Matrix3f b = new Matrix3f(m), t = new Matrix3f();
    t.transpose(m);
    b.mul(t, b);
    for (int i = 0; i < 5; i++) v.mul(stepJacobi(b));
    v.normalize();
    t.set(v);
    b.set(m);
    b.mul(t);
    // FIXME: this doesn't work correctly for some reason; not crucial, so disabling for now; investigate in the future.
    //sortSingularValues(b, v);
    Pair<Float, Float> p;
    float ul = 1f;
    p = qrGivensQuat(b.m00, b.m10);
    qt.set(0, 0, p.getLeft(), p.getRight());
    u.mul(qt);
    t.setIdentity();
    t.m00 = qt.w * qt.w - qt.z * qt.z;
    t.m11 = t.m00;
    t.m10 = -2 * qt.z * qt.w;
    t.m01 = -t.m10;
    t.m22 = qt.w * qt.w + qt.z * qt.z;
    ul *= t.m22;
    b.mul(t, b);
    p = qrGivensQuat(b.m00, b.m20);
    qt.set(0, -p.getLeft(), 0, p.getRight());
    u.mul(qt);
    t.setIdentity();
    t.m00 = qt.w * qt.w - qt.y * qt.y;
    t.m22 = t.m00;
    t.m20 = 2 * qt.y * qt.w;
    t.m02 = -t.m20;
    t.m11 = qt.w * qt.w + qt.y * qt.y;
    ul *= t.m11;
    b.mul(t, b);
    p = qrGivensQuat(b.m11, b.m21);
    qt.set(p.getLeft(), 0, 0, p.getRight());
    u.mul(qt);
    t.setIdentity();
    t.m11 = qt.w * qt.w - qt.x * qt.x;
    t.m22 = t.m11;
    t.m21 = -2 * qt.x * qt.w;
    t.m12 = -t.m21;
    t.m00 = qt.w * qt.w + qt.x * qt.x;
    ul *= t.m00;
    b.mul(t, b);
    ul = 1f / ul;
    u.scale((float) Math.sqrt(ul));
    Vector3f s = new Vector3f(b.m00 * ul, b.m11 * ul, b.m22 * ul);
    return Triple.of(u, s, v);
}

Example 14 with Quat4f

use of javax.vecmath.Quat4f in project MinecraftForge by MinecraftForge.

the class TRSRTransformation method sortSingularValues.

@SuppressWarnings("unused")
private static void sortSingularValues(Matrix3f b, Quat4f v) {
    float p0 = b.m00 * b.m00 + b.m10 * b.m10 + b.m20 * b.m20;
    float p1 = b.m01 * b.m01 + b.m11 * b.m11 + b.m21 * b.m21;
    float p2 = b.m02 * b.m02 + b.m12 * b.m12 + b.m22 * b.m22;
    Quat4f t = new Quat4f();
    if (p0 < p1) {
        swapNeg(b, 0, 1);
        t.set(0, 0, sq2, sq2);
        v.mul(t);
        float f = p0;
        p0 = p1;
        p1 = f;
    }
    if (p0 < p2) {
        swapNeg(b, 0, 2);
        t.set(0, sq2, 0, sq2);
        v.mul(t);
        float f = p0;
        p0 = p2;
        p2 = f;
    }
    if (p1 < p2) {
        swapNeg(b, 1, 2);
        t.set(sq2, 0, 0, sq2);
        v.mul(t);
    }
}

Example 15 with Quat4f

use of javax.vecmath.Quat4f in project MinecraftForge by MinecraftForge.

the class TRSRTransformation method quatFromXYZ.

public static Quat4f quatFromXYZ(float x, float y, float z) {
    Quat4f ret = new Quat4f(0, 0, 0, 1), t = new Quat4f();
    t.set((float) Math.sin(x / 2), 0, 0, (float) Math.cos(x / 2));
    ret.mul(t);
    t.set(0, (float) Math.sin(y / 2), 0, (float) Math.cos(y / 2));
    ret.mul(t);
    t.set(0, 0, (float) Math.sin(z / 2), (float) Math.cos(z / 2));
    ret.mul(t);
    return ret;
}