From aef484d5fc720febfad18c6966dee88dadea3d48 Mon Sep 17 00:00:00 2001 From: curt Date: Mon, 24 Aug 1998 20:04:08 +0000 Subject: [PATCH] Various "inline" code optimizations contributed by Norman Vine. --- Math/MAT3mat.c | 3 ++ Math/MAT3vec.c | 5 ++ Math/mat3.h | 127 +++++++++++++++++++++++++++++++++++++---------- Math/polar3d.cxx | 48 +++++++++++++++--- Math/polar3d.hxx | 18 +++++-- Math/vector.cxx | 38 ++++++++++++-- Math/vector.hxx | 29 +++++++++-- 7 files changed, 225 insertions(+), 43 deletions(-) diff --git a/Math/MAT3mat.c b/Math/MAT3mat.c index 7b1a24c2a..6eb74cb1c 100644 --- a/Math/MAT3mat.c +++ b/Math/MAT3mat.c @@ -28,6 +28,8 @@ /* -------------------------- Public Routines ---------------------------- */ +#if !defined( USE_XTRA_MAT3_INLINES ) + /* * Sets a matrix to identity. */ @@ -95,6 +97,7 @@ MAT3mult (double (*result_mat)[4], register double (*mat1)[4], register double ( mat1[i][3] * mat2[3][j]); MAT3copy (result_mat, tmp_mat); } +#endif // !defined( USE_XTRA_MAT3_INLINES ) /* * This returns the transpose of a matrix. The result matrix may be diff --git a/Math/MAT3vec.c b/Math/MAT3vec.c index 24f2939af..d760add5f 100644 --- a/Math/MAT3vec.c +++ b/Math/MAT3vec.c @@ -29,6 +29,7 @@ # define FALSE 0 #endif +#if !defined( USE_XTRA_MAT3_INLINES ) void MAT3mult_vec(double *result_vec, register double *vec, register double (*mat)[4]) @@ -45,6 +46,7 @@ MAT3mult_vec(double *result_vec, register double *vec, register double (*mat)[4] MAT3_COPY_VEC(result_vec, temp); } +#endif // !defined( USE_XTRA_MAT3_INLINES ) /* * Multiplies a vector of size 4 by a matrix, setting the result vector. @@ -92,6 +94,8 @@ MAT3mult_hvec(double *result_vec, register double *vec, register double (*mat)[4 return(ret); } +#if !defined( USE_XTRA_MAT3_INLINES ) + /* * Sets the first vector to be the cross-product of the last two vectors. */ @@ -108,6 +112,7 @@ MAT3cross_product(double *result_vec, register double *vec1, register double *ve MAT3_COPY_VEC(result_vec, temp); } +#endif // !defined( USE_XTRA_MAT3_INLINES ) /* * Finds a vector perpendicular to vec and stores it in result_vec. diff --git a/Math/mat3.h b/Math/mat3.h index 89148b1c5..781af311a 100644 --- a/Math/mat3.h +++ b/Math/mat3.h @@ -18,6 +18,7 @@ #endif #include +#include #ifdef __cplusplus extern "C" { @@ -26,7 +27,15 @@ extern "C" { #define MAT3_DET0 -1 /* Indicates singular mat */ #define MAT3_EPSILON 1e-12 /* Close enough to zero */ -#define MAT3_PI 3.141592653589793 /* Pi */ + +#ifdef M_PI +# define MAT3_PI M_PI +#else +# define MAT3_PI 3.14159265358979323846 +#endif + + +#define USE_XTRA_MAT3_INLINES /* ------------------------------ Types --------------------------------- */ @@ -119,34 +128,102 @@ typedef double MAT3hvec[4]; /* Vector with homogeneous coord */ /* In MAT3geom.c */ -void MAT3direction_matrix (MAT3mat result_mat, MAT3mat mat); -int MAT3normal_matrix (MAT3mat result_mat, MAT3mat mat); -void MAT3rotate (MAT3mat result_mat, MAT3vec axis, double angle_in_radians); -void MAT3translate (MAT3mat result_mat, MAT3vec trans); -void MAT3scale (MAT3mat result_mat, MAT3vec scale); -void MAT3shear(MAT3mat result_mat, double xshear, double yshear); +void MAT3direction_matrix (MAT3mat result_mat, MAT3mat mat); +int MAT3normal_matrix (MAT3mat result_mat, MAT3mat mat); +void MAT3rotate (MAT3mat result_mat, MAT3vec axis, double angle_in_radians); +void MAT3translate (MAT3mat result_mat, MAT3vec trans); +void MAT3scale (MAT3mat result_mat, MAT3vec scale); +void MAT3shear(MAT3mat result_mat, double xshear, double yshear); + +#if defined( USE_XTRA_MAT3_INLINES ) + +#define MAT3mult_vec( result_vec, vec, mat) { \ + MAT3vec tempvec; \ + tempvec[0]=vec[0]*mat[0][0]+vec[1]*mat[1][0]+vec[2]*mat[2][0]+mat[3][0]; \ + tempvec[1]=vec[0]*mat[0][1]+vec[1]*mat[1][1]+vec[2]*mat[2][1]+mat[3][1]; \ + tempvec[2]=vec[0]*mat[0][2]+vec[1]*mat[1][2]+vec[2]*mat[2][2]+mat[3][2]; \ + result_vec[0] = tempvec[0]; \ + result_vec[1] = tempvec[1]; \ + result_vec[2] = tempvec[2]; \ +} + +#define MAT3cross_product(result_vec, vec1, vec2) { \ + MAT3vec tempvec; \ + tempvec[0] = vec1[1] * vec2[2] - vec1[2] * vec2[1]; \ + tempvec[1] = vec1[2] * vec2[0] - vec1[0] * vec2[2]; \ + tempvec[2] = vec1[0] * vec2[1] - vec1[1] * vec2[0]; \ + result_vec[0] = tempvec[0]; \ + result_vec[1] = tempvec[1]; \ + result_vec[2] = tempvec[2]; \ +} + +#if defined( USE_MEM ) || defined( WIN32 ) +#define MAT3copy( to, from) memcpy(to, from, sizeof(MAT3mat)) +#define MAT3zero(mat) memset(mat,0x00, sizeof(MAT3mat)) +#define MAT3mult( result_mat, mat1, mat2) { \ + register int i, j; \ + MAT3mat tmp_mat; \ + for (i = 0; i < 4; i++) \ + for (j = 0; j < 4; j++) \ + tmp_mat[i][j] = (mat1[i][0] * mat2[0][j] + mat1[i][1] * mat2[1][j] \ + + mat1[i][2] * mat2[2][j] + mat1[i][3] * mat2[3][j]); \ + memcpy(result_mat, tmp_mat, sizeof(MAT3mat)); \ +} +#define MAT3identity(mat) { \ + register int i; \ + memset(mat, 0x00, sizeof(MAT3mat)); \ + for (i = 0; i < 4; i++) mat[i][i] = 1.0; \ +} + +#else !defined( USE_MEM ) || !defined( WIN32 ) + +#define MAT3copy( to, from) bcopy(from, to, sizeof(MAT3mat)) +#define MAT3zero(mat) bzero (mat, sizeof(MAT3mat)) +#define MAT3mult( result_mat, mat1, mat2) { \ + register int i, j; \ + MAT3mat tmp_mat; \ + for (i = 0; i < 4; i++) \ + for (j = 0; j < 4; j++) \ + tmp_mat[i][j] = (mat1[i][0] * mat2[0][j] + mat1[i][1] * mat2[1][j] \ + + mat1[i][2] * mat2[2][j] + mat1[i][3] * mat2[3][j]); \ + bcopy(tmp_mat, result_mat, sizeof(MAT3mat)); \ +} +#define MAT3identity(mat) { \ + register int i; \ + bzero(mat, sizeof(MAT3mat)); \ + for(i = 0; i < 4; i++) mat[i][i] = 1.0; \ +} +#endif + +#else // !defined( USE_XTRA_MAT3_INLINES ) /* In MAT3mat.c */ -void MAT3identity(MAT3mat); -void MAT3zero(MAT3mat); -void MAT3copy (MAT3mat to, MAT3mat from); -void MAT3mult (MAT3mat result, MAT3mat, MAT3mat); -void MAT3transpose (MAT3mat result, MAT3mat); -int MAT3invert (MAT3mat result, MAT3mat); -void MAT3print (MAT3mat, FILE *fp); -void MAT3print_formatted (MAT3mat, FILE *fp, - char *title, char *head, char *format, char *tail); -extern int MAT3equal( void ); -extern double MAT3trace( void ); -extern int MAT3power( void ); -extern int MAT3column_reduce( void ); -extern int MAT3kernel_basis( void ); +void MAT3identity(MAT3mat); +void MAT3zero(MAT3mat); + +void MAT3copy (MAT3mat to, MAT3mat from); +void MAT3mult (MAT3mat result, MAT3mat, MAT3mat); + +#endif // defined( USE_XTRA_MAT3_INLINES ) + +void MAT3transpose (MAT3mat result, MAT3mat); +int MAT3invert (MAT3mat result, MAT3mat); +void MAT3print (MAT3mat, FILE *fp); +void MAT3print_formatted (MAT3mat, FILE *fp, + char *title, char *head, char *format, char *tail); +int MAT3equal( void ); +double MAT3trace( void ); +int MAT3power( void ); +int MAT3column_reduce( void ); +int MAT3kernel_basis( void ); /* In MAT3vec.c */ -void MAT3mult_vec(MAT3vec result_vec, MAT3vec vec, MAT3mat mat); -int MAT3mult_hvec (MAT3hvec result_vec, MAT3hvec vec, MAT3mat mat, int normalize); -void MAT3cross_product(MAT3vec result,MAT3vec,MAT3vec); -void MAT3perp_vec(MAT3vec result_vec, MAT3vec vec, int is_unit); +int MAT3mult_hvec (MAT3hvec result_vec, MAT3hvec vec, MAT3mat mat, int normalize); +void MAT3perp_vec(MAT3vec result_vec, MAT3vec vec, int is_unit); +#if !defined( USE_XTRA_MAT3_INLINES ) +void MAT3mult_vec(MAT3vec result_vec, MAT3vec vec, MAT3mat mat); +void MAT3cross_product(MAT3vec result,MAT3vec,MAT3vec); +#endif // !defined( USE_XTRA_MAT3_INLINES ) #ifdef __cplusplus diff --git a/Math/polar3d.cxx b/Math/polar3d.cxx index 8052744fb..19fd26ab1 100644 --- a/Math/polar3d.cxx +++ b/Math/polar3d.cxx @@ -37,9 +37,12 @@ * to be specified in meters */ fgPoint3d fgPolarToCart3d(fgPoint3d p) { fgPoint3d pnew; + double tmp; - pnew.x = cos(p.lon) * cos(p.lat) * p.radius; - pnew.y = sin(p.lon) * cos(p.lat) * p.radius; + tmp = cos(p.lat) * p.radius; + + pnew.x = cos(p.lon) * tmp; + pnew.y = sin(p.lon) * tmp; pnew.z = sin(p.lat) * p.radius; return(pnew); @@ -61,12 +64,45 @@ fgPoint3d fgCartToPolar3d(fgPoint3d cp) { } +/* Find the Altitude above the Ellipsoid (WGS84) given the Earth + * Centered Cartesian coordinate vector Distances are specified in + * meters. */ +double fgGeodAltFromCart(fgPoint3d cp) +{ + double t_lat, x_alpha, mu_alpha; + double lat_geoc, radius; + double result; + + lat_geoc = FG_PI_2 - atan2( sqrt(cp.x*cp.x + cp.y*cp.y), cp.z ); + radius = sqrt(cp.x*cp.x + cp.y*cp.y + cp.z*cp.z); + + if( ( (FG_PI_2 - lat_geoc) < ONE_SECOND ) /* near North pole */ + || ( (FG_PI_2 + lat_geoc) < ONE_SECOND ) ) /* near South pole */ + { + result = radius - EQUATORIAL_RADIUS_M*E; + } else { + t_lat = tan(lat_geoc); + 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; + } + result = (radius - x_alpha/cos(lat_geoc))*cos(mu_alpha - lat_geoc); + } + + return(result); +} + + /* $Log$ -/* 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/08/24 20:04:11 curt +/* Various "inline" code optimizations contributed by Norman Vine. /* + * 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. * diff --git a/Math/polar3d.hxx b/Math/polar3d.hxx index a65579051..c568c31f3 100644 --- a/Math/polar3d.hxx +++ b/Math/polar3d.hxx @@ -33,6 +33,7 @@ #endif +#include #include @@ -47,15 +48,24 @@ fgPoint3d fgPolarToCart3d(fgPoint3d p); fgPoint3d fgCartToPolar3d(fgPoint3d cp); +/* Find the Altitude above the Ellipsoid (WGS84) given the Earth + * Centered Cartesian coordinate vector Distances are specified in + * meters. */ +double fgGeodAltFromCart(fgPoint3d cp); + + #endif /* _POLAR_HXX */ /* $Log$ -/* 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.2 1998/08/24 20:04:12 curt +/* Various "inline" code optimizations contributed by Norman Vine. /* + * 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] * diff --git a/Math/vector.cxx b/Math/vector.cxx index 95de6c58f..aac81abfc 100644 --- a/Math/vector.cxx +++ b/Math/vector.cxx @@ -34,6 +34,7 @@ #include "mat3.h" +#if !defined( USE_XTRA_MAT3_INLINES ) /* Map a vector onto the plane specified by normal */ void map_vec_onto_cur_surface_plane(MAT3vec normal, MAT3vec v0, MAT3vec vec, MAT3vec result) @@ -78,6 +79,34 @@ void map_vec_onto_cur_surface_plane(MAT3vec normal, MAT3vec v0, MAT3vec vec, /* printf(" result = %.2f, %.2f, %.2f\n", result[0], result[1], result[2]); */ } +#endif // !defined( USE_XTRA_MAT3_INLINES ) + + +// 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 ); +} // Given a point p, and a line through p0 with direction vector d, @@ -106,10 +135,13 @@ double fgPointLineSquared(MAT3vec p, MAT3vec p0, MAT3vec d) { /* $Log$ -/* Revision 1.2 1998/07/24 21:34:38 curt -/* fgPointLine() rewritten into fgPointLineSquared() ... this ultimately saves -/* us from doing a sqrt(). +/* Revision 1.3 1998/08/24 20:04:12 curt +/* Various "inline" code optimizations contributed by Norman Vine. /* + * Revision 1.2 1998/07/24 21:34:38 curt + * fgPointLine() rewritten into fgPointLineSquared() ... this ultimately saves + * us from doing a sqrt(). + * * 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 diff --git a/Math/vector.hxx b/Math/vector.hxx index 6cbf299d4..2dd37a046 100644 --- a/Math/vector.hxx +++ b/Math/vector.hxx @@ -33,12 +33,28 @@ #endif -#include +#include "mat3.h" /* Map a vector onto the plane specified by normal */ -void map_vec_onto_cur_surface_plane(MAT3vec normal, MAT3vec v0, MAT3vec vec, +#if defined( USE_XTRA_MAT3_INLINES ) +# define map_vec_onto_cur_surface_plane(normal, v0, vec, result) { \ + double scale = ((normal[0]*vec[0]+normal[1]*vec[1]+normal[2]*vec[2]) / \ + (normal[0]*normal[0]+normal[1]*normal[1]+normal[2]*normal[2])); \ + result[0] = vec[0]-normal[0]*scale; \ + result[1] = vec[1]-normal[1]*scale; \ + result[2] = vec[2]-normal[2]*scale; \ + } +#else + void map_vec_onto_cur_surface_plane(MAT3vec normal, MAT3vec v0, MAT3vec vec, MAT3vec result); +#endif //defined( USE_XTRA_MAT3_INLINES ) + + +// 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); + // Given a point p, and a line through p0 with direction vector d, // find the shortest distance (squared) from the point to the line @@ -49,10 +65,13 @@ double fgPointLineSquared(MAT3vec p, MAT3vec p0, MAT3vec d); /* $Log$ -/* Revision 1.2 1998/07/24 21:34:38 curt -/* fgPointLine() rewritten into fgPointLineSquared() ... this ultimately saves -/* us from doing a sqrt(). +/* Revision 1.3 1998/08/24 20:04:13 curt +/* Various "inline" code optimizations contributed by Norman Vine. /* + * Revision 1.2 1998/07/24 21:34:38 curt + * fgPointLine() rewritten into fgPointLineSquared() ... this ultimately saves + * us from doing a sqrt(). + * * 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