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      */
convertToOpenGlCoordinates(float[] location, int toRotate)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 
rotateCoordinates(float[] coordinates, int toRotate)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      */
setLookAtM(float[] rm, float eyeX, float eyeY, float eyeZ, float centerX, float centerY, float centerZ, float upX, float upY, float upZ)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      */
quaternionMatrixOpenGL(float[] quaternion)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      */
dotProduct(float[] vector1, float[] vector2, int dimensions)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      */
normalizeVector(float[] v)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      */
distanceCalculationOnXYPlane(float[] point1, float[] point2)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      */
distanceCalculationInXYZSpace(float[] point1, float[] point2)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      */
coordinatesToString(float[] coordinates)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 
getDeviceOrientationMatrix(int toRotate)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