#include "Wgs84.h" #include "Gaussian.h" namespace jf { double GetN(const double& radB) { double cosB = cos(radB); double V = sqrt(1.0 + ep2 * cosB * cosB); double N = c / V; return N; } double GetX(const double& radB) { double sinB = sin(radB); double cosB = cos(radB); double al = a0 * radB; double sc = sinB * cosB; double ss = sinB * sinB; double X = al - sc * ((a2 - a4 + a6) + (2.0 * a4 - a6 * 16.0 / 3.0) * ss + a6 * 16.0 * ss * ss / 3.0); return X; } double GetLatByX(const double& X) { double Bfi0 = X / a0; double Bfi1 = 0.0; bool num = true; double minAngle = PI * 1e-9; double menus_minAngle = 0 - minAngle; double sinB, sinB2, cosB, FBfi, deltaB; while (num == true) { num = false; sinB = sin(Bfi0); sinB2 = sinB * sinB; cosB = cos(Bfi0); FBfi = 0.0 - sinB * cosB * ((a2 - a4 + a6) + sinB2 * (2.0 * a4 - 16.0 * a6 / 3.0) + sinB2 * sinB2 * a6 * 16.0 / 3.0); Bfi1 = (X - FBfi) / a0; deltaB = Bfi1 - Bfi0; if (deltaB < menus_minAngle || deltaB > minAngle) { num = true; Bfi0 = Bfi1; } } return Bfi1; } double GetV(const double& lat) { double cosB = cos(lat); double V = sqrt(1 + ep2 * cosB * cosB); return V; } /* Array BLH2ECEF(const Array& ptBLH){ double cosB = cos( Degree2Rad( ptBLH[0] ) ); double sinB = sin( Degree2Rad( ptBLH[0] ) ); double cosL = cos( Degree2Rad( ptBLH[1] ) ); double sinL = sin( Degree2Rad( ptBLH[1] ) ); double N = GetN( Degree2Rad( ptBLH[0] ) ); double X = (N + ptBLH[2]) * cosB * cosL; double Y = (N + ptBLH[2]) * cosB * sinL; double Z = (N * (1 - e2) + ptBLH[2]) * sinB; return Array({X,Y,Z}); } Array ECEF2BLH(const Array& ptXYZ){ double L_radian = atan(ptXYZ[1] / ptXYZ[0]); if(L_radian < 0){ L_radian += PI; } double L = Rad2Degree(L_radian); double sqrtXY = sqrt(ptXYZ[0] * ptXYZ[0] + ptXYZ[1] * ptXYZ[1]); double t0 = ptXYZ[2] / sqrtXY; double p = c * e2 / sqrtXY; double k = 1 + ep2; double ti = t0; double ti1 = 0.0; int loop = 0; while(loop<10000){ loop += 1; ti1 = t0 + p * ti / sqrt(k + ti * ti); if(abs(ti1 - ti) < 1e-12) break; ti=ti1; } double B_radian = atan(ti1); double N = GetN(B_radian); double H = sqrtXY / cos(B_radian) - N; double B = Rad2Degree(B_radian); return Array({B,L,H}); } matrix GetR_ENU2ECEF(const double& radianL,const double& radianB){ double sinB = sin(radianB); double cosB = cos(radianB); double sinL = sin(radianL); double cosL = cos(radianL); matrix R4 = matrix({{-sinL, -sinB * cosL, cosB * cosL}, {cosL, -sinB * sinL, cosB * sinL}, {0, cosB, sinB}}); return R4; } double GetAngleDif(double distance){ double angleRafian = distance / a; return Rad2Degree(angleRafian); } Array GetAngleDif(Array& distance){ Array angleRafian = distance / a; return Rad2Degree(angleRafian); } */ } // namespace jf