diff --git a/Math/Makefile.am b/Math/Makefile.am index ee3eca0d5..a0fc42f09 100644 --- a/Math/Makefile.am +++ b/Math/Makefile.am @@ -10,7 +10,7 @@ libMath_la_SOURCES = \ fg_random.c fg_random.h \ interpolater.cxx interpolater.hxx \ mat3.h mat3defs.h mat3err.h \ - polar3d.c polar3d.h \ - vector.c vector.h + polar3d.cxx polar3d.hxx \ + vector.cxx vector.hxx INCLUDES += -I$(top_builddir) -I$(top_builddir)/Lib -I$(top_builddir)/Simulator diff --git a/Math/fg_geodesy.c b/Math/fg_geodesy.c index e86d8a3ec..c77c3daec 100644 --- a/Math/fg_geodesy.c +++ b/Math/fg_geodesy.c @@ -24,13 +24,13 @@ /* fgGeocToGeod(lat_geoc, radius, *lat_geod, *alt, *sea_level_r) * INPUTS: * lat_geoc Geocentric latitude, radians, + = North - * radius C.G. radius to earth center, ft + * radius C.G. radius to earth center (meters) * * OUTPUTS: * lat_geod Geodetic latitude, radians, + = North - * alt C.G. altitude above mean sea level, ft + * alt C.G. altitude above mean sea level (meters) * sea_level_r radius from earth center to sea level at - * local vertical (surface normal) of C.G. + * local vertical (surface normal) of C.G. (meters) */ void fgGeocToGeod( double lat_geoc, double radius, double @@ -43,12 +43,12 @@ void fgGeocToGeod( double lat_geoc, double radius, double || ( (FG_PI_2 + lat_geoc) < ONE_SECOND ) ) /* near South pole */ { *lat_geod = lat_geoc; - *sea_level_r = EQUATORIAL_RADIUS_KM*E; + *sea_level_r = EQUATORIAL_RADIUS_M*E; *alt = radius - *sea_level_r; } else { t_lat = tan(lat_geoc); - x_alpha = E*EQUATORIAL_RADIUS_KM/sqrt(t_lat*t_lat + E*E); - mu_alpha = atan2(sqrt(RESQ_KM - x_alpha*x_alpha),E*x_alpha); + x_alpha = E*EQUATORIAL_RADIUS_M/sqrt(t_lat*t_lat + E*E); + mu_alpha = atan2(sqrt(RESQ_M - x_alpha*x_alpha),E*x_alpha); if (lat_geoc < 0) mu_alpha = - mu_alpha; sin_mu_a = sin(mu_alpha); delt_lambda = mu_alpha - lat_geoc; @@ -56,14 +56,14 @@ void fgGeocToGeod( double lat_geoc, double radius, double l_point = radius - r_alpha; *alt = l_point*cos(delt_lambda); denom = sqrt(1-EPS*EPS*sin_mu_a*sin_mu_a); - rho_alpha = EQUATORIAL_RADIUS_KM*(1-EPS)/ + rho_alpha = EQUATORIAL_RADIUS_M*(1-EPS)/ (denom*denom*denom); delt_mu = atan2(l_point*sin(delt_lambda),rho_alpha + *alt); *lat_geod = mu_alpha - delt_mu; lambda_sl = atan( E*E * tan(*lat_geod) ); /* SL geoc. latitude */ sin_lambda_sl = sin( lambda_sl ); *sea_level_r = - sqrt(RESQ_KM / (1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl)); + sqrt(RESQ_M / (1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl)); } } @@ -71,11 +71,11 @@ void fgGeocToGeod( double lat_geoc, double radius, double /* fgGeodToGeoc( lat_geod, alt, *sl_radius, *lat_geoc ) * INPUTS: * lat_geod Geodetic latitude, radians, + = North - * alt C.G. altitude above mean sea level, ft + * alt C.G. altitude above mean sea level (meters) * * OUTPUTS: - * sl_radius SEA LEVEL radius to earth center, ft (add Altitude to - * get true distance from earth center. + * sl_radius SEA LEVEL radius to earth center (meters) + * (add Altitude to get true distance from earth center. * lat_geoc Geocentric latitude, radians, + = North * */ @@ -91,7 +91,7 @@ void fgGeodToGeoc( double lat_geod, double alt, double *sl_radius, sin_mu = sin(lat_geod); /* Geodetic (map makers') latitude */ cos_mu = cos(lat_geod); *sl_radius = - sqrt(RESQ_KM / (1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl)); + sqrt(RESQ_M / (1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl)); py = *sl_radius*sin_lambda_sl + alt*sin_mu; px = *sl_radius*cos_lambda_sl + alt*cos_mu; *lat_geoc = atan2( py, px ); @@ -140,6 +140,11 @@ void fgGeodToGeoc( double lat_geod, double alt, double *sl_radius, $Header$ $Log$ +Revision 1.6 1998/07/08 14:40:07 curt +polar3d.[ch] renamed to polar3d.[ch]xx, vector.[ch] renamed to vector.[ch]xx +Updated fg_geodesy comments to reflect that routines expect and produce + meters. + Revision 1.5 1998/04/25 22:06:23 curt Edited cvs log messages in source files ... bad bad bad! @@ -208,9 +213,14 @@ Initial Flight Gear revision. /* $Log$ -/* Revision 1.5 1998/04/25 22:06:23 curt -/* Edited cvs log messages in source files ... bad bad bad! +/* Revision 1.6 1998/07/08 14:40:07 curt +/* polar3d.[ch] renamed to polar3d.[ch]xx, vector.[ch] renamed to vector.[ch]xx +/* Updated fg_geodesy comments to reflect that routines expect and produce +/* meters. /* + * Revision 1.5 1998/04/25 22:06:23 curt + * Edited cvs log messages in source files ... bad bad bad! + * * Revision 1.4 1998/01/27 00:47:59 curt * Incorporated Paul Bleisch's new debug message * system and commandline/config file processing code. diff --git a/Math/fg_geodesy.h b/Math/fg_geodesy.h index a464516da..874acddd8 100644 --- a/Math/fg_geodesy.h +++ b/Math/fg_geodesy.h @@ -23,13 +23,13 @@ extern "C" { /* fgGeocToGeod(lat_geoc, radius, *lat_geod, *alt, *sea_level_r) * INPUTS: * lat_geoc Geocentric latitude, radians, + = North - * radius C.G. radius to earth center, ft + * radius C.G. radius to earth center (meters) * * OUTPUTS: * lat_geod Geodetic latitude, radians, + = North - * alt C.G. altitude above mean sea level, ft + * alt C.G. altitude above mean sea level (meters) * sea_level_r radius from earth center to sea level at - * local vertical (surface normal) of C.G. + * local vertical (surface normal) of C.G. (meters) */ void fgGeocToGeod( double lat_geoc, double radius, double @@ -38,17 +38,17 @@ void fgGeocToGeod( double lat_geoc, double radius, double /* fgGeodToGeoc( lat_geod, alt, *sl_radius, *lat_geoc ) * INPUTS: * lat_geod Geodetic latitude, radians, + = North - * alt C.G. altitude above mean sea level, ft + * alt C.G. altitude above mean sea level (meters) * * OUTPUTS: - * sl_radius SEA LEVEL radius to earth center, ft (add Altitude to - * get true distance from earth center. + * sl_radius SEA LEVEL radius to earth center (meters) + * (add Altitude to get true distance from earth center. * lat_geoc Geocentric latitude, radians, + = North * */ void fgGeodToGeoc( double lat_geod, double alt, double *sl_radius, - double *lat_geoc ); + double *lat_geoc ); @@ -94,6 +94,11 @@ void fgGeodToGeoc( double lat_geod, double alt, double *sl_radius, $Header$ $Log$ +Revision 1.4 1998/07/08 14:40:08 curt +polar3d.[ch] renamed to polar3d.[ch]xx, vector.[ch] renamed to vector.[ch]xx +Updated fg_geodesy comments to reflect that routines expect and produce + meters. + Revision 1.3 1998/04/21 17:03:48 curt Prepairing for C++ integration. @@ -160,9 +165,14 @@ Initial Flight Gear revision. /* $Log$ -/* Revision 1.3 1998/04/21 17:03:48 curt -/* Prepairing for C++ integration. +/* Revision 1.4 1998/07/08 14:40:08 curt +/* polar3d.[ch] renamed to polar3d.[ch]xx, vector.[ch] renamed to vector.[ch]xx +/* Updated fg_geodesy comments to reflect that routines expect and produce +/* meters. /* + * Revision 1.3 1998/04/21 17:03:48 curt + * Prepairing for C++ integration. + * * Revision 1.2 1998/01/22 02:59:38 curt * Changed #ifdef FILE_H to #ifdef _FILE_H * diff --git a/Math/polar3d.c b/Math/polar3d.cxx similarity index 84% rename from Math/polar3d.c rename to Math/polar3d.cxx index 53d8966bd..8052744fb 100644 --- a/Math/polar3d.c +++ b/Math/polar3d.cxx @@ -1,5 +1,5 @@ /************************************************************************** - * polar.c -- routines to deal with polar math and transformations + * polar.cxx -- routines to deal with polar math and transformations * * Written by Curtis Olson, started June 1997. * @@ -29,14 +29,14 @@ #include -#include "polar3d.h" +#include "polar3d.hxx" /* Convert a polar coordinate to a cartesian coordinate. Lon and Lat * must be specified in radians. The FG convention is for distances * to be specified in meters */ -fgCartesianPoint3d fgPolarToCart3d(fgPolarPoint3d p) { - fgCartesianPoint3d pnew; +fgPoint3d fgPolarToCart3d(fgPoint3d p) { + fgPoint3d pnew; pnew.x = cos(p.lon) * cos(p.lat) * p.radius; pnew.y = sin(p.lon) * cos(p.lat) * p.radius; @@ -48,8 +48,8 @@ fgCartesianPoint3d fgPolarToCart3d(fgPolarPoint3d p) { /* Convert a cartesian coordinate to polar coordinates (lon/lat * specified in radians. Distances are specified in meters. */ -fgPolarPoint3d fgCartToPolar3d(fgCartesianPoint3d cp) { - fgPolarPoint3d pp; +fgPoint3d fgCartToPolar3d(fgPoint3d cp) { + fgPoint3d pp; pp.lon = atan2( cp.y, cp.x ); pp.lat = FG_PI_2 - atan2( sqrt(cp.x*cp.x + cp.y*cp.y), cp.z ); @@ -62,9 +62,14 @@ fgPolarPoint3d fgCartToPolar3d(fgCartesianPoint3d cp) { /* $Log$ -/* Revision 1.2 1998/05/03 00:45:49 curt -/* Commented out a debugging printf. +/* Revision 1.1 1998/07/08 14:40:08 curt +/* polar3d.[ch] renamed to polar3d.[ch]xx, vector.[ch] renamed to vector.[ch]xx +/* Updated fg_geodesy comments to reflect that routines expect and produce +/* meters. /* + * Revision 1.2 1998/05/03 00:45:49 curt + * Commented out a debugging printf. + * * Revision 1.1 1998/05/02 01:50:11 curt * polar.[ch] renamed to polar3d.[ch] * diff --git a/Math/polar3d.h b/Math/polar3d.hxx similarity index 79% rename from Math/polar3d.h rename to Math/polar3d.hxx index 806f27e56..a65579051 100644 --- a/Math/polar3d.h +++ b/Math/polar3d.hxx @@ -1,5 +1,5 @@ /************************************************************************** - * polar.h -- routines to deal with polar math and transformations + * polar.hxx -- routines to deal with polar math and transformations * * Written by Curtis Olson, started June 1997. * @@ -24,12 +24,12 @@ **************************************************************************/ -#ifndef _POLAR_H -#define _POLAR_H +#ifndef _POLAR_HXX +#define _POLAR_HXX -#ifdef __cplusplus -extern "C" { +#ifndef __cplusplus +# error This library requires C++ #endif @@ -39,26 +39,26 @@ extern "C" { /* Convert a polar coordinate to a cartesian coordinate. Lon and Lat * must be specified in radians. The FG convention is for distances * to be specified in meters */ -fgCartesianPoint3d fgPolarToCart3d(fgPolarPoint3d p); +fgPoint3d fgPolarToCart3d(fgPoint3d p); /* Convert a cartesian coordinate to polar coordinates (lon/lat * specified in radians. Distances are specified in meters. */ -fgPolarPoint3d fgCartToPolar3d(fgCartesianPoint3d cp); +fgPoint3d fgCartToPolar3d(fgPoint3d cp); -#ifdef __cplusplus -} -#endif - - -#endif /* _POLAR_H */ +#endif /* _POLAR_HXX */ /* $Log$ -/* Revision 1.1 1998/05/02 01:50:11 curt -/* polar.[ch] renamed to polar3d.[ch] +/* Revision 1.1 1998/07/08 14:40:09 curt +/* polar3d.[ch] renamed to polar3d.[ch]xx, vector.[ch] renamed to vector.[ch]xx +/* Updated fg_geodesy comments to reflect that routines expect and produce +/* meters. /* + * Revision 1.1 1998/05/02 01:50:11 curt + * polar.[ch] renamed to polar3d.[ch] + * * Revision 1.9 1998/04/25 22:06:23 curt * Edited cvs log messages in source files ... bad bad bad! * diff --git a/Math/vector.c b/Math/vector.cxx similarity index 72% rename from Math/vector.c rename to Math/vector.cxx index 48fb9cd76..4da0af44f 100644 --- a/Math/vector.c +++ b/Math/vector.cxx @@ -27,9 +27,11 @@ #include #include -#include +#include -#include +#include "vector.hxx" + +#include "mat3.h" /* Map a vector onto the plane specified by normal */ @@ -78,18 +80,49 @@ void map_vec_onto_cur_surface_plane(MAT3vec normal, MAT3vec v0, MAT3vec vec, } +// Given a point p, and a line through p0 with direction vector d, +// find the shortest distance from the point to the line +double fgPointLine(MAT3vec p, MAT3vec p0, MAT3vec d) { + MAT3vec u, u1, v; + double ud, dd, tmp, dist; + + // u = p - p0 + MAT3_SUB_VEC(u, p, p0); + + // calculate the projection, u1, of u along d. + // u1 = ( dot_prod(u, d) / dot_prod(d, d) ) * d; + ud = MAT3_DOT_PRODUCT(u, d); + dd = MAT3_DOT_PRODUCT(d, d); + tmp = ud / dd; + + MAT3_SCALE_VEC(u1, d, tmp);; + + // v = u - u1 = vector from closest point on line, p1, to the + // original point, p. + MAT3_SUB_VEC(v, u, u1); + + dist = sqrt(MAT3_DOT_PRODUCT(v, v)); + + return( dist ); +} + + /* $Log$ -/* Revision 1.3 1998/05/07 23:04:28 curt -/* Added a blank formating line! +/* Revision 1.1 1998/07/08 14:40:10 curt +/* polar3d.[ch] renamed to polar3d.[ch]xx, vector.[ch] renamed to vector.[ch]xx +/* Updated fg_geodesy comments to reflect that routines expect and produce +/* meters. /* + * Revision 1.3 1998/05/07 23:04:28 curt + * Added a blank formating line! + * * Revision 1.2 1998/01/19 19:27:13 curt * Merged in make system changes from Bob Kuehne * This should simplify things tremendously. * * Revision 1.1 1997/12/22 04:13:17 curt * Initial revision. - * - */ + * */ diff --git a/Math/vector.h b/Math/vector.hxx similarity index 69% rename from Math/vector.h rename to Math/vector.hxx index 097a21245..d57a3108a 100644 --- a/Math/vector.h +++ b/Math/vector.hxx @@ -1,5 +1,5 @@ /************************************************************************** - * vector.h -- additional vector routines + * vector.hxx -- additional vector routines * * Written by Curtis Olson, started December 1997. * @@ -24,12 +24,12 @@ **************************************************************************/ -#ifndef _VECTOR_H -#define _VECTOR_H +#ifndef _VECTOR_HXX +#define _VECTOR_HXX -#ifdef __cplusplus -extern "C" { +#ifndef __cplusplus +# error This library requires C++ #endif @@ -40,19 +40,23 @@ extern "C" { void map_vec_onto_cur_surface_plane(MAT3vec normal, MAT3vec v0, MAT3vec vec, MAT3vec result); - -#ifdef __cplusplus -} -#endif +// Given a point p, and a line through p0 with direction vector d, +// find the shortest distance from the point to the line +double fgPointLine(MAT3vec p, MAT3vec p0, MAT3vec d); -#endif /* _VECTOR_H */ +#endif /* _VECTOR_HXX */ /* $Log$ -/* Revision 1.4 1998/04/21 17:03:51 curt -/* Prepairing for C++ integration. +/* Revision 1.1 1998/07/08 14:40:10 curt +/* polar3d.[ch] renamed to polar3d.[ch]xx, vector.[ch] renamed to vector.[ch]xx +/* Updated fg_geodesy comments to reflect that routines expect and produce +/* meters. /* + * Revision 1.4 1998/04/21 17:03:51 curt + * Prepairing for C++ integration. + * * Revision 1.3 1998/01/22 02:59:39 curt * Changed #ifdef FILE_H to #ifdef _FILE_H *