1 /*
2  * Copyright (C) 2017 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 android.location.cts.gnss.pseudorange;
18 
19 import android.location.cts.gnss.pseudorange.Ecef2LlaConverter.GeodeticLlaValues;
20 import org.apache.commons.math.linear.RealMatrix;
21 
22 /**
23  * Transformations from ECEF coordiantes to Topocentric coordinates
24  */
25 public class EcefToTopocentricConverter {
26   private static final double MIN_DISTANCE_MAGNITUDE_METERS = 1.0e-22;
27   private static final int EAST_IDX = 0;
28   private static final int NORTH_IDX = 1;
29   private static final int UP_IDX = 2;
30 
31   /**
32    * Transformation of {@code inputVectorMeters} with origin at {@code originECEFMeters} into
33    * topocentric coordinate system. The result is {@code TopocentricAEDValues} containing azimuth
34    * from north +ve clockwise, radians; elevation angle, radians; distance, vector length meters
35    *
36    * <p>Source: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates
37    * http://kom.aau.dk/~borre/life-l99/topocent.m
38    *
39    */
convertCartesianToTopocentericRadMeters( final double[] originECEFMeters, final double[] inputVectorMeters)40   public static TopocentricAEDValues convertCartesianToTopocentericRadMeters(
41       final double[] originECEFMeters, final double[] inputVectorMeters) {
42 
43     GeodeticLlaValues latLngAlt = Ecef2LlaConverter.convertECEFToLLACloseForm(originECEFMeters[0],
44         originECEFMeters[1], originECEFMeters[2]);
45 
46     RealMatrix rotationMatrix =
47         Ecef2EnuConverter.
48             getRotationMatrix(latLngAlt.latitudeRadians, latLngAlt.longitudeRadians).transpose();
49     double[] eastNorthUpVectorMeters = GpsMathOperations.matrixByColVectMultiplication(
50         rotationMatrix.transpose().getData(), inputVectorMeters);
51     double eastMeters = eastNorthUpVectorMeters[EAST_IDX];
52     double northMeters = eastNorthUpVectorMeters[NORTH_IDX];
53     double upMeters = eastNorthUpVectorMeters[UP_IDX];
54 
55     // calculate azimuth, elevation and height from the ENU values
56     double horizontalDistanceMeters = Math.hypot(eastMeters, northMeters);
57     double azimuthRadians;
58     double elevationRadians;
59 
60     if (horizontalDistanceMeters < MIN_DISTANCE_MAGNITUDE_METERS) {
61       elevationRadians = Math.PI / 2.0;
62       azimuthRadians = 0;
63     } else {
64       elevationRadians = Math.atan2(upMeters, horizontalDistanceMeters);
65       azimuthRadians = Math.atan2(eastMeters, northMeters);
66     }
67     if (azimuthRadians < 0) {
68       azimuthRadians += 2 * Math.PI;
69     }
70 
71     double distanceMeters = Math.sqrt(Math.pow(inputVectorMeters[0], 2)
72         + Math.pow(inputVectorMeters[1], 2) + Math.pow(inputVectorMeters[2], 2));
73     return new TopocentricAEDValues(elevationRadians, azimuthRadians, distanceMeters);
74   }
75 
76   /**
77    * Calculate azimuth, elevation in radians,and distance in meters between the user position in
78    * ECEF meters {@code userPositionECEFMeters} and the satellite position in ECEF meters
79    * {@code satPositionECEFMeters}
80    */
calculateElAzDistBetween2Points( double[] userPositionECEFMeters, double[] satPositionECEFMeters)81   public static TopocentricAEDValues calculateElAzDistBetween2Points(
82       double[] userPositionECEFMeters, double[] satPositionECEFMeters) {
83 
84     return convertCartesianToTopocentericRadMeters(userPositionECEFMeters,
85         GpsMathOperations.subtractTwoVectors(satPositionECEFMeters, userPositionECEFMeters));
86 
87   }
88 
89   /**
90    *
91    * Class containing topocenter coordinates: azimuth in radians, elevation in radians, and distance
92    * in meters
93    */
94   public static class TopocentricAEDValues {
95 
96     public final double elevationRadians;
97     public final double azimuthRadians;
98     public final double distanceMeters;
99 
TopocentricAEDValues(double elevationRadians, double azimuthRadians, double distanceMeters)100     public TopocentricAEDValues(double elevationRadians, double azimuthRadians,
101         double distanceMeters) {
102       this.elevationRadians = elevationRadians;
103       this.azimuthRadians = azimuthRadians;
104       this.distanceMeters = distanceMeters;
105     }
106   }
107 }
108