Java 3D slightly off matrices cause NaNs - Java javax.media.j3d

Java examples for javax.media.j3d:Transform3D

Description

Java 3D slightly off matrices cause NaNs

Demo Code

import java.math.BigDecimal;
import java.math.RoundingMode;
import java.util.Enumeration;
import javax.media.j3d.Appearance;
import javax.media.j3d.BoundingSphere;
import javax.media.j3d.BranchGroup;
import javax.media.j3d.Material;
import javax.media.j3d.Node;
import javax.media.j3d.PolygonAttributes;
import javax.media.j3d.Shape3D;
import javax.media.j3d.Transform3D;
import javax.media.j3d.TransformGroup;
import javax.vecmath.Color3f;
import javax.vecmath.Point3d;
import javax.vecmath.Point3f;
import javax.vecmath.Quat4f;
import javax.vecmath.Vector3f;

public class Main{
    /**// w  ww  . j a  va2s .  c om
     * slightly off matrices cause NaNs
     * @param t1
     * @param q1
     * @param doIt
     */
    public static void safeGetQuat(Transform3D t1, Quat4f q1) {
        float[] mat = new float[16];
        t1.get(mat);
        float[] rot = new float[9];
        rot[0] = mat[0];
        rot[1] = mat[1];
        rot[2] = mat[2];
        rot[3] = mat[4];
        rot[4] = mat[5];
        rot[5] = mat[6];
        rot[6] = mat[8];
        rot[7] = mat[9];
        rot[8] = mat[10];

        double ww = 0.25 * (1.0 + rot[0] + rot[4] + rot[8]);

        // no negatives or the sqrt below starts baking Nans
        ww = (ww < 0 ? 0 : ww);
        if (!(ww < 1.0e-10)) {
            q1.w = (float) Math.sqrt(ww);
            ww = 0.25 / q1.w;
            q1.x = (float) ((rot[7] - rot[5]) * ww);
            q1.y = (float) ((rot[2] - rot[6]) * ww);
            q1.z = (float) ((rot[3] - rot[1]) * ww);
            return;
        }

        q1.w = 0.0f;
        ww = -0.5 * (rot[4] + rot[8]);
        if (!((ww < 0 ? -ww : ww) < 1.0e-10)) {
            q1.x = (float) Math.sqrt(ww);
            ww = 0.5 / q1.x;
            q1.y = (float) (rot[3] * ww);
            q1.z = (float) (rot[6] * ww);
            return;
        }

        q1.x = 0.0f;
        ww = 0.5 * (1.0 - rot[8]);
        if (!((ww < 0 ? -ww : ww) < 1.0e-10)) {
            q1.y = (float) Math.sqrt(ww);
            q1.z = (float) (rot[7] / (2.0 * q1.y));
            return;
        }

        q1.y = 0.0f;
        q1.z = 1.0f;
    }
}

Related Tutorials