Main.java Source code

Introduction

Here is the source code for Main.java

Source

//package com.java2s;

import android.hardware.SensorManager;

public class Main {
    public static final float NS2S = 1.0f / 1000000000.0f;

    public static float[] getRotationFromGyro(float[] values, float timestamp, float nowTimeStamp,
            float[] currentRotMatrix, boolean swapX, boolean swapY, boolean swapZ) {
        float[] deltaRotationVector = new float[4];
        if (timestamp != 0) {
            final float dT = (nowTimeStamp - timestamp) * NS2S;
            float axisX = swapX ? -values[0] : values[0];
            float axisY = swapY ? -values[1] : values[1];
            float axisZ = swapZ ? -values[2] : values[2];

            float omegaMagnitude = (float) Math.sqrt(axisX * axisX + axisY * axisY + axisZ * axisZ);
            if (omegaMagnitude > 0.1f) {
                axisX /= omegaMagnitude;
                axisY /= omegaMagnitude;
                axisZ /= omegaMagnitude;
            }

            float thetaOverTwo = omegaMagnitude * dT / 2.0f;
            float sinThetaOverTwo = (float) Math.sin(thetaOverTwo);
            float cosThetaOverTwo = (float) Math.cos(thetaOverTwo);
            deltaRotationVector[0] = sinThetaOverTwo * axisX;
            deltaRotationVector[1] = sinThetaOverTwo * axisY;
            deltaRotationVector[2] = sinThetaOverTwo * axisZ;
            deltaRotationVector[3] = cosThetaOverTwo;
        }
        float[] deltaRotationMatrix = new float[16];
        SensorManager.getRotationMatrixFromVector(deltaRotationMatrix, deltaRotationVector);
        return naivMatrixMultiply(currentRotMatrix, deltaRotationMatrix);
    }

    public static float[] naivMatrixMultiply(float[] B, float[] A) {
        int mA, nA, mB, nB;
        mA = nA = (int) Math.sqrt(A.length);
        mB = nB = (int) Math.sqrt(B.length);
        if (nA != mB)
            throw new RuntimeException("Illegal matrix dimensions.");

        float[] C = new float[mA * nB];

        for (int i = 0; i < mA; i++)
            for (int j = 0; j < nB; j++)
                for (int k = 0; k < nA; k++)
                    C[i + nA * j] += (A[i + nA * k] * B[k + nB * j]);
        return C;
    }
}