452 lines
14 KiB
C
452 lines
14 KiB
C
/*
|
|
geometry.c
|
|
Copyright (C) 2003 Antoine Drouin
|
|
|
|
This file is part of paparazzi.
|
|
|
|
paparazzi 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, or (at your option)
|
|
any later version.
|
|
|
|
paparazzi 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 paparazzi; see the file COPYING. If not, write to
|
|
the Free Software Foundation, 59 Temple Place - Suite 330,
|
|
Boston, MA 02111-1307, USA.
|
|
*/
|
|
|
|
#include "geometry.h"
|
|
|
|
//#include <math.h>
|
|
#include "math_papabench.h"
|
|
|
|
const struct ellipsoid ellipsoids[ ELLIPSOID_NB ] = {
|
|
{-168., -60., 320., 6378249.2, 0.0034075495234250643, 0.08248325676}, /* NTF */
|
|
{ 0., 0., 0., 6378137.0, 0.0033528106647474805, 0.08181919106}, /* WGS84 */
|
|
{-87.0, -98., -121.0, 6378388.0, 0.003367003367003367, 0.08199188998}, /* ED50 */
|
|
{ 0.0, 125.0, 194.0, 6378067.6, 0.0033155460257, 0.08181919106} /* NAD27 */
|
|
};
|
|
/* From http://www.tandt.be/wis/WiS/eqntf.html and http://www.ign.fr/MP/GEOD/geodesie/coordonnees.html */
|
|
const struct lambert lamberts[ LAMBERT_NB ] = {
|
|
{ /* LAMBERTI */
|
|
&ellipsoids[ NTF ],
|
|
RAD_OF_DEG( DECIMAL( 49., 30., 0. ) ), /* phi0 */
|
|
0.991996665, /* lphi0 */
|
|
5457616.674, /* r0 */
|
|
RAD_OF_DEG( DECIMAL( 2., 20., 14.025 ) ), /* lambda0 */
|
|
600000, /* x0 */
|
|
200000, /* y0 */
|
|
5657617, /* ys */
|
|
0.99987734 /* k0 */
|
|
},
|
|
{ /* LAMBERTII */
|
|
&ellipsoids[ NTF ],
|
|
RAD_OF_DEG( DECIMAL( 46., 48., 0. ) ), /* phi0 */
|
|
0.921557361, /* lphi0 */
|
|
5999695.77, /* r0 */
|
|
RAD_OF_DEG( DECIMAL( 2., 20., 14.025 ) ), /* lambda0 */
|
|
600000, /* x0 */
|
|
2200000, /* y0 */
|
|
6199696, /* ys */
|
|
0.99987742 /* k0 */
|
|
},
|
|
{ /* LAMBERTIIE */
|
|
&ellipsoids[ NTF ],
|
|
RAD_OF_DEG( DECIMAL( 46., 48., 0. ) ), /* phi0 */
|
|
0.921557361, /* lphi0 */
|
|
5999695.77, /* r0 */
|
|
RAD_OF_DEG( DECIMAL( 2., 20., 14.025 ) ), /* lambda0 */
|
|
600000, /* x0 */
|
|
2200000, /* y0 */
|
|
8199696, /* ys */
|
|
0.99987742 /* k0 */
|
|
},
|
|
{ /* LAMBERTIII */
|
|
&ellipsoids[ NTF ],
|
|
RAD_OF_DEG( DECIMAL( 44., 6., 0. ) ), /* phi0 */
|
|
0.921557361, /* lphi0 */
|
|
6591905.08, /* r0 */
|
|
RAD_OF_DEG( DECIMAL( 2., 20., 14.025 ) ), /* lambda0 */
|
|
600000, /* x0 */
|
|
3200000, /* y0 */
|
|
6791905, /* ys */
|
|
0.99987750 /* k0 */
|
|
},
|
|
{ /* LAMBERTIV */
|
|
&ellipsoids[ NTF ],
|
|
RAD_OF_DEG( DECIMAL( 42., 9., 54. ) ), /* phi0 */
|
|
0.921557361, /* lphi0 */
|
|
7053300.18, /* r0 */
|
|
RAD_OF_DEG( DECIMAL( 2., 20., 14.025 ) ), /* lambda0 */
|
|
234, /* x0 */ /* FIXME mmmmmmm suspicious */
|
|
4185861, /* y0 */
|
|
7239162, /* ys */
|
|
0.99994471 /* k0 */
|
|
},
|
|
};
|
|
|
|
double distance_c2d( const struct double_c2d a, const struct double_c2d b )
|
|
{
|
|
return sqrt( ( a.x - b.x ) * ( a.x - b.x ) + ( a.y - b.y ) * ( a.y - b.y ) );
|
|
}
|
|
|
|
struct double_c2d vector_c2d( const struct double_c2d a,
|
|
const struct double_c2d b )
|
|
{
|
|
struct double_c2d v;
|
|
v.x = b.x - a.x;
|
|
v.y = b.y - a.y;
|
|
return v;
|
|
}
|
|
|
|
struct double_c2d unit_vector_c2d( const struct double_c2d a,
|
|
const struct double_c2d b )
|
|
{
|
|
struct double_c2d v = vector_c2d( a, b );
|
|
scale_c2d_s( &v, 1 / module_c2d( v ) );
|
|
return v;
|
|
}
|
|
|
|
struct double_c2d normal_vector_c2d( const struct double_c2d u )
|
|
{
|
|
struct double_c2d v;
|
|
v.x = -u.y;
|
|
v.y = u.x;
|
|
return v;
|
|
}
|
|
|
|
double vect_prod_c2d( const struct double_c2d a, const struct double_c2d b )
|
|
{
|
|
return a.x * b.y - a.y * b.x;
|
|
}
|
|
|
|
double scal_prod_c2d( const struct double_c2d a, const struct double_c2d b )
|
|
{
|
|
return a.x * b.x + a.y * b.y;
|
|
}
|
|
|
|
void rotate_c2d( struct double_c2d *a, double alpha )
|
|
{
|
|
double tmp;
|
|
tmp = a->x * cos( alpha ) - a->y * sin( alpha );
|
|
a->y = a->x * sin( alpha ) + a->y * cos( alpha );
|
|
a->x = tmp;
|
|
}
|
|
|
|
double norme2_c2d( const struct double_c2d a )
|
|
{
|
|
return a.x * a.x + a.y * a.y;
|
|
}
|
|
|
|
double module_c2d( const struct double_c2d a )
|
|
{
|
|
return sqrt( norme2_c2d( a ) );
|
|
}
|
|
|
|
void translate_c2d( struct double_c2d *a, const struct double_c2d v )
|
|
{
|
|
add_c2d_s( a, v );
|
|
}
|
|
|
|
void add_c2d_s( struct double_c2d *a, const struct double_c2d b )
|
|
{
|
|
a->x = a->x + b.x;
|
|
a->y = a->y + b.y;
|
|
}
|
|
|
|
struct double_c2d add_c2d( struct double_c2d a, const struct double_c2d b )
|
|
{
|
|
add_c2d_s( &a, b );
|
|
return a;
|
|
}
|
|
|
|
void normalise_c2d_s( struct double_c2d *a )
|
|
{
|
|
scale_c2d_s( a, 1 / module_c2d( *a ) );
|
|
}
|
|
|
|
struct double_c2d normalise_c2d( struct double_c2d a )
|
|
{
|
|
normalise_c2d_s( &a );
|
|
return a;
|
|
}
|
|
|
|
void minus_c2d_s( struct double_c2d *a, const struct double_c2d b )
|
|
{
|
|
a->x = a->x - b.x;
|
|
a->y = a->y - b.y;
|
|
}
|
|
|
|
struct double_c2d minus_c2d( struct double_c2d a, const struct double_c2d b )
|
|
{
|
|
minus_c2d_s( &a, b );
|
|
return a;
|
|
}
|
|
|
|
void scale_c2d_s( struct double_c2d *v, double factor )
|
|
{
|
|
v->x = v->x * factor;
|
|
v->y = v->y * factor;
|
|
}
|
|
|
|
struct double_c2d scale_c2d( struct double_c2d v, double factor )
|
|
{
|
|
scale_c2d_s( &v, factor );
|
|
return v;
|
|
}
|
|
|
|
|
|
|
|
#define MPI_PI(val)\
|
|
{\
|
|
while (val> M_PI) val -= 2. * M_PI;\
|
|
while (val<-M_PI) val += 2. * M_PI;\
|
|
}
|
|
|
|
double mpi_pi( double val )
|
|
{
|
|
MPI_PI( val );
|
|
return val;
|
|
};
|
|
|
|
struct double_c2d lin_interp_c2d( struct double_c2d a, double ta,
|
|
struct double_c2d b, double tb, double t )
|
|
{
|
|
double delta_t = tb - ta;
|
|
struct double_c2d res;
|
|
res.x = a.x + ( t - ta ) / delta_t *( b.x - a.x );
|
|
res.y = a.y + ( t - ta ) / delta_t *( b.y - a.y );
|
|
return res;
|
|
}
|
|
|
|
double wind_dir_from_angle_deg( double deg )
|
|
{
|
|
double w = 180. + 90. - deg;
|
|
while ( w > 360 ) w -= 360;
|
|
return w;
|
|
}
|
|
|
|
double wind_dir_from_angle_rad( double rad )
|
|
{
|
|
double w = M_PI + M_PI_2 - rad;
|
|
while ( w > 2 * M_PI ) w -= ( 2 * M_PI );
|
|
return w;
|
|
}
|
|
|
|
double heading_of_to_angle_deg( double angle )
|
|
{
|
|
double a = 90. - angle + 360.;
|
|
while ( a >= 180. ) a -= 360.;
|
|
return a;
|
|
}
|
|
|
|
double heading_of_to_angle_rad( double angle )
|
|
{
|
|
double two_pi = 2 * M_PI;
|
|
double a = M_PI_2 - angle + two_pi;
|
|
while ( a >= two_pi ) a -= two_pi;
|
|
return a;
|
|
}
|
|
|
|
|
|
|
|
#define G 9.81
|
|
#define ACCEL_DOWN 0
|
|
double compute_phi( struct double_c2d curspeed, struct double_c2d last_speed,
|
|
double delta_t )
|
|
{
|
|
double accel_norm, accel_tgt, phi;
|
|
struct double_c2d accel = minus_c2d( curspeed, last_speed );
|
|
// printf("%lf %lf %lf %lf %lf\n", curspeed.x, curspeed.y, last_speed.x, last_speed.y, delta_t);
|
|
scale_c2d_s( &accel, 1 / delta_t );
|
|
accel_norm = vect_prod_c2d( accel, curspeed ) / module_c2d( curspeed );
|
|
accel_tgt = scal_prod_c2d( accel, curspeed ) / module_c2d( curspeed );
|
|
phi = -atan2( accel_norm, G - ACCEL_DOWN );
|
|
return phi;
|
|
}
|
|
|
|
double angle_c2d( struct double_c2d u, struct double_c2d v )
|
|
{
|
|
double alpha1 = atan2( u.y, u.x );
|
|
double alpha2 = atan2( v.y, v.x );
|
|
double alpha = alpha2 - alpha1;
|
|
MPI_PI( alpha );
|
|
return alpha;
|
|
}
|
|
|
|
struct double_p2d p2d_of_c2d( const struct double_c2d c )
|
|
{
|
|
struct double_p2d p;
|
|
p.r = module_c2d( c );
|
|
p.teta = atan2( c.y, c.x );
|
|
return p;
|
|
}
|
|
|
|
struct double_c2d c2d_of_p2d( const struct double_p2d p )
|
|
{
|
|
struct double_c2d c;
|
|
c.x = p.r * cos( p.teta );
|
|
c.y = p.r * sin( p.teta );
|
|
return c;
|
|
}
|
|
|
|
struct double_p2d add_p2d( const struct double_p2d p1,
|
|
const struct double_p2d p2 )
|
|
{
|
|
/* p.r = sqrt(p1.r*p1.r + p2.r*p2.r + 2 * p1.r * p2.r * cos ( p1.teta - p2.teta )); */
|
|
/* p.teta = 0; */
|
|
struct double_c2d c1 = c2d_of_p2d( p1 );
|
|
struct double_c2d c2 = c2d_of_p2d( p2 );
|
|
struct double_c2d c3 = add_c2d( c1, c2 );
|
|
struct double_p2d p = p2d_of_c2d( c3 );
|
|
return p;
|
|
}
|
|
|
|
|
|
struct double_p2d minus_p2d( const struct double_p2d p1,
|
|
const struct double_p2d p2 )
|
|
{
|
|
struct double_c2d c1 = c2d_of_p2d( p1 );
|
|
struct double_c2d c2 = c2d_of_p2d( p2 );
|
|
struct double_c2d c3 = minus_c2d( c1, c2 );
|
|
struct double_p2d p = p2d_of_c2d( c3 );
|
|
return p;
|
|
}
|
|
|
|
|
|
struct double_c2d c2d_of_polar( double r, double teta )
|
|
{
|
|
struct double_c2d res = {r * cos( teta ), r * sin( teta )};
|
|
return res;
|
|
}
|
|
|
|
void translate_c3d( struct double_c3d *a, const struct double_c3d v )
|
|
{
|
|
a->x = a->x + v.x;
|
|
a->y = a->y + v.y;
|
|
a->z = a->z + v.z;
|
|
}
|
|
|
|
void scale_c3d( struct double_c3d *v, double factor )
|
|
{
|
|
v->x = v->x * factor;
|
|
v->y = v->y * factor;
|
|
v->z = v->z * factor;
|
|
}
|
|
|
|
static double inverse_latitude_isometrique( double lat, double e,
|
|
double epsilon )
|
|
{
|
|
double exp_l = exp( lat );
|
|
double phi0 = 2. * atan( exp_l - M_PI_2 );
|
|
double loop( double phi ) {
|
|
double sin_phi = e * sin( phi );
|
|
double new_phi = 2. * atan ( pow( ( 1. + sin_phi ) / ( 1. - sin_phi ),
|
|
e / 2 ) * exp_l ) - M_PI_2;
|
|
if ( fabs( phi - new_phi ) < epsilon ) return new_phi;
|
|
return loop( new_phi );
|
|
}
|
|
return loop( phi0 );
|
|
}
|
|
|
|
static double lambert_n ( const enum type_lambert lmb )
|
|
{
|
|
return sin( lamberts[ lmb ].phi0 );
|
|
}
|
|
static double lambert_c ( const enum type_lambert lmb )
|
|
{
|
|
return lamberts[ lmb ].r0 * exp( lamberts[ lmb ].phi0 * lambert_n( lmb ) );
|
|
}
|
|
|
|
void lambert_of_e2d ( const enum type_lambert lmb,
|
|
const enum type_ellipsoid elps,
|
|
struct double_e2d e2d_pos, struct double_c2d *lmb_pos )
|
|
{
|
|
double n, ll, r, gamma;
|
|
e2d_of_e2d( NTF, elps, &e2d_pos );
|
|
n = lambert_n( lmb );
|
|
ll = LATITUDE_ISOMETRIQUE( e2d_pos.lat, lamberts[ lmb ].ellipsoid->e );
|
|
r = lamberts[ lmb ].r0 * exp( lamberts[ lmb ].lphi0 * sin( lamberts[ lmb ].phi0 ) ) *
|
|
exp( -ll * n );
|
|
gamma = ( e2d_pos.lon - lamberts[ lmb ].lambda0 ) * n;
|
|
lmb_pos->x = ( double )lamberts[ lmb ].x0 + r * sin( gamma );
|
|
lmb_pos->y = ( double )lamberts[ lmb ].ys - r * cos( gamma );
|
|
}
|
|
|
|
void e2d_of_lambert ( const enum type_lambert lmb,
|
|
const enum type_ellipsoid elps,
|
|
struct double_c2d c_pos, struct double_e2d *e_pos )
|
|
{
|
|
double dx = c_pos.x - lamberts[ lmb ].x0;
|
|
double dy = c_pos.y - lamberts[ lmb ].ys;
|
|
double r = sqrt( dx * dx + dy * dy );
|
|
double gamma = atan2( dx, -dy );
|
|
double lambda = lamberts[ lmb ].lambda0 + gamma / lambert_n( lmb );
|
|
double ll = -1. / lambert_n( lmb ) * log( fabs( r / lambert_c( lmb ) ) );
|
|
double phi = inverse_latitude_isometrique( ll, lamberts[ lmb ].ellipsoid->e,
|
|
1e-11 );
|
|
e_pos->lon = lambda;
|
|
e_pos->lat = phi;
|
|
e2d_of_e2d( elps, NTF, e_pos );
|
|
}
|
|
|
|
void e2d_of_e2d( const enum type_ellipsoid dest, const enum type_ellipsoid src,
|
|
struct double_e2d *e2d_pos )
|
|
{
|
|
if ( dest != src ) {
|
|
double sin_lat = sin( e2d_pos->lat );
|
|
double cos_lat = cos( e2d_pos->lat );
|
|
double sin_lon = sin( e2d_pos->lon );
|
|
double cos_lon = cos( e2d_pos->lon );
|
|
double d16 = E_SQUARE( ellipsoids[ src ].df );
|
|
double d17 = E_SQUARE( ellipsoids[ dest ].df );
|
|
double d18 = ellipsoids[ src ].a / sqrt ( 1.0 - d16 * sin_lat * sin_lat );
|
|
double d20 = d18 * cos_lat * cos_lon;
|
|
double d21 = d18 * cos_lat * sin_lon;
|
|
double d22 = d18 * ( 1.0 - d16 ) * sin_lat;
|
|
double d23 = d20 - ellipsoids[ dest ].dx + ellipsoids[ src ].dx;
|
|
double d24 = d21 - ellipsoids[ dest ].dy + ellipsoids[ src ].dy;
|
|
double d25 = d22 - ellipsoids[ dest ].dz + ellipsoids[ src ].dz;
|
|
double d26 = sqrt ( d23 * d23 + d24 * d24 );
|
|
double d27 = atan2( d25, d26 * ( 1.0 - ellipsoids[ dest ].df ) );
|
|
double d28 = ellipsoids[ dest ].a * ( 1.0 - ellipsoids[ dest ].df );
|
|
double d29 = E_PRIME_SQUARE( ellipsoids[ dest ].df );
|
|
double d3 = atan2( ( d25 + d29 * d28 * sin( d27 ) * sin( d27 ) * sin( d27 ) ),
|
|
( d26 - d17 * ellipsoids[ dest ].a * cos( d27 ) * cos( d27 ) * cos( d27 ) ) );
|
|
double d4 = atan2( d24, d23 );
|
|
e2d_pos->lon = d4;
|
|
e2d_pos->lat = d3;
|
|
}
|
|
}
|
|
|
|
|
|
/*
|
|
converts 3d cartesian coordinate to 3d ellipsoidal
|
|
*/
|
|
void e3d_of_c3d( const enum type_ellipsoid elps, struct double_c3d c_pos,
|
|
struct double_e3d *e_pos )
|
|
{
|
|
#define WGS_84_SMA 6378137.00
|
|
#define WGS_84_SMB 6356752.31
|
|
#define E2 ((WGS_84_SMA*WGS_84_SMA-WGS_84_SMB*WGS_84_SMB)/(WGS_84_SMA*WGS_84_SMA))
|
|
#define PREC 0.000000000001
|
|
double dist = sqrt( c_pos.x * c_pos.x + c_pos.y * c_pos.y );
|
|
double v, old_phi, new_phi = atan2( ( double )c_pos.z, dist * ( 1 - E2 ) );
|
|
do {
|
|
old_phi = new_phi;
|
|
v = WGS_84_SMA / sqrt( 1 - ( E2 * sin( old_phi ) * sin( old_phi ) ) );
|
|
new_phi = atan2( ( ( double )c_pos.z + E2 * v * sin ( old_phi ) ), dist );
|
|
} while ( fabs( new_phi - old_phi ) > PREC );
|
|
e_pos->lat = new_phi;
|
|
e_pos->lon = atan2( c_pos.y, c_pos.x );
|
|
e_pos->h = ( dist / cos ( new_phi ) ) - v;
|
|
}
|
|
|
|
|
|
|