1 /*
2  * Copyright (C) 2022 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 
17 package com.android.internal.location.altitude;
18 
19 import android.annotation.NonNull;
20 
21 import java.util.Arrays;
22 import java.util.Locale;
23 
24 /**
25  * Provides lightweight S2 cell ID utilities without traditional geometry dependencies.
26  *
27  * <p>See <a href="https://s2geometry.io/">the S2 Geometry Library website</a> for more details.
28  */
29 public final class S2CellIdUtils {
30 
31     /** The level of all leaf S2 cells. */
32     public static final int MAX_LEVEL = 30;
33 
34     private static final int MAX_SIZE = 1 << MAX_LEVEL;
35     private static final double ONE_OVER_MAX_SIZE = 1.0 / MAX_SIZE;
36     private static final int NUM_FACES = 6;
37     private static final int POS_BITS = 2 * MAX_LEVEL + 1;
38     private static final int SWAP_MASK = 0x1;
39     private static final int LOOKUP_BITS = 4;
40     private static final int LOOKUP_MASK = (1 << LOOKUP_BITS) - 1;
41     private static final int INVERT_MASK = 0x2;
42     private static final int LEAF_MASK = 0x1;
43     private static final int[] LOOKUP_POS = new int[1 << (2 * LOOKUP_BITS + 2)];
44     private static final int[] LOOKUP_IJ = new int[1 << (2 * LOOKUP_BITS + 2)];
45     private static final int[] POS_TO_ORIENTATION = {SWAP_MASK, 0, 0, INVERT_MASK + SWAP_MASK};
46     private static final int[][] POS_TO_IJ =
47             {{0, 1, 3, 2}, {0, 2, 3, 1}, {3, 2, 0, 1}, {3, 1, 0, 2}};
48     private static final double UV_LIMIT = calculateUvLimit();
49     private static final UvTransform[] UV_TRANSFORMS = createUvTransforms();
50     private static final XyzTransform[] XYZ_TRANSFORMS = createXyzTransforms();
51 
52     // Used to encode (i, j, o) coordinates into primitive longs.
53     private static final int I_SHIFT = 33;
54     private static final int J_SHIFT = 2;
55     private static final long J_MASK = (1L << 31) - 1;
56 
57     static {
initLookupCells()58         initLookupCells();
59     }
60 
61     /** Prevents instantiation. */
S2CellIdUtils()62     private S2CellIdUtils() {
63     }
64 
65     /**
66      * Returns the leaf S2 cell ID for the specified latitude and longitude, both measured in
67      * degrees.
68      */
fromLatLngDegrees(double latDegrees, double lngDegrees)69     public static long fromLatLngDegrees(double latDegrees, double lngDegrees) {
70         return fromLatLngRadians(Math.toRadians(latDegrees), Math.toRadians(lngDegrees));
71     }
72 
73     /** Returns the leaf S2 cell ID of the specified (face, i, j) coordinate. */
fromFij(int face, int i, int j)74     public static long fromFij(int face, int i, int j) {
75         int bits = (face & SWAP_MASK);
76         // Update most significant bits.
77         long msb = ((long) face) << (POS_BITS - 33);
78         for (int k = 7; k >= 4; --k) {
79             bits = lookupBits(i, j, k, bits);
80             msb = updateBits(msb, k, bits);
81             bits = maskBits(bits);
82         }
83         // Update least significant bits.
84         long lsb = 0;
85         for (int k = 3; k >= 0; --k) {
86             bits = lookupBits(i, j, k, bits);
87             lsb = updateBits(lsb, k, bits);
88             bits = maskBits(bits);
89         }
90         return (((msb << 32) + lsb) << 1) + 1;
91     }
92 
93     /**
94      * Returns the face of the specified S2 cell. The returned face is in [0, 5] for valid S2 cell
95      * IDs. Behavior is undefined for invalid S2 cell IDs.
96      */
getFace(long s2CellId)97     public static int getFace(long s2CellId) {
98         return (int) (s2CellId >>> POS_BITS);
99     }
100 
101     /**
102      * Returns the ID of the parent of the specified S2 cell at the specified parent level.
103      * Behavior is undefined for invalid S2 cell IDs or parent levels not in
104      * [0, {@code getLevel(s2CellId)}[.
105      */
getParent(long s2CellId, int level)106     public static long getParent(long s2CellId, int level) {
107         long newLsb = getLowestOnBitForLevel(level);
108         return (s2CellId & -newLsb) | newLsb;
109     }
110 
111     /**
112      * Inserts into {@code neighbors} the four S2 cell IDs corresponding to the neighboring
113      * cells adjacent across the specified cell's four edges. This array must be of minimum
114      * length four, and elements at the tail end of the array not corresponding to a neighbor
115      * are set to zero. A reference to this array is returned.
116      *
117      * <p>Inserts in the order of down, right, up, and left directions, in that order. All
118      * neighbors are guaranteed to be distinct.
119      */
getEdgeNeighbors(long s2CellId, @NonNull long[] neighbors)120     public static void getEdgeNeighbors(long s2CellId, @NonNull long[] neighbors) {
121         int level = getLevel(s2CellId);
122         int size = levelToSizeIj(level);
123         int face = getFace(s2CellId);
124         long ijo = toIjo(s2CellId);
125         int i = ijoToI(ijo);
126         int j = ijoToJ(ijo);
127 
128         int iPlusSize = i + size;
129         int iMinusSize = i - size;
130         int jPlusSize = j + size;
131         int jMinusSize = j - size;
132         boolean iPlusSizeLtMax = iPlusSize < MAX_SIZE;
133         boolean iMinusSizeGteZero = iMinusSize >= 0;
134         boolean jPlusSizeLtMax = jPlusSize < MAX_SIZE;
135         boolean jMinusSizeGteZero = jMinusSize >= 0;
136 
137         int index = 0;
138         // Down direction.
139         neighbors[index++] = getParent(fromFijSame(face, i, jMinusSize, jMinusSizeGteZero),
140                 level);
141         // Right direction.
142         neighbors[index++] = getParent(fromFijSame(face, iPlusSize, j, iPlusSizeLtMax), level);
143         // Up direction.
144         neighbors[index++] = getParent(fromFijSame(face, i, jPlusSize, jPlusSizeLtMax), level);
145         // Left direction.
146         neighbors[index++] = getParent(fromFijSame(face, iMinusSize, j, iMinusSizeGteZero),
147                 level);
148 
149         // Pad end of neighbor array with zeros.
150         Arrays.fill(neighbors, index, neighbors.length, 0);
151     }
152 
153     /** Returns the "i" coordinate for the specified S2 cell. */
getI(long s2CellId)154     public static int getI(long s2CellId) {
155         return ijoToI(toIjo(s2CellId));
156     }
157 
158     /** Returns the "j" coordinate for the specified S2 cell. */
getJ(long s2CellId)159     public static int getJ(long s2CellId) {
160         return ijoToJ(toIjo(s2CellId));
161     }
162 
163     /**
164      * Returns the leaf S2 cell ID for the specified latitude and longitude, both measured in
165      * radians.
166      */
fromLatLngRadians(double latRadians, double lngRadians)167     private static long fromLatLngRadians(double latRadians, double lngRadians) {
168         double cosLat = Math.cos(latRadians);
169         double x = Math.cos(lngRadians) * cosLat;
170         double y = Math.sin(lngRadians) * cosLat;
171         double z = Math.sin(latRadians);
172         return fromXyz(x, y, z);
173     }
174 
175     /**
176      * Returns the level of the specified S2 cell. The returned level is in [0, 30] for valid
177      * S2 cell IDs. Behavior is undefined for invalid S2 cell IDs.
178      */
getLevel(long s2CellId)179     static int getLevel(long s2CellId) {
180         if (isLeaf(s2CellId)) {
181             return MAX_LEVEL;
182         }
183         return MAX_LEVEL - (Long.numberOfTrailingZeros(s2CellId) >> 1);
184     }
185 
186     /** Returns the lowest-numbered bit that is on for the specified S2 cell. */
getLowestOnBit(long s2CellId)187     static long getLowestOnBit(long s2CellId) {
188         return s2CellId & -s2CellId;
189     }
190 
191     /** Returns the lowest-numbered bit that is on for any S2 cell on the specified level. */
getLowestOnBitForLevel(int level)192     static long getLowestOnBitForLevel(int level) {
193         return 1L << (2 * (MAX_LEVEL - level));
194     }
195 
196     /**
197      * Returns the ID of the first S2 cell in a traversal of the children S2 cells at the specified
198      * level, in Hilbert curve order.
199      */
getTraversalStart(long s2CellId, int level)200     static long getTraversalStart(long s2CellId, int level) {
201         return s2CellId - getLowestOnBit(s2CellId) + getLowestOnBitForLevel(level);
202     }
203 
204     /** Returns the ID of the next S2 cell at the same level along the Hilbert curve. */
getTraversalNext(long s2CellId)205     static long getTraversalNext(long s2CellId) {
206         return s2CellId + (getLowestOnBit(s2CellId) << 1);
207     }
208 
209     /**
210      * Encodes the S2 cell id to compact text strings suitable for display or indexing. Cells at
211      * lower levels (i.e., larger cells) are encoded into fewer characters.
212      */
213     @NonNull
getToken(long s2CellId)214     static String getToken(long s2CellId) {
215         if (s2CellId == 0) {
216             return "X";
217         }
218 
219         // Convert to a hex string with as many digits as necessary.
220         String hex = Long.toHexString(s2CellId).toLowerCase(Locale.US);
221         // Prefix 0s to get a length 16 string.
222         String padded = padStart(hex);
223         // Trim zeroes off the end.
224         return padded.replaceAll("0*$", "");
225     }
226 
padStart(String string)227     private static String padStart(String string) {
228         if (string.length() >= 16) {
229             return string;
230         }
231         return "0".repeat(16 - string.length()) + string;
232     }
233 
234     /** Returns the leaf S2 cell ID of the specified (x, y, z) coordinate. */
fromXyz(double x, double y, double z)235     private static long fromXyz(double x, double y, double z) {
236         int face = xyzToFace(x, y, z);
237         UvTransform uvTransform = UV_TRANSFORMS[face];
238         double u = uvTransform.xyzToU(x, y, z);
239         double v = uvTransform.xyzToV(x, y, z);
240         return fromFuv(face, u, v);
241     }
242 
243     /** Returns the leaf S2 cell ID of the specified (face, u, v) coordinate. */
fromFuv(int face, double u, double v)244     private static long fromFuv(int face, double u, double v) {
245         int i = uToI(u);
246         int j = vToJ(v);
247         return fromFij(face, i, j);
248     }
249 
fromFijWrap(int face, int i, int j)250     private static long fromFijWrap(int face, int i, int j) {
251         double u = iToU(i);
252         double v = jToV(j);
253 
254         XyzTransform xyzTransform = XYZ_TRANSFORMS[face];
255         double x = xyzTransform.uvToX(u, v);
256         double y = xyzTransform.uvToY(u, v);
257         double z = xyzTransform.uvToZ(u, v);
258 
259         int newFace = xyzToFace(x, y, z);
260         UvTransform uvTransform = UV_TRANSFORMS[newFace];
261         double newU = uvTransform.xyzToU(x, y, z);
262         double newV = uvTransform.xyzToV(x, y, z);
263 
264         int newI = uShiftIntoI(newU);
265         int newJ = vShiftIntoJ(newV);
266         return fromFij(newFace, newI, newJ);
267     }
268 
fromFijSame(int face, int i, int j, boolean isSameFace)269     private static long fromFijSame(int face, int i, int j, boolean isSameFace) {
270         if (isSameFace) {
271             return fromFij(face, i, j);
272         }
273         return fromFijWrap(face, i, j);
274     }
275 
276     /**
277      * Returns the face associated with the specified (x, y, z) coordinate. For a coordinate
278      * on a face boundary, the returned face is arbitrary but repeatable.
279      */
xyzToFace(double x, double y, double z)280     private static int xyzToFace(double x, double y, double z) {
281         double absX = Math.abs(x);
282         double absY = Math.abs(y);
283         double absZ = Math.abs(z);
284         if (absX > absY) {
285             if (absX > absZ) {
286                 return (x < 0) ? 3 : 0;
287             }
288             return (z < 0) ? 5 : 2;
289         }
290         if (absY > absZ) {
291             return (y < 0) ? 4 : 1;
292         }
293         return (z < 0) ? 5 : 2;
294     }
295 
uToI(double u)296     private static int uToI(double u) {
297         double s;
298         if (u >= 0) {
299             s = 0.5 * Math.sqrt(1 + 3 * u);
300         } else {
301             s = 1 - 0.5 * Math.sqrt(1 - 3 * u);
302         }
303         return Math.max(0, Math.min(MAX_SIZE - 1, (int) Math.round(MAX_SIZE * s - 0.5)));
304     }
305 
vToJ(double v)306     private static int vToJ(double v) {
307         // Same calculation as uToI.
308         return uToI(v);
309     }
310 
lookupBits(int i, int j, int k, int bits)311     private static int lookupBits(int i, int j, int k, int bits) {
312         bits += ((i >> (k * LOOKUP_BITS)) & LOOKUP_MASK) << (LOOKUP_BITS + 2);
313         bits += ((j >> (k * LOOKUP_BITS)) & LOOKUP_MASK) << 2;
314         return LOOKUP_POS[bits];
315     }
316 
updateBits(long sb, int k, int bits)317     private static long updateBits(long sb, int k, int bits) {
318         return sb | ((((long) bits) >> 2) << ((k & 0x3) * 2 * LOOKUP_BITS));
319     }
320 
maskBits(int bits)321     private static int maskBits(int bits) {
322         return bits & (SWAP_MASK | INVERT_MASK);
323     }
324 
isLeaf(long s2CellId)325     private static boolean isLeaf(long s2CellId) {
326         return ((int) s2CellId & LEAF_MASK) != 0;
327     }
328 
iToU(int i)329     private static double iToU(int i) {
330         int satI = Math.max(-1, Math.min(MAX_SIZE, i));
331         return Math.max(
332                 -UV_LIMIT,
333                 Math.min(UV_LIMIT, ONE_OVER_MAX_SIZE * ((satI << 1) + 1 - MAX_SIZE)));
334     }
335 
jToV(int j)336     private static double jToV(int j) {
337         // Same calculation as iToU.
338         return iToU(j);
339     }
340 
toIjo(long s2CellId)341     private static long toIjo(long s2CellId) {
342         int face = getFace(s2CellId);
343         int bits = face & SWAP_MASK;
344         int i = 0;
345         int j = 0;
346         for (int k = 7; k >= 0; --k) {
347             int nbits = (k == 7) ? (MAX_LEVEL - 7 * LOOKUP_BITS) : LOOKUP_BITS;
348             bits += ((int) (s2CellId >>> (k * 2 * LOOKUP_BITS + 1)) & ((1 << (2 * nbits))
349                     - 1)) << 2;
350             bits = LOOKUP_IJ[bits];
351             i += (bits >> (LOOKUP_BITS + 2)) << (k * LOOKUP_BITS);
352             j += ((bits >> 2) & ((1 << LOOKUP_BITS) - 1)) << (k * LOOKUP_BITS);
353             bits &= (SWAP_MASK | INVERT_MASK);
354         }
355         int orientation =
356                 ((getLowestOnBit(s2CellId) & 0x1111111111111110L) != 0) ? (bits ^ SWAP_MASK)
357                         : bits;
358         return (((long) i) << I_SHIFT) | (((long) j) << J_SHIFT) | orientation;
359     }
360 
ijoToI(long ijo)361     private static int ijoToI(long ijo) {
362         return (int) (ijo >>> I_SHIFT);
363     }
364 
ijoToJ(long ijo)365     private static int ijoToJ(long ijo) {
366         return (int) ((ijo >>> J_SHIFT) & J_MASK);
367     }
368 
uShiftIntoI(double u)369     private static int uShiftIntoI(double u) {
370         double s = 0.5 * (u + 1);
371         return Math.max(0, Math.min(MAX_SIZE - 1, (int) Math.round(MAX_SIZE * s - 0.5)));
372     }
373 
vShiftIntoJ(double v)374     private static int vShiftIntoJ(double v) {
375         // Same calculation as uShiftIntoI.
376         return uShiftIntoI(v);
377     }
378 
levelToSizeIj(int level)379     private static int levelToSizeIj(int level) {
380         return 1 << (MAX_LEVEL - level);
381     }
382 
initLookupCells()383     private static void initLookupCells() {
384         initLookupCell(0, 0, 0, 0, 0, 0);
385         initLookupCell(0, 0, 0, SWAP_MASK, 0, SWAP_MASK);
386         initLookupCell(0, 0, 0, INVERT_MASK, 0, INVERT_MASK);
387         initLookupCell(0, 0, 0, SWAP_MASK | INVERT_MASK, 0, SWAP_MASK | INVERT_MASK);
388     }
389 
initLookupCell( int level, int i, int j, int origOrientation, int pos, int orientation)390     private static void initLookupCell(
391             int level, int i, int j, int origOrientation, int pos, int orientation) {
392         if (level == LOOKUP_BITS) {
393             int ij = (i << LOOKUP_BITS) + j;
394             LOOKUP_POS[(ij << 2) + origOrientation] = (pos << 2) + orientation;
395             LOOKUP_IJ[(pos << 2) + origOrientation] = (ij << 2) + orientation;
396         } else {
397             level++;
398             i <<= 1;
399             j <<= 1;
400             pos <<= 2;
401             for (int subPos = 0; subPos < 4; subPos++) {
402                 int ij = POS_TO_IJ[orientation][subPos];
403                 int orientationMask = POS_TO_ORIENTATION[subPos];
404                 initLookupCell(
405                         level,
406                         i + (ij >>> 1),
407                         j + (ij & 0x1),
408                         origOrientation,
409                         pos + subPos,
410                         orientation ^ orientationMask);
411             }
412         }
413     }
414 
calculateUvLimit()415     private static double calculateUvLimit() {
416         double machEps = 1.0;
417         do {
418             machEps /= 2.0f;
419         } while ((1.0 + (machEps / 2.0)) != 1.0);
420         return 1.0 + machEps;
421     }
422 
423     @NonNull
createUvTransforms()424     private static UvTransform[] createUvTransforms() {
425         UvTransform[] uvTransforms = new UvTransform[NUM_FACES];
426         uvTransforms[0] =
427                 new UvTransform() {
428 
429                     @Override
430                     public double xyzToU(double x, double y, double z) {
431                         return y / x;
432                     }
433 
434                     @Override
435                     public double xyzToV(double x, double y, double z) {
436                         return z / x;
437                     }
438                 };
439         uvTransforms[1] =
440                 new UvTransform() {
441 
442                     @Override
443                     public double xyzToU(double x, double y, double z) {
444                         return -x / y;
445                     }
446 
447                     @Override
448                     public double xyzToV(double x, double y, double z) {
449                         return z / y;
450                     }
451                 };
452         uvTransforms[2] =
453                 new UvTransform() {
454 
455                     @Override
456                     public double xyzToU(double x, double y, double z) {
457                         return -x / z;
458                     }
459 
460                     @Override
461                     public double xyzToV(double x, double y, double z) {
462                         return -y / z;
463                     }
464                 };
465         uvTransforms[3] =
466                 new UvTransform() {
467 
468                     @Override
469                     public double xyzToU(double x, double y, double z) {
470                         return z / x;
471                     }
472 
473                     @Override
474                     public double xyzToV(double x, double y, double z) {
475                         return y / x;
476                     }
477                 };
478         uvTransforms[4] =
479                 new UvTransform() {
480 
481                     @Override
482                     public double xyzToU(double x, double y, double z) {
483                         return z / y;
484                     }
485 
486                     @Override
487                     public double xyzToV(double x, double y, double z) {
488                         return -x / y;
489                     }
490                 };
491         uvTransforms[5] =
492                 new UvTransform() {
493 
494                     @Override
495                     public double xyzToU(double x, double y, double z) {
496                         return -y / z;
497                     }
498 
499                     @Override
500                     public double xyzToV(double x, double y, double z) {
501                         return -x / z;
502                     }
503                 };
504         return uvTransforms;
505     }
506 
507     @NonNull
createXyzTransforms()508     private static XyzTransform[] createXyzTransforms() {
509         XyzTransform[] xyzTransforms = new XyzTransform[NUM_FACES];
510         xyzTransforms[0] =
511                 new XyzTransform() {
512 
513                     @Override
514                     public double uvToX(double u, double v) {
515                         return 1;
516                     }
517 
518                     @Override
519                     public double uvToY(double u, double v) {
520                         return u;
521                     }
522 
523                     @Override
524                     public double uvToZ(double u, double v) {
525                         return v;
526                     }
527                 };
528         xyzTransforms[1] =
529                 new XyzTransform() {
530 
531                     @Override
532                     public double uvToX(double u, double v) {
533                         return -u;
534                     }
535 
536                     @Override
537                     public double uvToY(double u, double v) {
538                         return 1;
539                     }
540 
541                     @Override
542                     public double uvToZ(double u, double v) {
543                         return v;
544                     }
545                 };
546         xyzTransforms[2] =
547                 new XyzTransform() {
548 
549                     @Override
550                     public double uvToX(double u, double v) {
551                         return -u;
552                     }
553 
554                     @Override
555                     public double uvToY(double u, double v) {
556                         return -v;
557                     }
558 
559                     @Override
560                     public double uvToZ(double u, double v) {
561                         return 1;
562                     }
563                 };
564         xyzTransforms[3] =
565                 new XyzTransform() {
566 
567                     @Override
568                     public double uvToX(double u, double v) {
569                         return -1;
570                     }
571 
572                     @Override
573                     public double uvToY(double u, double v) {
574                         return -v;
575                     }
576 
577                     @Override
578                     public double uvToZ(double u, double v) {
579                         return -u;
580                     }
581                 };
582         xyzTransforms[4] =
583                 new XyzTransform() {
584 
585                     @Override
586                     public double uvToX(double u, double v) {
587                         return v;
588                     }
589 
590                     @Override
591                     public double uvToY(double u, double v) {
592                         return -1;
593                     }
594 
595                     @Override
596                     public double uvToZ(double u, double v) {
597                         return -u;
598                     }
599                 };
600         xyzTransforms[5] =
601                 new XyzTransform() {
602 
603                     @Override
604                     public double uvToX(double u, double v) {
605                         return v;
606                     }
607 
608                     @Override
609                     public double uvToY(double u, double v) {
610                         return u;
611                     }
612 
613                     @Override
614                     public double uvToZ(double u, double v) {
615                         return -1;
616                     }
617                 };
618         return xyzTransforms;
619     }
620 
621     /**
622      * Transform from (x, y, z) coordinates to (u, v) coordinates, indexed by face. For a
623      * (x, y, z) coordinate within a face, each element of the resulting (u, v) coordinate
624      * should lie in the inclusive range [-1, 1], with the face center having a (u, v)
625      * coordinate equal to (0, 0).
626      */
627     private interface UvTransform {
628 
629         /**
630          * Returns for the specified (x, y, z) coordinate the corresponding u-coordinate
631          * (which may lie outside the range [-1, 1]).
632          */
xyzToU(double x, double y, double z)633         double xyzToU(double x, double y, double z);
634 
635         /**
636          * Returns for the specified (x, y, z) coordinate the corresponding v-coordinate
637          * (which may lie outside the range [-1, 1]).
638          */
xyzToV(double x, double y, double z)639         double xyzToV(double x, double y, double z);
640     }
641 
642     /**
643      * Transform from (u, v) coordinates to (x, y, z) coordinates, indexed by face. The
644      * resulting vectors are not necessarily of unit length.
645      */
646     private interface XyzTransform {
647 
648         /** Returns for the specified (u, v) coordinate the corresponding x-coordinate. */
uvToX(double u, double v)649         double uvToX(double u, double v);
650 
651         /** Returns for the specified (u, v) coordinate the corresponding y-coordinate. */
uvToY(double u, double v)652         double uvToY(double u, double v);
653 
654         /** Returns for the specified (u, v) coordinate the corresponding z-coordinate. */
uvToZ(double u, double v)655         double uvToZ(double u, double v);
656     }
657 }
658