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++
33 #include <simgear/constants.h>
34 #include <simgear/math/point3d.hxx>
37 // Find the Altitude above the Ellipsoid (WGS84) given the Earth
38 // Centered Cartesian coordinate vector Distances are specified in
40 double fgGeodAltFromCart(const Point3D& cp);
43 // Convert a polar coordinate to a cartesian coordinate. Lon and Lat
44 // must be specified in radians. The FG convention is for distances
45 // to be specified in meters
46 inline Point3D fgPolarToCart3d(const Point3D& p) {
47 double tmp = cos( p.lat() ) * p.radius();
49 return Point3D( cos( p.lon() ) * tmp,
51 sin( p.lat() ) * p.radius() );
55 // Convert a cartesian coordinate to polar coordinates (lon/lat
56 // specified in radians. Distances are specified in meters.
57 inline Point3D fgCartToPolar3d(const Point3D& cp) {
58 return Point3D( atan2( cp.y(), cp.x() ),
60 atan2( sqrt(cp.x()*cp.x() + cp.y()*cp.y()), cp.z() ),
61 sqrt(cp.x()*cp.x() + cp.y()*cp.y() + cp.z()*cp.z()) );
65 // calc new lon/lat given starting lon/lat, and offset radial, and
66 // distance. NOTE: starting point is specifed in radians, distance is
67 // specified in meters (and converted internally to radians)
68 // ... assumes a spherical world
69 inline Point3D calc_lon_lat( const Point3D& orig, double course, double dist ) {
72 // lat=asin(sin(lat1)*cos(d)+cos(lat1)*sin(d)*cos(tc))
74 // lon=lon1 // endpoint a pole
76 // lon=mod(lon1-asin(sin(tc)*sin(d)/cos(lat))+pi,2*pi)-pi
79 // printf("calc_lon_lat() offset.theta = %.2f offset.dist = %.2f\n",
80 // offset.theta, offset.dist);
82 dist *= METER_TO_NM * NM_TO_RAD;
84 result.sety( asin( sin(orig.y()) * cos(dist) +
85 cos(orig.y()) * sin(dist) * cos(course) ) );
87 if ( cos(result.y()) < FG_EPSILON ) {
88 result.setx( orig.x() ); // endpoint a pole
91 fmod(orig.x() - asin( sin(course) * sin(dist) /
92 cos(result.y()) ) + FG_PI, FG_2PI) - FG_PI );