]> git.mxchange.org Git - simgear.git/blob - simgear/math/polar3d.hxx
SG-ified logstream.
[simgear.git] / simgear / math / polar3d.hxx
1 // polar.hxx -- routines to deal with polar math and transformations
2 //
3 // Written by Curtis Olson, started June 1997.
4 //
5 // Copyright (C) 1997  Curtis L. Olson  - curt@infoplane.com
6 //
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.
11 //
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.
16 //
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.
21 //
22 // $Id$
23
24
25 #ifndef _POLAR_HXX
26 #define _POLAR_HXX
27
28
29 #ifndef __cplusplus                                                          
30 # error This library requires C++
31 #endif                                   
32
33
34 #include <math.h>
35
36 #include <simgear/constants.h>
37 #include <simgear/math/point3d.hxx>
38
39
40 // Find the Altitude above the Ellipsoid (WGS84) given the Earth
41 // Centered Cartesian coordinate vector Distances are specified in
42 // meters.
43 double sgGeodAltFromCart(const Point3D& cp);
44
45
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();
51
52     return Point3D( cos( p.lon() ) * tmp,
53                     sin( p.lon() ) * tmp,
54                     sin( p.lat() ) * p.radius() );
55 }
56
57
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() ),
62                     SGD_PI_2 - 
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()) );
65 }
66
67
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,
73                                 double dist ) {
74     Point3D result;
75
76     // lat=asin(sin(lat1)*cos(d)+cos(lat1)*sin(d)*cos(tc))
77     // IF (cos(lat)=0)
78     //   lon=lon1      // endpoint a pole
79     // ELSE
80     //   lon=mod(lon1-asin(sin(tc)*sin(d)/cos(lat))+pi,2*pi)-pi
81     // ENDIF
82
83     // printf("calc_lon_lat()  offset.theta = %.2f offset.dist = %.2f\n",
84     //        offset.theta, offset.dist);
85
86     dist *= SG_METER_TO_NM * SG_NM_TO_RAD;
87     
88     result.sety( asin( sin(orig.y()) * cos(dist) + 
89                        cos(orig.y()) * sin(dist) * cos(course) ) );
90
91     if ( cos(result.y()) < SG_EPSILON ) {
92         result.setx( orig.x() );      // endpoint a pole
93     } else {
94         result.setx( 
95             fmod(orig.x() - asin( sin(course) * sin(dist) / 
96                                   cos(result.y()) )
97                  + SGD_PI, SGD_2PI) - SGD_PI );
98     }
99
100     return result;
101 }
102
103
104 // calc course/dist
105 inline void calc_gc_course_dist( const Point3D& start, const Point3D& dest, 
106                                  double *course, double *dist ) {
107     // d = 2*asin(sqrt((sin((lat1-lat2)/2))^2 + 
108     //            cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
109     double tmp1 = sin( (start.y() - dest.y()) / 2 );
110     double tmp2 = sin( (start.x() - dest.x()) / 2 );
111     double d = 2.0 * asin( sqrt( tmp1 * tmp1 + 
112                                  cos(start.y()) * cos(dest.y()) * tmp2 * tmp2));
113
114     // We obtain the initial course, tc1, (at point 1) from point 1 to
115     // point 2 by the following. The formula fails if the initial
116     // point is a pole. We can special case this with:
117     //
118     // IF (cos(lat1) < EPS)   // EPS a small number ~ machine precision
119     //   IF (lat1 > 0)
120     //     tc1= pi        //  starting from N pole
121     //   ELSE
122     //     tc1= 0         //  starting from S pole
123     //   ENDIF
124     // ENDIF
125     //
126     // For starting points other than the poles: 
127     // 
128     // IF sin(lon2-lon1)<0       
129     //   tc1=acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)))    
130     // ELSE       
131     //   tc1=2*pi-acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)))    
132     // ENDIF 
133
134     double tc1;
135
136     if ( cos(start.y()) < SG_EPSILON ) {
137         // EPS a small number ~ machine precision
138         if ( start.y() > 0 ) {
139             tc1 = SGD_PI;        // starting from N pole
140         } else {
141             tc1 = 0;            // starting from S pole
142         }
143     }
144
145     // For starting points other than the poles: 
146
147     double tmp3 = sin(d)*cos(start.y());
148     double tmp4 = sin(dest.y())-sin(start.y())*cos(d);
149     double tmp5 = acos(tmp4/tmp3);
150     if ( sin( dest.x() - start.x() ) < 0 ) {
151          tc1 = tmp5;
152     } else {
153          tc1 = 2 * SGD_PI - tmp5;
154     }
155
156     *course = tc1;
157     *dist = d * SG_RAD_TO_NM * SG_NM_TO_METER;
158 }
159
160 #endif // _POLAR_HXX