Adds complete U implementation of AltitudeConverter.

Relnote: N/A
Bug: 231327615
Test: atest CtsLocationNoneTestCases
Change-Id: I7bac8b12ddd68732f99ff04e30f169657c2d2e71

3 files changed, 1197 insertions, 0 deletions

diff --git a/location/java/android/location/altitude/AltitudeConverter.java b/location/java/android/location/altitude/AltitudeConverter.java
new file mode 100644
index 000000000000..506128e17740
--- /dev/null
+++ b/location/java/android/location/altitude/AltitudeConverter.java
@@ -0,0 +1,175 @@
+/*
+ * Copyright (C) 2022 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+package android.location.altitude;
+
+import android.annotation.NonNull;
+import android.annotation.WorkerThread;
+import android.content.Context;
+import android.location.Location;
+
+import com.android.internal.location.altitude.GeoidHeightMap;
+import com.android.internal.location.altitude.S2CellIdUtils;
+import com.android.internal.location.altitude.nano.MapParamsProto;
+import com.android.internal.util.Preconditions;
+
+import java.io.IOException;
+
+/**
+ * Converts altitudes reported above the World Geodetic System 1984 (WGS84) reference ellipsoid
+ * into ones above Mean Sea Level.
+ */
+public final class AltitudeConverter {
+
+    private static final double MAX_ABS_VALID_LATITUDE = 90;
+    private static final double MAX_ABS_VALID_LONGITUDE = 180;
+
+    /** Manages a mapping of geoid heights associated with S2 cells. */
+    private final GeoidHeightMap mGeoidHeightMap = new GeoidHeightMap();
+
+    /**
+     * Creates an instance that manages an independent cache to optimized conversions of locations
+     * in proximity to one another.
+     */
+    public AltitudeConverter() {
+    }
+
+    /**
+     * Throws an {@link IllegalArgumentException} if the {@code location} has an invalid latitude,
+     * longitude, or altitude above WGS84.
+     */
+    private static void validate(@NonNull Location location) {
+        Preconditions.checkArgument(
+                isFiniteAndAtAbsMost(location.getLatitude(), MAX_ABS_VALID_LATITUDE),
+                "Invalid latitude: %f", location.getLatitude());
+        Preconditions.checkArgument(
+                isFiniteAndAtAbsMost(location.getLongitude(), MAX_ABS_VALID_LONGITUDE),
+                "Invalid longitude: %f", location.getLongitude());
+        Preconditions.checkArgument(location.hasAltitude(), "Missing altitude above WGS84");
+        Preconditions.checkArgument(Double.isFinite(location.getAltitude()),
+                "Invalid altitude above WGS84: %f", location.getAltitude());
+    }
+
+    private static boolean isFiniteAndAtAbsMost(double value, double rhs) {
+        return Double.isFinite(value) && Math.abs(value) <= rhs;
+    }
+
+    /**
+     * Returns the four S2 cell IDs for the map square associated with the {@code location}.
+     *
+     * <p>The first map cell contains the location, while the others are located horizontally,
+     * vertically, and diagonally, in that order, with respect to the S2 (i,j) coordinate system. If
+     * the diagonal map cell does not exist (i.e., the location is near an S2 cube vertex), its
+     * corresponding ID is set to zero.
+     */
+    @NonNull
+    private static long[] findMapSquare(@NonNull MapParamsProto params,
+            @NonNull Location location) {
+        long s2CellId = S2CellIdUtils.fromLatLngDegrees(location.getLatitude(),
+                location.getLongitude());
+
+        // (0,0) cell.
+        long s0 = S2CellIdUtils.getParent(s2CellId, params.mapS2Level);
+        long[] edgeNeighbors = new long[4];
+        S2CellIdUtils.getEdgeNeighbors(s0, edgeNeighbors);
+
+        // (1,0) cell.
+        int i1 = S2CellIdUtils.getI(s2CellId) > S2CellIdUtils.getI(s0) ? -1 : 1;
+        long s1 = edgeNeighbors[i1 + 2];
+
+        // (0,1) cell.
+        int i2 = S2CellIdUtils.getJ(s2CellId) > S2CellIdUtils.getJ(s0) ? 1 : -1;
+        long s2 = edgeNeighbors[i2 + 1];
+
+        // (1,1) cell.
+        S2CellIdUtils.getEdgeNeighbors(s1, edgeNeighbors);
+        long s3 = 0;
+        for (int i = 0; i < edgeNeighbors.length; i++) {
+            if (edgeNeighbors[i] == s0) {
+                int i3 = (i + i1 * i2 + edgeNeighbors.length) % edgeNeighbors.length;
+                s3 = edgeNeighbors[i3] == s2 ? 0 : edgeNeighbors[i3];
+                break;
+            }
+        }
+
+        // Reuse edge neighbors' array to avoid an extra allocation.
+        edgeNeighbors[0] = s0;
+        edgeNeighbors[1] = s1;
+        edgeNeighbors[2] = s2;
+        edgeNeighbors[3] = s3;
+        return edgeNeighbors;
+    }
+
+    /**
+     * Adds to {@code location} the bilinearly interpolated Mean Sea Level altitude. In addition, a
+     * Mean Sea Level altitude accuracy is added if the {@code location} has a valid vertical
+     * accuracy; otherwise, does not add a corresponding accuracy.
+     */
+    private static void addMslAltitude(@NonNull MapParamsProto params, @NonNull long[] s2CellIds,
+            @NonNull double[] geoidHeightsMeters, @NonNull Location location) {
+        long s0 = s2CellIds[0];
+        double h0 = geoidHeightsMeters[0];
+        double h1 = geoidHeightsMeters[1];
+        double h2 = geoidHeightsMeters[2];
+        double h3 = s2CellIds[3] == 0 ? h0 : geoidHeightsMeters[3];
+
+        // Bilinear interpolation on an S2 square of size equal to that of a map cell. wi and wj
+        // are the normalized [0,1] weights in the i and j directions, respectively, allowing us to
+        // employ the simplified unit square formulation.
+        long s2CellId = S2CellIdUtils.fromLatLngDegrees(location.getLatitude(),
+                location.getLongitude());
+        double sizeIj = 1 << (S2CellIdUtils.MAX_LEVEL - params.mapS2Level);
+        double wi = Math.abs(S2CellIdUtils.getI(s2CellId) - S2CellIdUtils.getI(s0)) / sizeIj;
+        double wj = Math.abs(S2CellIdUtils.getJ(s2CellId) - S2CellIdUtils.getJ(s0)) / sizeIj;
+        double offsetMeters = h0 + (h1 - h0) * wi + (h2 - h0) * wj + (h3 - h1 - h2 + h0) * wi * wj;
+
+        location.setMslAltitudeMeters(location.getAltitude() - offsetMeters);
+        if (location.hasVerticalAccuracy()) {
+            double verticalAccuracyMeters = location.getVerticalAccuracyMeters();
+            if (Double.isFinite(verticalAccuracyMeters) && verticalAccuracyMeters >= 0) {
+                location.setMslAltitudeAccuracyMeters(
+                        (float) Math.hypot(verticalAccuracyMeters, params.modelRmseMeters));
+            }
+        }
+    }
+
+    /**
+     * Adds a Mean Sea Level altitude to the {@code location}. In addition, adds a Mean Sea Level
+     * altitude accuracy if the {@code location} has a finite and non-negative vertical accuracy;
+     * otherwise, does not add a corresponding accuracy.
+     *
+     * <p>Must be called off the main thread as data may be loaded from raw assets. Throws an
+     * {@link IOException} if an I/O error occurs when loading data.
+     *
+     * <p>Throws an {@link IllegalArgumentException} if the {@code location} has an invalid
+     * latitude, longitude, or altitude above WGS84. Specifically:
+     *
+     * <ul>
+     *     <li>The latitude must be between -90 and 90, both inclusive.
+     *     <li>The longitude must be between -180 and 180, both inclusive.
+     *     <li>The altitude above WGS84 must be finite.
+     * </ul>
+     */
+    @WorkerThread
+    public void addMslAltitude(@NonNull Context context, @NonNull Location location)
+            throws IOException {
+        validate(location);
+        MapParamsProto params = GeoidHeightMap.getParams(context);
+        long[] s2CellIds = findMapSquare(params, location);
+        double[] geoidHeightsMeters = mGeoidHeightMap.readGeoidHeights(params, context, s2CellIds);
+        addMslAltitude(params, s2CellIds, geoidHeightsMeters, location);
+    }
+}
diff --git a/location/java/com/android/internal/location/altitude/GeoidHeightMap.java b/location/java/com/android/internal/location/altitude/GeoidHeightMap.java
new file mode 100644
index 000000000000..e113ab411d98
--- /dev/null
+++ b/location/java/com/android/internal/location/altitude/GeoidHeightMap.java
@@ -0,0 +1,369 @@
+/*
+ * Copyright (C) 2022 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+package com.android.internal.location.altitude;
+
+import android.annotation.NonNull;
+import android.annotation.Nullable;
+import android.content.Context;
+import android.graphics.Bitmap;
+import android.graphics.BitmapFactory;
+import android.util.LruCache;
+
+import com.android.internal.annotations.GuardedBy;
+import com.android.internal.location.altitude.nano.MapParamsProto;
+import com.android.internal.location.altitude.nano.S2TileProto;
+import com.android.internal.util.Preconditions;
+
+import java.io.IOException;
+import java.io.InputStream;
+import java.nio.ByteBuffer;
+import java.util.Objects;
+
+/**
+ * Manages a mapping of geoid heights associated with S2 cells, referred to as MAP CELLS.
+ *
+ * <p>Tiles are used extensively to reduce the number of entries needed to be stored in memory and
+ * on disk. A tile associates geoid heights with all map cells of a common parent at a specified S2
+ * level.
+ *
+ * <p>Since bilinear interpolation considers at most four map cells at a time, at most four tiles
+ * are simultaneously stored in memory. These tiles, referred to as CACHE TILES, are each keyed by
+ * its common parent's S2 cell ID, referred to as a CACHE KEY.
+ *
+ * <p>Absent cache tiles needed for interpolation are constructed from larger tiles stored on disk.
+ * The latter tiles, referred to as DISK TILES, are each keyed by its common parent's S2 cell token,
+ * referred to as a DISK TOKEN.
+ */
+public final class GeoidHeightMap {
+
+    private static final Object sLock = new Object();
+
+    @GuardedBy("sLock")
+    @Nullable
+    private static MapParamsProto sParams;
+
+    /** Defines a cache large enough to hold all cache tiles needed for interpolation. */
+    private final LruCache<Long, S2TileProto> mCacheTiles = new LruCache<>(4);
+
+    /**
+     * Returns the singleton parameter instance for a spherically projected geoid height map and its
+     * corresponding tile management.
+     */
+    @NonNull
+    public static MapParamsProto getParams(@NonNull Context context) throws IOException {
+        synchronized (sLock) {
+            if (sParams == null) {
+                try (InputStream is = context.getApplicationContext().getAssets().open(
+                        "geoid_height_map/map-params.pb")) {
+                    sParams = MapParamsProto.parseFrom(is.readAllBytes());
+                }
+            }
+            return sParams;
+        }
+    }
+
+    private static long getCacheKey(@NonNull MapParamsProto params, long s2CellId) {
+        return S2CellIdUtils.getParent(s2CellId, params.cacheTileS2Level);
+    }
+
+    @NonNull
+    private static String getDiskToken(@NonNull MapParamsProto params, long s2CellId) {
+        return S2CellIdUtils.getToken(
+                S2CellIdUtils.getParent(s2CellId, params.diskTileS2Level));
+    }
+
+    /**
+     * Adds to {@code values} values in the unit interval [0, 1] for the map cells identified by
+     * {@code s2CellIds}. Returns true if values are present for all non-zero IDs; otherwise,
+     * returns false and adds NaNs for absent values.
+     */
+    private static boolean getUnitIntervalValues(@NonNull MapParamsProto params,
+            @NonNull TileFunction tileFunction,
+            @NonNull long[] s2CellIds, @NonNull double[] values) {
+        int len = s2CellIds.length;
+
+        S2TileProto[] tiles = new S2TileProto[len];
+        for (int i = 0; i < len; i++) {
+            if (s2CellIds[i] != 0) {
+                tiles[i] = tileFunction.getTile(s2CellIds[i]);
+            }
+            values[i] = Double.NaN;
+        }
+
+        for (int i = 0; i < len; i++) {
+            if (tiles[i] == null || !Double.isNaN(values[i])) {
+                continue;
+            }
+
+            mergeByteBufferValues(params, s2CellIds, tiles, i, values);
+            mergeByteJpegValues(params, s2CellIds, tiles, i, values);
+            mergeBytePngValues(params, s2CellIds, tiles, i, values);
+        }
+
+        boolean allFound = true;
+        for (int i = 0; i < len; i++) {
+            if (s2CellIds[i] == 0) {
+                continue;
+            }
+            if (Double.isNaN(values[i])) {
+                allFound = false;
+            } else {
+                values[i] = (((int) values[i]) & 0xFF) / 255.0;
+            }
+        }
+        return allFound;
+    }
+
+    @SuppressWarnings("ReferenceEquality")
+    private static void mergeByteBufferValues(@NonNull MapParamsProto params,
+            @NonNull long[] s2CellIds,
+            @NonNull S2TileProto[] tiles,
+            int tileIndex, @NonNull double[] values) {
+        byte[] bytes = tiles[tileIndex].byteBuffer;
+        if (bytes == null || bytes.length == 0) {
+            return;
+        }
+
+        ByteBuffer byteBuffer = ByteBuffer.wrap(bytes).asReadOnlyBuffer();
+        int tileS2Level = params.mapS2Level - Integer.numberOfTrailingZeros(byteBuffer.limit()) / 2;
+        int numBitsLeftOfTile = 2 * tileS2Level + 3;
+
+        for (int i = tileIndex; i < tiles.length; i++) {
+            if (tiles[i] != tiles[tileIndex]) {
+                continue;
+            }
+
+            long maskedS2CellId = s2CellIds[i] & (-1L >>> numBitsLeftOfTile);
+            int numBitsRightOfMap = 2 * (S2CellIdUtils.MAX_LEVEL - params.mapS2Level) + 1;
+            int bufferIndex = (int) (maskedS2CellId >>> numBitsRightOfMap);
+            values[i] = Double.isNaN(values[i]) ? 0 : values[i];
+            values[i] += ((int) byteBuffer.get(bufferIndex)) & 0xFF;
+        }
+    }
+
+    private static void mergeByteJpegValues(@NonNull MapParamsProto params,
+            @NonNull long[] s2CellIds,
+            @NonNull S2TileProto[] tiles,
+            int tileIndex, @NonNull double[] values) {
+        mergeByteImageValues(params, tiles[tileIndex].byteJpeg, s2CellIds, tiles, tileIndex,
+                values);
+    }
+
+    private static void mergeBytePngValues(@NonNull MapParamsProto params,
+            @NonNull long[] s2CellIds,
+            @NonNull S2TileProto[] tiles,
+            int tileIndex, @NonNull double[] values) {
+        mergeByteImageValues(params, tiles[tileIndex].bytePng, s2CellIds, tiles, tileIndex, values);
+    }
+
+    @SuppressWarnings("ReferenceEquality")
+    private static void mergeByteImageValues(@NonNull MapParamsProto params, @NonNull byte[] bytes,
+            @NonNull long[] s2CellIds,
+            @NonNull S2TileProto[] tiles, int tileIndex, @NonNull double[] values) {
+        if (bytes == null || bytes.length == 0) {
+            return;
+        }
+        Bitmap bitmap = BitmapFactory.decodeByteArray(bytes, 0, bytes.length);
+        if (bitmap == null) {
+            return;
+        }
+
+        for (int i = tileIndex; i < tiles.length; i++) {
+            if (s2CellIds[i] == 0 || tiles[i] != tiles[tileIndex]) {
+                continue;
+            }
+
+            values[i] = Double.isNaN(values[i]) ? 0 : values[i];
+            values[i] += bitmap.getPixel(getIndexX(params, s2CellIds[i], bitmap.getWidth()),
+                    getIndexY(params, s2CellIds[i], bitmap.getHeight())) & 0xFF;
+        }
+    }
+
+    /** Returns the X index for an S2 cell within an S2 tile image of specified width. */
+    private static int getIndexX(@NonNull MapParamsProto params, long s2CellId, int width) {
+        return getIndexXOrY(params, S2CellIdUtils.getI(s2CellId), width);
+    }
+
+    /** Returns the Y index for an S2 cell within an S2 tile image of specified height. */
+    private static int getIndexY(@NonNull MapParamsProto params, long s2CellId, int height) {
+        return getIndexXOrY(params, S2CellIdUtils.getJ(s2CellId), height);
+    }
+
+    private static int getIndexXOrY(@NonNull MapParamsProto params, int iOrJ, int widthOrHeight) {
+        return (iOrJ >> (S2CellIdUtils.MAX_LEVEL - params.mapS2Level)) % widthOrHeight;
+    }
+
+    /**
+     * Returns the geoid heights in meters associated with the map cells identified by
+     * {@code s2CellIds}. Throws an {@link IOException} if a geoid height cannot be calculated for a
+     * non-zero ID.
+     */
+    @NonNull
+    public double[] readGeoidHeights(@NonNull MapParamsProto params, @NonNull Context context,
+            @NonNull long[] s2CellIds) throws IOException {
+        Preconditions.checkArgument(s2CellIds.length == 4);
+        for (long s2CellId : s2CellIds) {
+            Preconditions.checkArgument(
+                    s2CellId == 0 || S2CellIdUtils.getLevel(s2CellId) == params.mapS2Level);
+        }
+
+        double[] heightsMeters = new double[s2CellIds.length];
+        if (getGeoidHeights(params, mCacheTiles::get, s2CellIds, heightsMeters)) {
+            return heightsMeters;
+        }
+
+        TileFunction loadedTiles = loadFromCacheAndDisk(params, context, s2CellIds);
+        if (getGeoidHeights(params, loadedTiles, s2CellIds, heightsMeters)) {
+            return heightsMeters;
+        }
+        throw new IOException("Unable to calculate geoid heights from raw assets.");
+    }
+
+    /**
+     * Adds to {@code heightsMeters} the geoid heights in meters associated with the map cells
+     * identified by {@code s2CellIds}. Returns true if heights are present for all non-zero IDs;
+     * otherwise, returns false and adds NaNs for absent heights.
+     */
+    private boolean getGeoidHeights(@NonNull MapParamsProto params,
+            @NonNull TileFunction tileFunction, @NonNull long[] s2CellIds,
+            @NonNull double[] heightsMeters) {
+        boolean allFound = getUnitIntervalValues(params, tileFunction, s2CellIds, heightsMeters);
+        for (int i = 0; i < heightsMeters.length; i++) {
+            // NaNs are properly preserved.
+            heightsMeters[i] *= params.modelAMeters;
+            heightsMeters[i] += params.modelBMeters;
+        }
+        return allFound;
+    }
+
+    @NonNull
+    private TileFunction loadFromCacheAndDisk(@NonNull MapParamsProto params,
+            @NonNull Context context, @NonNull long[] s2CellIds) throws IOException {
+        int len = s2CellIds.length;
+
+        // Enable batch loading by finding all cache keys upfront.
+        long[] cacheKeys = new long[len];
+        for (int i = 0; i < len; i++) {
+            if (s2CellIds[i] == 0) {
+                continue;
+            }
+            cacheKeys[i] = getCacheKey(params, s2CellIds[i]);
+        }
+
+        // Attempt to load tiles from cache.
+        S2TileProto[] loadedTiles = new S2TileProto[len];
+        String[] diskTokens = new String[len];
+        for (int i = 0; i < len; i++) {
+            if (s2CellIds[i] == 0 || diskTokens[i] != null) {
+                continue;
+            }
+            loadedTiles[i] = mCacheTiles.get(cacheKeys[i]);
+            diskTokens[i] = getDiskToken(params, cacheKeys[i]);
+
+            // Batch across common cache key.
+            for (int j = i + 1; j < len; j++) {
+                if (cacheKeys[j] == cacheKeys[i]) {
+                    loadedTiles[j] = loadedTiles[i];
+                    diskTokens[j] = diskTokens[i];
+                }
+            }
+        }
+
+        // Attempt to load tiles from disk.
+        for (int i = 0; i < len; i++) {
+            if (s2CellIds[i] == 0 || loadedTiles[i] != null) {
+                continue;
+            }
+
+            S2TileProto tile;
+            try (InputStream is = context.getApplicationContext().getAssets().open(
+                    "geoid_height_map/tile-" + diskTokens[i] + ".pb")) {
+                tile = S2TileProto.parseFrom(is.readAllBytes());
+            } catch (IOException e) {
+                throw new RuntimeException(e);
+            }
+            mergeFromDiskTile(params, tile, cacheKeys, diskTokens, i, loadedTiles);
+        }
+
+        return s2CellId -> {
+            if (s2CellId == 0) {
+                return null;
+            }
+            long cacheKey = getCacheKey(params, s2CellId);
+            for (int i = 0; i < cacheKeys.length; i++) {
+                if (cacheKeys[i] == cacheKey) {
+                    return loadedTiles[i];
+                }
+            }
+            return null;
+        };
+    }
+
+    private void mergeFromDiskTile(@NonNull MapParamsProto params, @NonNull S2TileProto diskTile,
+            @NonNull long[] cacheKeys, @NonNull String[] diskTokens, int diskTokenIndex,
+            @NonNull S2TileProto[] loadedTiles) throws IOException {
+        int len = cacheKeys.length;
+        int numMapCellsPerCacheTile = 1 << (2 * (params.mapS2Level - params.cacheTileS2Level));
+
+        // Reusable arrays.
+        long[] s2CellIds = new long[numMapCellsPerCacheTile];
+        double[] values = new double[numMapCellsPerCacheTile];
+
+        // Each cache key identifies a different sub-tile of the disk tile.
+        TileFunction diskTileFunction = s2CellId -> diskTile;
+        for (int i = diskTokenIndex; i < len; i++) {
+            if (!Objects.equals(diskTokens[i], diskTokens[diskTokenIndex])
+                    || loadedTiles[i] != null) {
+                continue;
+            }
+
+            // Find all map cells within the current cache tile.
+            long s2CellId = S2CellIdUtils.getTraversalStart(cacheKeys[i], params.mapS2Level);
+            for (int j = 0; j < numMapCellsPerCacheTile; j++) {
+                s2CellIds[j] = s2CellId;
+                s2CellId = S2CellIdUtils.getTraversalNext(s2CellId);
+            }
+
+            if (!getUnitIntervalValues(params, diskTileFunction, s2CellIds, values)) {
+                throw new IOException("Corrupted disk tile of disk token: " + diskTokens[i]);
+            }
+
+            loadedTiles[i] = new S2TileProto();
+            loadedTiles[i].byteBuffer = new byte[numMapCellsPerCacheTile];
+            for (int j = 0; j < numMapCellsPerCacheTile; j++) {
+                loadedTiles[i].byteBuffer[j] = (byte) Math.round(values[j] * 0xFF);
+            }
+
+            // Batch across common cache key.
+            for (int j = i + 1; j < len; j++) {
+                if (cacheKeys[j] == cacheKeys[i]) {
+                    loadedTiles[j] = loadedTiles[i];
+                }
+            }
+
+            // Side load into tile cache.
+            mCacheTiles.put(cacheKeys[i], loadedTiles[i]);
+        }
+    }
+
+    /** Defines a function-like object to retrieve tiles for map cells. */
+    private interface TileFunction {
+
+        @Nullable
+        S2TileProto getTile(long s2CellId);
+    }
+}
diff --git a/location/java/com/android/internal/location/altitude/S2CellIdUtils.java b/location/java/com/android/internal/location/altitude/S2CellIdUtils.java
new file mode 100644
index 000000000000..5f113877d416
--- /dev/null
+++ b/location/java/com/android/internal/location/altitude/S2CellIdUtils.java
@@ -0,0 +1,653 @@
+/*
+ * Copyright (C) 2022 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+package com.android.internal.location.altitude;
+
+import android.annotation.NonNull;
+
+import java.util.Arrays;
+import java.util.Locale;
+
+/**
+ * Provides lightweight S2 cell ID utilities without traditional geometry dependencies.
+ *
+ * <p>See <a href="https://s2geometry.io/">the S2 Geometry Library website</a> for more details.
+ */
+public final class S2CellIdUtils {
+
+    /** The level of all leaf S2 cells. */
+    public static final int MAX_LEVEL = 30;
+
+    private static final int MAX_SIZE = 1 << MAX_LEVEL;
+    private static final double ONE_OVER_MAX_SIZE = 1.0 / MAX_SIZE;
+    private static final int NUM_FACES = 6;
+    private static final int POS_BITS = 2 * MAX_LEVEL + 1;
+    private static final int SWAP_MASK = 0x1;
+    private static final int LOOKUP_BITS = 4;
+    private static final int LOOKUP_MASK = (1 << LOOKUP_BITS) - 1;
+    private static final int INVERT_MASK = 0x2;
+    private static final int LEAF_MASK = 0x1;
+    private static final int[] LOOKUP_POS = new int[1 << (2 * LOOKUP_BITS + 2)];
+    private static final int[] LOOKUP_IJ = new int[1 << (2 * LOOKUP_BITS + 2)];
+    private static final int[] POS_TO_ORIENTATION = {SWAP_MASK, 0, 0, INVERT_MASK + SWAP_MASK};
+    private static final int[][] POS_TO_IJ =
+            {{0, 1, 3, 2}, {0, 2, 3, 1}, {3, 2, 0, 1}, {3, 1, 0, 2}};
+    private static final double UV_LIMIT = calculateUvLimit();
+    private static final UvTransform[] UV_TRANSFORMS = createUvTransforms();
+    private static final XyzTransform[] XYZ_TRANSFORMS = createXyzTransforms();
+
+    // Used to encode (i, j, o) coordinates into primitive longs.
+    private static final int I_SHIFT = 33;
+    private static final int J_SHIFT = 2;
+    private static final long J_MASK = (1L << 31) - 1;
+
+    static {
+        initLookupCells();
+    }
+
+    /** Prevents instantiation. */
+    private S2CellIdUtils() {
+    }
+
+    /**
+     * Returns the leaf S2 cell ID for the specified latitude and longitude, both measured in
+     * degrees.
+     */
+    public static long fromLatLngDegrees(double latDegrees, double lngDegrees) {
+        return fromLatLngRadians(Math.toRadians(latDegrees), Math.toRadians(lngDegrees));
+    }
+
+    /**
+     * Returns the ID of the parent of the specified S2 cell at the specified parent level.
+     * Behavior is undefined for invalid S2 cell IDs or parent levels not in
+     * [0, {@code getLevel(s2CellId)}[.
+     */
+    public static long getParent(long s2CellId, int level) {
+        long newLsb = getLowestOnBitForLevel(level);
+        return (s2CellId & -newLsb) | newLsb;
+    }
+
+    /**
+     * Inserts into {@code neighbors} the four S2 cell IDs corresponding to the neighboring
+     * cells adjacent across the specified cell's four edges. This array must be of minimum
+     * length four, and elements at the tail end of the array not corresponding to a neighbor
+     * are set to zero. A reference to this array is returned.
+     *
+     * <p>Inserts in the order of down, right, up, and left directions, in that order. All
+     * neighbors are guaranteed to be distinct.
+     */
+    public static void getEdgeNeighbors(long s2CellId, @NonNull long[] neighbors) {
+        int level = getLevel(s2CellId);
+        int size = levelToSizeIj(level);
+        int face = getFace(s2CellId);
+        long ijo = toIjo(s2CellId);
+        int i = ijoToI(ijo);
+        int j = ijoToJ(ijo);
+
+        int iPlusSize = i + size;
+        int iMinusSize = i - size;
+        int jPlusSize = j + size;
+        int jMinusSize = j - size;
+        boolean iPlusSizeLtMax = iPlusSize < MAX_SIZE;
+        boolean iMinusSizeGteZero = iMinusSize >= 0;
+        boolean jPlusSizeLtMax = jPlusSize < MAX_SIZE;
+        boolean jMinusSizeGteZero = jMinusSize >= 0;
+
+        int index = 0;
+        // Down direction.
+        neighbors[index++] = getParent(fromFijSame(face, i, jMinusSize, jMinusSizeGteZero),
+                level);
+        // Right direction.
+        neighbors[index++] = getParent(fromFijSame(face, iPlusSize, j, iPlusSizeLtMax), level);
+        // Up direction.
+        neighbors[index++] = getParent(fromFijSame(face, i, jPlusSize, jPlusSizeLtMax), level);
+        // Left direction.
+        neighbors[index++] = getParent(fromFijSame(face, iMinusSize, j, iMinusSizeGteZero),
+                level);
+
+        // Pad end of neighbor array with zeros.
+        Arrays.fill(neighbors, index, neighbors.length, 0);
+    }
+
+    /** Returns the "i" coordinate for the specified S2 cell. */
+    public static int getI(long s2CellId) {
+        return ijoToI(toIjo(s2CellId));
+    }
+
+    /** Returns the "j" coordinate for the specified S2 cell. */
+    public static int getJ(long s2CellId) {
+        return ijoToJ(toIjo(s2CellId));
+    }
+
+    /**
+     * Returns the leaf S2 cell ID for the specified latitude and longitude, both measured in
+     * radians.
+     */
+    private static long fromLatLngRadians(double latRadians, double lngRadians) {
+        double cosLat = Math.cos(latRadians);
+        double x = Math.cos(lngRadians) * cosLat;
+        double y = Math.sin(lngRadians) * cosLat;
+        double z = Math.sin(latRadians);
+        return fromXyz(x, y, z);
+    }
+
+    /**
+     * Returns the level of the specified S2 cell. The returned level is in [0, 30] for valid
+     * S2 cell IDs. Behavior is undefined for invalid S2 cell IDs.
+     */
+    static int getLevel(long s2CellId) {
+        if (isLeaf(s2CellId)) {
+            return MAX_LEVEL;
+        }
+        return MAX_LEVEL - (Long.numberOfTrailingZeros(s2CellId) >> 1);
+    }
+
+    /** Returns the lowest-numbered bit that is on for the specified S2 cell. */
+    static long getLowestOnBit(long s2CellId) {
+        return s2CellId & -s2CellId;
+    }
+
+    /** Returns the lowest-numbered bit that is on for any S2 cell on the specified level. */
+    static long getLowestOnBitForLevel(int level) {
+        return 1L << (2 * (MAX_LEVEL - level));
+    }
+
+    /**
+     * Returns the ID of the first S2 cell in a traversal of the children S2 cells at the specified
+     * level, in Hilbert curve order.
+     */
+    static long getTraversalStart(long s2CellId, int level) {
+        return s2CellId - getLowestOnBit(s2CellId) + getLowestOnBitForLevel(level);
+    }
+
+    /** Returns the ID of the next S2 cell at the same level along the Hilbert curve. */
+    static long getTraversalNext(long s2CellId) {
+        return s2CellId + (getLowestOnBit(s2CellId) << 1);
+    }
+
+    /**
+     * Encodes the S2 cell id to compact text strings suitable for display or indexing. Cells at
+     * lower levels (i.e., larger cells) are encoded into fewer characters.
+     */
+    @NonNull
+    static String getToken(long s2CellId) {
+        if (s2CellId == 0) {
+            return "X";
+        }
+
+        // Convert to a hex string with as many digits as necessary.
+        String hex = Long.toHexString(s2CellId).toLowerCase(Locale.US);
+        // Prefix 0s to get a length 16 string.
+        String padded = padStart(hex);
+        // Trim zeroes off the end.
+        return padded.replaceAll("0*$", "");
+    }
+
+    private static String padStart(String string) {
+        if (string.length() >= 16) {
+            return string;
+        }
+        return "0".repeat(16 - string.length()) + string;
+    }
+
+    /** Returns the leaf S2 cell ID of the specified (x, y, z) coordinate. */
+    private static long fromXyz(double x, double y, double z) {
+        int face = xyzToFace(x, y, z);
+        UvTransform uvTransform = UV_TRANSFORMS[face];
+        double u = uvTransform.xyzToU(x, y, z);
+        double v = uvTransform.xyzToV(x, y, z);
+        return fromFuv(face, u, v);
+    }
+
+    /** Returns the leaf S2 cell ID of the specified (face, u, v) coordinate. */
+    private static long fromFuv(int face, double u, double v) {
+        int i = uToI(u);
+        int j = vToJ(v);
+        return fromFij(face, i, j);
+    }
+
+    /** Returns the leaf S2 cell ID of the specified (face, i, j) coordinate. */
+    private static long fromFij(int face, int i, int j) {
+        int bits = (face & SWAP_MASK);
+        // Update most significant bits.
+        long msb = ((long) face) << (POS_BITS - 33);
+        for (int k = 7; k >= 4; --k) {
+            bits = lookupBits(i, j, k, bits);
+            msb = updateBits(msb, k, bits);
+            bits = maskBits(bits);
+        }
+        // Update least significant bits.
+        long lsb = 0;
+        for (int k = 3; k >= 0; --k) {
+            bits = lookupBits(i, j, k, bits);
+            lsb = updateBits(lsb, k, bits);
+            bits = maskBits(bits);
+        }
+        return (((msb << 32) + lsb) << 1) + 1;
+    }
+
+    private static long fromFijWrap(int face, int i, int j) {
+        double u = iToU(i);
+        double v = jToV(j);
+
+        XyzTransform xyzTransform = XYZ_TRANSFORMS[face];
+        double x = xyzTransform.uvToX(u, v);
+        double y = xyzTransform.uvToY(u, v);
+        double z = xyzTransform.uvToZ(u, v);
+
+        int newFace = xyzToFace(x, y, z);
+        UvTransform uvTransform = UV_TRANSFORMS[newFace];
+        double newU = uvTransform.xyzToU(x, y, z);
+        double newV = uvTransform.xyzToV(x, y, z);
+
+        int newI = uShiftIntoI(newU);
+        int newJ = vShiftIntoJ(newV);
+        return fromFij(newFace, newI, newJ);
+    }
+
+    private static long fromFijSame(int face, int i, int j, boolean isSameFace) {
+        if (isSameFace) {
+            return fromFij(face, i, j);
+        }
+        return fromFijWrap(face, i, j);
+    }
+
+    /**
+     * Returns the face associated with the specified (x, y, z) coordinate. For a coordinate
+     * on a face boundary, the returned face is arbitrary but repeatable.
+     */
+    private static int xyzToFace(double x, double y, double z) {
+        double absX = Math.abs(x);
+        double absY = Math.abs(y);
+        double absZ = Math.abs(z);
+        if (absX > absY) {
+            if (absX > absZ) {
+                return (x < 0) ? 3 : 0;
+            }
+            return (z < 0) ? 5 : 2;
+        }
+        if (absY > absZ) {
+            return (y < 0) ? 4 : 1;
+        }
+        return (z < 0) ? 5 : 2;
+    }
+
+    private static int uToI(double u) {
+        double s;
+        if (u >= 0) {
+            s = 0.5 * Math.sqrt(1 + 3 * u);
+        } else {
+            s = 1 - 0.5 * Math.sqrt(1 - 3 * u);
+        }
+        return Math.max(0, Math.min(MAX_SIZE - 1, (int) Math.round(MAX_SIZE * s - 0.5)));
+    }
+
+    private static int vToJ(double v) {
+        // Same calculation as uToI.
+        return uToI(v);
+    }
+
+    private static int lookupBits(int i, int j, int k, int bits) {
+        bits += ((i >> (k * LOOKUP_BITS)) & LOOKUP_MASK) << (LOOKUP_BITS + 2);
+        bits += ((j >> (k * LOOKUP_BITS)) & LOOKUP_MASK) << 2;
+        return LOOKUP_POS[bits];
+    }
+
+    private static long updateBits(long sb, int k, int bits) {
+        return sb | ((((long) bits) >> 2) << ((k & 0x3) * 2 * LOOKUP_BITS));
+    }
+
+    private static int maskBits(int bits) {
+        return bits & (SWAP_MASK | INVERT_MASK);
+    }
+
+    private static int getFace(long s2CellId) {
+        return (int) (s2CellId >>> POS_BITS);
+    }
+
+    private static boolean isLeaf(long s2CellId) {
+        return ((int) s2CellId & LEAF_MASK) != 0;
+    }
+
+    private static double iToU(int i) {
+        int satI = Math.max(-1, Math.min(MAX_SIZE, i));
+        return Math.max(
+                -UV_LIMIT,
+                Math.min(UV_LIMIT, ONE_OVER_MAX_SIZE * ((satI << 1) + 1 - MAX_SIZE)));
+    }
+
+    private static double jToV(int j) {
+        // Same calculation as iToU.
+        return iToU(j);
+    }
+
+    private static long toIjo(long s2CellId) {
+        int face = getFace(s2CellId);
+        int bits = face & SWAP_MASK;
+        int i = 0;
+        int j = 0;
+        for (int k = 7; k >= 0; --k) {
+            int nbits = (k == 7) ? (MAX_LEVEL - 7 * LOOKUP_BITS) : LOOKUP_BITS;
+            bits += ((int) (s2CellId >>> (k * 2 * LOOKUP_BITS + 1)) & ((1 << (2 * nbits))
+                    - 1)) << 2;
+            bits = LOOKUP_IJ[bits];
+            i += (bits >> (LOOKUP_BITS + 2)) << (k * LOOKUP_BITS);
+            j += ((bits >> 2) & ((1 << LOOKUP_BITS) - 1)) << (k * LOOKUP_BITS);
+            bits &= (SWAP_MASK | INVERT_MASK);
+        }
+        int orientation =
+                ((getLowestOnBit(s2CellId) & 0x1111111111111110L) != 0) ? (bits ^ SWAP_MASK)
+                        : bits;
+        return (((long) i) << I_SHIFT) | (((long) j) << J_SHIFT) | orientation;
+    }
+
+    private static int ijoToI(long ijo) {
+        return (int) (ijo >>> I_SHIFT);
+    }
+
+    private static int ijoToJ(long ijo) {
+        return (int) ((ijo >>> J_SHIFT) & J_MASK);
+    }
+
+    private static int uShiftIntoI(double u) {
+        double s = 0.5 * (u + 1);
+        return Math.max(0, Math.min(MAX_SIZE - 1, (int) Math.round(MAX_SIZE * s - 0.5)));
+    }
+
+    private static int vShiftIntoJ(double v) {
+        // Same calculation as uShiftIntoI.
+        return uShiftIntoI(v);
+    }
+
+    private static int levelToSizeIj(int level) {
+        return 1 << (MAX_LEVEL - level);
+    }
+
+    private static void initLookupCells() {
+        initLookupCell(0, 0, 0, 0, 0, 0);
+        initLookupCell(0, 0, 0, SWAP_MASK, 0, SWAP_MASK);
+        initLookupCell(0, 0, 0, INVERT_MASK, 0, INVERT_MASK);
+        initLookupCell(0, 0, 0, SWAP_MASK | INVERT_MASK, 0, SWAP_MASK | INVERT_MASK);
+    }
+
+    private static void initLookupCell(
+            int level, int i, int j, int origOrientation, int pos, int orientation) {
+        if (level == LOOKUP_BITS) {
+            int ij = (i << LOOKUP_BITS) + j;
+            LOOKUP_POS[(ij << 2) + origOrientation] = (pos << 2) + orientation;
+            LOOKUP_IJ[(pos << 2) + origOrientation] = (ij << 2) + orientation;
+        } else {
+            level++;
+            i <<= 1;
+            j <<= 1;
+            pos <<= 2;
+            for (int subPos = 0; subPos < 4; subPos++) {
+                int ij = POS_TO_IJ[orientation][subPos];
+                int orientationMask = POS_TO_ORIENTATION[subPos];
+                initLookupCell(
+                        level,
+                        i + (ij >>> 1),
+                        j + (ij & 0x1),
+                        origOrientation,
+                        pos + subPos,
+                        orientation ^ orientationMask);
+            }
+        }
+    }
+
+    private static double calculateUvLimit() {
+        double machEps = 1.0;
+        do {
+            machEps /= 2.0f;
+        } while ((1.0 + (machEps / 2.0)) != 1.0);
+        return 1.0 + machEps;
+    }
+
+    @NonNull
+    private static UvTransform[] createUvTransforms() {
+        UvTransform[] uvTransforms = new UvTransform[NUM_FACES];
+        uvTransforms[0] =
+                new UvTransform() {
+
+                    @Override
+                    public double xyzToU(double x, double y, double z) {
+                        return y / x;
+                    }
+
+                    @Override
+                    public double xyzToV(double x, double y, double z) {
+                        return z / x;
+                    }
+                };
+        uvTransforms[1] =
+                new UvTransform() {
+
+                    @Override
+                    public double xyzToU(double x, double y, double z) {
+                        return -x / y;
+                    }
+
+                    @Override
+                    public double xyzToV(double x, double y, double z) {
+                        return z / y;
+                    }
+                };
+        uvTransforms[2] =
+                new UvTransform() {
+
+                    @Override
+                    public double xyzToU(double x, double y, double z) {
+                        return -x / z;
+                    }
+
+                    @Override
+                    public double xyzToV(double x, double y, double z) {
+                        return -y / z;
+                    }
+                };
+        uvTransforms[3] =
+                new UvTransform() {
+
+                    @Override
+                    public double xyzToU(double x, double y, double z) {
+                        return z / x;
+                    }
+
+                    @Override
+                    public double xyzToV(double x, double y, double z) {
+                        return y / x;
+                    }
+                };
+        uvTransforms[4] =
+                new UvTransform() {
+
+                    @Override
+                    public double xyzToU(double x, double y, double z) {
+                        return z / y;
+                    }
+
+                    @Override
+                    public double xyzToV(double x, double y, double z) {
+                        return -x / y;
+                    }
+                };
+        uvTransforms[5] =
+                new UvTransform() {
+
+                    @Override
+                    public double xyzToU(double x, double y, double z) {
+                        return -y / z;
+                    }
+
+                    @Override
+                    public double xyzToV(double x, double y, double z) {
+                        return -x / z;
+                    }
+                };
+        return uvTransforms;
+    }
+
+    @NonNull
+    private static XyzTransform[] createXyzTransforms() {
+        XyzTransform[] xyzTransforms = new XyzTransform[NUM_FACES];
+        xyzTransforms[0] =
+                new XyzTransform() {
+
+                    @Override
+                    public double uvToX(double u, double v) {
+                        return 1;
+                    }
+
+                    @Override
+                    public double uvToY(double u, double v) {
+                        return u;
+                    }
+
+                    @Override
+                    public double uvToZ(double u, double v) {
+                        return v;
+                    }
+                };
+        xyzTransforms[1] =
+                new XyzTransform() {
+
+                    @Override
+                    public double uvToX(double u, double v) {
+                        return -u;
+                    }
+
+                    @Override
+                    public double uvToY(double u, double v) {
+                        return 1;
+                    }
+
+                    @Override
+                    public double uvToZ(double u, double v) {
+                        return v;
+                    }
+                };
+        xyzTransforms[2] =
+                new XyzTransform() {
+
+                    @Override
+                    public double uvToX(double u, double v) {
+                        return -u;
+                    }
+
+                    @Override
+                    public double uvToY(double u, double v) {
+                        return -v;
+                    }
+
+                    @Override
+                    public double uvToZ(double u, double v) {
+                        return 1;
+                    }
+                };
+        xyzTransforms[3] =
+                new XyzTransform() {
+
+                    @Override
+                    public double uvToX(double u, double v) {
+                        return -1;
+                    }
+
+                    @Override
+                    public double uvToY(double u, double v) {
+                        return -v;
+                    }
+
+                    @Override
+                    public double uvToZ(double u, double v) {
+                        return -u;
+                    }
+                };
+        xyzTransforms[4] =
+                new XyzTransform() {
+
+                    @Override
+                    public double uvToX(double u, double v) {
+                        return v;
+                    }
+
+                    @Override
+                    public double uvToY(double u, double v) {
+                        return -1;
+                    }
+
+                    @Override
+                    public double uvToZ(double u, double v) {
+                        return -u;
+                    }
+                };
+        xyzTransforms[5] =
+                new XyzTransform() {
+
+                    @Override
+                    public double uvToX(double u, double v) {
+                        return v;
+                    }
+
+                    @Override
+                    public double uvToY(double u, double v) {
+                        return u;
+                    }
+
+                    @Override
+                    public double uvToZ(double u, double v) {
+                        return -1;
+                    }
+                };
+        return xyzTransforms;
+    }
+
+    /**
+     * Transform from (x, y, z) coordinates to (u, v) coordinates, indexed by face. For a
+     * (x, y, z) coordinate within a face, each element of the resulting (u, v) coordinate
+     * should lie in the inclusive range [-1, 1], with the face center having a (u, v)
+     * coordinate equal to (0, 0).
+     */
+    private interface UvTransform {
+
+        /**
+         * Returns for the specified (x, y, z) coordinate the corresponding u-coordinate
+         * (which may lie outside the range [-1, 1]).
+         */
+        double xyzToU(double x, double y, double z);
+
+        /**
+         * Returns for the specified (x, y, z) coordinate the corresponding v-coordinate
+         * (which may lie outside the range [-1, 1]).
+         */
+        double xyzToV(double x, double y, double z);
+    }
+
+    /**
+     * Transform from (u, v) coordinates to (x, y, z) coordinates, indexed by face. The
+     * resulting vectors are not necessarily of unit length.
+     */
+    private interface XyzTransform {
+
+        /** Returns for the specified (u, v) coordinate the corresponding x-coordinate. */
+        double uvToX(double u, double v);
+
+        /** Returns for the specified (u, v) coordinate the corresponding y-coordinate. */
+        double uvToY(double u, double v);
+
+        /** Returns for the specified (u, v) coordinate the corresponding z-coordinate. */
+        double uvToZ(double u, double v);
+    }
+}