// 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$
#define _POLAR3D_HXX
-#ifndef __cplusplus
+#ifndef __cplusplus
# error This library requires C++
-#endif
+#endif
-#include <math.h>
-
#include <simgear/constants.h>
#include <simgear/math/point3d.hxx>
+#include "SGMath.hxx"
/**
* Find the Altitude above the Ellipsoid (WGS84) given the Earth
* @param cp point specified in cartesian coordinates
* @return altitude above the (wgs84) earth in meters
*/
-double sgGeodAltFromCart(const Point3D& cp);
+inline double sgGeodAltFromCart(const Point3D& p)
+{
+ SGGeod geod;
+ SGGeodesy::SGCartToGeod(SGVec3<double>(p.x(), p.y(), p.z()), geod);
+ return geod.getElevationM();
+}
/**
* @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() );
+inline Point3D sgPolarToCart3d(const Point3D& p)
+{
+ SGVec3<double> cart;
+ SGGeodesy::SGGeocToCart(SGGeoc::fromRadM(p.lon(), p.lat(), p.radius()), cart);
+ return Point3D::fromSGVec3(cart);
}
* @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()) );
+inline Point3D sgCartToPolar3d(const Point3D& p)
+{
+ SGGeoc geoc;
+ SGGeodesy::SGCartToGeoc(SGVec3<double>(p.x(), p.y(), p.z()), geoc);
+ return Point3D::fromSGGeoc(geoc);
}
* @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;
-}
+Point3D calc_gc_lon_lat( const Point3D& orig, double course, double dist );
/**
* @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 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;
- }
- }
-}
+void calc_gc_course_dist( const Point3D& start, const Point3D& dest,
+ double *course, double *dist );
#if 0
/**
* @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;
-}
+void calc_gc_course_dist( const Point3D& start, const Point3D& dest,
+ double *course, double *dist );
#endif // 0
#endif // _POLAR3D_HXX
+
projects / simgear.git / blobdiff