toys/gps/docs/source/util/math_util.cpp

#include <cmath>

#include "basic/basic.h"
#include "geo/private/util/math_util.h"
#include "geo/private/util/angle_util.h"

namespace ns
{
namespace geo
{

double point_2_point_distance(const Position& point_1, const Position& point_2)
{
    double x_difference = point_1.x - point_2.x;
    double y_difference = point_1.y - point_2.y;
    return sqrt(x_difference * x_difference + y_difference * y_difference);
}

double calculate_relative_angle(const Position& reference_position,
                                const double& reference_angle,
                                const Position& target_position)
{
    double bearing_angle = atan2(target_position.y - reference_position.y,
                                 target_position.x - reference_position.x);

    return angle_modulus(angle_modulus(bearing_angle) -
                         angle_modulus(reference_angle));
}

double angle_modulus(double angle)
{
    return std::fmod(angle + 2 * c_pi(), 2 * c_pi());
}

bool is_inside_or_on_polygon(const Eigen::Vector2d& p, const Eigen::Matrix2Xd& v)
{
    const Eigen::Index n = v.cols();

    int cnt = 0;
    double py = p(1);
    bool rel_j = (v(1, n - 1) <= py);
    for (Eigen::Index i = 0, j = n - 1; i < n; ++i, j = i - 1)
    {
        bool rel_i = (v(1, i) <= py);
        if (rel_i != rel_j)
        {
            Eigen::Matrix2d m;
            m << p - v.col(i), v.col(j) - v.col(i);
            double det = m.determinant();
            if (std::abs(det) < 1.e-6)
            {
                // on the edge i => j
                return true;
            }

            // increase the count only when the point is on the left side of the
            // segment
            cnt += (int)((v(1, i) < v(1, j)) == (det < 0));
        }
        else if (v(1, i) == py)
        {
            if (v(1, j) == py)
            {
                // i => j is horizontal
                if ((p(0) - v(0, i)) * (p(0) - v(0, j)) <= 0)
                {
                    // on the edge
                    return true;
                }
            }
            else if (p(0) == v(0, i))
            {
                // exactly on the vertex
                return true;
            }
        }
        rel_j = rel_i;
    }

    return ((cnt & 1) == 1);
}

Eigen::Matrix2Xd get_outline(const Eigen::Vector2d& pos,
                             double yaw, double length, double width)
{
    Eigen::Matrix2Xd vehicle_outline(2, 5);

    vehicle_outline.col(0) = pos;

    const auto h_l = length / 2;
    const auto h_w = width / 2;
    const Eigen::Vector2d v_y(std::cos(yaw), std::sin(yaw));
    const Eigen::Vector2d v_v_y(std::cos(yaw + EIGEN_PI / 2),
                                std::sin(yaw + EIGEN_PI / 2));
    Eigen::Vector2d p_f = pos + h_l * v_y;
    Eigen::Vector2d p_r = pos - h_l * v_y;

    // point front left
    vehicle_outline.col(1) = p_f + h_w * v_v_y;
    // point front right
    vehicle_outline.col(2) = p_f - h_w * v_v_y;
    // point rear left
    vehicle_outline.col(3) = p_r + h_w * v_v_y;
    // point rear right
    vehicle_outline.col(4) = p_r - h_w * v_v_y;

    return vehicle_outline;
}

double distance(const Eigen::Vector2d& l_ps,
                const Eigen::Vector2d& l_pe,
                const Eigen::Vector2d& p)
{
    auto line = l_pe - l_ps;
    auto a = l_pe - p;
    auto b = l_ps - p;
    auto sqr = [](double x) { return x * x; };
    const double inv_sqrlen = 1. / line.squaredNorm();
    const double t = (line(0) * a(0) + line(1) * a(1)) * inv_sqrlen;
    double sqr_dist;
    if (t < 0.)
    {
        sqr_dist = a.squaredNorm();
    }
    else if (t <= 1.)
    {
        sqr_dist = sqr(line(0) * b(1) - line(1) * b(0)) * inv_sqrlen;
    }
    else
    {
        // t>1.
        sqr_dist = b.squaredNorm();
    }
    return std::sqrt(sqr_dist);
}

// Get vector to vector angel
double get_angle_v2v(const Eigen::Vector2d& v1, const Eigen::Vector2d& v2)
{
    Eigen::Matrix2d m;
    m << v1, v2;

    double a = std::atan2(m.determinant(), v1.dot(v2));

    // normalize to [0, 2π]
    if (a < 0)
    {
        a += 2 * EIGEN_PI;
    }

    return a;
}

double get_heading(const Eigen::Vector2d& v1, const Eigen::Vector2d& v2)
{
    double dx = v2.x() - v1.x();
    double dy = v2.y() - v1.y();

    return std::atan2(dy, dx);
}

} // namespace geo
} // namespace ns