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 java.util.ArrayList;
19 import java.util.Random;
20 
21 import static com.android.cts.verifier.sensors.sixdof.Utils.MathsUtils.VECTOR_2D;
22 import static com.android.cts.verifier.sensors.sixdof.Utils.MathsUtils.X;
23 import static com.android.cts.verifier.sensors.sixdof.Utils.MathsUtils.Y;
24 import static com.android.cts.verifier.sensors.sixdof.Utils.MathsUtils.Z;
25 import static com.android.cts.verifier.sensors.sixdof.Utils.MathsUtils.dotProduct;
26 
27 import com.android.cts.verifier.sensors.sixdof.Utils.MathsUtils;
28 import com.android.cts.verifier.sensors.sixdof.Utils.Exceptions.WaypointRingNotEnteredException;
29 import com.android.cts.verifier.sensors.sixdof.Utils.Path.PathUtilityClasses.Ring;
30 import com.android.cts.verifier.sensors.sixdof.Utils.Path.PathUtilityClasses.Waypoint;
31 
32 /**
33  * Handles all the path properties of the ComplexMovement Path.
34  */
35 public class ComplexMovementPath extends com.android.cts.verifier.sensors.sixdof.Utils.Path.Path {
36     public static final float DISTANCE_FOR_RING_POSITION = 0.25f;
37     public static final int RINGS_PER_PATH = 5;
38 
39     private ArrayList<Ring> mRings = new ArrayList<>();
40     private Random mRandomGenerator = new Random();
41     private int mCurrentLap = 0;
42     private float mLocationMapping[][];
43 
44     /**
45      * Possible locations for a ring.
46      */
47     private enum RingLocations {
48         ORIGINAL,
49         TOP,
50         DOWN,
51         LEFT,
52         RIGHT,
53         TOP_LEFT,
54         TOP_RIGHT,
55         BOTTOM_LEFT,
56         BOTTOM_RIGHT,
57     }
58 
59     /**
60      * Constructor for this class does the mapping and the creation of rings.
61      *
62      * @param referencePathDistances The distance between the markers in the reference path
63      * @param referencePath          The reference path
64      */
ComplexMovementPath( ArrayList<Float> referencePathDistances, ArrayList<Waypoint> referencePath)65     public ComplexMovementPath(
66             ArrayList<Float> referencePathDistances, ArrayList<Waypoint> referencePath) {
67         mapNineRingLocations();
68         generatePathRings(referencePathDistances, referencePath);
69     }
70 
71     /**
72      * Defines the different ring locations that can be used when adding the rings.
73      */
mapNineRingLocations()74     private void mapNineRingLocations() {
75         mLocationMapping = new float[RingLocations.values().length][2];
76         mLocationMapping[RingLocations.ORIGINAL.ordinal()] = new float[]{0f, 0f};
77         mLocationMapping[RingLocations.TOP.ordinal()] =
78                 new float[]{0f, DISTANCE_FOR_RING_POSITION};
79         mLocationMapping[RingLocations.DOWN.ordinal()] =
80                 new float[]{0f, -DISTANCE_FOR_RING_POSITION};
81         mLocationMapping[RingLocations.LEFT.ordinal()] =
82                 new float[]{-DISTANCE_FOR_RING_POSITION, 0f};
83         mLocationMapping[RingLocations.RIGHT.ordinal()] =
84                 new float[]{DISTANCE_FOR_RING_POSITION, 0f};
85         mLocationMapping[RingLocations.TOP_LEFT.ordinal()] =
86                 new float[]{-DISTANCE_FOR_RING_POSITION, DISTANCE_FOR_RING_POSITION};
87         mLocationMapping[RingLocations.TOP_RIGHT.ordinal()] =
88                 new float[]{DISTANCE_FOR_RING_POSITION, DISTANCE_FOR_RING_POSITION};
89         mLocationMapping[RingLocations.BOTTOM_LEFT.ordinal()] =
90                 new float[]{-DISTANCE_FOR_RING_POSITION, -DISTANCE_FOR_RING_POSITION};
91         mLocationMapping[RingLocations.BOTTOM_RIGHT.ordinal()] =
92                 new float[]{DISTANCE_FOR_RING_POSITION, -DISTANCE_FOR_RING_POSITION};
93     }
94 
95     /**
96      * Performs ComplexMovement path related checks on a marker.
97      *
98      * @param coordinates the coordinates for the waypoint
99      * @throws WaypointRingNotEnteredException if a ring is not entered
100      */
101     @Override
additionalChecks(float[] coordinates)102     public void additionalChecks(float[] coordinates) throws WaypointRingNotEnteredException {
103         if (mCurrentLap != 0) {
104             for (Ring ring : mRings) {
105                 if (ring.getPathNumber() == mCurrentLap && !ring.isEntered()) {
106                     throw new WaypointRingNotEnteredException();
107                 }
108             }
109         }
110         mCurrentLap++;
111     }
112 
113     /**
114      * Generates the rings for this path.
115      *
116      * @param referencePathDistances The distance between the markers in the reference path
117      * @param referencePath          The reference path
118      */
generatePathRings( ArrayList<Float> referencePathDistances, ArrayList<Waypoint> referencePath)119     private void generatePathRings(
120             ArrayList<Float> referencePathDistances, ArrayList<Waypoint> referencePath) {
121         ArrayList<Float> distanceBetweenRingSections;
122         distanceBetweenRingSections = calculateSectionDistance(referencePathDistances);
123         addRingsToPath(referencePath, distanceBetweenRingSections);
124     }
125 
126     /**
127      * Calculates the distance between the rings in a path.
128      *
129      * @param referencePathDistances The distance between the markers in the reference path.
130      * @return The length of a section in the different paths.
131      */
calculateSectionDistance(ArrayList<Float> referencePathDistances)132     private ArrayList<Float> calculateSectionDistance(ArrayList<Float> referencePathDistances) {
133         ArrayList<Float> arrayToReturn = new ArrayList<>();
134         for (Float distance : referencePathDistances) {
135             arrayToReturn.add(distance / (RINGS_PER_PATH + 1f));
136         }
137         return arrayToReturn;
138     }
139 
140     /**
141      * Calculates the location for the ring and adds it to the path.
142      *
143      * @param referencePath               The reference path.
144      * @param distanceBetweenRingSections The length of a section in the different paths.
145      */
addRingsToPath( ArrayList<Waypoint> referencePath, ArrayList<Float> distanceBetweenRingSections)146     private void addRingsToPath(
147             ArrayList<Waypoint> referencePath, ArrayList<Float> distanceBetweenRingSections) {
148         int currentPath = 0;
149         Waypoint currentWaypoint = referencePath.get(0);
150         for (Float pathIntervalDistance : distanceBetweenRingSections) {
151             currentPath++;
152             for (int i = 0; i < RINGS_PER_PATH; i++) {
153                 currentWaypoint = calculateRingLocationOnPath(
154                         referencePath, referencePath.indexOf(currentWaypoint), pathIntervalDistance);
155                 mRings.add(createRing(referencePath, currentWaypoint, currentPath));
156             }
157             while (!currentWaypoint.isUserGenerated()) {
158                 currentWaypoint = referencePath.get(referencePath.indexOf(currentWaypoint) + 1);
159             }
160         }
161     }
162 
163     /**
164      * Creates the ring that will be added onto the path.
165      *
166      * @param referencePath The reference path.
167      * @param waypoint      The waypoint which the ring will be located at.
168      * @param currentPath   The part of the lap in which the ring will be placed.
169      * @return A reference to the ring created.
170      */
createRing(ArrayList<Waypoint> referencePath, Waypoint waypoint, int currentPath)171     private Ring createRing(ArrayList<Waypoint> referencePath, Waypoint waypoint, int currentPath) {
172         float[] ringCenter = waypoint.getCoordinates();
173         float[] pointRotation = calculateRingRotation(ringCenter,
174                 referencePath.get(referencePath.indexOf(waypoint) - 1).getCoordinates());
175         int randomNumber = mRandomGenerator.nextInt(RingLocations.values().length);
176         RingLocations ringLocationDifference = RingLocations.values()[randomNumber];
177         ringCenter[X] += mLocationMapping[ringLocationDifference.ordinal()][0];
178         ringCenter[Z] += mLocationMapping[ringLocationDifference.ordinal()][1];
179         ArrayList<float[]> rotatedRect = calculateRectangleHitbox(ringCenter, pointRotation);
180         return new Ring(ringCenter, currentPath, pointRotation, rotatedRect);
181     }
182 
183     /**
184      * Calculates the orientation of the ring.
185      *
186      * @param location1 The location of the first point.
187      * @param location2 The location of the second point.
188      * @return the rotation needed to get the orientation of the ring.
189      */
calculateRingRotation(float[] location1, float[] location2)190     private float[] calculateRingRotation(float[] location1, float[] location2) {
191         float[] rotation = new float[3];
192         rotation[X] = location2[X] - location1[X];
193         rotation[Y] = location2[Y] - location1[Y];
194         rotation[Z] = location2[Z] - location1[Z];
195         return rotation;
196     }
197 
198     /**
199      * Calculates the next possible position for the ring to be placed at.
200      *
201      * @param referencePath        The reference path.
202      * @param currentLocation      The location to start calculating from.
203      * @param pathIntervalDistance The distance indicating how far apart the rings are going to be.
204      * @return The waypoint where the ring will be placed at.
205      */
calculateRingLocationOnPath( ArrayList<Waypoint> referencePath, int currentLocation, Float pathIntervalDistance)206     private Waypoint calculateRingLocationOnPath(
207             ArrayList<Waypoint> referencePath, int currentLocation, Float pathIntervalDistance) {
208         float pathRemaining = 0;
209         while (currentLocation < referencePath.size() - 1) {
210             pathRemaining += MathsUtils.distanceCalculationOnXYPlane(
211                     referencePath.get(currentLocation).getCoordinates(),
212                     referencePath.get(currentLocation + 1).getCoordinates());
213             if (pathRemaining >= pathIntervalDistance) {
214                 return referencePath.get(currentLocation);
215             }
216             currentLocation++;
217         }
218         throw new AssertionError(
219                 "calculateRingLocationOnPath: Ring number and section number don't seem to match up");
220     }
221 
222     /**
223      * Calculates the rectangular hit box for the ring.
224      *
225      * @param centre   the middle location of the ring.
226      * @param rotation the rotation to get the same orientation of the ring.
227      * @return The four corners of the rectangle.
228      */
calculateRectangleHitbox(float[] centre, float[] rotation)229     private ArrayList<float[]> calculateRectangleHitbox(float[] centre, float[] rotation) {
230         ArrayList<float[]> rectangle = new ArrayList<>();
231         float magnitude = (float) Math.sqrt(Math.pow(rotation[X], 2) +
232                 Math.pow(rotation[Z], 2));
233         float lengthScaleFactor = 0.02f / magnitude;
234         float widthScaleFactor = 0.17f / magnitude;
235 
236         float[] rotationInverse = {0 - rotation[X], 0 - rotation[Y]};
237         float[] rotationNinety = {rotation[Y], 0 - rotation[X]};
238         float[] rotationNinetyInverse = {0 - rotation[Y], rotation[X]};
239 
240         float[] midFront = new float[2];
241         midFront[X] = centre[X] + (lengthScaleFactor * rotation[X]);
242         midFront[Y] = centre[Y] + (lengthScaleFactor * rotation[Y]);
243         float[] midRear = new float[2];
244         midRear[X] = centre[X] + (lengthScaleFactor * rotationInverse[X]);
245         midRear[Y] = centre[Y] + (lengthScaleFactor * rotationInverse[Y]);
246 
247         float[] frontLeft = new float[3];
248         frontLeft[Z] = centre[Z];
249         frontLeft[X] = midFront[X] + (widthScaleFactor * rotationNinetyInverse[X]);
250         frontLeft[Y] = midFront[Y] + (widthScaleFactor * rotationNinetyInverse[Y]);
251         float[] frontRight = new float[3];
252         frontRight[Z] = centre[Z];
253         frontRight[X] = midFront[X] + (widthScaleFactor * rotationNinety[X]);
254         frontRight[Y] = midFront[Y] + (widthScaleFactor * rotationNinety[Y]);
255         float[] rearLeft = new float[3];
256         rearLeft[Z] = centre[Z];
257         rearLeft[X] = midRear[X] + (widthScaleFactor * rotationNinetyInverse[X]);
258         rearLeft[Y] = midRear[Y] + (widthScaleFactor * rotationNinetyInverse[Y]);
259         float[] rearRight = new float[3];
260         rearRight[Z] = centre[Z];
261         rearRight[X] = midRear[X] + (widthScaleFactor * rotationNinety[X]);
262         rearRight[Y] = midRear[Y] + (widthScaleFactor * rotationNinety[Y]);
263 
264         rectangle.add(frontLeft);
265         rectangle.add(frontRight);
266         rectangle.add(rearRight);
267         rectangle.add(rearLeft);
268         return rectangle;
269     }
270 
271     /**
272      * Check to see if a ring has been entered.
273      *
274      * @param location the location of the user to be tested.
275      */
hasRingBeenEntered(float[] location)276     public Ring hasRingBeenEntered(float[] location) {
277         float xDifference, yDifference, zDifference;
278         for (int i = 0; i < mRings.size(); i++) {
279             if (mRings.get(i).getPathNumber() == mCurrentLap) {
280                 xDifference = Math.abs(mRings.get(i).getLocation()[X] - location[X]);
281                 yDifference = Math.abs(mRings.get(i).getLocation()[Y] - location[Y]);
282                 zDifference = Math.abs(mRings.get(i).getLocation()[Z] - location[Z]);
283                 if (xDifference < 0.17 && yDifference < 0.17 && zDifference < 0.17) {
284                     if (checkCollision(mRings.get(i), location)) {
285                         return mRings.get(i);
286                     }
287                 }
288             }
289         }
290         return null;
291     }
292 
293     /**
294      * Calculates whether the location of the user is in the rectangular hit box or not.
295      *
296      * @param ring     the ring to be tested.
297      * @param location the location of the user.
298      * @return true if the ring is entered and false if it is not.
299      */
checkCollision(Ring ring, float[] location)300     private boolean checkCollision(Ring ring, float[] location) {
301         float[] rectangleVector1 = new float[2];
302         rectangleVector1[X] = ring.getRectangleHitBox().get(0)[X] - ring.getRectangleHitBox().get(3)[X];
303         rectangleVector1[Y] = ring.getRectangleHitBox().get(0)[Y] - ring.getRectangleHitBox().get(3)[Y];
304 
305         float[] rectangleVector2 = new float[2];
306         rectangleVector2[X] = ring.getRectangleHitBox().get(2)[X] - ring.getRectangleHitBox().get(3)[X];
307         rectangleVector2[Y] = ring.getRectangleHitBox().get(2)[Y] - ring.getRectangleHitBox().get(3)[Y];
308 
309         float[] locationVector = new float[2];
310         locationVector[X] = location[X] - ring.getRectangleHitBox().get(3)[X];
311         locationVector[Y] = location[Y] - ring.getRectangleHitBox().get(3)[Y];
312 
313         if (dotProduct(rectangleVector1, locationVector, VECTOR_2D) > 0) {
314             if (dotProduct(rectangleVector1, rectangleVector1, VECTOR_2D)
315                     > dotProduct(rectangleVector1, locationVector, VECTOR_2D)) {
316                 if (dotProduct(rectangleVector2, locationVector, VECTOR_2D) > 0) {
317                     if (dotProduct(rectangleVector2, rectangleVector2, VECTOR_2D)
318                             > dotProduct(rectangleVector2, locationVector, VECTOR_2D)) {
319                         return true;
320                     }
321                 }
322             }
323         }
324         return false;
325     }
326 
327     /**
328      * Returns the list of rings.
329      */
getRings()330     public ArrayList<Ring> getRings() {
331         return new ArrayList<>(mRings);
332     }
333 }
334