Changeset 15 in flair-src for trunk/lib/FlairSensorActuator/src/unexported/geodesie.h
- Timestamp:
- 04/08/16 15:40:57 (8 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/lib/FlairSensorActuator/src/unexported/geodesie.h
r3 r15 13 13 14 14 #ifndef M_PI 15 # 15 #define M_PI 3.14159265358979323846 16 16 #endif 17 17 #ifndef M_PI_2 18 # 18 #define M_PI_2 1.57079632679489661923 19 19 #endif 20 20 #ifndef M_PI_4 21 # 21 #define M_PI_4 0.78539816339744830962 22 22 #endif 23 23 24 24 //////////////////////////////////////////////////////////////////////// 25 25 struct Matrice { 26 Matrice(const Matrice & A); 27 Matrice(); 28 void Apply(double v0, double v1, double v2, double & Mv0, double & Mv1, double & Mv2); 29 double c0_l0;double c1_l0;double c2_l0; 30 double c0_l1;double c1_l1;double c2_l1; 31 double c0_l2;double c1_l2;double c2_l2; 26 Matrice(const Matrice &A); 27 Matrice(); 28 void Apply(double v0, double v1, double v2, double &Mv0, double &Mv1, 29 double &Mv2); 30 double c0_l0; 31 double c1_l0; 32 double c2_l0; 33 double c0_l1; 34 double c1_l1; 35 double c2_l1; 36 double c0_l2; 37 double c1_l2; 38 double c2_l2; 32 39 }; // class 33 40 34 41 Matrice TransMat(const Matrice A); 35 42 36 Matrice ProdMat(const Matrice A, const Matrice B);37 void Write(const Matrice A, std::ostream&out);43 Matrice ProdMat(const Matrice A, const Matrice B); 44 void Write(const Matrice A, std::ostream &out); 38 45 39 46 //////////////////////////////////////////////////////////////////////// 40 47 class Raf98 { 41 private : 42 std::vector<double> m_dvalues; 43 double LitGrille(unsigned int c,unsigned int l) const; 44 public : 45 ~Raf98(); 46 Raf98() {} 47 bool Load(const std::string & s); 48 bool Interpol(double longitude/*deg*/, double latitude/*deg*/, double* Hwgs84) const; 48 private: 49 std::vector<double> m_dvalues; 50 double LitGrille(unsigned int c, unsigned int l) const; 51 52 public: 53 ~Raf98(); 54 Raf98() {} 55 bool Load(const std::string &s); 56 bool Interpol(double longitude /*deg*/, double latitude /*deg*/, 57 double *Hwgs84) const; 49 58 }; // class 50 59 //////////////////////////////////////////////////////////////////////// 51 60 52 61 //////////////////////////////////////////////////////////////////////// 53 inline double Deg2Rad(double deg) { return deg*M_PI/180.0;}54 inline double Rad2Deg(double rad) { return rad*180.0/M_PI;}62 inline double Deg2Rad(double deg) { return deg * M_PI / 180.0; } 63 inline double Rad2Deg(double rad) { return rad * 180.0 / M_PI; } 55 64 //////////////////////////////////////////////////////////////////////// 56 65 57 const double a_Lambert93 =6378137;58 const double f_Lambert93 =1 / 298.257222101;59 const double e_Lambert93 =sqrt(f_Lambert93*(2-f_Lambert93));60 const double lambda0_Lambert93 =Deg2Rad(3.0);//degres61 const double phi0_Lambert93 =Deg2Rad(46.5);62 const double phi1_Lambert93 =Deg2Rad(44.0);63 const double phi2_Lambert93 =Deg2Rad(49.0);//degres64 const double X0_Lambert93 =700000;//65 const double Y0_Lambert93 =6600000;//66 const double a_Lambert93 = 6378137; 67 const double f_Lambert93 = 1 / 298.257222101; 68 const double e_Lambert93 = sqrt(f_Lambert93 * (2 - f_Lambert93)); 69 const double lambda0_Lambert93 = Deg2Rad(3.0); // degres 70 const double phi0_Lambert93 = Deg2Rad(46.5); 71 const double phi1_Lambert93 = Deg2Rad(44.0); 72 const double phi2_Lambert93 = Deg2Rad(49.0); // degres 73 const double X0_Lambert93 = 700000; // 74 const double Y0_Lambert93 = 6600000; // 66 75 const double n_Lambert93 = 0.7256077650; 67 76 const double c_Lambert93 = 11754255.426; … … 70 79 71 80 const double GRS_a = 6378137; 72 const double GRS_f = 1 /298.257222101;73 const double GRS_b = GRS_a *(1-GRS_f);74 const double GRS_e = sqrt((pow(GRS_a, 2) - pow(GRS_b,2)) / pow(GRS_a,2));81 const double GRS_f = 1 / 298.257222101; 82 const double GRS_b = GRS_a * (1 - GRS_f); 83 const double GRS_e = sqrt((pow(GRS_a, 2) - pow(GRS_b, 2)) / pow(GRS_a, 2)); 75 84 76 85 //////////////////////////////////////////////////////////////////////// 77 void Geographique_2_Lambert93(const Raf98& raf98,double lambda,double phi,double he,Matrice in,double& E,double& N,double& h,Matrice& out); 78 void Geographique_2_Lambert93(const Raf98& raf98,double lambda,double phi,double he,double& E,double& N,double& h); 79 void Lambert93_2_Geographique(const Raf98& raf98,double E,double N,double h,double& lambda,double& phi,double& he); 80 void Lambert93_2_Geographique(const Raf98& raf98,double E,double N,double h,Matrice in,double& lambda,double& phi,double& he,Matrice& out); 86 void Geographique_2_Lambert93(const Raf98 &raf98, double lambda, double phi, 87 double he, Matrice in, double &E, double &N, 88 double &h, Matrice &out); 89 void Geographique_2_Lambert93(const Raf98 &raf98, double lambda, double phi, 90 double he, double &E, double &N, double &h); 91 void Lambert93_2_Geographique(const Raf98 &raf98, double E, double N, double h, 92 double &lambda, double &phi, double &he); 93 void Lambert93_2_Geographique(const Raf98 &raf98, double E, double N, double h, 94 Matrice in, double &lambda, double &phi, 95 double &he, Matrice &out); 81 96 /** Convert from geographique to ECEF. 82 97 * @param[in] longitude Longitude in radian. … … 84 99 * @param[in] he Height in meter. 85 100 */ 86 void Geographique_2_ECEF(double longitude, double latitude, double he, double& x, double& y, double& z); 101 void Geographique_2_ECEF(double longitude, double latitude, double he, 102 double &x, double &y, double &z); 87 103 /** Convert from ECEF two ENU. 88 104 * @param[in] lon0 Longitude of the origin in radian. … … 90 106 * @param[in] he0 Height of the origin in radian. 91 107 */ 92 void ECEF_2_ENU(double x,double y,double z,double& e,double& n,double& u,double lon0,double lat0,double he0); 108 void ECEF_2_ENU(double x, double y, double z, double &e, double &n, double &u, 109 double lon0, double lat0, double he0); 93 110 //////////////////////////////////////////////////////////////////////// 94 111 95 // ALGO000196 double LatitueIsometrique(double latitude, double e);97 // ALGO000298 double LatitueIsometrique2Lat(double latitude_iso, double e,double epsilon);112 // ALGO0001 113 double LatitueIsometrique(double latitude, double e); 114 // ALGO0002 115 double LatitueIsometrique2Lat(double latitude_iso, double e, double epsilon); 99 116 100 //ALGO0003 101 void Geo2ProjLambert( 102 double lambda,double phi, 103 double n, double c,double e, 104 double lambdac,double xs,double ys, 105 double& X,double& Y); 106 //ALGO0004 107 void Proj2GeoLambert( 108 double X,double Y, 109 double n, double c,double e, 110 double lambdac,double xs,double ys, 111 double epsilon, 112 double& lambda,double& phi); 117 // ALGO0003 118 void Geo2ProjLambert(double lambda, double phi, double n, double c, double e, 119 double lambdac, double xs, double ys, double &X, 120 double &Y); 121 // ALGO0004 122 void Proj2GeoLambert(double X, double Y, double n, double c, double e, 123 double lambdac, double xs, double ys, double epsilon, 124 double &lambda, double &phi); 113 125 114 126 double ConvMerApp(double longitude); … … 118 130 */ 119 131 template <typename _T1, typename _T2> 120 void cartesianToPolar(const _T1 x, const _T1 y, _T2 & r, _T2 &theta) {121 r = std::sqrt(x*x + y*y);122 132 void cartesianToPolar(const _T1 x, const _T1 y, _T2 &r, _T2 &theta) { 133 r = std::sqrt(x * x + y * y); 134 theta = std::atan2(x, y); 123 135 } 124 136 … … 127 139 */ 128 140 template <typename _T1, typename _T2> 129 void polarToCartesian(const _T1 r, const _T1 theta, _T2 & x, _T2 &y) {130 131 141 void polarToCartesian(const _T1 r, const _T1 theta, _T2 &x, _T2 &y) { 142 x = r * std::cos(theta); 143 y = r * std::sin(theta); 132 144 } 133 145 134 146 /** 135 Converts Cartesian (x, y, z) coordinates to spherical coordinates (r, theta, phi) 147 Converts Cartesian (x, y, z) coordinates to spherical coordinates (r, theta, 148 phi) 136 149 Angles expressed in radians. 137 150 */ 138 151 template <typename _T1, typename _T2> 139 void cartesianToSpherical(const _T1 x, const _T1 y, const _T1 z, _T2 & r, _T2 & theta, _T2 & phi) { 140 r = std::sqrt(x*x + y*y + z*z); 141 theta = std::acos(z / r); 142 phi = std::atan2(y, x); 152 void cartesianToSpherical(const _T1 x, const _T1 y, const _T1 z, _T2 &r, 153 _T2 &theta, _T2 &phi) { 154 r = std::sqrt(x * x + y * y + z * z); 155 theta = std::acos(z / r); 156 phi = std::atan2(y, x); 143 157 } 144 158 145 159 /** 146 Converts spherical coordinates (r, theta, phi) to Cartesian (x, y, z) coordinates. 160 Converts spherical coordinates (r, theta, phi) to Cartesian (x, y, z) 161 coordinates. 147 162 Angles expressed in radians. 148 163 */ 149 164 template <typename _T1, typename _T2> 150 void sphericalToCartesian(const _T1 r, const _T1 theta, const _T1 phi, _T2 & x, _T2 & y, _T2 & z) { 151 x = r * std::sin(theta) * std::cos(phi); 152 y = r * std::sin(theta) * std::sin(phi); 153 z = r * std::cos(theta); 165 void sphericalToCartesian(const _T1 r, const _T1 theta, const _T1 phi, _T2 &x, 166 _T2 &y, _T2 &z) { 167 x = r * std::sin(theta) * std::cos(phi); 168 y = r * std::sin(theta) * std::sin(phi); 169 z = r * std::cos(theta); 154 170 } 155 171
Note:
See TracChangeset
for help on using the changeset viewer.