source: pacpusframework/trunk/src/PacpusTools/src/geodesie.cpp

// %pacpus:license{
// This file is part of the PACPUS framework distributed under the
// CECILL-C License, Version 1.0.
/// @author  Marek Kurdej <firstname.surname@utc.fr>
/// @author  Jean Laneurit <firstname.surname@utc.fr>
/// @date    April, 2010
// %pacpus:license}

#include <Pacpus/PacpusTools/geodesie.h>

#include <fstream>
#include <QMatrix4x4>
#include <QVector3D>

using ::boost::math::constants::pi;
using ::boost::math::constants::half_pi;
using ::std::abs;
using ::std::atan;
using ::std::exp;
using ::std::ifstream;
using ::std::ostream;
using ::std::log;
using ::std::sin;
using ::std::string;

#ifdef _MSC_VER
#pragma warning(disable : 4244)
#endif //_MSC_VER

namespace Geodesy
{

////////////////////////////////////////////////////////////////////////////////
Matrice::Matrice(const Matrice& A)
{
    c0_l0 = A.c0_l0;
    c1_l0 = A.c1_l0;
    c2_l0 = A.c2_l0;
    c0_l1 = A.c0_l1;
    c1_l1 = A.c1_l1;
    c2_l1 = A.c2_l1;
    c0_l2 = A.c0_l2;
    c1_l2 = A.c1_l2;
    c2_l2 = A.c2_l2;
}

////////////////////////////////////////////////////////////////////////////////
Matrice::Matrice()
{
    c0_l0 = 0.0;
    c1_l0 = 0.0;
    c2_l0 = 0.0;
    c0_l1 = 0.0;
    c1_l1 = 0.0;
    c2_l1 = 0.0;
    c0_l2 = 0.0;
    c1_l2 = 0.0;
    c2_l2 = 0.0;
}

////////////////////////////////////////////////////////////////////////////////
void Matrice::Apply(double v0, double v1, double v2, double& Mv0, double& Mv1, double& Mv2)
{
    Mv0 = c0_l0 * v0 + c1_l0 * v1 + c2_l0 * v2;
    Mv1 = c0_l1 * v0 + c1_l1 * v1 + c2_l1 * v2;
    Mv2 = c0_l2 * v0 + c1_l2 * v1 + c2_l2 * v2;
}

Matrice& Matrice::operator*=(Matrice const& other)
{
    // TODO: optimize
    *this = ProdMat(*this, other);
    return *this;
}

////////////////////////////////////////////////////////////////////////////////
Matrice ProdMat(Matrice const& A, Matrice const& B)
{
    Matrice out;

    out.c0_l0 = A.c0_l0 * B.c0_l0 + A.c1_l0 * B.c0_l1 + A.c2_l0 * B.c0_l2;
    out.c1_l0 = A.c0_l0 * B.c1_l0 + A.c1_l0 * B.c1_l1 + A.c2_l0 * B.c1_l2;
    out.c2_l0 = A.c0_l0 * B.c2_l0 + A.c1_l0 * B.c2_l1 + A.c2_l0 * B.c2_l2;

    out.c0_l1 = A.c0_l1 * B.c0_l0 + A.c1_l1 * B.c0_l1 + A.c2_l1 * B.c0_l2;
    out.c1_l1 = A.c0_l1 * B.c1_l0 + A.c1_l1 * B.c1_l1 + A.c2_l1 * B.c1_l2;
    out.c2_l1 = A.c0_l1 * B.c2_l0 + A.c1_l1 * B.c2_l1 + A.c2_l1 * B.c2_l2;

    out.c0_l2 = A.c0_l2 * B.c0_l0 + A.c1_l2 * B.c0_l1 + A.c2_l2 * B.c0_l2;
    out.c1_l2 = A.c0_l2 * B.c1_l0 + A.c1_l2 * B.c1_l1 + A.c2_l2 * B.c1_l2;
    out.c2_l2 = A.c0_l2 * B.c2_l0 + A.c1_l2 * B.c2_l1 + A.c2_l2 * B.c2_l2;
    return out;
}

////////////////////////////////////////////////////////////////////////////////
Matrice TransMat(Matrice const& A)
{
    Matrice out;
    out.c0_l0 = A.c0_l0;
    out.c1_l0 = A.c0_l1;
    out.c2_l0 = A.c0_l2;
    out.c0_l1 = A.c1_l0;
    out.c1_l1 = A.c1_l1;
    out.c2_l1 = A.c1_l2;
    out.c0_l2 = A.c2_l0;
    out.c1_l2 = A.c2_l1;
    out.c2_l2 = A.c2_l2;
    return out;
}

////////////////////////////////////////////////////////////////////////////////
ostream& operator<<(ostream& os, Matrice const& A)
{
    os << A.c0_l0 << "\t" << A.c1_l0 << "\t" << A.c2_l0 << "\n";
    os << A.c0_l1 << "\t" << A.c1_l1 << "\t" << A.c2_l1 << "\n";
    os << A.c0_l2 << "\t" << A.c1_l2 << "\t" << A.c2_l2 << "\n";
    return os;
}

void Write(Matrice const& A, ostream& out)
{
    out << A;
}

////////////////////////////////////////////////////////////////////////////////
Raf98::Raf98()
{
}

////////////////////////////////////////////////////////////////////////////////
Raf98::~Raf98()
{
    m_dvalues.clear();
}

////////////////////////////////////////////////////////////////////////////////
bool Raf98::Interpol(double longitude, double latitude, double* Hwgs84) const
{
    *Hwgs84 = 0.0;
    if (m_dvalues.size() == 0) {
        return false;
    }
    const double longitude_min = -5.5;
    const double longitude_max = 8.5;
    if (longitude < longitude_min) {
        return false;
    }
    if (longitude > longitude_max) {
        return false;
    }
    const double latitude_min = 42;
    const double latitude_max = 51.5;
    if (latitude < latitude_min) {
        return false;
    }
    if (latitude > latitude_max) {
        return false;
    }
    //conversion en position
    double longPix = (longitude - longitude_min) * 30.;
    double latPix = (latitude_max - latitude) * 40.;

    double RestCol, RestLig;
    double ColIni, LigIni;
    RestCol = modf(longPix, &ColIni);
    RestLig = modf(latPix, &LigIni);

    double Zbd = (1.0 - RestCol) * (1.0 - RestLig) * LitGrille(ColIni, LigIni);
    Zbd += RestCol * (1.0 - RestLig) * LitGrille(ColIni + 1, LigIni);
    Zbd += (1.0 - RestCol) * RestLig * LitGrille(ColIni, LigIni + 1);
    Zbd += RestCol * RestLig * LitGrille(ColIni + 1, LigIni + 1);
    *Hwgs84 = Zbd;

    return true;
}

////////////////////////////////////////////////////////////////////////////////
double Raf98::LitGrille(unsigned int c, unsigned int l) const
{
    const unsigned int w = 421;
    //    const unsigned int h=381;
    return m_dvalues.at(c + l * w);
}

////////////////////////////////////////////////////////////////////////////////
bool Raf98::Load(const string& s)
{
    ifstream in(s.c_str());
    unsigned int w = 421;
    unsigned int h = 381;

    m_dvalues.reserve(w * h);

    char entete[1024]; //sur 3 lignes
    in.getline(entete, 1023);
    in.getline(entete, 1023);
    in.getline(entete, 1023);

    char bidon[1024];
    double val;
    for (unsigned int i = 0; i < h; ++i) {
        for (unsigned int j = 0; j < 52; ++j) {
            for (unsigned int k = 0; k < 8; ++k) {
                in >> val;
                m_dvalues.push_back(val);
            }
            in.getline(bidon, 1023);
        }
        for (unsigned int k = 0; k < 5; ++k) {
            in >> val;
            m_dvalues.push_back(val);
        }
        in.getline(bidon, 1023);
        if (!in.good()) {
            m_dvalues.clear();
            return false;
        }
    }
    return in.good();
}

} // namespace Geodesy

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

////////////////////////////////////////////////////////////////////////////////
//ALGO0001
double Geodesy::LatitueIsometrique(double latitude, double e)
{
    double li = log(tan(pi<double>() / 4. + latitude / 2.)) + e * log((1 - e * sin(latitude)) / (1 + e * sin(latitude))) / 2;
    return li;
}

////////////////////////////////////////////////////////////////////////////////
//ALGO0002
double Geodesy::LatitueIsometrique2Lat(double latitude_iso, double e, double epsilon)
{
    double latitude_i = 2 * atan(exp(latitude_iso)) - half_pi<double>();
    double latitude_ip1 = latitude_i + epsilon * 2;
    while (abs(latitude_i - latitude_ip1) > epsilon) {
        latitude_i = latitude_ip1;
        latitude_ip1 = 2 * atan(
                               exp(e * 0.5 * log(
                                                 (1 + e * sin(latitude_i)) / (1 - e * sin(latitude_i))))
                               * exp(latitude_iso)) - half_pi<double>();
    }
    return latitude_ip1;
}

////////////////////////////////////////////////////////////////////////////////
void Geodesy::Geo2ProjLambert(
    double lambda, double phi,
    double n, double c, double e,
    double lambdac, double xs, double ys,
    double& X, double& Y)
{
    double lat_iso = LatitueIsometrique(phi, e);
    X = xs + c * exp(-n * lat_iso) * sin(n * (lambda - lambdac));
    Y = ys - c * exp(-n * lat_iso) * cos(n * (lambda - lambdac));
}

////////////////////////////////////////////////////////////////////////////////
//ALGO0004
void Geodesy::Proj2GeoLambert(
    double X, double Y,
    double n, double c, double e,
    double lambdac, double xs, double ys,
    double epsilon,
    double& lambda, double& phi)
{
    double X_xs = X - xs;
    double ys_Y = ys - Y;
    double R = sqrt(X_xs * X_xs + ys_Y * ys_Y);
    double gamma = atan(X_xs / ys_Y);
    lambda = lambdac + gamma / n;
    double lat_iso = -1 / n * log(fabs(R / c));
    phi = LatitueIsometrique2Lat(lat_iso, e, epsilon);
}

////////////////////////////////////////////////////////////////////////////////
double Geodesy::ConvMerApp(double longitude)
{
    double phi0_Lambert93 = Deg2Rad(46.5);
    double lambda0_Lambert93 = Deg2Rad(3.0);
    double conv = -sin(phi0_Lambert93) * (longitude - lambda0_Lambert93);
    return conv;
}

////////////////////////////////////////////////////////////////////
void Geodesy::Geographique_2_Lambert93(const Raf98& raf98, double lambda, double phi, double he, Matrice in, double& E, double& N, double& h, Matrice& out)
{
    Matrice passage;
    double conv = Geodesy::ConvMerApp(lambda);
    double c_ = cos(conv);
    double s_ = sin(conv);

    passage.c0_l0 = c_;
    passage.c0_l1 = s_;
    passage.c0_l2 = 0.0;

    passage.c1_l0 = -s_;
    passage.c1_l1 = c_;
    passage.c1_l2 = 0.0;

    passage.c2_l0 = 0.0;
    passage.c2_l1 = 0.0;
    passage.c2_l2 = 1.0;

    out = passage * in;
    double diff_h;
    raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi), &diff_h);
    h = he - diff_h;

    Geodesy::Geo2ProjLambert(
        lambda, phi,
        n_Lambert93, c_Lambert93, e_Lambert93,
        lambda0_Lambert93, xs_Lambert93, ys_Lambert93,
        E, N);
}

////////////////////////////////////////////////////////////////////////
void Geodesy::Geographique_2_Lambert93(const Raf98& raf98, double lambda, double phi, double he, double& E, double& N, double& h)
{
    Geodesy::Geo2ProjLambert(
        lambda, phi,
        n_Lambert93, c_Lambert93, e_Lambert93,
        lambda0_Lambert93, xs_Lambert93, ys_Lambert93,
        E, N);

    double diff_h;
    raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi), &diff_h);
    h = he - diff_h;
}

/// Converts Lambert93 coordinates (East, North, Height) into geographical coordinates in radians (Longitude = Rad2Deg(lambda), Latitude = Rad2Deg(phi), Height)
void Geodesy::Lambert93_2_Geographique(const Raf98& raf98, double E, double N, double h, double& lambda, double& phi, double& he)
{
    Geodesy::Proj2GeoLambert(
        E, N,
        n_Lambert93, c_Lambert93, e_Lambert93,
        lambda0_Lambert93, xs_Lambert93, ys_Lambert93,
        0.0000000000000001,
        lambda, phi);

    double diff_h;
    raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi), &diff_h);
    he = h + diff_h;
}

////////////////////////////////////////////////////////////////////////
void Geodesy::Lambert93_2_Geographique(const Raf98& raf98, double E, double N, double h, Matrice in, double& lambda, double& phi, double& he, Matrice& out)
{
    Geodesy::Proj2GeoLambert(
        E, N,
        n_Lambert93, c_Lambert93, e_Lambert93,
        lambda0_Lambert93, xs_Lambert93, ys_Lambert93,
        0.0000000000000001,
        lambda, phi);

    Matrice passage;
    double conv = Geodesy::ConvMerApp(lambda);
    double c_ = cos(conv);
    double s_ = sin(conv);

    passage.c0_l0 = c_;
    passage.c0_l1 = -s_;
    passage.c0_l2 = 0.0;

    passage.c1_l0 = s_;
    passage.c1_l1 = c_;
    passage.c1_l2 = 0.0;

    passage.c2_l0 = 0.0;
    passage.c2_l1 = 0.0;
    passage.c2_l2 = 1.0;

    out = passage * in;

    double diff_h;
    raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi), &diff_h);
    he = h + diff_h;
}

////////////////////////////////////////////////////////////////////////
void Geodesy::Geographique_2_ECEF(double longitude, double latitude, double he, double& x, double& y, double& z)
{
    const double n = GRS_a / sqrt(1.0 - pow(GRS_e, 2) * pow(sin(latitude), 2));
    x = (n + he) * cos(latitude) * cos(longitude);
    y = (n + he) * cos(latitude) * sin(longitude);
    z = (n * (1.0 - pow(GRS_e, 2)) + he) * sin(latitude);
}

////////////////////////////////////////////////////////////////////////
void Geodesy::ECEF_2_ENU(double x, double y, double z, double& e, double& n, double& u, double lon0, double lat0, double he0)
{
    double slat = sin(lat0);
    double clat = cos(lat0);
    double slon = sin(lon0);
    double clon = cos(lon0);

    Matrice C;
    C.c0_l0 = -slon;
    C.c1_l0 = clon;

    C.c0_l1 = -clon * slat;
    C.c1_l1 = -slon * slat;
    C.c2_l1 = clat;

    C.c0_l2 = clon * clat;
    C.c1_l2 = slon * clat;
    C.c2_l2 = slat;

    double x0, y0, z0;
    Geographique_2_ECEF(lon0, lat0, he0, x0, y0, z0);

    x -= x0;
    y -= y0;
    z -= z0;

    C.Apply(x, y, z, e, n, u);
}

QMatrix4x4 Geodesy::yprenuToMatrix(QVector3D angle, QVector3D position)
{
    float c1 = cos(angle.x());
    float c2 = cos(angle.y());
    float c3 = cos(angle.z());

    float s1 = sin(angle.x());
    float s2 = sin(angle.y());
    float s3 = sin(angle.z());

    // Source : https://en.wikipedia.org/wiki/Euler_angles
    return QMatrix4x4(c1 * c2, c1 * s2 * s3 - c3 * s1, s1 * s3 + c1 * c3 * s2, position.x(),
                      c2 * s1, c1 * c3 + s1 * s2 * s3, c3 * s1 * s2 - c1 * s3, position.y(),
                      -s2, c2 * s3, c2 * c3, position.z(),
                      0, 0, 0, 1);
}