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

// %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