X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=simgear%2Fmath%2Fpolar3d.hxx;h=9721dc728b0bf51281727f03bd813487c764573c;hb=6a7c2000027cd22eea603e936ddbad1a5bfc8b04;hp=a2c863f27a26581b2f47acf07c3546cff08525a6;hpb=48a219473e4e453b3543a9620c1a00a08e5a31e6;p=simgear.git diff --git a/simgear/math/polar3d.hxx b/simgear/math/polar3d.hxx index a2c863f2..9721dc72 100644 --- a/simgear/math/polar3d.hxx +++ b/simgear/math/polar3d.hxx @@ -5,7 +5,7 @@ // Written by Curtis Olson, started June 1997. // -// Copyright (C) 1997 Curtis L. Olson - curt@infoplane.com +// Copyright (C) 1997 Curtis L. Olson - http://www.flightgear.org/~curt // // This library is free software; you can redistribute it and/or // modify it under the terms of the GNU Library General Public @@ -17,10 +17,9 @@ // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // Library General Public License for more details. // -// 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. +// 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. // // $Id$ @@ -29,56 +28,12 @@ #define _POLAR3D_HXX -#ifndef __cplusplus +#ifndef __cplusplus # error This library requires C++ -#endif +#endif - -#include - -#include #include - - -/** - * Find the Altitude above the Ellipsoid (WGS84) given the Earth - * Centered Cartesian coordinate vector Distances are specified in - * meters. - * @param cp point specified in cartesian coordinates - * @return altitude above the (wgs84) earth in meters - */ -double sgGeodAltFromCart(const Point3D& cp); - - -/** - * Convert a polar coordinate to a cartesian coordinate. Lon and Lat - * must be specified in radians. The SG convention is for distances - * to be specified in meters - * @param p point specified in polar coordinates - * @return the same point in cartesian coordinates - */ -inline Point3D sgPolarToCart3d(const Point3D& p) { - double tmp = cos( p.lat() ) * p.radius(); - - return Point3D( cos( p.lon() ) * tmp, - sin( p.lon() ) * tmp, - sin( p.lat() ) * p.radius() ); -} - - -/** - * Convert a cartesian coordinate to polar coordinates (lon/lat - * specified in radians. Distances are specified in meters. - * @param cp point specified in cartesian coordinates - * @return the same point in polar coordinates - */ -inline Point3D sgCartToPolar3d(const Point3D& cp) { - return Point3D( atan2( cp.y(), cp.x() ), - SGD_PI_2 - - atan2( sqrt(cp.x()*cp.x() + cp.y()*cp.y()), cp.z() ), - sqrt(cp.x()*cp.x() + cp.y()*cp.y() + cp.z()*cp.z()) ); -} - +#include "SGMath.hxx" /** * Calculate new lon/lat given starting lon/lat, and offset radial, and @@ -90,36 +45,8 @@ inline Point3D sgCartToPolar3d(const Point3D& cp) { * @param dist offset distance * @return destination point in polar coordinates */ -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)) - // IF (cos(lat)=0) - // lon=lon1 // endpoint a pole - // ELSE - // lon=mod(lon1-asin(sin(tc)*sin(d)/cos(lat))+pi,2*pi)-pi - // ENDIF - - // printf("calc_lon_lat() offset.theta = %.2f offset.dist = %.2f\n", - // offset.theta, offset.dist); - - dist *= SG_METER_TO_NM * SG_NM_TO_RAD; - - result.sety( asin( sin(orig.y()) * cos(dist) + - cos(orig.y()) * sin(dist) * cos(course) ) ); - - if ( cos(result.y()) < SG_EPSILON ) { - result.setx( orig.x() ); // endpoint a pole - } else { - result.setx( - fmod(orig.x() - asin( sin(course) * sin(dist) / - cos(result.y()) ) - + SGD_PI, SGD_2PI) - SGD_PI ); - } - - return result; -} +inline Point3D calc_gc_lon_lat(const Point3D& orig, double course, double dist) +{ return Point3D::fromSGGeoc(orig.toSGGeoc().advanceRadM(course, dist)); } /** @@ -132,131 +59,11 @@ inline Point3D calc_gc_lon_lat( const Point3D& orig, double course, 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 cos_start_y = cos( start.y() ); - volatile double tmp1 = sin( (start.y() - dest.y()) * 0.5 ); - volatile double tmp2 = sin( (start.x() - dest.x()) * 0.5 ); - double d = 2.0 * asin( sqrt( tmp1 * tmp1 + - cos_start_y * cos(dest.y()) * tmp2 * tmp2)); - - *dist = d * SG_RAD_TO_NM * SG_NM_TO_METER; - - // 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 - - // if ( cos(start.y()) < SG_EPSILON ) { - // doing it this way saves a transcendental call - double sin_start_y = sin( start.y() ); - if ( fabs(1.0-sin_start_y) < SG_EPSILON ) { - // EPS a small number ~ machine precision - if ( start.y() > 0 ) { - *course = SGD_PI; // starting from N pole - } else { - *course = 0; // starting from S pole - } - } else { - // 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); - - // Doing this way gaurentees that the temps are - // not stored into memory - double tmp5 = acos( (sin(dest.y()) - sin_start_y * cos(d)) / - (sin(d) * cos_start_y) ); - - // if ( sin( dest.x() - start.x() ) < 0 ) { - // the sin of the negative angle is just the opposite sign - // of the sin of the angle so tmp2 will have the opposite - // sign of sin( dest.x() - start.x() ) - if ( tmp2 >= 0 ) { - *course = tmp5; - } else { - *course = 2 * SGD_PI - tmp5; - } - } -} - -#if 0 -/** - * Calculate course/dist given two spherical points. - * @param start starting point - * @param dest ending point - * @param course resulting course - * @param dist resulting distance - */ -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()) < SG_EPSILON ) { - // EPS a small number ~ machine precision - if ( start.y() > 0 ) { - tc1 = SGD_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 * SGD_PI - tmp5; - } - - *course = tc1; - *dist = d * SG_RAD_TO_NM * SG_NM_TO_METER; + SGGeoc gs = start.toSGGeoc(); + SGGeoc gd = dest.toSGGeoc(); + *course = SGGeoc::courseRad(gs, gd); + *dist = SGGeoc::distanceM(gs, gd); } -#endif // 0 #endif // _POLAR3D_HXX +