/* -------------------------- Public Routines ---------------------------- */
+#if !defined( USE_XTRA_MAT3_INLINES )
+
/*
* Sets a matrix to identity.
*/
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
# define FALSE 0
#endif
+#if !defined( USE_XTRA_MAT3_INLINES )
void
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.
return(ret);
}
+#if !defined( USE_XTRA_MAT3_INLINES )
+
/*
* Sets the first vector to be the cross-product of the last two vectors.
*/
MAT3_COPY_VEC(result_vec, temp);
}
+#endif // !defined( USE_XTRA_MAT3_INLINES )
/*
* Finds a vector perpendicular to vec and stores it in result_vec.
#endif
#include <stdio.h>
+#include <string.h>
#ifdef __cplusplus
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 --------------------------------- */
/* 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
* 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);
}
+/* 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.
*
#endif
+#include <Include/fg_constants.h>
#include <Include/fg_types.h>
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]
*
#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)
/* 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,
/* $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
#endif
-#include <Math/mat3.h>
+#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
/* $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
projects / flightgear.git / commitdiff