1 : /******************************************************************************
2 : * $Id: ogr_xplane_geo_utils.cpp
3 : *
4 : * Project: X-Plane aeronautical data reader
5 : * Purpose: Geo-computations
6 : * Author: Even Rouault, even dot rouault at mines dash paris dot org
7 : *
8 : ******************************************************************************
9 : * Copyright (c) 2008, Even Rouault
10 : *
11 : * Permission is hereby granted, free of charge, to any person obtaining a
12 : * copy of this software and associated documentation files (the "Software"),
13 : * to deal in the Software without restriction, including without limitation
14 : * the rights to use, copy, modify, merge, publish, distribute, sublicense,
15 : * and/or sell copies of the Software, and to permit persons to whom the
16 : * Software is furnished to do so, subject to the following conditions:
17 : *
18 : * The above copyright notice and this permission notice shall be included
19 : * in all copies or substantial portions of the Software.
20 : *
21 : * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
22 : * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
23 : * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
24 : * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
25 : * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
26 : * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
27 : * DEALINGS IN THE SOFTWARE.
28 : ****************************************************************************/
29 :
30 : #include "ogr_xplane_geo_utils.h"
31 : #include <math.h>
32 : #include "cpl_port.h"
33 :
34 : CPL_CVSID("$Id: ogr_xplane_geo_utils.cpp 18430 2010-01-03 15:10:02Z rouault $");
35 :
36 : #ifndef M_PI
37 : # define M_PI 3.1415926535897932384626433832795
38 : #endif
39 :
40 : #define RAD2METER ((180./M_PI)*60.*1852.)
41 : #define METER2RAD (1/RAD2METER)
42 :
43 : #define DEG2RAD (M_PI/180.)
44 : #define RAD2DEG (1/DEG2RAD)
45 :
46 : static
47 9755 : double OGRXPlane_Safe_acos(double x)
48 : {
49 9755 : if (x > 1)
50 20 : x = 1;
51 9735 : else if (x < -1)
52 0 : x = -1;
53 9755 : return acos(x);
54 : }
55 :
56 : /************************************************************************/
57 : /* OGRXPlane_Distance() */
58 : /************************************************************************/
59 :
60 49 : double OGRXPlane_Distance(double LatA_deg, double LonA_deg,
61 : double LatB_deg, double LonB_deg)
62 : {
63 : double LatA_rad, LatB_rad;
64 : double cosa, cosb, sina, sinb, cosP;
65 : double cos_angle;
66 :
67 49 : cosP = cos((LonB_deg - LonA_deg) * DEG2RAD);
68 49 : LatA_rad = LatA_deg * DEG2RAD;
69 49 : LatB_rad = LatB_deg * DEG2RAD;
70 49 : cosa = cos(LatA_rad);
71 49 : sina = sin(LatA_rad);
72 49 : cosb = cos(LatB_rad);
73 49 : sinb = sin(LatB_rad);
74 49 : cos_angle = sina*sinb + cosa*cosb*cosP;
75 49 : return OGRXPlane_Safe_acos(cos_angle) * RAD2METER;
76 : }
77 :
78 : /************************************************************************/
79 : /* OGRXPlane_Track() */
80 : /************************************************************************/
81 :
82 108 : double OGRXPlane_Track(double LatA_deg, double LonA_deg,
83 : double LatB_deg, double LonB_deg)
84 : {
85 108 : if (fabs (LatA_deg - 90) < 1e-10 || fabs (LatB_deg + 90) < 1e-10)
86 : {
87 0 : return 180;
88 : }
89 108 : else if (fabs (LatA_deg + 90) < 1e-10 || fabs (LatB_deg - 90) < 1e-10)
90 : {
91 0 : return 0;
92 : }
93 : else
94 : {
95 : double cos_LatA, sin_LatA, diffG, cos_diffG, sin_diffG;
96 : double denom;
97 : double track;
98 108 : double LatA_rad = LatA_deg * DEG2RAD;
99 108 : double LatB_rad = LatB_deg * DEG2RAD;
100 :
101 108 : cos_LatA = cos(LatA_rad);
102 108 : sin_LatA = sin(LatA_rad);
103 :
104 108 : diffG = (LonA_deg - LonB_deg) * DEG2RAD;
105 108 : cos_diffG = cos(diffG);
106 108 : sin_diffG = sin(diffG);
107 :
108 108 : denom = sin_LatA * cos_diffG - cos_LatA * tan(LatB_rad);
109 :
110 108 : track = atan (sin_diffG / denom) * RAD2DEG;
111 :
112 108 : if (denom > 0.0)
113 : {
114 53 : track = 180 + track;
115 : }
116 55 : else if (track < 0)
117 : {
118 12 : track = 360 + track;
119 : }
120 :
121 108 : return track;
122 : }
123 : }
124 :
125 : /************************************************************************/
126 : /* OGRXPlane_ExtendPosition() */
127 : /************************************************************************/
128 :
129 4853 : int OGRXPlane_ExtendPosition(double dfLatA_deg, double dfLonA_deg,
130 : double dfDistance, double dfHeading,
131 : double* pdfLatB_deg, double* pdfLonB_deg)
132 : {
133 : double dfHeadingRad, cos_Heading, sin_Heading;
134 : double dfDistanceRad, cos_Distance, sin_Distance;
135 : double dfLatA_rad, cos_complement_LatA, sin_complement_LatA;
136 : double cos_complement_latB, complement_latB;
137 : double Cos_dG, dG_deg;
138 :
139 4853 : dfHeadingRad = dfHeading * DEG2RAD;
140 4853 : cos_Heading = cos (dfHeadingRad);
141 4853 : sin_Heading = sin (dfHeadingRad);
142 :
143 4853 : dfDistanceRad = dfDistance * METER2RAD;
144 4853 : cos_Distance = cos (dfDistanceRad);
145 4853 : sin_Distance = sin (dfDistanceRad);
146 :
147 4853 : dfLatA_rad = dfLatA_deg * DEG2RAD;
148 4853 : cos_complement_LatA = sin(dfLatA_rad);
149 4853 : sin_complement_LatA = cos(dfLatA_rad);
150 :
151 : cos_complement_latB = cos_Distance * cos_complement_LatA +
152 4853 : sin_Distance * sin_complement_LatA * cos_Heading;
153 :
154 4853 : complement_latB = OGRXPlane_Safe_acos(cos_complement_latB);
155 :
156 : Cos_dG = (cos_Distance - cos_complement_latB * cos_complement_LatA) /
157 4853 : (sin(complement_latB) * sin_complement_LatA);
158 4853 : *pdfLatB_deg = 90 - complement_latB * RAD2DEG;
159 :
160 4853 : dG_deg = OGRXPlane_Safe_acos(Cos_dG) * RAD2DEG;
161 :
162 4853 : if (sin_Heading < 0)
163 2328 : *pdfLonB_deg = dfLonA_deg - dG_deg;
164 : else
165 2525 : *pdfLonB_deg = dfLonA_deg + dG_deg;
166 :
167 4853 : if (*pdfLonB_deg > 180)
168 0 : *pdfLonB_deg -= 360;
169 4853 : else if (*pdfLonB_deg <= -180)
170 0 : *pdfLonB_deg += 360;
171 :
172 4853 : return 1;
173 : }
|
