| 34 | } |
| 35 | |
| 36 | double GetDistance(ms::LatLon const & point1, ms::LatLon const & point2) |
| 37 | { |
| 38 | using namespace base; |
| 39 | using namespace std; |
| 40 | using math::DegToRad, math::Pow2; |
| 41 | |
| 42 | m2::PointD const p1 = {DegToRad(point1.m_lon), DegToRad(point1.m_lat)}; |
| 43 | m2::PointD const p2 = {DegToRad(point2.m_lon), DegToRad(point2.m_lat)}; |
| 44 | double const U1 = ReducedLatitude(p1.y); |
| 45 | double const U2 = ReducedLatitude(p2.y); |
| 46 | double const sinU1 = sin(U1); |
| 47 | double const cosU1 = cos(U1); |
| 48 | double const sinU2 = sin(U2); |
| 49 | double const cosU2 = cos(U2); |
| 50 | |
| 51 | // Difference in longitude of two points. |
| 52 | double const L = p2.x - p1.x; |
| 53 | // Difference in longitude on the auxiliary sphere. |
| 54 | double lambda = L; |
| 55 | double lambdaPrev = std::numeric_limits<double>::max(); |
| 56 | int iterations = kIterations; |
| 57 | double sinSigma = 1.; |
| 58 | double cosSigma = 0.; |
| 59 | double sigma = 0.; |
| 60 | double cosAlphaSquare = 0.; |
| 61 | double cosDoubleSigmaMid = 0.; |
| 62 | double cosDoubleSigmaMidSquare = 0.; |
| 63 | |
| 64 | while (iterations-- > 0 && !AlmostEqualAbs(lambda, lambdaPrev, kEps)) |
| 65 | { |
| 66 | sinSigma = sqrt(Pow2(cosU2 * sin(lambda)) + Pow2(cosU1 * sinU2 - sinU1 * cosU2 * cos(lambda))); |
| 67 | cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cos(lambda); |
| 68 | sigma = atan2(sinSigma, cosSigma); |
| 69 | double const sinAlpha = cosU1 * cosU2 * sin(lambda) / sinSigma; |
| 70 | cosAlphaSquare = 1 - Pow2(sinAlpha); |
| 71 | |
| 72 | // Cosine of SigmaMid - angular separation between the midpoint of the line and the equator. |
| 73 | if (fabs(cosAlphaSquare) < DBL_EPSILON) |
| 74 | cosDoubleSigmaMid = 0; |
| 75 | else |
| 76 | cosDoubleSigmaMid = cos(sigma) - 2 * sinU1 * sinU2 / cosAlphaSquare; |
| 77 | cosDoubleSigmaMidSquare = Pow2(cosDoubleSigmaMid); |
| 78 | |
| 79 | double const C = (kF / 16.0) * cosAlphaSquare * (4.0 + kF * (4.0 - 3.0 * cosAlphaSquare)); |
| 80 | |
| 81 | lambdaPrev = lambda; |
| 82 | lambda = L + (1 - C) * kF * sinAlpha * |
| 83 | (sigma + C * sinSigma * (cosDoubleSigmaMid + C * cosSigma * (-1 + 2 * cosDoubleSigmaMidSquare))); |
| 84 | } |
| 85 | |
| 86 | // Fallback solution. |
| 87 | if (!AlmostEqualAbs(lambda, lambdaPrev, kEps)) |
| 88 | return DistanceOnEarth(point1, point2); |
| 89 | |
| 90 | double constexpr aSquare = kA * kA; |
| 91 | double constexpr bSquare = kB * kB; |
| 92 | |
| 93 | double const uSquare = cosAlphaSquare * (aSquare - bSquare) / bSquare; |