Changeset 210 in pacpusframework
- Timestamp:
- Nov 6, 2013, 11:35:22 AM (11 years ago)
- Location:
- trunk
- Files:
-
- 2 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/include/Pacpus/PacpusTools/geodesie.h
r209 r210 18 18 #include "PacpusToolsConfig.h" 19 19 20 #include <Pacpus/kernel/pacpus.h> 21 20 22 #include <boost/math/constants/constants.hpp> 21 23 #include <cmath> … … 36 38 { 37 39 /// Copy ctor 38 Matrice( const Matrice& A);40 Matrice(Matrice const& A); 39 41 /// Ctor 40 42 Matrice(); 41 43 /// @todo Documentation 42 void Apply(double v0, double v1, double v2, double & Mv0, double & Mv1, double& Mv2);44 void Apply(double v0, double v1, double v2, double& Mv0, double& Mv1, double& Mv2); 43 45 44 46 /// @todo Documentation … … 64 66 }; 65 67 66 PACPUSTOOLS_API Matrice TransMat(const Matrice A); 67 PACPUSTOOLS_API Matrice ProdMat(const Matrice A, const Matrice B); 68 PACPUSTOOLS_API void Write(const Matrice A, std::ostream & out); 68 PACPUSTOOLS_API Matrice TransMat(Matrice const& A); 69 PACPUSTOOLS_API Matrice ProdMat(Matrice const& A, Matrice const& B); 70 PACPUSTOOLS_API PACPUS_DEPRECATED_MSG(void Write(Matrice const& A, std::ostream& out), "use operator<<"); 71 PACPUSTOOLS_API std::ostream& operator<<(std::ostream& os, Matrice const& A); 69 72 70 73 //////////////////////////////////////////////////////////////////////// … … 74 77 public: 75 78 /// Ctor of Raf98 class. 76 Raf98() {} 79 Raf98(); 80 77 81 /// Dtor of Raf98 class. 78 82 ~Raf98(); 79 83 80 84 /// @todo Documentation 81 85 /// @param s filepath 82 bool Load(const std::string 83 86 bool Load(const std::string& s); 87 84 88 /// @todo Documentation 85 89 /// @param longitude [degrees] 86 90 /// @param latitude [degrees] 87 91 /// @param Hwgs84 Output: interpolated altitude using WGS84 geoid model [meters] 88 bool Interpol(double longitude /*deg*/, double latitude/*deg*/, double* Hwgs84) const;89 92 bool Interpol(double longitude /*deg*/, double latitude /*deg*/, double* Hwgs84) const; 93 90 94 private: 91 95 std::vector<double> m_dvalues; 92 double LitGrille(unsigned int c, unsigned int l) const;96 double LitGrille(unsigned int c, unsigned int l) const; 93 97 }; 94 98 … … 110 114 //////////////////////////////////////////////////////////////////////// 111 115 112 const double a_Lambert93 =6378137;113 const double f_Lambert93 =1 / 298.257222101;114 const double e_Lambert93 =sqrt(f_Lambert93*(2-f_Lambert93));115 const double lambda0_Lambert93 =Deg2Rad(3.0);//degres116 const double phi0_Lambert93 =Deg2Rad(46.5);117 const double phi1_Lambert93 =Deg2Rad(44.0);118 const double phi2_Lambert93 =Deg2Rad(49.0);//degres119 const double X0_Lambert93 =700000;//120 const double Y0_Lambert93 =6600000;//116 const double a_Lambert93 = 6378137; 117 const double f_Lambert93 = 1 / 298.257222101; 118 const double e_Lambert93 = sqrt(f_Lambert93 * (2 - f_Lambert93)); 119 const double lambda0_Lambert93 = Deg2Rad(3.0); //degres 120 const double phi0_Lambert93 = Deg2Rad(46.5); 121 const double phi1_Lambert93 = Deg2Rad(44.0); 122 const double phi2_Lambert93 = Deg2Rad(49.0); //degres 123 const double X0_Lambert93 = 700000; // 124 const double Y0_Lambert93 = 6600000; // 121 125 const double n_Lambert93 = 0.7256077650; 122 126 const double c_Lambert93 = 11754255.426; … … 125 129 126 130 const double GRS_a = 6378137; 127 const double GRS_f = 1 /298.257222101;128 const double GRS_b = GRS_a *(1-GRS_f);129 const double GRS_e = sqrt((pow(GRS_a, 2) - pow(GRS_b,2)) / pow(GRS_a,2));130 131 //////////////////////////////////////////////////////////////////////// 132 PACPUSTOOLS_API void Geographique_2_Lambert93(const Raf98& raf98, double lambda,double phi,double he,Matrice in,double& E,double& N,double& h,Matrice& out);133 PACPUSTOOLS_API void Geographique_2_Lambert93(const Raf98& raf98, double lambda,double phi,double he,double& E,double& N,double& h);134 PACPUSTOOLS_API void Lambert93_2_Geographique(const Raf98& raf98, double E,double N,double h,double& lambda,double& phi,double& he);135 PACPUSTOOLS_API void Lambert93_2_Geographique(const Raf98& raf98, double E,double N,double h,Matrice in,double& lambda,double& phi,double& he,Matrice& out);131 const double GRS_f = 1 / 298.257222101; 132 const double GRS_b = GRS_a * (1 - GRS_f); 133 const double GRS_e = sqrt((pow(GRS_a, 2) - pow(GRS_b, 2)) / pow(GRS_a, 2)); 134 135 //////////////////////////////////////////////////////////////////////// 136 PACPUSTOOLS_API void Geographique_2_Lambert93(const Raf98& raf98, double lambda, double phi, double he, Matrice in, double& E, double& N, double& h, Matrice& out); 137 PACPUSTOOLS_API void Geographique_2_Lambert93(const Raf98& raf98, double lambda, double phi, double he, double& E, double& N, double& h); 138 PACPUSTOOLS_API void Lambert93_2_Geographique(const Raf98& raf98, double E, double N, double h, double& lambda, double& phi, double& he); 139 PACPUSTOOLS_API void Lambert93_2_Geographique(const Raf98& raf98, double E, double N, double h, Matrice in, double& lambda, double& phi, double& he, Matrice& out); 136 140 /** Convert from geographique to ECEF. 137 141 * @param[in] longitude Longitude in radian. … … 145 149 * @param[in] he0 Height of the origin in radian. 146 150 */ 147 PACPUSTOOLS_API void ECEF_2_ENU(double x, double y,double z,double& e,double& n,double& u,double lon0,double lat0,double he0);151 PACPUSTOOLS_API void ECEF_2_ENU(double x, double y, double z, double& e, double& n, double& u, double lon0, double lat0, double he0); 148 152 //////////////////////////////////////////////////////////////////////// 149 153 150 154 ///ALGO0001 151 155 /// @todo Rename 152 PACPUSTOOLS_API double LatitueIsometrique(double latitude, double e);156 PACPUSTOOLS_API double LatitueIsometrique(double latitude, double e); 153 157 ///ALGO0002 154 158 /// @todo Rename 155 PACPUSTOOLS_API double LatitueIsometrique2Lat(double latitude_iso, double e,double epsilon);159 PACPUSTOOLS_API double LatitueIsometrique2Lat(double latitude_iso, double e, double epsilon); 156 160 157 161 ///ALGO0003 158 162 PACPUSTOOLS_API void Geo2ProjLambert( 159 double lambda, double phi,160 double n, double c, double e,161 double lambdac, double xs,double ys,162 double& X, double& Y);163 double lambda, double phi, 164 double n, double c, double e, 165 double lambdac, double xs, double ys, 166 double& X, double& Y); 163 167 ///ALGO0004 164 168 PACPUSTOOLS_API void Proj2GeoLambert( 165 double X, double Y,166 double n, double c, double e,167 double lambdac, double xs,double ys,169 double X, double Y, 170 double n, double c, double e, 171 double lambdac, double xs, double ys, 168 172 double epsilon, 169 double& lambda, double& phi);173 double& lambda, double& phi); 170 174 171 175 PACPUSTOOLS_API double ConvMerApp(double longitude); … … 175 179 */ 176 180 template <typename _T1, typename _T2> 177 void cartesianToPolar(const _T1 x, const _T1 y, _T2 & r, _T2 & theta) { 178 r = std::sqrt(x*x + y*y); 181 void cartesianToPolar(const _T1 x, const _T1 y, _T2& r, _T2& theta) 182 { 183 r = std::sqrt(x * x + y * y); 179 184 theta = std::atan2(x, y); 180 185 } … … 184 189 */ 185 190 template <typename _T1, typename _T2> 186 void polarToCartesian(const _T1 r, const _T1 theta, _T2 & x, _T2 & y) { 191 void polarToCartesian(const _T1 r, const _T1 theta, _T2& x, _T2& y) 192 { 187 193 x = r * std::cos(theta); 188 194 y = r * std::sin(theta); … … 194 200 */ 195 201 template <typename _T1, typename _T2> 196 void cartesianToSpherical(const _T1 x, const _T1 y, const _T1 z, _T2 & r, _T2 & theta, _T2 & phi) { 197 r = std::sqrt(x*x + y*y + z*z); 202 void cartesianToSpherical(const _T1 x, const _T1 y, const _T1 z, _T2& r, _T2& theta, _T2& phi) 203 { 204 r = std::sqrt(x * x + y * y + z * z); 198 205 theta = std::acos(z / r); 199 206 phi = std::atan2(y, x); … … 205 212 */ 206 213 template <typename _T1, typename _T2> 207 void sphericalToCartesian(const _T1 r, const _T1 theta, const _T1 phi, _T2 & x, _T2 & y, _T2 & z) { 214 void sphericalToCartesian(const _T1 r, const _T1 theta, const _T1 phi, _T2& x, _T2& y, _T2& z) 215 { 208 216 x = r * std::sin(theta) * std::cos(phi); 209 217 y = r * std::sin(theta) * std::sin(phi); … … 217 225 namespace Geodesie 218 226 { 219 227 using namespace Geodesy; 220 228 } // namespace Geodesie 221 229 -
trunk/src/PacpusTools/src/geodesie.cpp
r209 r210 11 11 #include <fstream> 12 12 13 using boost::math::constants::pi; 14 using boost::math::constants::half_pi; 13 using ::boost::math::constants::pi; 14 using ::boost::math::constants::half_pi; 15 using ::std::abs; 16 using ::std::atan; 17 using ::std::exp; 18 using ::std::ifstream; 19 using ::std::ostream; 20 using ::std::log; 21 using ::std::sin; 22 using ::std::string; 15 23 16 24 #ifdef _MSC_VER 17 # pragma warning(disable:4244)25 #pragma warning(disable : 4244) 18 26 #endif //_MSC_VER 19 27 … … 21 29 { 22 30 23 /// //////////////////////////////////////////////////////////////////// 24 Matrice::Matrice(const Matrice & A) 25 { 26 c0_l0=A.c0_l0;c1_l0=A.c1_l0;c2_l0=A.c2_l0; 27 c0_l1=A.c0_l1;c1_l1=A.c1_l1;c2_l1=A.c2_l1; 28 c0_l2=A.c0_l2;c1_l2=A.c1_l2;c2_l2=A.c2_l2; 29 } 30 31 /// //////////////////////////////////////////////////////////////////// 31 //////////////////////////////////////////////////////////////////////////////// 32 Matrice::Matrice(const Matrice& A) 33 { 34 c0_l0 = A.c0_l0; 35 c1_l0 = A.c1_l0; 36 c2_l0 = A.c2_l0; 37 c0_l1 = A.c0_l1; 38 c1_l1 = A.c1_l1; 39 c2_l1 = A.c2_l1; 40 c0_l2 = A.c0_l2; 41 c1_l2 = A.c1_l2; 42 c2_l2 = A.c2_l2; 43 } 44 45 //////////////////////////////////////////////////////////////////////////////// 32 46 Matrice::Matrice() 33 47 { 34 c0_l0=0.0;c1_l0=0.0;c2_l0=0.0; 35 c0_l1=0.0;c1_l1=0.0;c2_l1=0.0; 36 c0_l2=0.0;c1_l2=0.0;c2_l2=0.0; 37 } 38 39 /// //////////////////////////////////////////////////////////////////// 40 void Matrice::Apply(double v0,double v1,double v2, double& Mv0,double& Mv1,double& Mv2) 41 { 42 Mv0 = c0_l0*v0 + c1_l0*v1 + c2_l0*v2; 43 Mv1 = c0_l1*v0 + c1_l1*v1 + c2_l1*v2; 44 Mv2 = c0_l2*v0 + c1_l2*v1 + c2_l2*v2; 45 } 46 47 /// //////////////////////////////////////////////////////////////////// 48 Matrice ProdMat(const Matrice A, const Matrice B) 48 c0_l0 = 0.0; 49 c1_l0 = 0.0; 50 c2_l0 = 0.0; 51 c0_l1 = 0.0; 52 c1_l1 = 0.0; 53 c2_l1 = 0.0; 54 c0_l2 = 0.0; 55 c1_l2 = 0.0; 56 c2_l2 = 0.0; 57 } 58 59 //////////////////////////////////////////////////////////////////////////////// 60 void Matrice::Apply(double v0, double v1, double v2, double& Mv0, double& Mv1, double& Mv2) 61 { 62 Mv0 = c0_l0 * v0 + c1_l0 * v1 + c2_l0 * v2; 63 Mv1 = c0_l1 * v0 + c1_l1 * v1 + c2_l1 * v2; 64 Mv2 = c0_l2 * v0 + c1_l2 * v1 + c2_l2 * v2; 65 } 66 67 //////////////////////////////////////////////////////////////////////////////// 68 Matrice ProdMat(Matrice const& A, Matrice const& B) 49 69 { 50 70 Matrice out; 51 71 52 out.c0_l0 =A.c0_l0 * B.c0_l0 + A.c1_l0 * B.c0_l1 + A.c2_l0 * B.c0_l2;53 out.c1_l0 =A.c0_l0 * B.c1_l0 + A.c1_l0 * B.c1_l1 + A.c2_l0 * B.c1_l2;54 out.c2_l0 =A.c0_l0 * B.c2_l0 + A.c1_l0 * B.c2_l1 + A.c2_l0 * B.c2_l2;55 56 out.c0_l1 =A.c0_l1 * B.c0_l0 + A.c1_l1 * B.c0_l1 + A.c2_l1 * B.c0_l2;57 out.c1_l1 =A.c0_l1 * B.c1_l0 + A.c1_l1 * B.c1_l1 + A.c2_l1 * B.c1_l2;58 out.c2_l1 =A.c0_l1 * B.c2_l0 + A.c1_l1 * B.c2_l1 + A.c2_l1 * B.c2_l2;59 60 out.c0_l2 =A.c0_l2 * B.c0_l0 + A.c1_l2 * B.c0_l1 + A.c2_l2 * B.c0_l2;61 out.c1_l2 =A.c0_l2 * B.c1_l0 + A.c1_l2 * B.c1_l1 + A.c2_l2 * B.c1_l2;62 out.c2_l2 =A.c0_l2 * B.c2_l0 + A.c1_l2 * B.c2_l1 + A.c2_l2 * B.c2_l2;72 out.c0_l0 = A.c0_l0 * B.c0_l0 + A.c1_l0 * B.c0_l1 + A.c2_l0 * B.c0_l2; 73 out.c1_l0 = A.c0_l0 * B.c1_l0 + A.c1_l0 * B.c1_l1 + A.c2_l0 * B.c1_l2; 74 out.c2_l0 = A.c0_l0 * B.c2_l0 + A.c1_l0 * B.c2_l1 + A.c2_l0 * B.c2_l2; 75 76 out.c0_l1 = A.c0_l1 * B.c0_l0 + A.c1_l1 * B.c0_l1 + A.c2_l1 * B.c0_l2; 77 out.c1_l1 = A.c0_l1 * B.c1_l0 + A.c1_l1 * B.c1_l1 + A.c2_l1 * B.c1_l2; 78 out.c2_l1 = A.c0_l1 * B.c2_l0 + A.c1_l1 * B.c2_l1 + A.c2_l1 * B.c2_l2; 79 80 out.c0_l2 = A.c0_l2 * B.c0_l0 + A.c1_l2 * B.c0_l1 + A.c2_l2 * B.c0_l2; 81 out.c1_l2 = A.c0_l2 * B.c1_l0 + A.c1_l2 * B.c1_l1 + A.c2_l2 * B.c1_l2; 82 out.c2_l2 = A.c0_l2 * B.c2_l0 + A.c1_l2 * B.c2_l1 + A.c2_l2 * B.c2_l2; 63 83 return out; 64 84 } 65 85 66 /// 67 Matrice TransMat( const MatriceA)86 //////////////////////////////////////////////////////////////////////////////// 87 Matrice TransMat(Matrice const& A) 68 88 { 69 89 Matrice out; 70 out.c0_l0=A.c0_l0 ; out.c1_l0 = A.c0_l1 ; out.c2_l0 = A.c0_l2 ; 71 out.c0_l1=A.c1_l0 ; out.c1_l1 = A.c1_l1 ; out.c2_l1 = A.c1_l2 ; 72 out.c0_l2=A.c2_l0 ; out.c1_l2 = A.c2_l1 ; out.c2_l2 = A.c2_l2 ; 90 out.c0_l0 = A.c0_l0; 91 out.c1_l0 = A.c0_l1; 92 out.c2_l0 = A.c0_l2; 93 out.c0_l1 = A.c1_l0; 94 out.c1_l1 = A.c1_l1; 95 out.c2_l1 = A.c1_l2; 96 out.c0_l2 = A.c2_l0; 97 out.c1_l2 = A.c2_l1; 98 out.c2_l2 = A.c2_l2; 73 99 return out; 74 100 } 75 101 76 /// //////////////////////////////////////////////////////////////////// 77 void Write(const Matrice A,std::ostream& out) { 78 out<< A.c0_l0<<"\t"<< A.c1_l0<<"\t"<< A.c2_l0<<"\n"; 79 out<< A.c0_l1<<"\t"<< A.c1_l1<<"\t"<< A.c2_l1<<"\n"; 80 out<< A.c0_l2<<"\t"<< A.c1_l2<<"\t"<< A.c2_l2<<"\n"; 81 } 82 83 /// //////////////////////////////////////////////////////////////////// 84 Raf98::~Raf98() { 102 //////////////////////////////////////////////////////////////////////////////// 103 ostream& operator<<(ostream& os, Matrice const& A) 104 { 105 os << A.c0_l0 << "\t" << A.c1_l0 << "\t" << A.c2_l0 << "\n"; 106 os << A.c0_l1 << "\t" << A.c1_l1 << "\t" << A.c2_l1 << "\n"; 107 os << A.c0_l2 << "\t" << A.c1_l2 << "\t" << A.c2_l2 << "\n"; 108 return os; 109 } 110 111 void Write(Matrice const& A, ostream& out) 112 { 113 out << A; 114 } 115 116 //////////////////////////////////////////////////////////////////////////////// 117 Raf98::Raf98() 118 { 119 } 120 121 //////////////////////////////////////////////////////////////////////////////// 122 Raf98::~Raf98() 123 { 85 124 m_dvalues.clear(); 86 125 } 87 126 88 //----------------------------------------------------------------------------- 89 bool Raf98::Interpol(double longitude, double latitude, double* Hwgs84) const { 127 //////////////////////////////////////////////////////////////////////////////// 128 bool Raf98::Interpol(double longitude, double latitude, double* Hwgs84) const 129 { 90 130 *Hwgs84 = 0.0; 91 if (m_dvalues.size() ==0)131 if (m_dvalues.size() == 0) { 92 132 return false; 93 const double longitude_min = -5.5; 94 const double longitude_max = 8.5; 95 if (longitude < longitude_min) 133 } 134 const double longitude_min = -5.5; 135 const double longitude_max = 8.5; 136 if (longitude < longitude_min) { 96 137 return false; 97 if (longitude > longitude_max) 138 } 139 if (longitude > longitude_max) { 98 140 return false; 99 100 const double latitude_min = 101 const double latitude_max = 102 if (latitude < latitude_min) 141 } 142 const double latitude_min = 42; 143 const double latitude_max = 51.5; 144 if (latitude < latitude_min) { 103 145 return false; 104 if (latitude > latitude_max) 146 } 147 if (latitude > latitude_max) { 105 148 return false; 106 149 } 107 150 //conversion en position 108 151 double longPix = (longitude - longitude_min) * 30.; 109 double latPix = (latitude_max - latitude) * 40.; 110 111 double RestCol,RestLig; 112 double ColIni,LigIni; 113 RestCol = modf(longPix,&ColIni); 114 RestLig = modf(latPix,&LigIni); 115 116 117 double Zbd = (1.0-RestCol) * (1.0-RestLig) * LitGrille(ColIni , LigIni ); 118 Zbd += RestCol * (1.0-RestLig) * LitGrille(ColIni+1, LigIni ); 119 Zbd += (1.0-RestCol) * RestLig * LitGrille(ColIni , LigIni+1); 120 Zbd += RestCol * RestLig * LitGrille(ColIni+1, LigIni+1); 152 double latPix = (latitude_max - latitude) * 40.; 153 154 double RestCol, RestLig; 155 double ColIni, LigIni; 156 RestCol = modf(longPix, &ColIni); 157 RestLig = modf(latPix, &LigIni); 158 159 double Zbd = (1.0 - RestCol) * (1.0 - RestLig) * LitGrille(ColIni, LigIni); 160 Zbd += RestCol * (1.0 - RestLig) * LitGrille(ColIni + 1, LigIni); 161 Zbd += (1.0 - RestCol) * RestLig * LitGrille(ColIni, LigIni + 1); 162 Zbd += RestCol * RestLig * LitGrille(ColIni + 1, LigIni + 1); 121 163 *Hwgs84 = Zbd; 122 164 123 124 165 return true; 125 166 } 126 167 127 /// 128 double Raf98::LitGrille(unsigned int c, unsigned int l) const129 { 130 const unsigned int w =421;168 //////////////////////////////////////////////////////////////////////////////// 169 double Raf98::LitGrille(unsigned int c, unsigned int l) const 170 { 171 const unsigned int w = 421; 131 172 // const unsigned int h=381; 132 return m_dvalues.at(c +l*w);133 } 134 135 /// 136 bool Raf98::Load(const st d::string& s)137 { 138 std::ifstream in(s.c_str());173 return m_dvalues.at(c + l * w); 174 } 175 176 //////////////////////////////////////////////////////////////////////////////// 177 bool Raf98::Load(const string& s) 178 { 179 ifstream in(s.c_str()); 139 180 unsigned int w = 421; 140 181 unsigned int h = 381; 141 142 m_dvalues.reserve(w *h);143 144 char entete[1024]; //sur 3 lignes145 in.getline(entete, 1023);146 in.getline(entete, 1023);147 in.getline(entete, 1023);182 183 m_dvalues.reserve(w * h); 184 185 char entete[1024]; //sur 3 lignes 186 in.getline(entete, 1023); 187 in.getline(entete, 1023); 188 in.getline(entete, 1023); 148 189 149 190 char bidon[1024]; 150 191 double val; 151 for (unsigned int i =0; i< h; ++i) {152 for (unsigned int j =0; j< 52; ++j){153 for (unsigned int k =0; k< 8; ++k) {192 for (unsigned int i = 0; i < h; ++i) { 193 for (unsigned int j = 0; j < 52; ++j) { 194 for (unsigned int k = 0; k < 8; ++k) { 154 195 in >> val; 155 m_dvalues.push_back( val);196 m_dvalues.push_back(val); 156 197 } 157 in.getline(bidon, 1023);198 in.getline(bidon, 1023); 158 199 } 159 for (unsigned int k =0; k< 5; ++k) {200 for (unsigned int k = 0; k < 5; ++k) { 160 201 in >> val; 161 m_dvalues.push_back( val);202 m_dvalues.push_back(val); 162 203 } 163 in.getline(bidon, 1023);164 if (!in.good()) 204 in.getline(bidon, 1023); 205 if (!in.good()) { 165 206 m_dvalues.clear(); 166 207 return false; … … 172 213 } // namespace Geodesy 173 214 174 /// 175 /// 176 177 /// 215 //////////////////////////////////////////////////////////////////////////////// 216 //////////////////////////////////////////////////////////////////////////////// 217 218 //////////////////////////////////////////////////////////////////////////////// 178 219 //ALGO0001 179 220 double Geodesy::LatitueIsometrique(double latitude, double e) 180 221 { 181 using ::std::log; 182 using ::std::sin; 183 using ::std::tan; 184 185 double li = log(tan(pi<double>() / 4. + latitude/2.)) + e*log( (1-e*sin(latitude))/(1+e*sin(latitude)) )/2; 222 double li = log(tan(pi<double>() / 4. + latitude / 2.)) + e * log((1 - e * sin(latitude)) / (1 + e * sin(latitude))) / 2; 186 223 return li; 187 224 } 188 225 189 /// 226 //////////////////////////////////////////////////////////////////////////////// 190 227 //ALGO0002 191 double Geodesy::LatitueIsometrique2Lat(double latitude_iso,double e,double epsilon) 192 { 193 using ::std::atan; 194 using ::std::exp; 195 using ::std::abs; 196 using ::std::log; 197 using ::std::sin; 198 228 double Geodesy::LatitueIsometrique2Lat(double latitude_iso, double e, double epsilon) 229 { 199 230 double latitude_i = 2 * atan(exp(latitude_iso)) - half_pi<double>(); 200 231 double latitude_ip1 = latitude_i + epsilon * 2; 201 while (abs(latitude_i -latitude_ip1) > epsilon) {232 while (abs(latitude_i - latitude_ip1) > epsilon) { 202 233 latitude_i = latitude_ip1; 203 234 latitude_ip1 = 2 * atan( 204 exp(e*0.5* 205 log( 206 (1+e*sin(latitude_i))/(1-e*sin(latitude_i)) 207 ) 208 ) 209 *exp(latitude_iso) 210 ) - half_pi<double>(); 235 exp(e * 0.5 * log( 236 (1 + e * sin(latitude_i)) / (1 - e * sin(latitude_i)))) 237 * exp(latitude_iso)) - half_pi<double>(); 211 238 } 212 239 return latitude_ip1; 213 240 } 214 /// //////////////////////////////////////////////////////////////////// 241 242 //////////////////////////////////////////////////////////////////////////////// 215 243 void Geodesy::Geo2ProjLambert( 216 double lambda,double phi, 217 double n, double c,double e, 218 double lambdac,double xs,double ys, 219 double& X,double& Y) 220 { 221 double lat_iso=LatitueIsometrique(phi,e); 222 X=xs+c*exp(-n*lat_iso)*sin(n*(lambda-lambdac)); 223 Y=ys-c*exp(-n*lat_iso)*cos(n*(lambda-lambdac)); 224 } 225 /// //////////////////////////////////////////////////////////////////// 244 double lambda, double phi, 245 double n, double c, double e, 246 double lambdac, double xs, double ys, 247 double& X, double& Y) 248 { 249 double lat_iso = LatitueIsometrique(phi, e); 250 X = xs + c * exp(-n * lat_iso) * sin(n * (lambda - lambdac)); 251 Y = ys - c * exp(-n * lat_iso) * cos(n * (lambda - lambdac)); 252 } 253 254 //////////////////////////////////////////////////////////////////////////////// 226 255 //ALGO0004 227 256 void Geodesy::Proj2GeoLambert( 228 double X, double Y,229 double n, double c, double e,230 double lambdac, double xs,double ys,257 double X, double Y, 258 double n, double c, double e, 259 double lambdac, double xs, double ys, 231 260 double epsilon, 232 double& lambda,double& phi) 233 { 234 double X_xs=X-xs; 235 double ys_Y=ys-Y; 236 double R=sqrt(X_xs*X_xs+ys_Y*ys_Y); 237 double gamma=atan(X_xs/ys_Y); 238 lambda=lambdac+gamma/n; 239 double lat_iso=-1/n*log(fabs(R/c)); 240 phi=LatitueIsometrique2Lat(lat_iso,e,epsilon); 241 } 242 /// //////////////////////////////////////////////////////////////////// 243 double Geodesy::ConvMerApp(double longitude) { 261 double& lambda, double& phi) 262 { 263 double X_xs = X - xs; 264 double ys_Y = ys - Y; 265 double R = sqrt(X_xs * X_xs + ys_Y * ys_Y); 266 double gamma = atan(X_xs / ys_Y); 267 lambda = lambdac + gamma / n; 268 double lat_iso = -1 / n * log(fabs(R / c)); 269 phi = LatitueIsometrique2Lat(lat_iso, e, epsilon); 270 } 271 272 //////////////////////////////////////////////////////////////////////////////// 273 double Geodesy::ConvMerApp(double longitude) 274 { 244 275 double phi0_Lambert93 = Deg2Rad(46.5); 245 276 double lambda0_Lambert93 = Deg2Rad(3.0); 246 double conv =-sin(phi0_Lambert93)*(longitude-lambda0_Lambert93);247 return 277 double conv = -sin(phi0_Lambert93) * (longitude - lambda0_Lambert93); 278 return conv; 248 279 } 249 280 250 281 //////////////////////////////////////////////////////////////////// 251 void Geodesy::Geographique_2_Lambert93(const Raf98& raf98, double lambda,double phi,double he,Matrice in,double& E,double& N,double& h,Matrice& out)282 void Geodesy::Geographique_2_Lambert93(const Raf98& raf98, double lambda, double phi, double he, Matrice in, double& E, double& N, double& h, Matrice& out) 252 283 { 253 284 Matrice passage; 254 double conv =Geodesy::ConvMerApp(lambda);255 double c_ =cos(conv);256 double s_ =sin(conv);285 double conv = Geodesy::ConvMerApp(lambda); 286 double c_ = cos(conv); 287 double s_ = sin(conv); 257 288 258 289 passage.c0_l0 = c_; 259 290 passage.c0_l1 = s_; 260 passage.c0_l2 = 291 passage.c0_l2 = 0.0; 261 292 262 293 passage.c1_l0 = -s_; 263 294 passage.c1_l1 = c_; 264 passage.c1_l2 = 265 266 passage.c2_l0 = 0.0;267 passage.c2_l1 = 0.0;268 passage.c2_l2 = 1.0;269 270 out =ProdMat(passage,in);295 passage.c1_l2 = 0.0; 296 297 passage.c2_l0 = 0.0; 298 passage.c2_l1 = 0.0; 299 passage.c2_l2 = 1.0; 300 301 out = ProdMat(passage, in); 271 302 double diff_h; 272 raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi),&diff_h);273 h =he-diff_h;274 303 raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi), &diff_h); 304 h = he - diff_h; 305 275 306 Geodesy::Geo2ProjLambert( 276 lambda,phi, 277 n_Lambert93,c_Lambert93,e_Lambert93, 278 lambda0_Lambert93,xs_Lambert93,ys_Lambert93, 279 E,N); 280 } 307 lambda, phi, 308 n_Lambert93, c_Lambert93, e_Lambert93, 309 lambda0_Lambert93, xs_Lambert93, ys_Lambert93, 310 E, N); 311 } 312 281 313 //////////////////////////////////////////////////////////////////////// 282 void Geodesy::Geographique_2_Lambert93(const Raf98& raf98,double lambda,double phi,double he,double& E,double& N,double& h) { 314 void Geodesy::Geographique_2_Lambert93(const Raf98& raf98, double lambda, double phi, double he, double& E, double& N, double& h) 315 { 283 316 Geodesy::Geo2ProjLambert( 284 lambda,phi,285 n_Lambert93,c_Lambert93,e_Lambert93,286 lambda0_Lambert93,xs_Lambert93,ys_Lambert93,287 E,N);317 lambda, phi, 318 n_Lambert93, c_Lambert93, e_Lambert93, 319 lambda0_Lambert93, xs_Lambert93, ys_Lambert93, 320 E, N); 288 321 289 322 double diff_h; 290 raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi),&diff_h);291 h =he-diff_h;323 raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi), &diff_h); 324 h = he - diff_h; 292 325 } 293 326 294 327 /// Converts Lambert93 coordinates (East, North, Height) into geographical coordinates in radians (Longitude = Rad2Deg(lambda), Latitude = Rad2Deg(phi), Height) 295 void Geodesy::Lambert93_2_Geographique(const Raf98& raf98,double E,double N,double h,double& lambda,double& phi,double& he) { 328 void Geodesy::Lambert93_2_Geographique(const Raf98& raf98, double E, double N, double h, double& lambda, double& phi, double& he) 329 { 296 330 Geodesy::Proj2GeoLambert( 297 E,N,298 n_Lambert93,c_Lambert93,e_Lambert93,299 lambda0_Lambert93,xs_Lambert93,ys_Lambert93,300 301 lambda,phi);331 E, N, 332 n_Lambert93, c_Lambert93, e_Lambert93, 333 lambda0_Lambert93, xs_Lambert93, ys_Lambert93, 334 0.0000000000000001, 335 lambda, phi); 302 336 303 337 double diff_h; 304 raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi),&diff_h);305 he =h+diff_h;338 raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi), &diff_h); 339 he = h + diff_h; 306 340 } 307 341 308 342 //////////////////////////////////////////////////////////////////////// 309 void Geodesy::Lambert93_2_Geographique(const Raf98& raf98,double E,double N,double h,Matrice in,double& lambda,double& phi,double& he,Matrice& out) { 343 void Geodesy::Lambert93_2_Geographique(const Raf98& raf98, double E, double N, double h, Matrice in, double& lambda, double& phi, double& he, Matrice& out) 344 { 310 345 Geodesy::Proj2GeoLambert( 311 E,N,312 n_Lambert93,c_Lambert93,e_Lambert93,313 lambda0_Lambert93,xs_Lambert93,ys_Lambert93,314 315 lambda,phi);346 E, N, 347 n_Lambert93, c_Lambert93, e_Lambert93, 348 lambda0_Lambert93, xs_Lambert93, ys_Lambert93, 349 0.0000000000000001, 350 lambda, phi); 316 351 317 352 Matrice passage; 318 double conv =Geodesy::ConvMerApp(lambda);319 double c_ =cos(conv);320 double s_ =sin(conv);353 double conv = Geodesy::ConvMerApp(lambda); 354 double c_ = cos(conv); 355 double s_ = sin(conv); 321 356 322 357 passage.c0_l0 = c_; 323 358 passage.c0_l1 = -s_; 324 passage.c0_l2 = 359 passage.c0_l2 = 0.0; 325 360 326 361 passage.c1_l0 = s_; 327 362 passage.c1_l1 = c_; 328 passage.c1_l2 = 329 330 passage.c2_l0 = 0.0;331 passage.c2_l1 = 0.0;332 passage.c2_l2 = 1.0;333 334 out =ProdMat(passage,in);335 363 passage.c1_l2 = 0.0; 364 365 passage.c2_l0 = 0.0; 366 passage.c2_l1 = 0.0; 367 passage.c2_l2 = 1.0; 368 369 out = ProdMat(passage, in); 370 336 371 double diff_h; 337 raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi),&diff_h);338 he =h+diff_h;372 raf98.Interpol(Rad2Deg(lambda), Rad2Deg(phi), &diff_h); 373 he = h + diff_h; 339 374 } 340 375 341 376 //////////////////////////////////////////////////////////////////////// 342 void Geodesy::Geographique_2_ECEF(double longitude,double latitude,double he,double& x,double& y,double& z) { 343 const double n = GRS_a / sqrt(1.0 - pow(GRS_e,2) * pow(sin(latitude),2)); 377 void Geodesy::Geographique_2_ECEF(double longitude, double latitude, double he, double& x, double& y, double& z) 378 { 379 const double n = GRS_a / sqrt(1.0 - pow(GRS_e, 2) * pow(sin(latitude), 2)); 344 380 x = (n + he) * cos(latitude) * cos(longitude); 345 381 y = (n + he) * cos(latitude) * sin(longitude); 346 z = (n * (1.0 - pow(GRS_e, 2)) + he) * sin(latitude);382 z = (n * (1.0 - pow(GRS_e, 2)) + he) * sin(latitude); 347 383 } 348 384 349 385 //////////////////////////////////////////////////////////////////////// 350 void Geodesy::ECEF_2_ENU(double x,double y,double z,double& e,double& n,double& u,double lon0,double lat0,double he0) { 351 double slat = std::sin(lat0); 352 double clat = std::cos(lat0); 353 double slon = std::sin(lon0); 354 double clon = std::cos(lon0); 355 356 Geodesy::Matrice C; 386 void Geodesy::ECEF_2_ENU(double x, double y, double z, double& e, double& n, double& u, double lon0, double lat0, double he0) 387 { 388 double slat = sin(lat0); 389 double clat = cos(lat0); 390 double slon = sin(lon0); 391 double clon = cos(lon0); 392 393 Matrice C; 357 394 C.c0_l0 = -slon; 358 395 C.c1_l0 = clon; … … 367 404 368 405 double x0, y0, z0; 369 Geographique_2_ECEF(lon0, lat0,he0, x0,y0,z0);406 Geographique_2_ECEF(lon0, lat0, he0, x0, y0, z0); 370 407 371 408 x -= x0; … … 373 410 z -= z0; 374 411 375 C.Apply(x, y,z, e,n,u);412 C.Apply(x, y, z, e, n, u); 376 413 } 377 414 … … 386 423 float s3 = sin(angle.z()); 387 424 388 389 425 // Source : https://en.wikipedia.org/wiki/Euler_angles 390 return QMatrix4x4 ( c1*c2, c1*s2*s3-c3*s1, s1*s3+c1*c3*s2, position.x(),391 c2*s1, c1*c3+s1*s2*s3, c3*s1*s2-c1*s3, position.y(),392 -s2, c2*s3, c2*c3, position.z(),393 0, 0, 0,1);394 } 426 return QMatrix4x4(c1 * c2, c1 * s2 * s3 - c3 * s1, s1 * s3 + c1 * c3 * s2, position.x(), 427 c2 * s1, c1 * c3 + s1 * s2 * s3, c3 * s1 * s2 - c1 * s3, position.y(), 428 -s2, c2 * s3, c2 * c3, position.z(), 429 0, 0, 0, 1); 430 }
Note:
See TracChangeset
for help on using the changeset viewer.