FGFS-4.1/simgear
3
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Moved everything from Lib to simgear

Browse Source
This commit is contained in:
Tim Moore
2009-09-16 08:06:28 +02:00
parent 708e745cfd
commit 778c3ab25c
156 changed files with 0 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

168
lib/math/MAT3geom.c Normal file
View File

@@ -0,0 +1,168 @@
/* #include "HEADERS.h" */
/* Copyright 1988, Brown Computer Graphics Group. All Rights Reserved. */
/* --------------------------------------------------------------------------
* This file contains routines that perform geometry-related operations
* on matrices.
* -------------------------------------------------------------------------*/
#include <Math/mat3defs.h>
/* -------------------------- Static Routines ---------------------------- */
/* ------------------------- Internal Routines --------------------------- */
/* -------------------------- Public Routines ---------------------------- */
/*
* This takes a matrix used to transform points, and returns a corresponding
* matrix that can be used to transform direction vectors (between points).
*/
void
MAT3direction_matrix(register double (*result_mat)[4], register double (*mat)[4])
{
register int i;
MAT3copy(result_mat, mat);
for (i = 0; i < 4; i++) result_mat[i][3] = result_mat[3][i] = 0.0;
result_mat[3][3] = 1.0;
}
/*
* This takes a matrix used to transform points, and returns a corresponding
* matrix that can be used to transform vectors that must remain perpendicular
* to planes defined by the points. It is useful when you are transforming
* some object that has both points and normals in its definition, and you
* only have the transformation matrix for the points. This routine returns
* FALSE if the normal matrix is uncomputable. Otherwise, it returns TRUE.
*
* Spike sez: "This is the adjoint for the non-homogeneous part of the
* transformation."
*/
int
MAT3normal_matrix(register double (*result_mat)[4], register double (*mat)[4])
{
register int ret;
MAT3mat tmp_mat;
MAT3direction_matrix(result_mat, mat);
if ( (ret = MAT3invert(tmp_mat, tmp_mat)) ) {
MAT3transpose(result_mat, tmp_mat);
}
return(ret);
}
/*
* Sets the given matrix to be a scale matrix for the given vector of
* scale values.
*/
void
MAT3scale(double (*result_mat)[4], double *scale)
{
MAT3identity(result_mat);
result_mat[0][0] = scale[0];
result_mat[1][1] = scale[1];
result_mat[2][2] = scale[2];
}
/*
* Sets up a matrix for a rotation about an axis given by the line from
* (0,0,0) to axis, through an angle (in radians).
* Looking along the axis toward the origin, the rotation is counter-clockwise.
*/
#define SELECT .7071 /* selection constant (roughly .5*sqrt(2) */
void
MAT3rotate(double (*result_mat)[4], double *axis, double angle_in_radians)
{
MAT3vec naxis, /* Axis of rotation, normalized */
base2, /* 2nd unit basis vec, perp to axis */
base3; /* 3rd unit basis vec, perp to axis & base2 */
double dot;
MAT3mat base_mat, /* Change-of-basis matrix */
base_mat_trans; /* Inverse of c-o-b matrix */
register int i;
/* Step 1: extend { axis } to a basis for 3-space: { axis, base2, base3 }
* which is orthonormal (all three have unit length, and all three are
* mutually orthogonal). Also should be oriented, i.e. axis cross base2 =
* base3, rather than -base3.
*
* Method: Find a vector linearly independent from axis. For this we
* either use the y-axis, or, if that is too close to axis, the
* z-axis. 'Too close' means that the dot product is too near to 1.
*/
MAT3_COPY_VEC(naxis, axis);
MAT3_NORMALIZE_VEC(naxis, dot);
if (dot == 0.0) {
/* ERR_ERROR(MAT3_errid, ERR_SEVERE,
(ERR_S, "Zero-length axis vector given to MAT3rotate")); */
return;
}
MAT3perp_vec(base2, naxis, TRUE);
MAT3cross_product(base3, naxis, base2);
/* Set up the change-of-basis matrix, and its inverse */
MAT3identity(base_mat);
MAT3identity(base_mat_trans);
MAT3identity(result_mat);
for (i = 0; i < 3; i++){
base_mat_trans[i][0] = base_mat[0][i] = naxis[i];
base_mat_trans[i][1] = base_mat[1][i] = base2[i];
base_mat_trans[i][2] = base_mat[2][i] = base3[i];
}
/* If T(u) = uR, where R is base_mat, then T(x-axis) = naxis,
* T(y-axis) = base2, and T(z-axis) = base3. The inverse of base_mat is
* its transpose. OK?
*/
result_mat[1][1] = result_mat[2][2] = cos(angle_in_radians);
result_mat[2][1] = -(result_mat[1][2] = sin(angle_in_radians));
MAT3mult(result_mat, base_mat_trans, result_mat);
MAT3mult(result_mat, result_mat, base_mat);
}
/*
* Sets the given matrix to be a translation matrix for the given vector of
* translation values.
*/
void
MAT3translate(double (*result_mat)[4], double *trans)
{
MAT3identity(result_mat);
result_mat[3][0] = trans[0];
result_mat[3][1] = trans[1];
result_mat[3][2] = trans[2];
}
/*
* Sets the given matrix to be a shear matrix for the given x and y shear
* values.
*/
void
MAT3shear(double (*result_mat)[4], double xshear, double yshear)
{
MAT3identity(result_mat);
result_mat[2][0] = xshear;
result_mat[2][1] = yshear;
}

311
lib/math/MAT3inv.c Normal file
View File

@@ -0,0 +1,311 @@
/* Copyright 1988, Brown Computer Graphics Group. All Rights Reserved. */
/* --------------------------------------------------------------------------
* This file contains routines that operate solely on matrices.
* -------------------------------------------------------------------------*/
#include <Math/mat3defs.h>
/* -------------------------- Static Routines ---------------------------- */
#define SMALL 1e-20 /* Small enough to be considered zero */
/*
* Shuffles rows in inverse of 3x3. See comment in MAT3_inv3_second_col().
*/
static void
MAT3_inv3_swap( register double inv[3][3], int row0, int row1, int row2)
{
register int i, tempi;
double temp;
#define SWAP_ROWS(a, b) \
for (i = 0; i < 3; i++) SWAP(inv[a][i], inv[b][i], temp); \
SWAP(a, b, tempi)
if (row0 != 0){
if (row1 == 0) {
SWAP_ROWS(row0, row1);
}
else {
SWAP_ROWS(row0, row2);
}
}
if (row1 != 1) {
SWAP_ROWS(row1, row2);
}
}
/*
* Does Gaussian elimination on second column.
*/
static int
MAT3_inv3_second_col (register double source[3][3], register double inv[3][3], int row0)
{
register int row1, row2, i1, i2, i;
double temp;
double a, b;
/* Find which row to use */
if (row0 == 0) i1 = 1, i2 = 2;
else if (row0 == 1) i1 = 0, i2 = 2;
else i1 = 0, i2 = 1;
/* Find which is larger in abs. val.:the entry in [i1][1] or [i2][1] */
/* and use that value for pivoting. */
a = source[i1][1]; if (a < 0) a = -a;
b = source[i2][1]; if (b < 0) b = -b;
if (a > b) row1 = i1;
else row1 = i2;
row2 = (row1 == i1 ? i2 : i1);
/* Scale row1 in source */
if ((source[row1][1] < SMALL) && (source[row1][1] > -SMALL)) return(FALSE);
temp = 1.0 / source[row1][1];
source[row1][1] = 1.0;
source[row1][2] *= temp; /* source[row1][0] is zero already */
/* Scale row1 in inv */
inv[row1][row1] = temp; /* it used to be a 1.0 */
inv[row1][row0] *= temp;
/* Clear column one, source, and make corresponding changes in inv */
for (i = 0; i < 3; i++) if (i != row1) { /* for i = all rows but row1 */
temp = -source[i][1];
source[i][1] = 0.0;
source[i][2] += temp * source[row1][2];
inv[i][row1] = temp * inv[row1][row1];
inv[i][row0] += temp * inv[row1][row0];
}
/* Scale row2 in source */
if ((source[row2][2] < SMALL) && (source[row2][2] > -SMALL)) return(FALSE);
temp = 1.0 / source[row2][2];
source[row2][2] = 1.0; /* source[row2][*] is zero already */
/* Scale row2 in inv */
inv[row2][row2] = temp; /* it used to be a 1.0 */
inv[row2][row0] *= temp;
inv[row2][row1] *= temp;
/* Clear column one, source, and make corresponding changes in inv */
for (i = 0; i < 3; i++) if (i != row2) { /* for i = all rows but row2 */
temp = -source[i][2];
source[i][2] = 0.0;
inv[i][row0] += temp * inv[row2][row0];
inv[i][row1] += temp * inv[row2][row1];
inv[i][row2] += temp * inv[row2][row2];
}
/*
* Now all is done except that the inverse needs to have its rows shuffled.
* row0 needs to be moved to inv[0][*], row1 to inv[1][*], etc.
*
* We *didn't* do the swapping before the elimination so that we could more
* easily keep track of what ops are needed to be done in the inverse.
*/
MAT3_inv3_swap(inv, row0, row1, row2);
return(TRUE);
}
/*
* Fast inversion routine for 3 x 3 matrices. - Written by jfh.
*
* This takes 30 multiplies/divides, as opposed to 39 for Cramer's Rule.
* The algorithm consists of performing fast gaussian elimination, by never
* doing any operations where the result is guaranteed to be zero, or where
* one operand is guaranteed to be zero. This is done at the cost of clarity,
* alas.
*
* Returns 1 if the inverse was successful, 0 if it failed.
*/
static int
MAT3_invert3 (register double source[3][3], register double inv[3][3])
{
register int i, row0;
double temp;
double a, b, c;
inv[0][0] = inv[1][1] = inv[2][2] = 1.0;
inv[0][1] = inv[0][2] = inv[1][0] = inv[1][2] = inv[2][0] = inv[2][1] = 0.0;
/* attempt to find the largest entry in first column to use as pivot */
a = source[0][0]; if (a < 0) a = -a;
b = source[1][0]; if (b < 0) b = -b;
c = source[2][0]; if (c < 0) c = -c;
if (a > b) {
if (a > c) row0 = 0;
else row0 = 2;
}
else {
if (b > c) row0 = 1;
else row0 = 2;
}
/* Scale row0 of source */
if ((source[row0][0] < SMALL) && (source[row0][0] > -SMALL)) return(FALSE);
temp = 1.0 / source[row0][0];
source[row0][0] = 1.0;
source[row0][1] *= temp;
source[row0][2] *= temp;
/* Scale row0 of inverse */
inv[row0][row0] = temp; /* other entries are zero -- no effort */
/* Clear column zero of source, and make corresponding changes in inverse */
for (i = 0; i < 3; i++) if (i != row0) { /* for i = all rows but row0 */
temp = -source[i][0];
source[i][0] = 0.0;
source[i][1] += temp * source[row0][1];
source[i][2] += temp * source[row0][2];
inv[i][row0] = temp * inv[row0][row0];
}
/*
* We've now done gaussian elimination so that the source and
* inverse look like this:
*
* 1 * * * 0 0
* 0 * * * 1 0
* 0 * * * 0 1
*
* We now proceed to do elimination on the second column.
*/
if (! MAT3_inv3_second_col(source, inv, row0)) return(FALSE);
return(TRUE);
}
/*
* Finds a new pivot for a non-simple 4x4. See comments in MAT3invert().
*/
static int
MAT3_inv4_pivot (register MAT3mat src, MAT3vec r, double *s, int *swap)
{
register int i, j;
double temp, max;
*swap = -1;
if (MAT3_IS_ZERO(src[3][3])) {
/* Look for a different pivot element: one with largest abs value */
max = 0.0;
for (i = 0; i < 4; i++) {
if (src[i][3] > max) max = src[*swap = i][3];
else if (src[i][3] < -max) max = -src[*swap = i][3];
}
/* No pivot element available ! */
if (*swap < 0) return(FALSE);
else for (j = 0; j < 4; j++) SWAP(src[*swap][j], src[3][j], temp);
}
MAT3_SET_VEC (r, -src[0][3], -src[1][3], -src[2][3]);
*s = 1.0 / src[3][3];
src[0][3] = src[1][3] = src[2][3] = 0.0;
src[3][3] = 1.0;
MAT3_SCALE_VEC(src[3], src[3], *s);
for (i = 0; i < 3; i++) {
src[0][i] += r[0] * src[3][i];
src[1][i] += r[1] * src[3][i];
src[2][i] += r[2] * src[3][i];
}
return(TRUE);
}
/* ------------------------- Internal Routines --------------------------- */
/* -------------------------- Public Routines ---------------------------- */
/*
* This returns the inverse of the given matrix. The result matrix
* may be the same as the one to invert.
*
* Fast inversion routine for 4 x 4 matrices, written by jfh.
*
* Returns 1 if the inverse was successful, 0 if it failed.
*
* This routine has been specially tweaked to notice the following:
* If the matrix has the form
* * * * 0
* * * * 0
* * * * 0
* * * * 1
*
* (as do many matrices in graphics), then we compute the inverse of
* the upper left 3x3 matrix and use this to find the general inverse.
*
* In the event that the right column is not 0-0-0-1, we do gaussian
* elimination to make it so, then use the 3x3 inverse, and then do
* our gaussian elimination.
*/
int
MAT3invert(double (*result_mat)[4], double (*mat)[4])
{
MAT3mat src, inv;
register int i, j, simple;
double m[3][3], inv3[3][3], s, temp;
MAT3vec r, t;
int swap;
MAT3copy(src, mat);
MAT3identity(inv);
/* If last column is not (0,0,0,1), use special code */
simple = (mat[0][3] == 0.0 && mat[1][3] == 0.0 &&
mat[2][3] == 0.0 && mat[3][3] == 1.0);
if (! simple && ! MAT3_inv4_pivot(src, r, &s, &swap)) return(FALSE);
MAT3_COPY_VEC(t, src[3]); /* Translation vector */
/* Copy upper-left 3x3 matrix */
for (i = 0; i < 3; i++) for (j = 0; j < 3; j++) m[i][j] = src[i][j];
if (! MAT3_invert3(m, inv3)) return(FALSE);
for (i = 0; i < 3; i++) for (j = 0; j < 3; j++) inv[i][j] = inv3[i][j];
for (i = 0; i < 3; i++) for (j = 0; j < 3; j++)
inv[3][i] -= t[j] * inv3[j][i];
if (! simple) {
/* We still have to undo our gaussian elimination from earlier on */
/* add r0 * first col to last col */
/* add r1 * 2nd col to last col */
/* add r2 * 3rd col to last col */
for (i = 0; i < 4; i++) {
inv[i][3] += r[0] * inv[i][0] + r[1] * inv[i][1] + r[2] * inv[i][2];
inv[i][3] *= s;
}
if (swap >= 0)
for (i = 0; i < 4; i++) SWAP(inv[i][swap], inv[i][3], temp);
}
MAT3copy(result_mat, inv);
return(TRUE);
}

120
lib/math/MAT3mat.c Normal file
View File

@@ -0,0 +1,120 @@
/* #include "HEADERS.h" */
/* Copyright 1988, Brown Computer Graphics Group. All Rights Reserved. */
/* --------------------------------------------------------------------------
* This file contains routines that operate solely on matrices.
* -------------------------------------------------------------------------*/
#ifdef HAVE_CONFIG_H
# include <config.h>
#endif
#ifdef WIN32
# ifndef HAVE_STL_SGI_PORT
# ifdef __BORLANDC__
# include <mem.h>
# else
# include <memory.h> /* required for memset() and memcpy() */
# endif
# endif
#endif
#include <string.h>
#include <Math/mat3defs.h>
MAT3mat identityMatrix = {
{ 1.0, 0.0, 0.0, 0.0 },
{ 0.0, 1.0, 0.0, 0.0 },
{ 0.0, 0.0, 1.0, 0.0 },
{ 0.0, 0.0, 0.0, 1.0 }
};
/* #include "macros.h" */
/* -------------------------- Static Routines ---------------------------- */
/* ------------------------- Internal Routines --------------------------- */
/* -------------------------- Public Routines ---------------------------- */
#if !defined( USE_XTRA_MAT3_INLINES )
/*
* This multiplies two matrices, producing a third, which may the same as
* either of the first two.
*/
void
MAT3mult (double (*result_mat)[4], register double (*mat1)[4], register double (*mat2)[4])
{
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]);
MAT3copy (result_mat, tmp_mat);
}
#endif // !defined( USE_XTRA_MAT3_INLINES )
/*
* This returns the transpose of a matrix. The result matrix may be
* the same as the one to transpose.
*/
void
MAT3transpose (double (*result_mat)[4], register double (*mat)[4])
{
register int i, j;
MAT3mat tmp_mat;
for (i = 0; i < 4; i++)
for (j = 0; j < 4; j++)
tmp_mat[i][j] = mat[j][i];
MAT3copy (result_mat, tmp_mat);
}
/*
* This prints the given matrix to the given file pointer.
*/
void
MAT3print(double (*mat)[4], FILE *fp)
{
MAT3print_formatted(mat, fp, CNULL, CNULL, CNULL, CNULL);
}
/*
* This prints the given matrix to the given file pointer.
* use the format string to pass to fprintf. head and tail
* are printed at the beginning and end of each line.
*/
void
MAT3print_formatted(double (*mat)[4], FILE *fp, char *title, char *head, char *format, char *tail)
{
register int i, j;
/* This is to allow this to be called easily from a debugger */
if (fp == NULL) fp = stderr;
if (title == NULL) title = "MAT3 matrix:\n";
if (head == NULL) head = " ";
if (format == NULL) format = "%#8.4lf ";
if (tail == NULL) tail = "\n";
(void) fprintf(fp, title);
for (i = 0; i < 4; i++) {
(void) fprintf(fp, head);
for (j = 0; j < 4; j++) (void) fprintf(fp, format, mat[i][j]);
(void) fprintf(fp, tail);
}
}

154
lib/math/MAT3vec.c Normal file
View File

@@ -0,0 +1,154 @@
/* Copyright 1988, Brown Computer Graphics Group. All Rights Reserved. */
/* --------------------------------------------------------------------------
* This file contains routines that operate on matrices and vectors, or
* vectors and vectors.
* -------------------------------------------------------------------------*/
/* #include "sphigslocal.h" */
/* -------------------------- Static Routines ---------------------------- */
/* ------------------------- Internal Routines --------------------------- */
/* -------------------------- Public Routines ---------------------------- */
/*
* Multiplies a vector by a matrix, setting the result vector.
* It assumes all homogeneous coordinates are 1.
* The two vectors involved may be the same.
*/
#include <Math/mat3.h>
#ifndef TRUE
# define TRUE 1
#endif
#ifndef FALSE
# define FALSE 0
#endif
#if !defined( USE_XTRA_MAT3_INLINES )
void
MAT3mult_vec(double *result_vec, register double *vec, register double (*mat)[4])
{
MAT3vec tempvec;
register double *temp = tempvec;
temp[0] = vec[0] * mat[0][0] + vec[1] * mat[1][0] +
vec[2] * mat[2][0] + mat[3][0];
temp[1] = vec[0] * mat[0][1] + vec[1] * mat[1][1] +
vec[2] * mat[2][1] + mat[3][1];
temp[2] = vec[0] * mat[0][2] + vec[1] * mat[1][2] +
vec[2] * mat[2][2] + mat[3][2];
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.
* The fourth element of the vector is the homogeneous coordinate, which
* may or may not be 1. If the "normalize" parameter is TRUE, then the
* result vector will be normalized so that the homogeneous coordinate is 1.
* The two vectors involved may be the same.
* This returns zero if the vector was to be normalized, but couldn't be.
*/
int
MAT3mult_hvec(double *result_vec, register double *vec, register double (*mat)[4], int normalize)
{
MAT3hvec tempvec;
double norm_fac;
register double *temp = tempvec;
register int ret = TRUE;
temp[0] = vec[0] * mat[0][0] + vec[1] * mat[1][0] +
vec[2] * mat[2][0] + vec[3] * mat[3][0];
temp[1] = vec[0] * mat[0][1] + vec[1] * mat[1][1] +
vec[2] * mat[2][1] + vec[3] * mat[3][1];
temp[2] = vec[0] * mat[0][2] + vec[1] * mat[1][2] +
vec[2] * mat[2][2] + vec[3] * mat[3][2];
temp[3] = vec[0] * mat[0][3] + vec[1] * mat[1][3] +
vec[2] * mat[2][3] + vec[3] * mat[3][3];
/* Normalize if asked for, possible, and necessary */
if (normalize) {
if (MAT3_IS_ZERO(temp[3])) {
#ifndef THINK_C
fprintf (stderr,
"Can't normalize vector: homogeneous coordinate is 0");
#endif
ret = FALSE;
}
else {
norm_fac = 1.0 / temp[3];
MAT3_SCALE_VEC(result_vec, temp, norm_fac);
result_vec[3] = 1.0;
}
}
else MAT3_COPY_HVEC(result_vec, temp);
return(ret);
}
#if !defined( USE_XTRA_MAT3_INLINES )
/*
* Sets the first vector to be the cross-product of the last two vectors.
*/
void
MAT3cross_product(double *result_vec, register double *vec1, register double *vec2)
{
MAT3vec tempvec;
register double *temp = tempvec;
temp[0] = vec1[1] * vec2[2] - vec1[2] * vec2[1];
temp[1] = vec1[2] * vec2[0] - vec1[0] * vec2[2];
temp[2] = vec1[0] * vec2[1] - vec1[1] * vec2[0];
MAT3_COPY_VEC(result_vec, temp);
}
#endif // !defined( USE_XTRA_MAT3_INLINES )
/*
* Finds a vector perpendicular to vec and stores it in result_vec.
* Method: take any vector (we use <0,1,0>) and subtract the
* portion of it pointing in the vec direction. This doesn't
* work if vec IS <0,1,0> or is very near it. So if this is
* the case, use <0,0,1> instead.
* If "is_unit" is TRUE, the given vector is assumed to be unit length.
*/
#define SELECT .7071 /* selection constant (roughly .5*sqrt(2) */
void
MAT3perp_vec(double *result_vec, double *vec, int is_unit)
{
MAT3vec norm;
double dot;
MAT3_SET_VEC(result_vec, 0.0, 1.0, 0.0);
MAT3_COPY_VEC(norm, vec);
if (! is_unit) MAT3_NORMALIZE_VEC(norm, dot);
/* See if vector is too close to <0,1,0>. If so, use <0,0,1> */
if ((dot = MAT3_DOT_PRODUCT(norm, result_vec)) > SELECT || dot < -SELECT) {
result_vec[1] = 0.0;
result_vec[2] = 1.0;
dot = MAT3_DOT_PRODUCT(norm, result_vec);
}
/* Subtract off non-perpendicular part */
result_vec[0] -= dot * norm[0];
result_vec[1] -= dot * norm[1];
result_vec[2] -= dot * norm[2];
/* Make result unit length */
MAT3_NORMALIZE_VEC(result_vec, dot);
}

17
lib/math/Makefile.am Normal file
View File

@@ -0,0 +1,17 @@
noinst_LIBRARIES = libMath.a
libMath_a_SOURCES = \
MAT3geom.c \
MAT3inv.c \
MAT3mat.c \
MAT3vec.c \
fg_geodesy.cxx fg_geodesy.hxx \
fg_random.c fg_random.h \
interpolater.cxx interpolater.hxx \
leastsqs.cxx leastsqs.hxx \
mat3.h mat3defs.h mat3err.h \
point3d.hxx \
polar3d.cxx polar3d.hxx \
vector.cxx vector.hxx
INCLUDES += -I$(top_builddir) -I$(top_builddir)/Lib -I$(top_builddir)/Simulator

211
lib/math/fg_geodesy.cxx Normal file
View File

@@ -0,0 +1,211 @@
// fg_geodesy.cxx -- routines to convert between geodetic and geocentric
// coordinate systems.
//
// Copied and adapted directly from LaRCsim/ls_geodesy.c
//
// See below for the complete original LaRCsim comments.
//
// $Id$
#include "Include/compiler.h"
#ifdef FG_HAVE_STD_INCLUDES
# include <cmath>
# include <cerrno>
#else
# include <math.h>
# include <errno.h>
#endif
#include <Debug/logstream.hxx>
#include <Include/fg_constants.h>
#include <Math/fg_geodesy.hxx>
#include <Math/point3d.hxx>
#ifndef FG_HAVE_NATIVE_SGI_COMPILERS
FG_USING_STD(cout);
#endif
// ONE_SECOND is pi/180/60/60, or about 100 feet at earths' equator
#define ONE_SECOND 4.848136811E-6
// fgGeocToGeod(lat_geoc, radius, *lat_geod, *alt, *sea_level_r)
// INPUTS:
// lat_geoc Geocentric latitude, radians, + = North
// radius C.G. radius to earth center (meters)
//
// OUTPUTS:
// lat_geod Geodetic latitude, radians, + = North
// 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. (meters)
void fgGeocToGeod( double lat_geoc, double radius, double
*lat_geod, double *alt, double *sea_level_r )
{
double t_lat, x_alpha, mu_alpha, delt_mu, r_alpha, l_point, rho_alpha;
double sin_mu_a, denom,delt_lambda, lambda_sl, sin_lambda_sl;
if( ( (FG_PI_2 - lat_geoc) < ONE_SECOND ) // near North pole
|| ( (FG_PI_2 + lat_geoc) < ONE_SECOND ) ) // near South pole
{
*lat_geod = lat_geoc;
*sea_level_r = EQUATORIAL_RADIUS_M*E;
*alt = radius - *sea_level_r;
} else {
t_lat = tan(lat_geoc);
x_alpha = E*EQUATORIAL_RADIUS_M/sqrt(t_lat*t_lat + E*E);
double tmp = RESQ_M - x_alpha * x_alpha;
if ( tmp < 0.0 ) { tmp = 0.0; }
mu_alpha = atan2(sqrt(tmp),E*x_alpha);
if (lat_geoc < 0) mu_alpha = - mu_alpha;
sin_mu_a = sin(mu_alpha);
delt_lambda = mu_alpha - lat_geoc;
r_alpha = x_alpha/cos(lat_geoc);
l_point = radius - r_alpha;
*alt = l_point*cos(delt_lambda);
// check for domain error
if ( errno == EDOM ) {
FG_LOG( FG_GENERAL, FG_ALERT, "Domain ERROR in fgGeocToGeod!!!!" );
*alt = 0.0;
}
denom = sqrt(1-EPS*EPS*sin_mu_a*sin_mu_a);
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_M / (1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl));
// check for domain error
if ( errno == EDOM ) {
FG_LOG( FG_GENERAL, FG_ALERT, "Domain ERROR in fgGeocToGeod!!!!" );
*sea_level_r = 0.0;
}
}
}
// fgGeodToGeoc( lat_geod, alt, *sl_radius, *lat_geoc )
// INPUTS:
// lat_geod Geodetic latitude, radians, + = North
// alt C.G. altitude above mean sea level (meters)
//
// OUTPUTS:
// 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 lambda_sl, sin_lambda_sl, cos_lambda_sl, sin_mu, cos_mu, px, py;
lambda_sl = atan( E*E * tan(lat_geod) ); // sea level geocentric latitude
sin_lambda_sl = sin( lambda_sl );
cos_lambda_sl = cos( lambda_sl );
sin_mu = sin(lat_geod); // Geodetic (map makers') latitude
cos_mu = cos(lat_geod);
*sl_radius =
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 );
}
/***************************************************************************
TITLE: ls_geodesy
----------------------------------------------------------------------------
FUNCTION: Converts geocentric coordinates to geodetic positions
----------------------------------------------------------------------------
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Written as part of LaRCSim project by E. B. Jackson
----------------------------------------------------------------------------
DESIGNED BY: E. B. Jackson
CODED BY: E. B. Jackson
MAINTAINED BY: E. B. Jackson
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
930208 Modified to avoid singularity near polar region. EBJ
930602 Moved backwards calcs here from ls_step. EBJ
931214 Changed erroneous Latitude and Altitude variables to
*lat_geod and *alt in routine ls_geoc_to_geod. EBJ
940111 Changed header files from old ls_eom.h style to ls_types,
and ls_constants. Also replaced old DATA type with new
SCALAR type. EBJ
CURRENT RCS HEADER:
$Header$
* Revision 1.5 1994/01/11 18:47:05 bjax
* Changed include files to use types and constants, not ls_eom.h
* Also changed DATA type to SCALAR type.
*
* Revision 1.4 1993/12/14 21:06:47 bjax
* Removed global variable references Altitude and Latitude. EBJ
*
* Revision 1.3 1993/06/02 15:03:40 bjax
* Made new subroutine for calculating geodetic to geocentric; changed name
* of forward conversion routine from ls_geodesy to ls_geoc_to_geod.
*
----------------------------------------------------------------------------
REFERENCES:
[ 1] Stevens, Brian L.; and Lewis, Frank L.: "Aircraft
Control and Simulation", Wiley and Sons, 1992.
ISBN 0-471-61397-5
----------------------------------------------------------------------------
CALLED BY: ls_aux
----------------------------------------------------------------------------
CALLS TO:
----------------------------------------------------------------------------
INPUTS:
lat_geoc Geocentric latitude, radians, + = North
radius C.G. radius to earth center, ft
----------------------------------------------------------------------------
OUTPUTS:
lat_geod Geodetic latitude, radians, + = North
alt C.G. altitude above mean sea level, ft
sea_level_r radius from earth center to sea level at
local vertical (surface normal) of C.G.
--------------------------------------------------------------------------*/

162
lib/math/fg_geodesy.hxx Normal file
View File

@@ -0,0 +1,162 @@
// fg_geodesy.hxx -- routines to convert between geodetic and geocentric
// coordinate systems.
//
// Copied and adapted directly from LaRCsim/ls_geodesy.c
//
// See below for the complete original LaRCsim comments.
//
// $Id$
#ifndef _FG_GEODESY_HXX
#define _FG_GEODESY_HXX
#ifndef __cplusplus
# error This library requires C++
#endif
#include <Math/point3d.hxx>
#include <Math/polar3d.hxx>
// fgGeocToGeod(lat_geoc, radius, *lat_geod, *alt, *sea_level_r)
// INPUTS:
// lat_geoc Geocentric latitude, radians, + = North
// radius C.G. radius to earth center (meters)
//
// OUTPUTS:
// lat_geod Geodetic latitude, radians, + = North
// 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. (meters)
void fgGeocToGeod( double lat_geoc, double radius, double
*lat_geod, double *alt, double *sea_level_r );
// fgGeodToGeoc( lat_geod, alt, *sl_radius, *lat_geoc )
// INPUTS:
// lat_geod Geodetic latitude, radians, + = North
// alt C.G. altitude above mean sea level (meters)
//
// OUTPUTS:
// 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 );
// convert a geodetic point lon(radians), lat(radians), elev(meter) to
// a cartesian point
inline Point3D fgGeodToCart(const Point3D& geod) {
double gc_lon, gc_lat, sl_radius;
// printf("A geodetic point is (%.2f, %.2f, %.2f)\n",
// geod[0], geod[1], geod[2]);
gc_lon = geod.lon();
fgGeodToGeoc(geod.lat(), geod.radius(), &sl_radius, &gc_lat);
// printf("A geocentric point is (%.2f, %.2f, %.2f)\n", gc_lon,
// gc_lat, sl_radius+geod[2]);
Point3D pp = Point3D( gc_lon, gc_lat, sl_radius + geod.radius());
return fgPolarToCart3d(pp);
}
/***************************************************************************
TITLE: ls_geodesy
----------------------------------------------------------------------------
FUNCTION: Converts geocentric coordinates to geodetic positions
----------------------------------------------------------------------------
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Written as part of LaRCSim project by E. B. Jackson
----------------------------------------------------------------------------
DESIGNED BY: E. B. Jackson
CODED BY: E. B. Jackson
MAINTAINED BY: E. B. Jackson
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
930208 Modified to avoid singularity near polar region. EBJ
930602 Moved backwards calcs here from ls_step. EBJ
931214 Changed erroneous Latitude and Altitude variables to
*lat_geod and *alt in routine ls_geoc_to_geod. EBJ
940111 Changed header files from old ls_eom.h style to ls_types,
and ls_constants. Also replaced old DATA type with new
SCALAR type. EBJ
CURRENT RCS HEADER:
$Header$
* Revision 1.5 1994/01/11 18:47:05 bjax
* Changed include files to use types and constants, not ls_eom.h
* Also changed DATA type to SCALAR type.
*
* Revision 1.4 1993/12/14 21:06:47 bjax
* Removed global variable references Altitude and Latitude. EBJ
*
* Revision 1.3 1993/06/02 15:03:40 bjax
* Made new subroutine for calculating geodetic to geocentric; changed name
* of forward conversion routine from ls_geodesy to ls_geoc_to_geod.
*
----------------------------------------------------------------------------
REFERENCES:
[ 1] Stevens, Brian L.; and Lewis, Frank L.: "Aircraft
Control and Simulation", Wiley and Sons, 1992.
ISBN 0-471-61397-5
----------------------------------------------------------------------------
CALLED BY: ls_aux
----------------------------------------------------------------------------
CALLS TO:
----------------------------------------------------------------------------
INPUTS:
lat_geoc Geocentric latitude, radians, + = North
radius C.G. radius to earth center, ft
----------------------------------------------------------------------------
OUTPUTS:
lat_geod Geodetic latitude, radians, + = North
alt C.G. altitude above mean sea level, ft
sea_level_r radius from earth center to sea level at
local vertical (surface normal) of C.G.
--------------------------------------------------------------------------*/
#endif // _FG_GEODESY_HXX

71
lib/math/fg_random.c Normal file
View File

@@ -0,0 +1,71 @@
// fg_random.c -- routines to handle random number generation
//
// Written by Curtis Olson, started July 1997.
//
// Copyright (C) 1997 Curtis L. Olson - curt@infoplane.com
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of the
// License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
//
// $Id$
#ifdef HAVE_CONFIG_H
# include <config.h>
#endif
#include <stdio.h>
#include <stdlib.h> // for random(), srandom()
#include <time.h> // for time() to seed srandom()
#include "fg_random.h"
#ifndef HAVE_RAND
# ifdef sgi
# undef RAND_MAX
# define RAND_MAX 2147483647
# endif
#endif
#ifdef __SUNPRO_CC
extern "C" {
long int random(void);
void srandom(unsigned int seed);
}
#endif
// Seed the random number generater with time() so we don't see the
// same sequence every time
void fg_srandom(void) {
// fgPrintf( FG_MATH, FG_INFO, "Seeding random number generater\n");
#ifdef HAVE_RAND
srand(time(NULL));
#else
srandom(time(NULL));
#endif
}
// return a random number between [0.0, 1.0)
double fg_random(void) {
#ifdef HAVE_RAND
return(rand() / (double)RAND_MAX);
#else
return(random() / (double)RAND_MAX);
#endif
}

48
lib/math/fg_random.h Normal file
View File

@@ -0,0 +1,48 @@
// fg_random.h -- routines to handle random number generation
//
// Written by Curtis Olson, started July 1997.
//
// Copyright (C) 1997 Curtis L. Olson - curt@infoplane.com
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of the
// License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
//
// $Id$
#ifndef _FG_RANDOM_H
#define _FG_RANDOM_H
#ifdef __cplusplus
extern "C" {
#endif
// Seed the random number generater with time() so we don't see the
// same sequence every time
void fg_srandom(void);
// return a random number between [0.0, 1.0)
double fg_random(void);
#ifdef __cplusplus
}
#endif
#endif // _FG_RANDOM_H

30
lib/math/geotest.c Normal file
View File

@@ -0,0 +1,30 @@
#include <Include/fg_constants.h>
#include <Math/fg_geodesy.h>
#include <stdio.h>
void
main( void )
{
double Lon, Alt, sl_radius;
double geodetic_Lat;
double geocentric_Lat;
Lon = -87.75 * DEG_TO_RAD;
geodetic_Lat = 41.83 * DEG_TO_RAD;
Alt = 1.5; /* km */
printf("Geodetic position = (%.8f, %.8f, %.8f)\n", Lon, geodetic_Lat, Alt);
fgGeodToGeoc( geodetic_Lat, Alt, &sl_radius, &geocentric_Lat );
printf("Geocentric position = (%.8f, %.8f, %.8f)\n", Lon, geocentric_Lat,
sl_radius + Alt);
printf("new sl_radius = %.8f\n", sl_radius);
fgGeocToGeod( geocentric_Lat, sl_radius + Alt, &geodetic_Lat,
&Alt, &sl_radius );
printf("Geodetic position = (%.8f, %.8f, %.8f)\n", Lon, geodetic_Lat, Alt);
printf("new sl_radius = %.8f\n", sl_radius);
}

107
lib/math/interpolater.cxx Normal file
View File

@@ -0,0 +1,107 @@
//
// interpolater.cxx -- routines to handle linear interpolation from a table of
// x,y The table must be sorted by "x" in ascending order
//
// Written by Curtis Olson, started April 1998.
//
// Copyright (C) 1998 Curtis L. Olson - curt@me.umn.edu
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of the
// License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
//
// $Id$
#include <Include/compiler.h>
#ifdef __MWERKS__
#include <stdlib.h> // for exit()
#endif
#include STL_STRING
#include <Debug/logstream.hxx>
#include <Include/fg_zlib.h>
#include <Misc/fgstream.hxx>
#include "interpolater.hxx"
// Constructor -- loads the interpolation table from the specified
// file
fgINTERPTABLE::fgINTERPTABLE( const string& file ) {
FG_LOG( FG_MATH, FG_INFO, "Initializing Interpolator for " << file );
fg_gzifstream in( file );
if ( !in ) {
FG_LOG( FG_GENERAL, FG_ALERT, "Cannot open file: " << file );
exit(-1);
}
size = 0;
in >> skipcomment;
while ( in ) {
if ( size < MAX_TABLE_SIZE ) {
in >> table[size][0] >> table[size][1];
size++;
} else {
FG_LOG( FG_MATH, FG_ALERT,
"fgInterpolateInit(): Exceed max table size = "
<< MAX_TABLE_SIZE );
exit(-1);
}
}
}
// Given an x value, linearly interpolate the y value from the table
double fgINTERPTABLE::interpolate(double x) {
int i;
double y;
i = 0;
while ( (x > table[i][0]) && (i < size) ) {
i++;
}
// printf ("i = %d ", i);
if ( (i == 0) && (x < table[0][0]) ) {
FG_LOG( FG_MATH, FG_ALERT,
"fgInterpolateInit(): lookup error, x to small = " << x );
return(0.0);
}
if ( x > table[i][0] ) {
FG_LOG( FG_MATH, FG_ALERT,
"fgInterpolateInit(): lookup error, x to big = " << x );
return(0.0);
}
// y = y1 + (y0 - y1)(x - x1) / (x0 - x1)
y = table[i][1] +
( (table[i-1][1] - table[i][1]) *
(x - table[i][0]) ) /
(table[i-1][0] - table[i][0]);
return(y);
}
// Destructor
fgINTERPTABLE::~fgINTERPTABLE( void ) {
}

62
lib/math/interpolater.hxx Normal file
View File

@@ -0,0 +1,62 @@
//
// interpolater.hxx -- routines to handle linear interpolation from a table of
// x,y The table must be sorted by "x" in ascending order
//
// Written by Curtis Olson, started April 1998.
//
// Copyright (C) 1998 Curtis L. Olson - curt@me.umn.edu
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of the
// License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
//
// $Id$
#ifndef _INTERPOLATER_H
#define _INTERPOLATER_H
#ifndef __cplusplus
# error This library requires C++
#endif
#include "Include/compiler.h"
#include STL_STRING
FG_USING_STD(string);
#define MAX_TABLE_SIZE 32
class fgINTERPTABLE {
int size;
double table[MAX_TABLE_SIZE][2];
public:
// Constructor -- loads the interpolation table from the specified
// file
fgINTERPTABLE( const string& file );
// Given an x value, linearly interpolate the y value from the table
double interpolate(double x);
// Destructor
~fgINTERPTABLE( void );
};
#endif // _INTERPOLATER_H

23
lib/math/inttest.cxx Normal file
View File

@@ -0,0 +1,23 @@
#include <stdio.h>
#include <Debug/fg_debug.h>
#include "interpolater.hxx"
main() {
fgINTERPTABLE test("test.table");
fgInitDebug();
printf("-1.0 = %.2f\n", test.interpolate(-1.0));
printf("0.0 = %.2f\n", test.interpolate(0.0));
printf("2.9 = %.2f\n", test.interpolate(2.9));
printf("3.0 = %.2f\n", test.interpolate(3.0));
printf("3.5 = %.2f\n", test.interpolate(3.5));
printf("4.0 = %.2f\n", test.interpolate(4.0));
printf("4.5 = %.2f\n", test.interpolate(4.5));
printf("5.2 = %.2f\n", test.interpolate(5.2));
printf("8.0 = %.2f\n", test.interpolate(8.0));
printf("8.5 = %.2f\n", test.interpolate(8.5));
printf("9.0 = %.2f\n", test.interpolate(9.0));
printf("10.0 = %.2f\n", test.interpolate(10.0));
}

135
lib/math/leastsqs.cxx Normal file
View File

@@ -0,0 +1,135 @@
// leastsqs.c -- Implements a simple linear least squares best fit routine
//
// Written by Curtis Olson, started September 1997.
//
// Copyright (C) 1997 Curtis L. Olson - curt@infoplane.com
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
//
// $Id$
//
#include <stdio.h>
#include "leastsqs.hxx"
/*
Least squares fit:
y = b0 + b1x
n*sum(xi*yi) - (sum(xi)*sum(yi))
b1 = --------------------------------
n*sum(xi^2) - (sum(xi))^2
b0 = sum(yi)/n - b1*(sum(xi)/n)
*/
double sum_xi, sum_yi, sum_xi_2, sum_xi_yi;
int sum_n;
void least_squares(double *x, double *y, int n, double *m, double *b) {
int i;
sum_xi = sum_yi = sum_xi_2 = sum_xi_yi = 0.0;
sum_n = n;
for ( i = 0; i < n; i++ ) {
sum_xi += x[i];
sum_yi += y[i];
sum_xi_2 += x[i] * x[i];
sum_xi_yi += x[i] * y[i];
}
/* printf("sum(xi)=%.2f sum(yi)=%.2f sum(xi^2)=%.2f sum(xi*yi)=%.2f\n",
sum_xi, sum_yi, sum_xi_2, sum_xi_yi); */
*m = ( (double)sum_n * sum_xi_yi - sum_xi * sum_yi ) /
( (double)sum_n * sum_xi_2 - sum_xi * sum_xi );
*b = (sum_yi / (double)sum_n) - (*m) * (sum_xi / (double)sum_n);
/* printf("slope = %.2f intercept = %.2f\n", *m, *b); */
}
/* incrimentally update existing values with a new data point */
void least_squares_update(double x, double y, double *m, double *b) {
++sum_n;
sum_xi += x;
sum_yi += y;
sum_xi_2 += x * x;
sum_xi_yi += x * y;
/* printf("sum(xi)=%.2f sum(yi)=%.2f sum(xi^2)=%.2f sum(xi*yi)=%.2f\n",
sum_xi, sum_yi, sum_xi_2, sum_xi_yi); */
*m = ( (double)sum_n * sum_xi_yi - sum_xi * sum_yi ) /
( (double)sum_n * sum_xi_2 - sum_xi * sum_xi );
*b = (sum_yi / (double)sum_n) - (*m) * (sum_xi / (double)sum_n);
/* printf("slope = %.2f intercept = %.2f\n", *m, *b); */
}
/*
return the least squares error:
(y[i] - y_hat[i])^2
-------------------
n
*/
double least_squares_error(double *x, double *y, int n, double m, double b) {
int i;
double error, sum;
sum = 0.0;
for ( i = 0; i < n; i++ ) {
error = y[i] - (m * x[i] + b);
sum += error * error;
// printf("%.2f %.2f\n", error, sum);
}
return ( sum / (double)n );
}
/*
return the maximum least squares error:
(y[i] - y_hat[i])^2
*/
double least_squares_max_error(double *x, double *y, int n, double m, double b){
int i;
double error, max_error;
max_error = 0.0;
for ( i = 0; i < n; i++ ) {
error = y[i] - (m * x[i] + b);
error = error * error;
if ( error > max_error ) {
max_error = error;
}
}
return ( max_error );
}

73
lib/math/leastsqs.hxx Normal file
View File

@@ -0,0 +1,73 @@
// leastsqs.h -- Implements a simple linear least squares best fit routine
//
// Written by Curtis Olson, started September 1997.
//
// Copyright (C) 1997 Curtis L. Olson - curt@infoplane.com
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
//
// $Id$
///
#ifndef _LEASTSQS_H
#define _LEASTSQS_H
#ifndef __cplusplus
# error This library requires C++
#endif
/*
Least squares fit:
y = b0 + b1x
n*sum(xi*yi) - (sum(xi)*sum(yi))
b1 = --------------------------------
n*sum(xi^2) - (sum(xi))^2
b0 = sum(yi)/n - b1*(sum(xi)/n)
*/
void least_squares(double *x, double *y, int n, double *m, double *b);
/* incrimentally update existing values with a new data point */
void least_squares_update(double x, double y, double *m, double *b);
/*
return the least squares error:
(y[i] - y_hat[i])^2
-------------------
n
*/
double least_squares_error(double *x, double *y, int n, double m, double b);
/*
return the maximum least squares error:
(y[i] - y_hat[i])^2
*/
double least_squares_max_error(double *x, double *y, int n, double m, double b);
#endif // _LEASTSQS_H

233
lib/math/mat3.h Normal file
View File

@@ -0,0 +1,233 @@
/* Copyright 1988, Brown Computer Graphics Group. All Rights Reserved. */
/* -------------------------------------------------------------------------
Public MAT3 include file
------------------------------------------------------------------------- */
#ifndef MAT3_HAS_BEEN_INCLUDED
#define MAT3_HAS_BEEN_INCLUDED
/* ----------------------------- Constants ------------------------------ */
/*
* Make sure the math library .h file is included, in case it wasn't.
*/
#ifndef HUGE
#include <math.h>
#endif
#include <stdio.h>
#include <string.h>
#include "Include/fg_memory.h"
#ifdef __cplusplus
extern "C" {
#endif
#define MAT3_DET0 -1 /* Indicates singular mat */
#define MAT3_EPSILON 1e-12 /* Close enough to zero */
#ifdef M_PI
# define MAT3_PI M_PI
#else
# define MAT3_PI 3.14159265358979323846
#endif
#define USE_XTRA_MAT3_INLINES
#if defined(i386)
#define USE_X86_ASM
#endif
#if defined(USE_X86_ASM)
static __inline__ int FloatToInt(float f)
{
int r;
__asm__ ("fistpl %0" : "=m" (r) : "t" (f) : "st");
return r;
}
#elif defined(__MSC__) && defined(__WIN32__)
static __inline int FloatToInt(float f)
{
int r;
_asm {
fld f
fistp r
}
return r;
}
#else
#define FloatToInt(F) ((int) ((F) < 0.0f ? (F)-0.5f : (F)+0.5f))
#endif
/* ------------------------------ Types --------------------------------- */
typedef double MAT3mat[4][4]; /* 4x4 matrix */
typedef double MAT3vec[3]; /* Vector */
typedef double MAT3hvec[4]; /* Vector with homogeneous coord */
/* ------------------------------ Macros -------------------------------- */
extern MAT3mat identityMatrix;
/* Tests if a number is within EPSILON of zero */
#define MAT3_IS_ZERO(N) ((N) < MAT3_EPSILON && (N) > -MAT3_EPSILON)
/* Sets a vector to the three given values */
#define MAT3_SET_VEC(V,X,Y,Z) ((V)[0]=(X), (V)[1]=(Y), (V)[2]=(Z))
/* Tests a vector for all components close to zero */
#define MAT3_IS_ZERO_VEC(V) (MAT3_IS_ZERO((V)[0]) && \
MAT3_IS_ZERO((V)[1]) && \
MAT3_IS_ZERO((V)[2]))
/* Dot product of two vectors */
#define MAT3_DOT_PRODUCT(V1,V2) \
((V1)[0]*(V2)[0] + (V1)[1]*(V2)[1] + (V1)[2]*(V2)[2])
/* Copy one vector to other */
#define MAT3_COPY_VEC(TO,FROM) ((TO)[0] = (FROM)[0], \
(TO)[1] = (FROM)[1], \
(TO)[2] = (FROM)[2])
/* Normalize vector to unit length, using TEMP as temporary variable.
* TEMP will be zero if vector has zero length */
#define MAT3_NORMALIZE_VEC(V,TEMP) \
if ((TEMP = sqrt(MAT3_DOT_PRODUCT(V,V))) > MAT3_EPSILON) { \
TEMP = 1.0 / TEMP; \
MAT3_SCALE_VEC(V,V,TEMP); \
} else TEMP = 0.0
/* Scale vector by given factor, storing result vector in RESULT_V */
#define MAT3_SCALE_VEC(RESULT_V,V,SCALE) \
MAT3_SET_VEC(RESULT_V, (V)[0]*(SCALE), (V)[1]*(SCALE), (V)[2]*(SCALE))
/* Adds vectors V1 and V2, storing result in RESULT_V */
#define MAT3_ADD_VEC(RESULT_V,V1,V2) \
MAT3_SET_VEC(RESULT_V, (V1)[0]+(V2)[0], (V1)[1]+(V2)[1], \
(V1)[2]+(V2)[2])
/* Subtracts vector V2 from V1, storing result in RESULT_V */
#define MAT3_SUB_VEC(RESULT_V,V1,V2) \
MAT3_SET_VEC(RESULT_V, (V1)[0]-(V2)[0], (V1)[1]-(V2)[1], \
(V1)[2]-(V2)[2])
/* Multiplies vectors V1 and V2, storing result in RESULT_V */
#define MAT3_MULT_VEC(RESULT_V,V1,V2) \
MAT3_SET_VEC(RESULT_V, (V1)[0]*(V2)[0], (V1)[1]*(V2)[1], \
(V1)[2]*(V2)[2])
/* Sets RESULT_V to the linear combination of V1 and V2, scaled by
* SCALE1 and SCALE2, respectively */
#define MAT3_LINEAR_COMB(RESULT_V,SCALE1,V1,SCALE2,V2) \
MAT3_SET_VEC(RESULT_V, (SCALE1)*(V1)[0] + (SCALE2)*(V2)[0], \
(SCALE1)*(V1)[1] + (SCALE2)*(V2)[1], \
(SCALE1)*(V1)[2] + (SCALE2)*(V2)[2])
/* Several of the vector macros are useful for homogeneous-coord vectors */
#define MAT3_SET_HVEC(V,X,Y,Z,W) ((V)[0]=(X), (V)[1]=(Y), \
(V)[2]=(Z), (V)[3]=(W))
#define MAT3_COPY_HVEC(TO,FROM) ((TO)[0] = (FROM)[0], \
(TO)[1] = (FROM)[1], \
(TO)[2] = (FROM)[2], \
(TO)[3] = (FROM)[3])
#define MAT3_SCALE_HVEC(RESULT_V,V,SCALE) \
MAT3_SET_HVEC(RESULT_V, (V)[0]*(SCALE), (V)[1]*(SCALE), \
(V)[2]*(SCALE), (V)[3]*(SCALE))
#define MAT3_ADD_HVEC(RESULT_V,V1,V2) \
MAT3_SET_HVEC(RESULT_V, (V1)[0]+(V2)[0], (V1)[1]+(V2)[1], \
(V1)[2]+(V2)[2], (V1)[3]+(V2)[3])
#define MAT3_SUB_HVEC(RESULT_V,V1,V2) \
MAT3_SET_HVEC(RESULT_V, (V1)[0]-(V2)[0], (V1)[1]-(V2)[1], \
(V1)[2]-(V2)[2], (V1)[3]-(V2)[3])
#define MAT3_MULT_HVEC(RESULT_V,V1,V2) \
MAT3_SET_HVEC(RESULT_V, (V1)[0]*(V2)[0], (V1)[1]*(V2)[1], \
(V1)[2]*(V2)[2], (V1)[3]*(V2)[3])
/* ------------------------------ Entries ------------------------------- */
#define MAT3identity(mat) fgmemcpy( mat, identityMatrix, sizeof(MAT3mat) )
#define MAT3zero(mat) fgmemzero( mat, sizeof(MAT3mat) )
#define MAT3copy(to, from) fgmemcpy( to, from, sizeof(MAT3mat) )
#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]; \
}
# 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]); \
fgmemcpy(result_mat, tmp_mat, sizeof(MAT3mat)); \
}
#else // !defined( USE_XTRA_MAT3_INLINES )
/* In MAT3mat.c */
void MAT3mult(MAT3mat result, MAT3mat, MAT3mat);
void MAT3mult_vec(MAT3vec result_vec, MAT3vec vec, MAT3mat mat);
void MAT3cross_product(MAT3vec result,MAT3vec,MAT3vec);
#endif // defined( USE_XTRA_MAT3_INLINES )
/* 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 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 */
int MAT3mult_hvec (MAT3hvec result_vec, MAT3hvec vec, MAT3mat mat, int normalize);
void MAT3perp_vec(MAT3vec result_vec, MAT3vec vec, int is_unit);
#ifdef __cplusplus
}
#endif
#endif /* MAT3_HAS_BEEN_INCLUDED */

56
lib/math/mat3defs.h Normal file
View File

@@ -0,0 +1,56 @@
/* Copyright 1988, Brown Computer Graphics Group. All Rights Reserved. */
#ifndef _MAT3DEFS_H
#define _MAT3DEFS_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdio.h>
/* #include <Math/mat3err.h> */
#include <Math/mat3.h>
/* ----------------------------- Constants ------------------------------ */
#define FALSE 0
#define TRUE 1
#define CNULL ((char *) NULL)
/* ------------------------------ Macros -------------------------------- */
#define ALLOCN(P,T,N,M) \
if ((P = (T *) malloc((unsigned) (N) * sizeof(T))) == NULL) \
ERR_ERROR(MAT3_errid, ERR_FATAL, (ERR_ALLOC1, M)); \
else
#define FREE(P) free((char *) (P))
#define ABS(A) ((A) > 0 ? (A) : -(A))
#define MIN(A,B) ((A) < (B) ? (A) : (B))
#define MAX(A,B) ((A) > (B) ? (A) : (B))
#define SWAP(A,B,T) (T = A, A = B, B = T)
/* Is N within EPS of zero ? */
#define IS_ZERO(N,EPS) ((N) < EPS && (N) > -EPS)
/* Macros for lu routines */
#define LU_PERMUTE(p,i,j) { int LU_T; LU_T = p[i]; p[i] = p[j]; p[j] = LU_T; }
/* ------------------------- Internal Entries ---------------------------- */
/* ------------------------- Global Variables ---------------------------- */
/* extern ERRid *MAT3_errid; */
#ifdef __cplusplus
}
#endif
#endif /* _MAT3DEFS_H */

41
lib/math/mat3err.h Normal file
View File

@@ -0,0 +1,41 @@
#ifndef _MAT3ERR_H
#define _MAT3ERR_H
#ifdef __cplusplus
extern "C" {
#endif
#include "sph_errtypes.h"
#ifdef THINK_C
/* We hide this from gnu's compiler, which doesn't understand it. */
void SPH__error (int errtype, ...);
#endif
#define ERR_ERROR(A,B,C) \
if (1) {char cstr[256]; sprintf C; SPH__error(ERR_MAT3_PACKAGE, cstr); } else
#define ERR_S cstr,"%s\n"
#define ERR_SI cstr,"%s: %d\n"
#define ERR_SS cstr,"%s: %s\n"
#define ERR_SEVERE 0
#define ERR_FATAL 0
#define ERR_ALLOC1 0
typedef int ERRid;
#define ERRregister_package(S) 100
#ifdef __cplusplus
}
#endif
#endif /* _MAT3ERR_H */

341
lib/math/point3d.hxx Normal file
View File

@@ -0,0 +1,341 @@
// point3d.hxx -- a 3d point class.
//
// Adapted from algebra3 by Jean-Francois Doue, started October 1998.
//
// Copyright (C) 1998 Curtis L. Olson - curt@me.umn.edu
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of the
// License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
//
// $Id$
#ifndef _POINT3D_HXX
#define _POINT3D_HXX
#ifndef __cplusplus
# error This library requires C++
#endif
#include <Include/compiler.h>
#ifdef FG_MATH_EXCEPTION_CLASH
# define exception c_exception
#endif
#ifdef FG_HAVE_STD_INCLUDES
# include <iostream>
# include <cassert>
# include <cmath>
#else
# include <iostream.h>
# include <assert.h>
# include <math.h>
#endif
// I don't understand ... <math.h> or <cmath> should be included
// already depending on how you defined FG_HAVE_STD_INCLUDES, but I
// can go ahead and add this -- CLO
#ifdef __MWERKS__
FG_USING_NAMESPACE(std);
#endif
#ifndef FG_HAVE_NATIVE_SGI_COMPILERS
FG_USING_STD(ostream);
FG_USING_STD(istream);
#endif
const double fgPoint3_Epsilon = 0.0000001;
enum {PX, PY, PZ}; // axes
// Kludge for msvc++ 6.0 - requires forward decls of friend functions.
class Point3D;
istream& operator>> ( istream&, Point3D& );
ostream& operator<< ( ostream&, const Point3D& );
Point3D operator- (const Point3D& p); // -p1
bool operator== (const Point3D& a, const Point3D& b); // p1 == p2?
///////////////////////////
//
// 3D Point
//
///////////////////////////
class Point3D {
protected:
double n[3];
public:
// Constructors
Point3D();
Point3D(const double x, const double y, const double z);
explicit Point3D(const double d);
Point3D(const Point3D &p);
// Assignment operators
Point3D& operator = ( const Point3D& p ); // assignment of a Point3D
Point3D& operator += ( const Point3D& p ); // incrementation by a Point3D
Point3D& operator -= ( const Point3D& p ); // decrementation by a Point3D
Point3D& operator *= ( const double d ); // multiplication by a constant
Point3D& operator /= ( const double d ); // division by a constant
void setx(const double x);
void sety(const double y);
void setz(const double z);
// Queries
double& operator [] ( int i); // indexing
double operator[] (int i) const; // read-only indexing
inline const double *get_n() const { return n; };
double x() const; // cartesian x
double y() const; // cartesian y
double z() const; // cartesian z
double lon() const; // polar longitude
double lat() const; // polar latitude
double radius() const; // polar radius
double elev() const; // geodetic elevation (if specifying a surface point)
// friends
friend Point3D operator - (const Point3D& p); // -p1
friend bool operator == (const Point3D& a, const Point3D& b); // p1 == p2?
friend istream& operator>> ( istream&, Point3D& );
friend ostream& operator<< ( ostream&, const Point3D& );
// Special functions
double distance3D(const Point3D& a) const; // distance between
double distance3Dsquared(const Point3D& a) const; // distance between ^ 2
};
// input from stream
inline istream&
operator >> ( istream& in, Point3D& p)
{
char c;
in >> p.n[PX];
// read past optional comma
while ( in.get(c) ) {
if ( (c != ' ') && (c != ',') ) {
// push back on the stream
in.putback(c);
break;
}
}
in >> p.n[PY];
// read past optional comma
while ( in.get(c) ) {
if ( (c != ' ') && (c != ',') ) {
// push back on the stream
in.putback(c);
break;
}
}
in >> p.n[PZ];
return in;
}
inline ostream&
operator<< ( ostream& out, const Point3D& p )
{
return out << p.n[PX] << ", " << p.n[PY] << ", " << p.n[PZ];
}
///////////////////////////
//
// Point3D Member functions
//
///////////////////////////
// CONSTRUCTORS
inline Point3D::Point3D() {}
inline Point3D::Point3D(const double x, const double y, const double z)
{
n[PX] = x; n[PY] = y; n[PZ] = z;
}
inline Point3D::Point3D(const double d)
{
n[PX] = n[PY] = n[PZ] = d;
}
inline Point3D::Point3D(const Point3D& p)
{
n[PX] = p.n[PX]; n[PY] = p.n[PY]; n[PZ] = p.n[PZ];
}
// ASSIGNMENT OPERATORS
inline Point3D& Point3D::operator = (const Point3D& p)
{
n[PX] = p.n[PX]; n[PY] = p.n[PY]; n[PZ] = p.n[PZ]; return *this;
}
inline Point3D& Point3D::operator += ( const Point3D& p )
{
n[PX] += p.n[PX]; n[PY] += p.n[PY]; n[PZ] += p.n[PZ]; return *this;
}
inline Point3D& Point3D::operator -= ( const Point3D& p )
{
n[PX] -= p.n[PX]; n[PY] -= p.n[PY]; n[PZ] -= p.n[PZ]; return *this;
}
inline Point3D& Point3D::operator *= ( const double d )
{
n[PX] *= d; n[PY] *= d; n[PZ] *= d; return *this;
}
inline Point3D& Point3D::operator /= ( const double d )
{
double d_inv = 1./d; n[PX] *= d_inv; n[PY] *= d_inv; n[PZ] *= d_inv;
return *this;
}
inline void Point3D::setx(const double x) {
n[PX] = x;
}
inline void Point3D::sety(const double y) {
n[PY] = y;
}
inline void Point3D::setz(const double z) {
n[PZ] = z;
}
// QUERIES
inline double& Point3D::operator [] ( int i)
{
assert(! (i < PX || i > PZ));
return n[i];
}
inline double Point3D::operator [] ( int i) const {
assert(! (i < PX || i > PZ));
return n[i];
}
inline double Point3D::x() const { return n[PX]; }
inline double Point3D::y() const { return n[PY]; }
inline double Point3D::z() const { return n[PZ]; }
inline double Point3D::lon() const { return n[PX]; }
inline double Point3D::lat() const { return n[PY]; }
inline double Point3D::radius() const { return n[PZ]; }
inline double Point3D::elev() const { return n[PZ]; }
// FRIENDS
inline Point3D operator - (const Point3D& a)
{
return Point3D(-a.n[PX],-a.n[PY],-a.n[PZ]);
}
inline Point3D operator + (const Point3D& a, const Point3D& b)
{
return Point3D(a) += b;
}
inline Point3D operator - (const Point3D& a, const Point3D& b)
{
return Point3D(a) -= b;
}
inline Point3D operator * (const Point3D& a, const double d)
{
return Point3D(a) *= d;
}
inline Point3D operator * (const double d, const Point3D& a)
{
return a*d;
}
inline Point3D operator / (const Point3D& a, const double d)
{
return Point3D(a) *= (1.0 / d );
}
inline bool operator == (const Point3D& a, const Point3D& b)
{
return
fabs(a.n[PX] - b.n[PX]) < fgPoint3_Epsilon &&
fabs(a.n[PY] - b.n[PY]) < fgPoint3_Epsilon &&
fabs(a.n[PZ] - b.n[PZ]) < fgPoint3_Epsilon;
}
inline bool operator != (const Point3D& a, const Point3D& b)
{
return !(a == b);
}
// Special functions
inline double
Point3D::distance3D(const Point3D& a ) const
{
double x, y, z;
x = n[PX] - a.n[PX];
y = n[PY] - a.n[PY];
z = n[PZ] - a.n[PZ];
return sqrt(x*x + y*y + z*z);
}
inline double
Point3D::distance3Dsquared(const Point3D& a ) const
{
double x, y, z;
x = n[PX] - a.n[PX];
y = n[PY] - a.n[PY];
z = n[PZ] - a.n[PZ];
return(x*x + y*y + z*z);
}
#endif // _POINT3D_HXX

61
lib/math/polar3d.cxx Normal file
View File

@@ -0,0 +1,61 @@
// polar.cxx -- routines to deal with polar math and transformations
//
// Written by Curtis Olson, started June 1997.
//
// Copyright (C) 1997 Curtis L. Olson - curt@infoplane.com
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of the
// License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
//
// $Id$
#include <math.h>
#include <stdio.h>
#include <Include/fg_constants.h>
#include "polar3d.hxx"
// Find the Altitude above the Ellipsoid (WGS84) given the Earth
// Centered Cartesian coordinate vector Distances are specified in
// meters.
double fgGeodAltFromCart(const Point3D& 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);
}

67
lib/math/polar3d.hxx Normal file
View File

@@ -0,0 +1,67 @@
// polar.hxx -- routines to deal with polar math and transformations
//
// Written by Curtis Olson, started June 1997.
//
// Copyright (C) 1997 Curtis L. Olson - curt@infoplane.com
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of the
// License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
//
// $Id$
#ifndef _POLAR_HXX
#define _POLAR_HXX
#ifndef __cplusplus
# error This library requires C++
#endif
#include <Include/fg_constants.h>
#include <Math/point3d.hxx>
// Find the Altitude above the Ellipsoid (WGS84) given the Earth
// Centered Cartesian coordinate vector Distances are specified in
// meters.
double fgGeodAltFromCart(const Point3D& cp);
// 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
inline Point3D fgPolarToCart3d(const Point3D& p) {
double tmp = cos( p.lat() ) * p.radius();
return Point3D( cos( p.lon() ) * tmp,
sin( p.lon() ) * tmp,
sin( p.lat() ) * p.radius() );
}
// Convert a cartesian coordinate to polar coordinates (lon/lat
// specified in radians. Distances are specified in meters.
inline Point3D fgCartToPolar3d(const Point3D& cp) {
return Point3D( atan2( cp.y(), cp.x() ),
FG_PI_2 -
atan2( sqrt(cp.x()*cp.x() + cp.y()*cp.y()), cp.z() ),
sqrt(cp.x()*cp.x() + cp.y()*cp.y() + cp.z()*cp.z()) );
}
#endif // _POLAR_HXX

129
lib/math/vector.cxx Normal file
View File

@@ -0,0 +1,129 @@
// vector.cxx -- additional vector routines
//
// Written by Curtis Olson, started December 1997.
//
// Copyright (C) 1997 Curtis L. Olson - curt@infoplane.com
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of the
// License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
//
// $Id$
#include <math.h>
#include <stdio.h>
// #include <Include/fg_types.h>
#include "vector.hxx"
#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)
{
MAT3vec u1, v, tmp;
// calculate a vector "u1" representing the shortest distance from
// the plane specified by normal and v0 to a point specified by
// "vec". "u1" represents both the direction and magnitude of
// this desired distance.
// u1 = ( (normal <dot> vec) / (normal <dot> normal) ) * normal
MAT3_SCALE_VEC( u1,
normal,
( MAT3_DOT_PRODUCT(normal, vec) /
MAT3_DOT_PRODUCT(normal, normal)
)
);
// printf(" vec = %.2f, %.2f, %.2f\n", vec[0], vec[1], vec[2]);
// printf(" v0 = %.2f, %.2f, %.2f\n", v0[0], v0[1], v0[2]);
// printf(" u1 = %.2f, %.2f, %.2f\n", u1[0], u1[1], u1[2]);
// calculate the vector "v" which is the vector "vec" mapped onto
// the plane specified by "normal" and "v0".
// v = v0 + vec - u1
MAT3_ADD_VEC(tmp, v0, vec);
MAT3_SUB_VEC(v, tmp, u1);
// printf(" v = %.2f, %.2f, %.2f\n", v[0], v[1], v[2]);
// Calculate the vector "result" which is "v" - "v0" which is a
// directional vector pointing from v0 towards v
// result = v - v0
MAT3_SUB_VEC(result, v, v0);
// 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;
// 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);
return sqrt(MAT3_DOT_PRODUCT(v, v));
}
// Given a point p, and a line through p0 with direction vector d,
// find the shortest distance (squared) from the point to the line
double fgPointLineSquared(MAT3vec p, MAT3vec p0, MAT3vec d) {
MAT3vec u, u1, v;
double ud, dd, tmp;
// 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);
return ( MAT3_DOT_PRODUCT(v, v) );
}

63
lib/math/vector.hxx Normal file
View File

@@ -0,0 +1,63 @@
// vector.hxx -- additional vector routines
//
// Written by Curtis Olson, started December 1997.
//
// Copyright (C) 1997 Curtis L. Olson - curt@infoplane.com
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of the
// License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
//
// $Id$
#ifndef _VECTOR_HXX
#define _VECTOR_HXX
#ifndef __cplusplus
# error This library requires C++
#endif
#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
double fgPointLineSquared(MAT3vec p, MAT3vec p0, MAT3vec d);
#endif // _VECTOR_HXX