Various "inline" code optimizations contributed by Norman Vine.

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

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.

Math/mat3.h

@@ -18,6 +18,7 @@
 #endif
 #include <stdio.h>
 
+#include <string.h>
 
 #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  --------------------------------- */
 
@@ -126,27 +135,95 @@ 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);
+
+#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);
-extern int		MAT3equal( void );
+int	MAT3equal( void );
-extern double		MAT3trace( void );
+double  MAT3trace( void );
-extern int		MAT3power( void );
+int	MAT3power( void );
-extern int		MAT3column_reduce( void );
+int	MAT3column_reduce( void );
-extern int		MAT3kernel_basis( 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);
+#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

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;
+    tmp = cos(p.lat) * p.radius;
-    pnew.y = sin(p.lon) * 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
+/* Revision 1.2  1998/08/24 20:04:11  curt
-/* polar3d.[ch] renamed to polar3d.[ch]xx, vector.[ch] renamed to vector.[ch]xx
+/* Various "inline" code optimizations contributed by Norman Vine.
-/* Updated fg_geodesy comments to reflect that routines expect and produce
-/*   meters.
 /*
+ * 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.
  *

Math/polar3d.hxx

@@ -33,6 +33,7 @@
 #endif                                   
 
 
+#include <Include/fg_constants.h>
 #include <Include/fg_types.h>
 
 
@@ -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
+/* Revision 1.2  1998/08/24 20:04:12  curt
-/* polar3d.[ch] renamed to polar3d.[ch]xx, vector.[ch] renamed to vector.[ch]xx
+/* Various "inline" code optimizations contributed by Norman Vine.
-/* Updated fg_geodesy comments to reflect that routines expect and produce
-/*   meters.
 /*
+ * 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]
  *

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
+/* Revision 1.3  1998/08/24 20:04:12  curt
-/* fgPointLine() rewritten into fgPointLineSquared() ... this ultimately saves
+/* Various "inline" code optimizations contributed by Norman Vine.
-/* us from doing a sqrt().
 /*
+ * 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

Math/vector.hxx

@@ -33,12 +33,28 @@
 #endif                                   
 
 
-#include <Math/mat3.h>
+#include "mat3.h"
 
 
 /* Map a vector onto the plane specified by normal */
+#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
+/* Revision 1.3  1998/08/24 20:04:13  curt
-/* fgPointLine() rewritten into fgPointLineSquared() ... this ultimately saves
+/* Various "inline" code optimizations contributed by Norman Vine.
-/* us from doing a sqrt().
 /*
+ * 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