1 // polar.hxx -- routines to deal with polar math and transformations
3 // Written by Curtis Olson, started June 1997.
5 // Copyright (C) 1997 Curtis L. Olson - curt@infoplane.com
7 // This program is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU General Public License as
9 // published by the Free Software Foundation; either version 2 of the
10 // License, or (at your option) any later version.
12 // This program is distributed in the hope that it will be useful, but
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 // General Public License for more details.
17 // You should have received a copy of the GNU General Public License
18 // along with this program; if not, write to the Free Software
19 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
29 # error This library requires C++
35 #include <simgear/constants.h>
36 #include <simgear/math/point3d.hxx>
39 // Find the Altitude above the Ellipsoid (WGS84) given the Earth
40 // Centered Cartesian coordinate vector Distances are specified in
42 double fgGeodAltFromCart(const Point3D& cp);
45 // Convert a polar coordinate to a cartesian coordinate. Lon and Lat
46 // must be specified in radians. The FG convention is for distances
47 // to be specified in meters
48 inline Point3D fgPolarToCart3d(const Point3D& p) {
49 double tmp = cos( p.lat() ) * p.radius();
51 return Point3D( cos( p.lon() ) * tmp,
53 sin( p.lat() ) * p.radius() );
57 // Convert a cartesian coordinate to polar coordinates (lon/lat
58 // specified in radians. Distances are specified in meters.
59 inline Point3D fgCartToPolar3d(const Point3D& cp) {
60 return Point3D( atan2( cp.y(), cp.x() ),
62 atan2( sqrt(cp.x()*cp.x() + cp.y()*cp.y()), cp.z() ),
63 sqrt(cp.x()*cp.x() + cp.y()*cp.y() + cp.z()*cp.z()) );
67 // calc new lon/lat given starting lon/lat, and offset radial, and
68 // distance. NOTE: starting point is specifed in radians, distance is
69 // specified in meters (and converted internally to radians)
70 // ... assumes a spherical world
71 inline Point3D calc_gc_lon_lat( const Point3D& orig, double course,
75 // lat=asin(sin(lat1)*cos(d)+cos(lat1)*sin(d)*cos(tc))
77 // lon=lon1 // endpoint a pole
79 // lon=mod(lon1-asin(sin(tc)*sin(d)/cos(lat))+pi,2*pi)-pi
82 // printf("calc_lon_lat() offset.theta = %.2f offset.dist = %.2f\n",
83 // offset.theta, offset.dist);
85 dist *= METER_TO_NM * NM_TO_RAD;
87 result.sety( asin( sin(orig.y()) * cos(dist) +
88 cos(orig.y()) * sin(dist) * cos(course) ) );
90 if ( cos(result.y()) < FG_EPSILON ) {
91 result.setx( orig.x() ); // endpoint a pole
94 fmod(orig.x() - asin( sin(course) * sin(dist) /
95 cos(result.y()) ) + FG_PI, FG_2PI) - FG_PI );
103 inline void calc_gc_course_dist( const Point3D& start, const Point3D& dest,
104 double *course, double *dist ) {
105 // d = 2*asin(sqrt((sin((lat1-lat2)/2))^2 +
106 // cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
107 double tmp1 = sin( (start.y() - dest.y()) / 2 );
108 double tmp2 = sin( (start.x() - dest.x()) / 2 );
109 double d = 2.0 * asin( sqrt( tmp1 * tmp1 +
110 cos(start.y()) * cos(dest.y()) * tmp2 * tmp2));
112 // We obtain the initial course, tc1, (at point 1) from point 1 to
113 // point 2 by the following. The formula fails if the initial
114 // point is a pole. We can special case this with:
116 // IF (cos(lat1) < EPS) // EPS a small number ~ machine precision
118 // tc1= pi // starting from N pole
120 // tc1= 0 // starting from S pole
124 // For starting points other than the poles:
126 // IF sin(lon2-lon1)<0
127 // tc1=acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)))
129 // tc1=2*pi-acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)))
134 if ( cos(start.y()) < FG_EPSILON ) {
135 // EPS a small number ~ machine precision
136 if ( start.y() > 0 ) {
137 tc1 = FG_PI; // starting from N pole
139 tc1 = 0; // starting from S pole
143 // For starting points other than the poles:
145 double tmp3 = sin(d)*cos(start.y());
146 double tmp4 = sin(dest.y())-sin(start.y())*cos(d);
147 double tmp5 = acos(tmp4/tmp3);
148 if ( sin( dest.x() - start.x() ) < 0 ) {
151 tc1 = 2 * FG_PI - tmp5;
155 *dist = d * RAD_TO_NM * NM_TO_METER;