Fix a build order problem.

[simgear.git] / simgear / math / polar3d.hxx

diff --git a/simgear/math/polar3d.hxx b/simgear/math/polar3d.hxx
index ee0dd2c38645d90dda51bcf0d6810c2b6e6960ef..acd24ae7a9d5c23e2411fca511981d922298e1ba 100644 (file)
--- a/simgear/math/polar3d.hxx
+++ b/simgear/math/polar3d.hxx
@@ -4,19 +4,20 @@
 //
 // Copyright (C) 1997  Curtis L. Olson  - curt@infoplane.com
 //
-// This program is free software; you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of the
-// License, or (at your option) any later version.
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Library General Public
+// License as published by the Free Software Foundation; either
+// version 2 of the License, or (at your option) any later version.
 //
-// This program is distributed in the hope that it will be useful, but
-// WITHOUT ANY WARRANTY; without even the implied warranty of
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
-// General Public License for more details.
+// Library General Public License for more details.
 //
-// You should have received a copy of the GNU General Public License
-// along with this program; if not, write to the Free Software
-// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+// You should have received a copy of the GNU Library General Public
+// License along with this library; if not, write to the
+// Free Software Foundation, Inc., 59 Temple Place - Suite 330,
+// Boston, MA  02111-1307, USA.
 //
 // $Id$
 
@@ -30,6 +31,8 @@
 #endif                                   
 
 
+#include <math.h>
+
 #include <simgear/constants.h>
 #include <simgear/math/point3d.hxx>
 
@@ -37,13 +40,13 @@
 // Find the Altitude above the Ellipsoid (WGS84) given the Earth
 // Centered Cartesian coordinate vector Distances are specified in
 // meters.
-double fgGeodAltFromCart(const Point3D& cp);
+double sgGeodAltFromCart(const Point3D& cp);
 
 
 // Convert a polar coordinate to a cartesian coordinate.  Lon and Lat
-// must be specified in radians.  The FG convention is for distances
+// must be specified in radians.  The SG convention is for distances
 // to be specified in meters
-inline Point3D fgPolarToCart3d(const Point3D& p) {
+inline Point3D sgPolarToCart3d(const Point3D& p) {
     double tmp = cos( p.lat() ) * p.radius();
 
     return Point3D( cos( p.lon() ) * tmp,
@@ -54,7 +57,7 @@ inline Point3D fgPolarToCart3d(const Point3D& p) {
 
 // Convert a cartesian coordinate to polar coordinates (lon/lat
 // specified in radians.  Distances are specified in meters.
-inline Point3D fgCartToPolar3d(const Point3D& cp) {
+inline Point3D sgCartToPolar3d(const Point3D& cp) {
     return Point3D( atan2( cp.y(), cp.x() ),
                    FG_PI_2 - 
                    atan2( sqrt(cp.x()*cp.x() + cp.y()*cp.y()), cp.z() ),
@@ -66,7 +69,8 @@ inline Point3D fgCartToPolar3d(const Point3D& cp) {
 // distance.  NOTE: starting point is specifed in radians, distance is
 // specified in meters (and converted internally to radians)
 // ... assumes a spherical world
-inline Point3D calc_lon_lat( const Point3D& orig, double course, double dist ) {
+inline Point3D calc_gc_lon_lat( const Point3D& orig, double course,
+                               double dist ) {
     Point3D result;
 
     // lat=asin(sin(lat1)*cos(d)+cos(lat1)*sin(d)*cos(tc))
@@ -96,6 +100,60 @@ inline Point3D calc_lon_lat( const Point3D& orig, double course, double dist ) {
 }
 
 
-#endif // _POLAR_HXX
+// calc course/dist
+inline void calc_gc_course_dist( const Point3D& start, const Point3D& dest, 
+                                double *course, double *dist ) {
+    // d = 2*asin(sqrt((sin((lat1-lat2)/2))^2 + 
+    //            cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2))
+    double tmp1 = sin( (start.y() - dest.y()) / 2 );
+    double tmp2 = sin( (start.x() - dest.x()) / 2 );
+    double d = 2.0 * asin( sqrt( tmp1 * tmp1 + 
+                                cos(start.y()) * cos(dest.y()) * tmp2 * tmp2));
+
+    // We obtain the initial course, tc1, (at point 1) from point 1 to
+    // point 2 by the following. The formula fails if the initial
+    // point is a pole. We can special case this with:
+    //
+    // IF (cos(lat1) < EPS)   // EPS a small number ~ machine precision
+    //   IF (lat1 > 0)
+    //     tc1= pi        //  starting from N pole
+    //   ELSE
+    //     tc1= 0         //  starting from S pole
+    //   ENDIF
+    // ENDIF
+    //
+    // For starting points other than the poles: 
+    // 
+    // IF sin(lon2-lon1)<0       
+    //   tc1=acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)))    
+    // ELSE       
+    //   tc1=2*pi-acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1)))    
+    // ENDIF 
+
+    double tc1;
+
+    if ( cos(start.y()) < FG_EPSILON ) {
+       // EPS a small number ~ machine precision
+       if ( start.y() > 0 ) {
+           tc1 = FG_PI;        // starting from N pole
+       } else {
+           tc1 = 0;            // starting from S pole
+       }
+    }
+
+    // For starting points other than the poles: 
 
+    double tmp3 = sin(d)*cos(start.y());
+    double tmp4 = sin(dest.y())-sin(start.y())*cos(d);
+    double tmp5 = acos(tmp4/tmp3);
+    if ( sin( dest.x() - start.x() ) < 0 ) {
+        tc1 = tmp5;
+    } else {
+        tc1 = 2 * FG_PI - tmp5;
+    }
 
+    *course = tc1;
+    *dist = d * RAD_TO_NM * NM_TO_METER;
+}
+
+#endif // _POLAR_HXX