Various "inline" code optimizations contributed by Norman Vine.
This commit is contained in:
curt
parent
dacc051a57
commit
aef484d5fc
@ -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
|
||||
|
||||
@ -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.
|
||||
|
||||
127
Math/mat3.h
127
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 --------------------------------- */
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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.
|
||||
*
|
||||
|
||||
@ -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
|
||||
/* 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]
|
||||
*
|
||||
|
||||
@ -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
|
||||
|
||||
@ -33,12 +33,28 @@
|
||||
#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
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user