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

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

namespace ns
{
namespace geo
{

namespace
{
constexpr auto epsilon = 1e-8;
constexpr auto fairly_epsilon = 0.01;          // 1cm. Heuristic
constexpr auto angle_epsilon = c_pi() / 180.0; // 1⁰. Heuristic

enum class EpsilonType
{
    Normal,
    Fair,
    Angle
};

template <EpsilonType>
struct EpsilonTraits;

template <>
struct EpsilonTraits<EpsilonType::Normal>
{
    static constexpr auto value = epsilon;
};
template <>
struct EpsilonTraits<EpsilonType::Fair>
{
    static constexpr auto value = fairly_epsilon;
};
template <>
struct EpsilonTraits<EpsilonType::Angle>
{
    static constexpr auto value = angle_epsilon;
};

template <EpsilonType Type>
bool are_equal(const double value1, const double value2)
{
    const auto abs_diff = std::abs(value1 - value2);
    return abs_diff <= EpsilonTraits<Type>::value;
}

} // namespace

bool are_fairly_equal(const double value1, const double value2)
{
    return are_equal<EpsilonType::Fair>(value1, value2);
}

bool are_equal(const double value1, const double value2)
{
    return are_equal<EpsilonType::Normal>(value1, value2);
}

bool is_zero(const double value)
{
    return std::fpclassify(value) == FP_ZERO;
}

int32_t JudgeSideness(const PointD& p1, const PointD& p2, const PointD& p)
{
    PointD v1(p2.x - p1.x, p2.y - p1.y);
    PointD v2(p.x - p1.x, p.y - p1.y);
    double value = v1.x * v2.y - v2.x * v1.y;
    if (value > 0)
        return 1;
    else if (value < 0)
        return -1;
    else
        return 0;
}

double vectorAngleDiff(const PointD& p1, const PointD& p2, const PointD& q1, const PointD& q2)
{
    double angle1 = cal_east_angle(p1, p2);
    double angle2 = cal_east_angle(q1, q2);
    return diff_angle(angle1, angle2);
}

double linearDis2Begin(double sValue, double eValue, double length, double mValue)
{
    if (sValue < eValue)
    {
        if (mValue < sValue)
            return 0;
        else if (mValue > eValue)
            return length;
        else
            return length * (mValue - sValue) / (eValue - sValue);
    }
    else if (sValue > eValue)
    {
        if (mValue < eValue)
            return 0;
        else if (mValue > sValue)
            return length;
        else
            return length * (mValue - sValue) / (eValue - sValue);
    }
    else
    {
        return -1;
    }
}

double linearValueAtS(double sValue, double eValue, double length, double s)
{
    if (is_zero(length) || is_zero(s) || are_equal(sValue, eValue))
    {
        return sValue;
    }
    else
    {
        return s / length * (eValue - sValue) + sValue;
    }
}

} // namespace geo
} // namespace ns