Add wasm tacle-bench targets
This commit is contained in:
451
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/geometry.c
Normal file
451
targets/wasm-tacle/parallel/PapaBench/sw/lib/c/geometry.c
Normal file
@ -0,0 +1,451 @@
|
||||
/*
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user