- Timestamp:
- Nov 6, 2013, 11:35:22 AM (11 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
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.