source: flair-src/trunk/lib/FlairSensorActuator/src/unexported/geodesie.h@ 15

// %flair:license{

// This file is part of the Flair framework distributed under the

// CECILL-C License, Version 1.0.

// %flair:license}

#ifndef GEODESIE_H

#define GEODESIE_H



#include <cmath>

#include <iostream>

#include <vector>



namespace Geodesie {



#ifndef M_PI

#define M_PI 3.14159265358979323846

#endif

#ifndef M_PI_2

#define M_PI_2 1.57079632679489661923

#endif

#ifndef M_PI_4

#define M_PI_4 0.78539816339744830962

#endif



////////////////////////////////////////////////////////////////////////

struct Matrice {

  Matrice(const Matrice &A);

  Matrice();

  void Apply(double v0, double v1, double v2, double &Mv0, double &Mv1,

             double &Mv2);

  double c0_l0;

  double c1_l0;

  double c2_l0;

  double c0_l1;

  double c1_l1;

  double c2_l1;

  double c0_l2;

  double c1_l2;

  double c2_l2;

}; // class



Matrice TransMat(const Matrice A);



Matrice ProdMat(const Matrice A, const Matrice B);

void Write(const Matrice A, std::ostream &out);



////////////////////////////////////////////////////////////////////////

class Raf98 {

private:

  std::vector<double> m_dvalues;

  double LitGrille(unsigned int c, unsigned int l) const;



public:

  ~Raf98();

  Raf98() {}

  bool Load(const std::string &s);

  bool Interpol(double longitude /*deg*/, double latitude /*deg*/,

                double *Hwgs84) const;

}; // class

////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////

inline double Deg2Rad(double deg) { return deg * M_PI / 180.0; }

inline double Rad2Deg(double rad) { return rad * 180.0 / M_PI; }

////////////////////////////////////////////////////////////////////////



const double a_Lambert93 = 6378137;

const double f_Lambert93 = 1 / 298.257222101;

const double e_Lambert93 = sqrt(f_Lambert93 * (2 - f_Lambert93));

const double lambda0_Lambert93 = Deg2Rad(3.0); // degres

const double phi0_Lambert93 = Deg2Rad(46.5);

const double phi1_Lambert93 = Deg2Rad(44.0);

const double phi2_Lambert93 = Deg2Rad(49.0); // degres

const double X0_Lambert93 = 700000;  //

const double Y0_Lambert93 = 6600000; //

const double n_Lambert93 = 0.7256077650;

const double c_Lambert93 = 11754255.426;

const double xs_Lambert93 = 700000;

const double ys_Lambert93 = 12655612.050;



const double GRS_a = 6378137;

const double GRS_f = 1 / 298.257222101;

const double GRS_b = GRS_a * (1 - GRS_f);

const double GRS_e = sqrt((pow(GRS_a, 2) - pow(GRS_b, 2)) / pow(GRS_a, 2));



////////////////////////////////////////////////////////////////////////

void Geographique_2_Lambert93(const Raf98 &raf98, double lambda, double phi,

                              double he, Matrice in, double &E, double &N,

                              double &h, Matrice &out);

void Geographique_2_Lambert93(const Raf98 &raf98, double lambda, double phi,

                              double he, double &E, double &N, double &h);

void Lambert93_2_Geographique(const Raf98 &raf98, double E, double N, double h,

                              double &lambda, double &phi, double &he);

void Lambert93_2_Geographique(const Raf98 &raf98, double E, double N, double h,

                              Matrice in, double &lambda, double &phi,

                              double &he, Matrice &out);

/** Convert from geographique to ECEF.

 * @param[in] longitude Longitude in radian.

 * @param[in] latitude Latitude in radian.

 * @param[in] he Height in meter.

 */

void Geographique_2_ECEF(double longitude, double latitude, double he,

                         double &x, double &y, double &z);

/** Convert from ECEF two ENU.

 * @param[in] lon0 Longitude of the origin in radian.

 * @param[in] lat0 Latitude of the origin in radian.

 * @param[in] he0 Height of the origin in radian.

 */

void ECEF_2_ENU(double x, double y, double z, double &e, double &n, double &u,

                double lon0, double lat0, double he0);

////////////////////////////////////////////////////////////////////////



// ALGO0001

double LatitueIsometrique(double latitude, double e);

// ALGO0002

double LatitueIsometrique2Lat(double latitude_iso, double e, double epsilon);



// ALGO0003

void Geo2ProjLambert(double lambda, double phi, double n, double c, double e,

                     double lambdac, double xs, double ys, double &X,

                     double &Y);

// ALGO0004

void Proj2GeoLambert(double X, double Y, double n, double c, double e,

                     double lambdac, double xs, double ys, double epsilon,

                     double &lambda, double &phi);



double ConvMerApp(double longitude);



/**

Converts Cartesian (x, y) coordinates to polar coordinates (r, theta)

*/

template <typename _T1, typename _T2>

void cartesianToPolar(const _T1 x, const _T1 y, _T2 &r, _T2 &theta) {

  r = std::sqrt(x * x + y * y);

  theta = std::atan2(x, y);

}



/**

Converts polar coordinates (r, theta) to Cartesian (x, y) coordinates

*/

template <typename _T1, typename _T2>

void polarToCartesian(const _T1 r, const _T1 theta, _T2 &x, _T2 &y) {

  x = r * std::cos(theta);

  y = r * std::sin(theta);

}



/**

Converts Cartesian (x, y, z) coordinates to spherical coordinates (r, theta,

phi)

Angles expressed in radians.

*/

template <typename _T1, typename _T2>

void cartesianToSpherical(const _T1 x, const _T1 y, const _T1 z, _T2 &r,

                          _T2 &theta, _T2 &phi) {

  r = std::sqrt(x * x + y * y + z * z);

  theta = std::acos(z / r);

  phi = std::atan2(y, x);

}



/**

Converts spherical coordinates (r, theta, phi) to Cartesian (x, y, z)

coordinates.

Angles expressed in radians.

*/

template <typename _T1, typename _T2>

void sphericalToCartesian(const _T1 r, const _T1 theta, const _T1 phi, _T2 &x,

                          _T2 &y, _T2 &z) {

  x = r * std::sin(theta) * std::cos(phi);

  y = r * std::sin(theta) * std::sin(phi);

  z = r * std::cos(theta);

}



} // namespace Geodesie



#endif // GEODESIE_H