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 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.
30 # error This library requires C++
36 #include <simgear/constants.h>
37 #include <simgear/math/point3d.hxx>
40 // Find the Altitude above the Ellipsoid (WGS84) given the Earth
41 // Centered Cartesian coordinate vector Distances are specified in
43 double sgGeodAltFromCart(const Point3D& cp);
46 // Convert a polar coordinate to a cartesian coordinate. Lon and Lat
47 // must be specified in radians. The SG convention is for distances
48 // to be specified in meters
49 inline Point3D sgPolarToCart3d(const Point3D& p) {
50 double tmp = cos( p.lat() ) * p.radius();
52 return Point3D( cos( p.lon() ) * tmp,
54 sin( p.lat() ) * p.radius() );
58 // Convert a cartesian coordinate to polar coordinates (lon/lat
59 // specified in radians. Distances are specified in meters.
60 inline Point3D sgCartToPolar3d(const Point3D& cp) {
61 return Point3D( atan2( cp.y(), cp.x() ),
63 atan2( sqrt(cp.x()*cp.x() + cp.y()*cp.y()), cp.z() ),
64 sqrt(cp.x()*cp.x() + cp.y()*cp.y() + cp.z()*cp.z()) );
68 // calc new lon/lat given starting lon/lat, and offset radial, and
69 // distance. NOTE: starting point is specifed in radians, distance is
70 // specified in meters (and converted internally to radians)
71 // ... assumes a spherical world
72 inline Point3D calc_gc_lon_lat( const Point3D& orig, double course,
76 // lat=asin(sin(lat1)*cos(d)+cos(lat1)*sin(d)*cos(tc))
78 // lon=lon1 // endpoint a pole
80 // lon=mod(lon1-asin(sin(tc)*sin(d)/cos(lat))+pi,2*pi)-pi
83 // printf("calc_lon_lat() offset.theta = %.2f offset.dist = %.2f\n",
84 // offset.theta, offset.dist);
86 dist *= METER_TO_NM * NM_TO_RAD;
88 result.sety( asin( sin(orig.y()) * cos(dist) +
89 cos(orig.y()) * sin(dist) * cos(course) ) );
91 if ( cos(result.y()) < FG_EPSILON ) {
92 result.setx( orig.x() ); // endpoint a pole
95 fmod(orig.x() - asin( sin(course) * sin(dist) /
96 cos(result.y()) ) + FG_PI, FG_2PI) - FG_PI );
104 inline void calc_gc_course_dist( const Point3D& start, const Point3D& dest,
105 double *course, double *dist ) {
106 // d = 2*asin(sqrt((sin((lat1-lat2)/2))^2 +
107 // cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
108 double tmp1 = sin( (start.y() - dest.y()) / 2 );
109 double tmp2 = sin( (start.x() - dest.x()) / 2 );
110 double d = 2.0 * asin( sqrt( tmp1 * tmp1 +
111 cos(start.y()) * cos(dest.y()) * tmp2 * tmp2));
113 // We obtain the initial course, tc1, (at point 1) from point 1 to
114 // point 2 by the following. The formula fails if the initial
115 // point is a pole. We can special case this with:
117 // IF (cos(lat1) < EPS) // EPS a small number ~ machine precision
119 // tc1= pi // starting from N pole
121 // tc1= 0 // starting from S pole
125 // For starting points other than the poles:
127 // IF sin(lon2-lon1)<0
128 // tc1=acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)))
130 // tc1=2*pi-acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)))
135 if ( cos(start.y()) < FG_EPSILON ) {
136 // EPS a small number ~ machine precision
137 if ( start.y() > 0 ) {
138 tc1 = FG_PI; // starting from N pole
140 tc1 = 0; // starting from S pole
144 // For starting points other than the poles:
146 double tmp3 = sin(d)*cos(start.y());
147 double tmp4 = sin(dest.y())-sin(start.y())*cos(d);
148 double tmp5 = acos(tmp4/tmp3);
149 if ( sin( dest.x() - start.x() ) < 0 ) {
152 tc1 = 2 * FG_PI - tmp5;
156 *dist = d * RAD_TO_NM * NM_TO_METER;