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

#include "geo/private/util/range_util.h"
#include "geo/private/util/intersection_util.h"

namespace ns
{
namespace geo
{

/**
 * {first index of segment, second index of segment, intersect point}
 */
typedef std::tuple<Eigen::Index, Eigen::Index, Eigen::Vector2d> LineSegmentPair;

namespace
{

Eigen::Matrix2Xd resample_polyline(const Eigen::Matrix2Xd& pl, double dist)
{
    (void)(dist);
    return pl;
    //    Eigen::Matrix2Xd resample;
    //    for (int idx = 1; idx < pl.cols(); idx++) {
    //        Eigen::Vector2d p1 = pl.col(idx - 1);
    //        Eigen::Vector2d p2 = pl.col(idx);
    //        int last_cols = pl.cols();
    //        int sub_cols = std::floor((p1 - p2).norm() / dist);
    //        resample.conservativeResize(2, last_cols + sub_cols);
    //        for (int jdx = 0; jdx < sub_cols; jdx++) {
    //            resample.col(last_cols + jdx) = p1 + jdx * (p2 - p1) / sub_cols;
    //        }
    //    }
    //    return resample;
}

/**
 * calculate distance from point to polyline
 * if point can't project on polyline, the cloest point will be first or last
 * node
 *
 * @return
 *  1) the neighbour node's index of closest point lies on polyline
 *  2) the ratio of the project point between polyline nodes
 *  3) closest point's distance
 */
std::tuple<std::size_t, double, double> p2pl_distance(
    const Eigen::Matrix2Xd& polyline, const Eigen::Vector2d& point)
{
    auto sqr = [](double x) { return x * x; };
    const auto count = polyline.cols();
    assert(count > 0);

    Eigen::Vector2d b = polyline.col(0);
    Eigen::Vector2d dbq = b - point;

    auto p_idx = -1;
    auto p_dt = 0.;
    double dist = dbq.squaredNorm();
    for (auto i = 1; i < count; ++i)
    {
        Eigen::Vector2d a = b;
        Eigen::Vector2d daq = dbq;
        b = polyline.col(i);
        dbq = b - point;
        const Eigen::Vector2d dab = a - b;
        const double inv_sqrlen = 1. / dab.squaredNorm();
        const double t = (dab(0) * daq(0) + dab(1) * daq(1)) * inv_sqrlen;
        if (t < 0.)
        {
            continue;
        }

        double current_dist;
        if (t <= 1.)
        {
            current_dist = sqr(dab(0) * dbq(1) - dab(1) * dbq(0)) * inv_sqrlen;
        }
        else
        {
            // t>1.
            current_dist = dbq.squaredNorm();
        }
        if (current_dist < dist)
        {
            dist = current_dist;
            p_idx = i;
            p_dt = t;
        }
    }
    return std::make_tuple(p_idx, p_dt, dist);
}

} // namespace

bool l2l_intersect(const Eigen::Matrix2d& l1, const Eigen::Matrix2d& l2)
{
    Eigen::Vector2d pt;
    return l2l_intersect(l1, l2, pt);
}

bool l2l_intersect(const Eigen::Matrix2d& l1, const Eigen::Matrix2d& l2,
                   Eigen::Vector2d& intersect_point)
{
    Eigen::Vector2d v1 = l1.col(1) - l1.col(0);
    Eigen::Vector2d v2 = l2.col(1) - l2.col(0);

    Eigen::Matrix2d m;
    m << v1, v2;

    double denominator = m.determinant();
    if (std::abs(denominator) < 1e-16)
    {
        // parallel
        return false;
    }

    m << v2, l1.col(0) - l2.col(0);
    double r = m.determinant() / denominator;
    if (r < 0 || r > 1)
    {
        return false;
    }

    m.col(0) = v1;
    double s = m.determinant() / denominator;
    if (s < 0 || s > 1)
    {
        return false;
    }

    intersect_point = l1.col(0) + r * v1;

    return true;
}

/**
 * get intersection points among i-th line segment
 * @param line_segments represented by points, eg. (0, 1) is the first line
 * segment, (2, 3) is the second, (2i, 2i+1) is the i-th segment
 * @return a list of LineSegmentPair
 */
std::vector<LineSegmentPair> get_intersection(
    const Eigen::Matrix2Xd& line_segments)
{
    assert(line_segments.cols() % 2 == 0);

    std::vector<LineSegmentPair> ret;

    int n = static_cast<int>(line_segments.cols() / 2);
    for (int i = 0; i < n - 1; ++i)
    {
        for (int j = i + 1; j < n; ++j)
        {
            Eigen::Vector2d intersect_pnt;
            const Eigen::Matrix2d& l1 =
                line_segments.block(0, static_cast<Eigen::Index>(2 * i), 2, 2);
            const Eigen::Matrix2d& l2 = line_segments.block(0, static_cast<Eigen::Index>(2 * j), 2, 2);
            if (l2l_intersect(l1, l2, intersect_pnt))
            {
                ret.emplace_back(i, j, intersect_pnt);
            }
        }
    }
    return ret;
}

/**
 * get self-intersection of a polyline
 * @param polyline polyline represented by matrix
 * @return a list of LineSegmentPair
 */
std::vector<LineSegmentPair> get_self_intersection(
    const Eigen::Matrix2Xd& polyline)
{
    Eigen::Index n = polyline.cols();
    Eigen::Index m = n - 1;
    Eigen::Matrix2Xd segments(2, 2 * m);

    for (Eigen::Index i = 0; i < n - 1; ++i)
    {
        Eigen::Index j = 2 * i;
        segments.middleCols(j, 2) = polyline.middleCols(i, 2);
    }

    std::vector<LineSegmentPair> ret;
    auto intersects = get_intersection(segments);

    for (const LineSegmentPair& item : intersects)
    {
        Eigen::Index i, j;
        std::tie(i, j, std::ignore) = item;
        if (j != i + 1 && j != i - 1)
        {
            ret.push_back(item);
        }
    }

    return ret;
}

void remove_zero_length_segments(Eigen::Matrix2Xd& polyline)
{
    Eigen::Index n = polyline.cols();

    if (n < 2)
    {
        return;
    }

    Eigen::VectorXd length =
        (polyline.rightCols(n - 1) - polyline.leftCols(n - 1)).colwise().norm();
    std::vector<Eigen::Index> zero_nodes;
    for (Eigen::Index i = 0; i < n - 1; ++i)
    {
        if (length(i) < 1.e-6)
        {
            zero_nodes.push_back(i);
        }
    }

    Eigen::Matrix2Xd new_polyline;
    if (!zero_nodes.empty())
    {
        Eigen::Index m = zero_nodes.size();
        Eigen::Index j = zero_nodes.front();
        Eigen::Index cnt;
        for (Eigen::Index i = 0; i < m - 1; ++i)
        {
            cnt = zero_nodes[i + 1] - zero_nodes[i] - 1;
            polyline.middleCols(j, cnt) =
                polyline.middleCols(zero_nodes[i] + 1, cnt);
            j += cnt;
        }
        cnt = n - zero_nodes.back() - 1;
        polyline.middleCols(j, cnt) = polyline.rightCols(cnt);
        polyline.conservativeResize(2, n - m);
    }
}

void resolve_self_intersection(Eigen::Matrix2Xd& polyline)
{
    remove_zero_length_segments(polyline);

    Eigen::Index n = polyline.cols();

    if (n <= 3)
    {
        return;
    }

    // the result is sorted by the index
    std::vector<LineSegmentPair> intersections =
        get_self_intersection(polyline);

    if (intersections.empty())
    {
        return;
    }

    Eigen::Matrix2Xd new_polyline(2, n);

    Eigen::Index read_pos = 0;
    Eigen::Index write_pos = 0;
    for (std::size_t i = 0; i < intersections.size(); ++i)
    {
        const LineSegmentPair& rcd = intersections[i];
        Eigen::Index idx = std::get<0>(rcd);

        /// if current intersection and next one has same first segment,
        /// continue
        if ((i + 1) != intersections.size() &&
            std::get<0>(intersections[i + 1]) == idx)
        {
            continue;
        }

        if (i > 0 && (read_pos - 1) == idx)
        {
            new_polyline.col(write_pos) = std::get<2>(rcd);
            ++write_pos;
            read_pos = std::get<1>(rcd) + 1;
        }
        else if (idx >= read_pos)
        {
            Eigen::Index cnt = idx - read_pos + 1;
            new_polyline.middleCols(write_pos, cnt) =
                polyline.middleCols(read_pos, cnt);
            write_pos += cnt;
            new_polyline.col(write_pos) = std::get<2>(rcd);
            ++write_pos;
            read_pos = std::get<1>(rcd) + 1;
        }
    }

    // copy the rest of the polyline
    Eigen::Index cnt = n - read_pos;
    new_polyline.middleCols(write_pos, cnt) = polyline.middleCols(read_pos, cnt);
    write_pos += cnt;

    new_polyline.conservativeResize(2, write_pos);
    polyline.swap(new_polyline);
}

bool is_path_intersect(const Eigen::Matrix2Xd& path_1,
                       const Eigen::Matrix2Xd& path_2,
                       Eigen::Vector2d* intersect_point)
{
    Eigen::Index n = path_1.cols();
    Eigen::Index m = path_2.cols();

    Eigen::Matrix2Xd segments(2, 2 * (n - 1 + m - 1));
    for (Eigen::Index i = 0; i < n - 1; ++i)
    {
        segments.middleCols(2 * i, 2) = path_1.middleCols(i, 2);
    }

    for (Eigen::Index i = 0; i < m - 1; ++i)
    {
        segments.middleCols(2 * (n - 1 + i), 2) = path_2.middleCols(i, 2);
    }

    auto intersects = get_intersection(segments);

    for (auto item : intersects)
    {
        Eigen::Index i = std::get<0>(item);
        Eigen::Index j = std::get<1>(item);

        if (i < n - 1 && j >= n - 1)
        {
            if (intersect_point)
            {
                *intersect_point = std::get<2>(item);
            }
            return true;
        }
    }

    return false;
}

bool is_path_intersect_dis(const Eigen::Matrix2Xd& path1,
                           const Eigen::Matrix2Xd& path2,
                           Eigen::Vector2d* point1, Eigen::Vector2d* point2,
                           double distance_threshold)
{
    distance_threshold = std::max(distance_threshold, 0.5);
    Eigen::Matrix2Xd resample_path1 =
        resample_polyline(path1, distance_threshold);
    Eigen::Matrix2Xd resample_path2 =
        resample_polyline(path2, distance_threshold);
    // it is a n * m implementation currently
    auto sqr_threshold = distance_threshold * distance_threshold;
    const auto n = resample_path1.cols();
    for (auto i = 0; i < n; ++i)
    {
        double dist;
        int index;
        std::tie(index, std::ignore, dist) =
            p2pl_distance(resample_path2, resample_path1.col(i));
        if (dist < sqr_threshold && index >= 0)
        {
            *point1 = resample_path1.col(i);
            *point2 = resample_path2.col(index);
            return true;
        }
    }
    return false;
}

} // namespace geo
} // namespace ns