toys/gps/source/geography.cpp

/**
 *  @file geography.cpp
 *  @author donkey (anjingyu_ws@foxmail.com)
 *  @brief Implementation of basic geography functions in \c ns::geo
 *  @version 0.1.0
 *  @date 2020-05-15
 *
 *  @copyright Copyright (c) 2020- donkey <anjingyu_ws@foxmail.com>.
 *
 */

#include "geo/geography.h"

#include "geo/math.h"

#include <cmath>  /**< std::sqrt std::fabs */
#include <cstdio> /**< sprintf */

#include "geodesic.h" /**< GeographicLib::Geodesic::WGS84 */

namespace ns {
namespace geo {

double Geography::distanceUnitConvertFactor(DistanceUnit from, DistanceUnit to)
{
    static const double cvtArray[] = {1.0, 1000.0, 100000.0, 100000000.0};
    return (cvtArray[static_cast<int>(from)] / cvtArray[static_cast<int>(to)]);
}

double Geography::distance(double lng1, double lat1, double lng2, double lat2)
{
    double distance = 0.0;
    GeographicLib::Geodesic::WGS84().Inverse(lat1, lng1, lat2, lng2, distance);
    return distance;
}

double Geography::angleToNorth(double lng1, double lat1, double lng2,
                               double lat2)
{
    double ec = Wgs84MinorRadius +
                (Wgs84MajorRadius - Wgs84MinorRadius) * (90.0 - lat1) / 90.0;
    double ed = ec * cos(ns::geo::Math::degree2Radian(lat1));
    double dx = (ns::geo::Math::degree2Radian(lng2) -
                 ns::geo::Math::degree2Radian(lng1)) *
                ed;
    double dy = (ns::geo::Math::degree2Radian(lat2) -
                 ns::geo::Math::degree2Radian(lat1)) *
                ec;
    double angle = atan(fabs(dx / dy)) * 180.0 / ns::geo::Math::pi();
    double dLng = lng2 - lng1;
    double dLat = lat2 - lat1;

    if (dLng > 0.0 && dLat <= 0.0)
    {
        angle = (90.0 - angle) + 90.0;
    }
    else if (dLng <= 0.0 && dLat < 0.0)
    {
        angle = angle + 180.0;
    }
    else if (dLng < 0.0 && dLat >= 0.0)
    {
        angle = (90.0 - angle) + 270.0;
    }

    return angle;
}

} // namespace geo
} // namespace ns