toys/gps/docs/source/math/angle.cpp

#include <cfloat>
#include <cmath>

#include "geo/private/math/angle.h"

namespace ns
{
namespace geo
{

double normalize_angle(double angle)
{
    while (angle < 0.0)
    {
        angle += 360.0;
    }

    while (angle > 360.0)
    {
        angle -= 360.0;
    }
    return angle;
}

double average_angle(double angle1, double angle2, double ratio)
{
    double a1 = normalize_angle(angle1);
    double a2 = normalize_angle(angle2);

    return a1 + ratio * (a2 - a1);
}

double opposite_angle(const double angle)
{
    return normalize_angle(angle + 180);
}

double diff_angle(double a1, double a2)
{
    double a11 = normalize_angle(a1);
    double a22 = normalize_angle(a2);

    double d1 = fabs(a11 - a22);
    if (d1 > 180)
        d1 = 360 - d1;

    return d1;
}

double cal_north_angle(PointD pt1, PointD pt2)
{
    return cal_north_angle(pt2.x - pt1.x, pt2.y - pt1.y);
}

double cal_north_angle(double dx, double dy)
{
    if (fabs(dx) < DBL_EPSILON)
        return dy >= 0 ? 0 : 180;
    if (fabs(dy) < DBL_EPSILON)
        return dx >= 0 ? 90 : 270;

    double ratio = std::abs(dy / dx);
    /// 第一象限
    if (dx > 0 && dy > 0)
        return 90 - Radian2Degree(atan(ratio));
    /// 第二象限
    else if (dx > 0 && dy < 0)
        return 90 + Radian2Degree(atan(ratio));
    /// 第三象限
    else if (dx < 0 && dy < 0)
        return 270 - Radian2Degree(atan(ratio));
    /// 第四象限
    else
        return 270 + Radian2Degree(atan(ratio));
}

double cal_east_angle(const PointD& pt1, const PointD& pt2)
{
    return normalize_angle(Radian2Degree(atan2((pt2.y - pt1.y), (pt2.x - pt1.x))));
}

double cal_lon_lat_angle(double lat1, double lon1, double lat2, double lon2)
{
    double numerator = sin(Degree2Radian(lon2 - lon1)) * cos(Degree2Radian(lat2));
    double denominator =
        cos(Degree2Radian(lat1)) * sin(Degree2Radian(lat2)) -
        sin(Degree2Radian(lat1)) * cos(Degree2Radian(lat2)) * cos(Degree2Radian(lon2 - lon1));

    double x = atan2(abs(numerator), abs(denominator));
    double result = x;

    if (lon2 > lon1) // right quadrant
    {
        if (lat2 > lat1) // first quadrant
            result = x;
        else if (lat2 < lat1) // forth quadrant
            result = c_pi() - x;
        else
            result = c_pi_2(); // in positive-x axis
    }
    else if (lon2 < lon1) // left quadrant
    {
        if (lat2 > lat1) // second quadrant
            result = 2 * c_pi() - x;
        else if (lat2 < lat1) // third quadrant
            result = c_pi() + x;
        else
            result = c_pi() * 3 / 2; // in negative-x axis
    }
    else // same longitude
    {
        if (lat2 > lat1) // in positive-y axis
            result = 0;
        else if (lat2 < lat1)
            result = c_pi(); // in negative-y axis
        else
            throw "Shouldn't be same location!";
    }

    return result * 180 / c_pi();
}

} // namespace geo
} // namespace ns