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 */ 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 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 257 public ArrayList<Long> getMarkerTimeStamp() { 258 return new ArrayList<>(mMarkerTimeStamp); 259 } 260 261 /** 262 * Returns the number of timestamps collected. 263 */ 264 public int getMarkerTimeStampSize() { 265 return mMarkerTimeStamp.size(); 266 } 267 268 /** 269 * Returns the rotations recorded for this path. 270 */ 271 public int getRobustnessPathRotationsSize() { 272 return mPathRotations.size(); 273 } 274 275 /** 276 * Returns the number of failed rotations. 277 */ 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 */ 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
