]> git.mxchange.org Git - simgear.git/blob - simgear/math/polar3d.hxx
Display a warning message for Irix users.
[simgear.git] / simgear / math / polar3d.hxx
1 /**
2  * \file polar3d.hxx
3  * Routines to deal with polar math and transformations.
4  */
5
6 // Written by Curtis Olson, started June 1997.
7 //
8 // Copyright (C) 1997  Curtis L. Olson  - curt@infoplane.com
9 //
10 // This library is free software; you can redistribute it and/or
11 // modify it under the terms of the GNU Library General Public
12 // License as published by the Free Software Foundation; either
13 // version 2 of the License, or (at your option) any later version.
14 //
15 // This library is distributed in the hope that it will be useful,
16 // but WITHOUT ANY WARRANTY; without even the implied warranty of
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
18 // Library General Public License for more details.
19 //
20 // You should have received a copy of the GNU Library General Public
21 // License along with this library; if not, write to the
22 // Free Software Foundation, Inc., 59 Temple Place - Suite 330,
23 // Boston, MA  02111-1307, USA.
24 //
25 // $Id$
26
27
28 #ifndef _POLAR3D_HXX
29 #define _POLAR3D_HXX
30
31
32 #ifndef __cplusplus                                                          
33 # error This library requires C++
34 #endif                                   
35
36
37 #include <math.h>
38
39 #include <simgear/constants.h>
40 #include <simgear/math/point3d.hxx>
41
42
43 /** 
44  * Find the Altitude above the Ellipsoid (WGS84) given the Earth
45  * Centered Cartesian coordinate vector Distances are specified in
46  * meters.
47  * @param cp point specified in cartesian coordinates
48  * @return altitude above the (wgs84) earth in meters
49  */
50 double sgGeodAltFromCart(const Point3D& cp);
51
52
53 /**
54  * Convert a polar coordinate to a cartesian coordinate.  Lon and Lat
55  * must be specified in radians.  The SG convention is for distances
56  * to be specified in meters
57  * @param p point specified in polar coordinates
58  * @return the same point in cartesian coordinates
59  */
60 inline Point3D sgPolarToCart3d(const Point3D& p) {
61     double tmp = cos( p.lat() ) * p.radius();
62
63     return Point3D( cos( p.lon() ) * tmp,
64                     sin( p.lon() ) * tmp,
65                     sin( p.lat() ) * p.radius() );
66 }
67
68
69 /**
70  * Convert a cartesian coordinate to polar coordinates (lon/lat
71  * specified in radians.  Distances are specified in meters.
72  * @param cp point specified in cartesian coordinates
73  * @return the same point in polar coordinates
74  */
75 inline Point3D sgCartToPolar3d(const Point3D& cp) {
76     return Point3D( atan2( cp.y(), cp.x() ),
77                     SGD_PI_2 - 
78                     atan2( sqrt(cp.x()*cp.x() + cp.y()*cp.y()), cp.z() ),
79                     sqrt(cp.x()*cp.x() + cp.y()*cp.y() + cp.z()*cp.z()) );
80 }
81
82
83 /**
84  * Calculate new lon/lat given starting lon/lat, and offset radial, and
85  * distance.  NOTE: starting point is specifed in radians, distance is
86  * specified in meters (and converted internally to radians)
87  * ... assumes a spherical world.
88  * @param orig specified in polar coordinates
89  * @param course offset radial
90  * @param dist offset distance
91  * @return destination point in polar coordinates
92  */
93 inline Point3D calc_gc_lon_lat( const Point3D& orig, double course,
94                                 double dist ) {
95     Point3D result;
96
97     // lat=asin(sin(lat1)*cos(d)+cos(lat1)*sin(d)*cos(tc))
98     // IF (cos(lat)=0)
99     //   lon=lon1      // endpoint a pole
100     // ELSE
101     //   lon=mod(lon1-asin(sin(tc)*sin(d)/cos(lat))+pi,2*pi)-pi
102     // ENDIF
103
104     // printf("calc_lon_lat()  offset.theta = %.2f offset.dist = %.2f\n",
105     //        offset.theta, offset.dist);
106
107     dist *= SG_METER_TO_NM * SG_NM_TO_RAD;
108     
109     result.sety( asin( sin(orig.y()) * cos(dist) + 
110                        cos(orig.y()) * sin(dist) * cos(course) ) );
111
112     if ( cos(result.y()) < SG_EPSILON ) {
113         result.setx( orig.x() );      // endpoint a pole
114     } else {
115         result.setx( 
116             fmod(orig.x() - asin( sin(course) * sin(dist) / 
117                                   cos(result.y()) )
118                  + SGD_PI, SGD_2PI) - SGD_PI );
119     }
120
121     return result;
122 }
123
124
125 /**
126  * Calculate course/dist given two spherical points.
127  * @param start starting point
128  * @param dest ending point
129  * @param course resulting course
130  * @param dist resulting distance
131  */
132 inline void calc_gc_course_dist( const Point3D& start, const Point3D& dest, 
133                                  double *course, double *dist ) {
134     // d = 2*asin(sqrt((sin((lat1-lat2)/2))^2 + 
135     //            cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
136     double tmp1 = sin( (start.y() - dest.y()) / 2 );
137     double tmp2 = sin( (start.x() - dest.x()) / 2 );
138     double d = 2.0 * asin( sqrt( tmp1 * tmp1 + 
139                                  cos(start.y()) * cos(dest.y()) * tmp2 * tmp2));
140
141     // We obtain the initial course, tc1, (at point 1) from point 1 to
142     // point 2 by the following. The formula fails if the initial
143     // point is a pole. We can special case this with:
144     //
145     // IF (cos(lat1) < EPS)   // EPS a small number ~ machine precision
146     //   IF (lat1 > 0)
147     //     tc1= pi        //  starting from N pole
148     //   ELSE
149     //     tc1= 0         //  starting from S pole
150     //   ENDIF
151     // ENDIF
152     //
153     // For starting points other than the poles: 
154     // 
155     // IF sin(lon2-lon1)<0       
156     //   tc1=acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)))    
157     // ELSE       
158     //   tc1=2*pi-acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)))    
159     // ENDIF 
160
161     double tc1;
162
163     if ( cos(start.y()) < SG_EPSILON ) {
164         // EPS a small number ~ machine precision
165         if ( start.y() > 0 ) {
166             tc1 = SGD_PI;        // starting from N pole
167         } else {
168             tc1 = 0;            // starting from S pole
169         }
170     }
171
172     // For starting points other than the poles: 
173
174     double tmp3 = sin(d)*cos(start.y());
175     double tmp4 = sin(dest.y())-sin(start.y())*cos(d);
176     double tmp5 = acos(tmp4/tmp3);
177     if ( sin( dest.x() - start.x() ) < 0 ) {
178          tc1 = tmp5;
179     } else {
180          tc1 = 2 * SGD_PI - tmp5;
181     }
182
183     *course = tc1;
184     *dist = d * SG_RAD_TO_NM * SG_NM_TO_METER;
185 }
186
187 #endif // _POLAR3D_HXX