1 // Copyright 2008, Google Inc. All rights reserved.
2 //
3 // Redistribution and use in source and binary forms, with or without
4 // modification, are permitted provided that the following conditions are met:
5 //
6 // 1. Redistributions of source code must retain the above copyright notice,
7 // this list of conditions and the following disclaimer.
8 // 2. Redistributions in binary form must reproduce the above copyright notice,
9 // this list of conditions and the following disclaimer in the documentation
10 // and/or other materials provided with the distribution.
11 // 3. Neither the name of Google Inc. nor the names of its contributors may be
12 // used to endorse or promote products derived from this software without
13 // specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
16 // WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
17 // MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO
18 // EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
19 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
20 // PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
21 // OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
22 // WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
23 // OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
24 // ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25
26 #include "kml/base/math_util.h"
27
28 // The mean radius of the Earth in meters.
29 // Equatorial = 6378137, polar = 6356752.
30 static const unsigned int kEarthRadius = 6366710;
31
32 namespace kmlbase {
33
AzimuthBetweenPoints(double lat1,double lng1,double lat2,double lng2)34 double AzimuthBetweenPoints(double lat1, double lng1,
35 double lat2, double lng2) {
36 const double lat1_r = DegToRad(lat1);
37 const double lng1_r = DegToRad(lng1);
38 const double lat2_r = DegToRad(lat2);
39 const double lng2_r = DegToRad(lng2);
40 return RadToDeg(fmod(atan2(sin(lng2_r - lng1_r) * cos(lat2_r),
41 cos(lat1_r) * sin(lat2_r) - sin(lat1_r) *
42 cos(lat2_r) * cos(lng2_r - lng1_r)), 2 * M_PI));
43 }
44
DistanceBetweenPoints(double lat1,double lng1,double lat2,double lng2)45 double DistanceBetweenPoints(double lat1, double lng1,
46 double lat2, double lng2) {
47 const double lat1_r = DegToRad(lat1);
48 const double lng1_r = DegToRad(lng1);
49 const double lat2_r = DegToRad(lat2);
50 const double lng2_r = DegToRad(lng2);
51 return RadiansToMeters(2 * asin(sqrt(pow(sin((lat1_r - lat2_r)/2), 2) +
52 cos(lat1_r) * cos(lat2_r) *
53 pow(sin((lng1_r - lng2_r) / 2), 2))));
54 }
55
DistanceBetweenPoints3d(double lat1,double lng1,double alt1,double lat2,double lng2,double alt2)56 double DistanceBetweenPoints3d(double lat1, double lng1,
57 double alt1, double lat2,
58 double lng2, double alt2) {
59 double surface_distance = DistanceBetweenPoints(lat1, lng1, lat2, lng2);
60 return sqrt(pow(surface_distance, 2) + pow(alt2 - alt1, 2));
61 }
62
ElevationBetweenPoints(double lat1,double lng1,double alt1,double lat2,double lng2,double alt2)63 double ElevationBetweenPoints(double lat1, double lng1, double alt1,
64 double lat2, double lng2, double alt2) {
65 // Naive implementation, accurate only over short distances.
66 // TODO: see header comment about curvature.
67 double distance = DistanceBetweenPoints(lat1, lng1, lat2, lng2);
68 return RadToDeg(atan((alt2 - alt1) / distance));
69 }
70
GroundDistanceFromRangeAndElevation(double range,double elevation)71 double GroundDistanceFromRangeAndElevation(double range, double elevation) {
72 return fabs(cos(DegToRad(elevation)) * range);
73 }
74
HeightFromRangeAndElevation(double range,double elevation)75 double HeightFromRangeAndElevation(double range, double elevation) {
76 return fabs(sin(DegToRad(elevation)) * range);
77 }
78
LatLngOnRadialFromPoint(double lat,double lng,double distance,double radial)79 Vec3 LatLngOnRadialFromPoint(double lat, double lng,
80 double distance, double radial) {
81 const double lat_r = DegToRad(lat);
82 const double lng_r = DegToRad(lng);
83 const double distance_r = MetersToRadians(distance);
84 const double radial_r = DegToRad(radial);
85 const double radial_lat = asin(sin(lat_r) * cos(distance_r) +
86 cos(lat_r) * sin(distance_r) * cos(radial_r));
87 const double delta_lng = atan2(sin(radial_r) * sin(distance_r) * cos(lat_r),
88 cos(distance_r) - sin(lat_r) * sin(radial_lat));
89 const double radial_lng = fmod(lng_r + delta_lng + M_PI, 2 * M_PI) - M_PI;
90 return Vec3(RadToDeg(radial_lng), RadToDeg(radial_lat));
91 }
92
DegToRad(double degrees)93 double DegToRad(double degrees) { return degrees * M_PI / 180.0; }
RadToDeg(double radians)94 double RadToDeg(double radians) { return radians * 180.0 / M_PI; }
MetersToRadians(double meters)95 double MetersToRadians(double meters) { return meters / kEarthRadius; }
RadiansToMeters(double radians)96 double RadiansToMeters(double radians) { return radians * kEarthRadius; }
97
98 } // end namespace kmlbase
99