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.Path;
17 
18 import com.android.cts.verifier.sensors.sixdof.Activities.TestActivity;
19 import com.android.cts.verifier.sensors.sixdof.Utils.MathsUtils;
20 import com.android.cts.verifier.sensors.sixdof.Utils.Exceptions.WaypointAreaCoveredException;
21 import com.android.cts.verifier.sensors.sixdof.Utils.Exceptions.WaypointDistanceException;
22 import com.android.cts.verifier.sensors.sixdof.Utils.Exceptions.WaypointStartPointException;
23 import com.android.cts.verifier.sensors.sixdof.Utils.Path.PathUtilityClasses.RotationData;
24 import com.android.cts.verifier.sensors.sixdof.Utils.Path.PathUtilityClasses.Waypoint;
25 
26 import android.opengl.Matrix;
27 import java.util.ArrayList;
28 import java.util.Timer;
29 import java.util.TimerTask;
30 
31 /**
32  * Handles all the path properties of the robustness path.
33  */
34 public class RobustnessPath extends Path {
35     public static final long TIME_TO_ADD_MARKER = 20000;
36     private static final int MAXIMUM_ROTATION_ANGLE = 40;
37     private static final int MINIMUM_ROTATION_ANGLE = MAXIMUM_ROTATION_ANGLE * -1;
38     private static final long TARGET_ROTATION_TIMER_INTERVALS = 100;
39     private static final float CHANGE_IN_ANGLE = 0.5f;
40     private static final int MAXIMUM_ROTATION_INACCURACY = 10;
41     private static final double DISTANCE_FROM_MARKER = 0.5F;
42 
43 
44     private static final float[] X_AXIS = new float[]{1, 0, 0, 0};
45 
46     private float mTargetRotation = 0;
47     private boolean mRotationPhase = true;
48     private ArrayList<RotationData> mPathRotations = new ArrayList<>();
49     private ArrayList<Long> mMarkerTimeStamp = new ArrayList<>();
50     private float mDistanceOfPathFailedRotation = 0;
51 
52     private int mOpenGlRotation = 0;
53 
54     /**
55      * Constructor which starts the timer which changes the targetRotation.
56      */
RobustnessPath(int openGlRotation)57     public RobustnessPath(int openGlRotation) {
58         mOpenGlRotation = openGlRotation;
59         startChangingTargetRotation();
60     }
61 
62     /**
63      * Performs robustness path related checks on a marker.
64      *
65      * @param coordinates the coordinates to use for the waypoint
66      * @throws WaypointDistanceException    if the location is too close to another.
67      * @throws WaypointAreaCoveredException if the area covered by the user is too little.
68      * @throws WaypointStartPointException  if the location is not close enough to the start.
69      */
70     @Override
additionalChecks(float[] coordinates)71     public void additionalChecks(float[] coordinates)
72             throws WaypointStartPointException, WaypointDistanceException,
73             WaypointAreaCoveredException {
74         mMarkerTimeStamp.add(System.currentTimeMillis());
75         if (mPathMarkers.size() == 0) {
76             mTargetRotation = 0;
77         }
78     }
79 
80     /**
81      * Starts a timer which changes the target rotation at specified intervals.
82      */
startChangingTargetRotation()83     private void startChangingTargetRotation() {
84         Timer timer = new Timer();
85         timer.scheduleAtFixedRate(new TimerTask() {
86 
87             @Override
88             public void run() {
89                 synchronized (TestActivity.POSE_LOCK) {
90                     setRotationToMake();
91                 }
92             }
93         }, 0, TARGET_ROTATION_TIMER_INTERVALS);
94     }
95 
96     /**
97      * Performs the change to the target rotation.
98      */
setRotationToMake()99     private void setRotationToMake() {
100         if (mRotationPhase) {
101             mTargetRotation = mTargetRotation - CHANGE_IN_ANGLE;
102             if (mTargetRotation <= MINIMUM_ROTATION_ANGLE) {
103                 mRotationPhase = false;
104             }
105         } else {
106             mTargetRotation = mTargetRotation + CHANGE_IN_ANGLE;
107             if (mTargetRotation >= MAXIMUM_ROTATION_ANGLE) {
108                 mRotationPhase = true;
109             }
110         }
111     }
112 
113     /**
114      * Calculates the time left for the user to place the waypoint.
115      *
116      * @return the time left based on the current timestamp and the timestamp of the last marker.
117      */
calculateTimeRemaining()118     public long calculateTimeRemaining() {
119         long timeRemaining;
120         if (!mMarkerTimeStamp.isEmpty()) {
121             int lastTimestamp = mMarkerTimeStamp.size() - 1;
122             timeRemaining = System.currentTimeMillis() - mMarkerTimeStamp.get(lastTimestamp);
123             return TIME_TO_ADD_MARKER - timeRemaining;
124         }
125         return TIME_TO_ADD_MARKER;
126     }
127 
128     /**
129      * Converts the rotation from quaternion to euler.
130      *
131      * @param rotationQuaternion The quaternions of the current rotation.
132      * @return The euler rotation.
133      */
calculateRotation(float[] rotationQuaternion)134     private float calculateRotation(float[] rotationQuaternion) {
135         float qx = rotationQuaternion[0];
136         float qy = rotationQuaternion[1];
137         float qz = rotationQuaternion[2];
138         float qw = rotationQuaternion[3];
139 
140         // Set initial Vector to be -(X Axis).
141         double x = -X_AXIS[0];
142         double y = X_AXIS[1];
143         double z = X_AXIS[2];
144 
145         // Create quaternion based rotation matrix and extract the values that we need.
146         final double X = x * (qy * qy + qx * qx - qz * qz - qw * qw)
147                 + y * (2 * qy * qz - 2 * qx * qw)
148                 + z * (2 * qy * qw + 2 * qx * qz);
149         final double Y = x * (2 * qx * qw + 2 * qy * qz)
150                 + y * (qx * qx - qy * qy + qz * qz - qw * qw)
151                 + z * (-2 * qx * qy + 2 * qz * qw);
152         final double Z = x * (-2 * qx * qz + 2 * qy * qw)
153                 + y * (2 * qx * qy + 2 * qz * qw)
154                 + z * (qx * qx - qy * qy - qz * qz + qw * qw);
155 
156         // Invert X and Z axis.
157         float[] values = {(float) Z, (float) Y, (float) X, 0.0f};
158         MathsUtils.normalizeVector(values);
159 
160         // Rotate the X axis based on the orientation of the device.
161         float[] adjustedXAxis = new float[4];
162         Matrix.multiplyMV(adjustedXAxis, 0, MathsUtils.getDeviceOrientationMatrix(mOpenGlRotation),
163                 0, X_AXIS, 0);
164 
165         // Calculate angle between current pose and adjusted X axis.
166         double angle = Math.acos(MathsUtils.dotProduct(values, adjustedXAxis, MathsUtils.VECTOR_3D));
167 
168         // Set our angle to be 0 based when upright.
169         angle = Math.toDegrees(angle) - MathsUtils.ORIENTATION_90_ANTI_CLOCKWISE;
170         angle *= -1;
171 
172         return (float) angle;
173     }
174 
175     /**
176      * Test the rotation and create a rotation object.
177      *
178      * @param rotationQuaternion    The quaternions of the current rotation.
179      * @param rotationLocation      The location of the point with the rotation.
180      * @param referencePathMarkers  The list of markers in the reference path.
181      * @param maximumDistanceToFail The distance that auto fails the test.
182      * @return The rotation data if the rotation doesn't cause the test to be invalid, null if the
183      * rotation causes the rest to be invalid.
184      */
handleRotation(float[] rotationQuaternion, float[] rotationLocation, ArrayList<Waypoint> referencePathMarkers, float maximumDistanceToFail)185     public RotationData handleRotation(float[] rotationQuaternion, float[] rotationLocation,
186                                        ArrayList<Waypoint> referencePathMarkers,
187                                        float maximumDistanceToFail) {
188         float eulerRotation = calculateRotation(rotationQuaternion);
189         boolean rotationTest = testRotation(eulerRotation, rotationLocation);
190         boolean rotationTestable = checkIfRotationTestable(rotationLocation, referencePathMarkers);
191         if (mDistanceOfPathFailedRotation > maximumDistanceToFail) {
192             return null;
193         } else {
194             return createRotation(eulerRotation, rotationTest, rotationLocation, rotationTestable);
195         }
196     }
197 
198     /**
199      * Tests the current rotation against the target rotation.
200      *
201      * @param eulerRotation    The rotation as a euler angle.
202      * @param rotationLocation The location of the current rotation.
203      * @return True if the rotation passes, and false if the rotation fails.
204      */
testRotation(double eulerRotation, float[] rotationLocation)205     private boolean testRotation(double eulerRotation, float[] rotationLocation) {
206         boolean rotationTestState = true;
207         double rotationDifference = Math.abs(eulerRotation - mTargetRotation);
208         if (rotationDifference > MAXIMUM_ROTATION_INACCURACY) {
209             mDistanceOfPathFailedRotation += MathsUtils.distanceCalculationOnXYPlane(
210                     rotationLocation, mCurrentPath.get(mCurrentPath.size() - 1).getCoordinates());
211             rotationTestState = false;
212         }
213         return rotationTestState;
214     }
215 
216     /**
217      * Checks to make sure the rotation not close to other markers.
218      *
219      * @param rotationLocation     The location of the point to validate the distance.
220      * @param referencePathMarkers The list of markers in the reference path.
221      * @return true if the location is not close to a marker, false if the location is close to a
222      * marker.
223      */
checkIfRotationTestable( float[] rotationLocation, ArrayList<Waypoint> referencePathMarkers)224     private boolean checkIfRotationTestable(
225             float[] rotationLocation, ArrayList<Waypoint> referencePathMarkers) {
226         for (Waypoint marker : referencePathMarkers) {
227             if (MathsUtils.distanceCalculationInXYZSpace(marker.getCoordinates(),
228                     rotationLocation) < DISTANCE_FROM_MARKER) {
229                 return false;
230             }
231         }
232         return true;
233     }
234 
235     /**
236      * Creates a rotation data object.
237      *
238      * @param currentRotation       The rotation of the current point.
239      * @param rotationTestState     Indicates whether the rotation fails or passes the test.
240      * @param rotationLocation      The location of the current point.
241      * @param testableRotationState Indicates whether the rotation is valid for testing.
242      * @return Reference to the rotation data object which contains the rotation.
243      */
createRotation( float currentRotation, boolean rotationTestState, float[] rotationLocation, boolean testableRotationState)244     private RotationData createRotation(
245             float currentRotation, boolean rotationTestState, float[] rotationLocation,
246             boolean testableRotationState) {
247         RotationData rotationData = new RotationData(
248                 mTargetRotation, currentRotation, rotationTestState, rotationLocation,
249                 testableRotationState);
250         mPathRotations.add(rotationData);
251         return rotationData;
252     }
253 
254     /**
255      * Returns the timestamps for the markers in the path.
256      */
getMarkerTimeStamp()257     public ArrayList<Long> getMarkerTimeStamp() {
258         return new ArrayList<>(mMarkerTimeStamp);
259     }
260 
261     /**
262      * Returns the number of timestamps collected.
263      */
getMarkerTimeStampSize()264     public int getMarkerTimeStampSize() {
265         return mMarkerTimeStamp.size();
266     }
267 
268     /**
269      * Returns the rotations recorded for this path.
270      */
getRobustnessPathRotationsSize()271     public int getRobustnessPathRotationsSize() {
272         return mPathRotations.size();
273     }
274 
275     /**
276      * Returns the number of failed rotations.
277      */
getFailedRotationsSize()278     public int getFailedRotationsSize() {
279         ArrayList<RotationData> failedRotations = new ArrayList<>();
280         for (RotationData rotationObject : mPathRotations) {
281             if (!rotationObject.getRotationTestState() && rotationObject.getRotationState()) {
282                 failedRotations.add(rotationObject);
283             }
284         }
285         return failedRotations.size();
286     }
287 
288     /**
289      * Returns the number of passed rotations.
290      */
getPassedRotationsSize()291     public int getPassedRotationsSize() {
292         ArrayList<RotationData> passedRotations = new ArrayList<>();
293         for (RotationData rotationObject : mPathRotations) {
294             if (rotationObject.getRotationTestState() && rotationObject.getRotationState()) {
295                 passedRotations.add(rotationObject);
296             }
297         }
298         return passedRotations.size();
299     }
300 }
301