1 // polar.cxx -- routines to deal with polar math and transformations
3 // Written by Curtis Olson, started June 1997.
5 // Copyright (C) 1997 Curtis L. Olson - http://www.flightgear.org/~curt
7 // This library is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU Library General Public
9 // License as published by the Free Software Foundation; either
10 // version 2 of the License, or (at your option) any later version.
12 // This library is distributed in the hope that it will be useful,
13 // but WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 // Library General Public License for more details.
17 // You should have received a copy of the GNU Library General Public
18 // License along with this library; if not, write to the
19 // Free Software Foundation, Inc., 59 Temple Place - Suite 330,
20 // Boston, MA 02111-1307, USA.
25 # include <simgear_config.h>
30 #include <simgear/constants.h>
32 #include "polar3d.hxx"
35 * Calculate new lon/lat given starting lon/lat, and offset radial, and
36 * distance. NOTE: starting point is specifed in radians, distance is
37 * specified in meters (and converted internally to radians)
38 * ... assumes a spherical world.
39 * @param orig specified in polar coordinates
40 * @param course offset radial
41 * @param dist offset distance
42 * @return destination point in polar coordinates
44 Point3D calc_gc_lon_lat( const Point3D& orig, double course,
48 // lat=asin(sin(lat1)*cos(d)+cos(lat1)*sin(d)*cos(tc))
50 // lon=lon1 // endpoint a pole
52 // lon=mod(lon1-asin(sin(tc)*sin(d)/cos(lat))+pi,2*pi)-pi
55 // printf("calc_lon_lat() offset.theta = %.2f offset.dist = %.2f\n",
56 // offset.theta, offset.dist);
58 dist *= SG_METER_TO_NM * SG_NM_TO_RAD;
60 result.sety( asin( sin(orig.y()) * cos(dist) +
61 cos(orig.y()) * sin(dist) * cos(course) ) );
63 if ( cos(result.y()) < SG_EPSILON ) {
64 result.setx( orig.x() ); // endpoint a pole
67 fmod(orig.x() - asin( sin(course) * sin(dist) /
69 + SGD_PI, SGD_2PI) - SGD_PI );
76 * Calculate course/dist given two spherical points.
77 * @param start starting point
78 * @param dest ending point
79 * @param course resulting course
80 * @param dist resulting distance
82 void calc_gc_course_dist( const Point3D& start, const Point3D& dest,
83 double *course, double *dist )
90 // d = 2*asin(sqrt((sin((lat1-lat2)/2))^2 +
91 // cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
92 double cos_start_y = cos( start.y() );
93 double tmp1 = sin( (start.y() - dest.y()) * 0.5 );
94 double tmp2 = sin( (start.x() - dest.x()) * 0.5 );
95 double d = 2.0 * asin( sqrt( tmp1 * tmp1 +
96 cos_start_y * cos(dest.y()) * tmp2 * tmp2));
98 *dist = d * SG_RAD_TO_NM * SG_NM_TO_METER;
102 cos(dest.y())*sin(dest.x()-start.x()),
103 cos(start.y())*sin(dest.y())-
104 sin(start.y())*cos(dest.y())*cos(dest.x()-start.x()));
106 *course = SGD_2PI-c1;
110 // We obtain the initial course, tc1, (at point 1) from point 1 to
111 // point 2 by the following. The formula fails if the initial
112 // point is a pole. We can special case this with:
114 // IF (cos(lat1) < EPS) // EPS a small number ~ machine precision
116 // tc1= pi // starting from N pole
118 // tc1= 0 // starting from S pole
122 // For starting points other than the poles:
124 // IF sin(lon2-lon1)<0
125 // tc1=acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)))
127 // tc1=2*pi-acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)))
130 // if ( cos(start.y()) < SG_EPSILON ) {
131 // doing it this way saves a transcendental call
132 double sin_start_y = sin( start.y() );
133 if ( fabs(1.0-sin_start_y) < SG_EPSILON ) {
134 // EPS a small number ~ machine precision
135 if ( start.y() > 0 ) {
136 *course = SGD_PI; // starting from N pole
138 *course = 0; // starting from S pole
141 // For starting points other than the poles:
142 // double tmp3 = sin(d)*cos_start_y);
143 // double tmp4 = sin(dest.y())-sin(start.y())*cos(d);
144 // double tmp5 = acos(tmp4/tmp3);
146 // Doing this way gaurentees that the temps are
147 // not stored into memory
148 double tmp5 = acos( (sin(dest.y()) - sin_start_y * cos(d)) /
149 (sin(d) * cos_start_y) );
151 // if ( sin( dest.x() - start.x() ) < 0 ) {
152 // the sin of the negative angle is just the opposite sign
153 // of the sin of the angle so tmp2 will have the opposite
154 // sign of sin( dest.x() - start.x() )
158 *course = SGD_2PI - tmp5;
167 * Calculate course/dist given two spherical points.
168 * @param start starting point
169 * @param dest ending point
170 * @param course resulting course
171 * @param dist resulting distance
173 void calc_gc_course_dist( const Point3D& start, const Point3D& dest,
174 double *course, double *dist ) {
175 // d = 2*asin(sqrt((sin((lat1-lat2)/2))^2 +
176 // cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
177 double tmp1 = sin( (start.y() - dest.y()) / 2 );
178 double tmp2 = sin( (start.x() - dest.x()) / 2 );
179 double d = 2.0 * asin( sqrt( tmp1 * tmp1 +
180 cos(start.y()) * cos(dest.y()) * tmp2 * tmp2));
181 // We obtain the initial course, tc1, (at point 1) from point 1 to
182 // point 2 by the following. The formula fails if the initial
183 // point is a pole. We can special case this with:
185 // IF (cos(lat1) < EPS) // EPS a small number ~ machine precision
187 // tc1= pi // starting from N pole
189 // tc1= 0 // starting from S pole
193 // For starting points other than the poles:
195 // IF sin(lon2-lon1)<0
196 // tc1=acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)))
198 // tc1=2*pi-acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)))
203 if ( cos(start.y()) < SG_EPSILON ) {
204 // EPS a small number ~ machine precision
205 if ( start.y() > 0 ) {
206 tc1 = SGD_PI; // starting from N pole
208 tc1 = 0; // starting from S pole
212 // For starting points other than the poles:
214 double tmp3 = sin(d)*cos(start.y());
215 double tmp4 = sin(dest.y())-sin(start.y())*cos(d);
216 double tmp5 = acos(tmp4/tmp3);
217 if ( sin( dest.x() - start.x() ) < 0 ) {
220 tc1 = SGD_2PI - tmp5;
224 *dist = d * SG_RAD_TO_NM * SG_NM_TO_METER;