Home | History | Annotate | Download | only in Utils 
      1 /*
      2  * Copyright (C) 2016 The Android Open Source Project
      3  *
      4  * Licensed under the Apache License, Version 2.0 (the "License");
      5  * you may not use this file except in compliance with the License.
      6  * You may obtain a copy of the License at
      7  *
      8  *      http://www.apache.org/licenses/LICENSE-2.0
      9  *
     10  * Unless required by applicable law or agreed to in writing, software
     11  * distributed under the License is distributed on an "AS IS" BASIS,
     12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
     13  * See the License for the specific language governing permissions and
     14  * limitations under the License.
     15  */
     16 package com.android.cts.verifier.sensors.sixdof.Utils;
     17 
     18 import android.opengl.Matrix;
     19 import android.util.Log;
     20 
     21 import com.android.cts.verifier.sensors.sixdof.Utils.PoseProvider.PoseData;
     22 
     23 import java.text.DecimalFormat;
     24 
     25 /**
     26  * Contains functions that are used throughout the app.
     27  */
     28 public class MathsUtils {
     29     public static final int X = PoseData.INDEX_TRANSLATION_X;
     30     public static final int Y = PoseData.INDEX_TRANSLATION_Y;
     31     public static final int Z = PoseData.INDEX_TRANSLATION_Z;
     32 
     33     public static final int MATRIX_4X4 = 16;
     34     public static final int VECTOR_2D = 2;
     35     public static final int VECTOR_3D = 3;
     36 
     37     public static final int ORIENTATION_0 = 0;
     38     public static final int ORIENTATION_90_ANTI_CLOCKWISE = 90;
     39     public static final int ORIENTATION_180_ANTI_CLOCKWISE = 180;
     40     public static final int ORIENTATION_270_ANTI_CLOCKWISE = 270;
     41     public static final int ORIENTATION_360_ANTI_CLOCKWISE = 360;
     42 
     43     /**
     44      * Converts from float array in 6DoF coordinate system to a Vector3 in OpenGl coordinate
     45      * system.
     46      *
     47      * @param location float array to convert.
     48      * @return the Vector3 in OpenGL coord system.
     49      */
     50     public static float[] convertToOpenGlCoordinates(float[] location, int toRotate) {
     51         // Have to swap Y and Z as they are different in OpenGl and 6DoF. Also invert Z.
     52         float[] inDefaultOrientation = new float[]{location[X], location[Z], -location[Y]};
     53 
     54         return rotateCoordinates(inDefaultOrientation, toRotate);
     55     }
     56 
     57     public static float[] rotateCoordinates(float[] coordinates, int toRotate) {
     58         final float[] inCurrentOrientation;
     59 
     60         switch (toRotate) {
     61             case ORIENTATION_0:
     62             case ORIENTATION_360_ANTI_CLOCKWISE:
     63                 inCurrentOrientation = coordinates;
     64                 break;
     65             case ORIENTATION_90_ANTI_CLOCKWISE:
     66                 inCurrentOrientation = new float[]{coordinates[Y], -coordinates[X],
     67                         coordinates[Z]};
     68                 break;
     69             case ORIENTATION_180_ANTI_CLOCKWISE:
     70                 inCurrentOrientation = new float[]{coordinates[X], coordinates[Y],
     71                         coordinates[Z]};
     72                 break;
     73             case ORIENTATION_270_ANTI_CLOCKWISE:
     74                 inCurrentOrientation = new float[]{-coordinates[Y], coordinates[X],
     75                         coordinates[Z]};
     76                 break;
     77             default:
     78                 throw new RuntimeException("Unexpected orientation that cannot be dealt with!");
     79         }
     80 
     81         return inCurrentOrientation;
     82     }
     83 
     84     /**
     85      * Produce a rotation transformation that looks at a point 'center' from a point 'eye'.
     86      */
     87     public static void setLookAtM(float[] rm, float eyeX, float eyeY, float eyeZ,
     88                                   float centerX, float centerY, float centerZ,
     89                                   float upX, float upY, float upZ) {
     90         // Algorithm taken from DirectX documentation.
     91         // https://msdn.microsoft.com/en-us/library/bb205343.aspx
     92 
     93         float zAxisX = eyeX - centerX;
     94         float zAxisY = eyeY - centerY;
     95         float zAxisZ = eyeZ - centerZ;
     96 
     97         // Normalize zAxis
     98         float rlf = 1.0f / Matrix.length(zAxisX, zAxisY, zAxisZ);
     99         zAxisX *= rlf;
    100         zAxisY *= rlf;
    101         zAxisZ *= rlf;
    102 
    103         // compute xAxis = up x zAxis (x means "cross product")
    104         float xAxisX = upY * zAxisZ - upZ * zAxisY;
    105         float xAxisY = upZ * zAxisX - upX * zAxisZ;
    106         float xAxisZ = upX * zAxisY - upY * zAxisX;
    107 
    108         // and normalize xAxis
    109         float rls = 1.0f / Matrix.length(xAxisX, xAxisY, xAxisZ);
    110         xAxisX *= rls;
    111         xAxisY *= rls;
    112         xAxisZ *= rls;
    113 
    114         // compute yAxis = zAxis x xAxis
    115         float yAxisX = zAxisY * xAxisZ - zAxisZ * xAxisY;
    116         float yAxisY = zAxisZ * xAxisX - zAxisX * xAxisZ;
    117         float yAxisZ = zAxisX * xAxisY - zAxisY * xAxisX;
    118 
    119         rm[0] = xAxisX;
    120         rm[1] = xAxisY;
    121         rm[2] = xAxisZ;
    122         rm[3] = (xAxisX * eyeX) + (xAxisY * eyeY) + (xAxisZ * eyeZ);
    123 
    124         rm[4] = yAxisX;
    125         rm[5] = yAxisY;
    126         rm[6] = yAxisZ;
    127         rm[7] = (yAxisX * eyeX) + (yAxisY * eyeY) + (yAxisZ * eyeZ);
    128 
    129         rm[8] = zAxisX;
    130         rm[9] = zAxisY;
    131         rm[10] = zAxisZ;
    132         rm[11] = (zAxisX * eyeX) + (zAxisY * eyeY) + (zAxisZ * eyeZ);
    133 
    134         rm[12] = 0.0f;
    135         rm[13] = 0.0f;
    136         rm[14] = 0.0f;
    137         rm[15] = 1.0f;
    138     }
    139 
    140     /**
    141      * A function to convert a quaternion to quaternion Matrix. Please note that Opengl.Matrix is
    142      * Column Major and so we construct the matrix in Column Major Format. - - - - | 0 4 8 12 | | 1
    143      * 5 9 13 | | 2 6 10 14 | | 3 7 11 15 | - - - -
    144      *
    145      * @param quaternion Input quaternion with float[4]
    146      * @return Quaternion Matrix of float[16]
    147      */
    148     public static float[] quaternionMatrixOpenGL(float[] quaternion) {
    149         float[] matrix = new float[16];
    150         normalizeVector(quaternion);
    151 
    152         float x = quaternion[0];
    153         float y = quaternion[1];
    154         float z = quaternion[2];
    155         float w = quaternion[3];
    156 
    157         float x2 = x * x;
    158         float y2 = y * y;
    159         float z2 = z * z;
    160         float xy = x * y;
    161         float xz = x * z;
    162         float yz = y * z;
    163         float wx = w * x;
    164         float wy = w * y;
    165         float wz = w * z;
    166 
    167         matrix[0] = 1f - 2f * (y2 + z2);
    168         matrix[4] = 2f * (xy - wz);
    169         matrix[8] = 2f * (xz + wy);
    170         matrix[12] = 0f;
    171 
    172         matrix[1] = 2f * (xy + wz);
    173         matrix[5] = 1f - 2f * (x2 + z2);
    174         matrix[9] = 2f * (yz - wx);
    175         matrix[13] = 0f;
    176 
    177         matrix[2] = 2f * (xz - wy);
    178         matrix[6] = 2f * (yz + wx);
    179         matrix[10] = 1f - 2f * (x2 + y2);
    180         matrix[14] = 0f;
    181 
    182         matrix[3] = 0f;
    183         matrix[7] = 0f;
    184         matrix[11] = 0f;
    185         matrix[15] = 1f;
    186 
    187         return matrix;
    188     }
    189 
    190     /**
    191      * Calculates the dot product between two vectors.
    192      *
    193      * @param vector1 the first vector.
    194      * @param vector2 the second vector.
    195      * @return the dot product.
    196      */
    197     public static double dotProduct(float[] vector1, float[] vector2, int dimensions) {
    198         double total = 0.0;
    199         for (int i = 0; i < dimensions; i++) {
    200             total += vector1[i] * vector2[i];
    201         }
    202         return total;
    203     }
    204 
    205     /**
    206      * Creates a unit vector in the direction of an arbitrary vector. The original vector is
    207      * modified in place.
    208      *
    209      * @param v the vector to normalize.
    210      */
    211     public static void normalizeVector(float[] v) {
    212         float mag2 = v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3];
    213         if (Math.abs(mag2) > 0.00001f && Math.abs(mag2 - 1.0f) > 0.00001f) {
    214             float mag = (float) Math.sqrt(mag2);
    215             v[0] = v[0] / mag;
    216             v[1] = v[1] / mag;
    217             v[2] = v[2] / mag;
    218             v[3] = v[3] / mag;
    219         }
    220     }
    221 
    222     /**
    223      * Calculates the distance between 2 points in 2D space.
    224      *
    225      * @param point1 the mCoordinates of the first point.
    226      * @param point2 the mCoordinates of the second point.
    227      * @return the distance between the 2 points.
    228      */
    229     public static float distanceCalculationOnXYPlane(float[] point1, float[] point2) {
    230         float yDifference = point2[Y] - point1[Y];
    231         float xDifference = point2[X] - point1[X];
    232         return (float) Math.sqrt((yDifference * yDifference) + (xDifference * xDifference));
    233     }
    234 
    235     /**
    236      * Calculates the distance between 2 points in 3D space.
    237      *
    238      * @param point1 the mCoordinates of the first point.
    239      * @param point2 the mCoordinates of the second point.
    240      * @return the distance between the 2 points.
    241      */
    242     public static float distanceCalculationInXYZSpace(float[] point1, float[] point2) {
    243         float zDifference = point2[Z] - point1[Z];
    244         float yDifference = point2[Y] - point1[Y];
    245         float xDifference = point2[X] - point1[X];
    246         return (float) Math.sqrt((zDifference * zDifference) + (yDifference * yDifference) +
    247                 (xDifference * xDifference));
    248     }
    249 
    250     /**
    251      * Puts the given coordinates in a printable format.
    252      *
    253      * @param coordinates the mCoordinates to print.
    254      * @return the mCoordinates formatted.
    255      */
    256     public static String coordinatesToString(float[] coordinates) {
    257         DecimalFormat threeDec = new DecimalFormat("0.0");
    258         String formattedCoordinates = "[";
    259         for (int i = 0; i < coordinates.length; i++) {
    260             formattedCoordinates += threeDec.format(coordinates[i]);
    261             if (i < (coordinates.length - 1)) {
    262                 formattedCoordinates += ", ";
    263             } else {
    264                 formattedCoordinates += "]";
    265             }
    266         }
    267         return formattedCoordinates;
    268     }
    269 
    270     public static float[] getDeviceOrientationMatrix(int toRotate) {
    271         float[] deviceMatrix;
    272 
    273         switch (toRotate) {
    274             case ORIENTATION_0:
    275             case ORIENTATION_360_ANTI_CLOCKWISE:
    276                 deviceMatrix = new float[]{1.0f, 0.0f, 0.0f, 0.0f,
    277                         0.0f, 1.0f, 0.0f, 0.0f,
    278                         0.0f, 0.0f, 1.0f, 0.0f,
    279                         0.0f, 0.0f, 0.0f, 1.0f};
    280                 break;
    281             case ORIENTATION_90_ANTI_CLOCKWISE:
    282                 deviceMatrix = new float[]{
    283                         0.0f, -1.0f, 0.0f, 0.0f,
    284                         1.0f, 0.0f, 0.0f, 0.0f,
    285                         0.0f, 0.0f, 1.0f, 0.0f,
    286                         0.0f, 0.0f, 0.0f, 1.0f};
    287                 break;
    288             case ORIENTATION_180_ANTI_CLOCKWISE:
    289                 deviceMatrix = new float[]{
    290                         -1.0f, 0.0f, 0.0f, 0.0f,
    291                         0.0f, -1.0f, 0.0f, 0.0f,
    292                         0.0f, 0.0f, 1.0f, 0.0f,
    293                         0.0f, 0.0f, 0.0f, 1.0f};
    294                 break;
    295             case ORIENTATION_270_ANTI_CLOCKWISE:
    296                 deviceMatrix = new float[]{
    297                         0.0f, 1.0f, 0.0f, 0.0f,
    298                         -1.0f, 0.0f, 0.0f, 0.0f,
    299                         0.0f, 0.0f, 1.0f, 0.0f,
    300                         0.0f, 0.0f, 0.0f, 1.0f};
    301                 break;
    302             default:
    303                 throw new RuntimeException("Unexpected orientation that cannot be dealt with!");
    304         }
    305 
    306         return deviceMatrix;
    307     }
    308 }
    309 
    310