rotation Matrix To Quaternion - Android java.lang

Android examples for java.lang:Math Matrix

  1. HOME
  2. Android
  3. java.lang
  4. Math Matrix

Description

rotation Matrix To Quaternion

Demo Code


//package com.java2s;

public class Main {
    public static void rotationMatrixToQuaternion(float[] R, float[] q) {
        final float m00 = R[0];
        final float m10 = R[1];
        final float m20 = R[2];
        final float m01 = R[4];
        final float m11 = R[5];
        final float m21 = R[6];
        final float m02 = R[8];
        final float m12 = R[9];
        final float m22 = R[10];

        float tr = m00 + m11 + m22;

        if (tr > 0) {
            float S = (float) Math.sqrt(tr + 1.0) * 2; // S=4*qw 
            q[0] = 0.25f * S;/* w  w  w.j ava  2s.co m*/
            q[1] = (m21 - m12) / S;
            q[2] = (m02 - m20) / S;
            q[3] = (m10 - m01) / S;
        } else if ((m00 > m11) & (m00 > m22)) {
            float S = (float) Math.sqrt(1.0 + m00 - m11 - m22) * 2; //
            // S=4*q[1]
            q[0] = (m21 - m12) / S;
            q[1] = 0.25f * S;
            q[2] = (m01 + m10) / S;
            q[3] = (m02 + m20) / S;
        } else if (m11 > m22) {
            float S = (float) Math.sqrt(1.0 + m11 - m00 - m22) * 2; //
            // S=4*q[2]
            q[0] = (m02 - m20) / S;
            q[1] = (m01 + m10) / S;
            q[2] = 0.25f * S;
            q[3] = (m12 + m21) / S;
        } else {
            float S = (float) Math.sqrt(1.0 + m22 - m00 - m11) * 2; //
            // S=4*q[3]
            q[0] = (m10 - m01) / S;
            q[1] = (m02 + m20) / S;
            q[2] = (m12 + m21) / S;
            q[3] = 0.25f * S;
        }

    }
}

Related Tutorials