toys/gps/docs/source/math/sweep_line_intersect_solver.h

#ifndef SWEEP_LINE_INTERSECT_SOLVER_H
#define SWEEP_LINE_INTERSECT_SOLVER_H

#include <vector>

#include "Eigen/Dense"
#include "geo/private/util/range_util.h"
#include "sweep_line_intersect_solver_base.h"

using namespace Eigen;

namespace ns
{
namespace geo
{

class SweepLineIntersectSolver : public SweepLineIntersectSolverBase
{
public:
    SweepLineIntersectSolver(const Matrix2Xd& points)
        : SweepLineIntersectSolverBase(points)
    {
    }

    std::vector<std::tuple<Index, Index, Vector2d>> solve()
    {
        if (!init_events())
            return {};
        proc_coincide_ends();
        proc_events();

        std::vector<std::tuple<Index, Index, Vector2d>> ret;
        for (auto& rcd : m_checked_pairs)
        {
            if (rcd.second >= 0)
            {
                ret.push_back(
                    std::make_tuple(rcd.first.first / 2, rcd.first.second / 2,
                                    std::get<2>(m_intersections[rcd.second])));
            }
        }
        return ret;
    }

private:
    // check intersection between sweep line i and j
    void check_sweep_line_intersect(Index sl_i, Index sl_j)
    {
        Index num_sl = (Index)m_sweep_lines.size();
        if (sl_i < 0 || sl_i >= num_sl || sl_j < 0 || sl_j >= num_sl)
        {
            return;
        }

        // line index
        Index line_i, line_j;
        line_i = m_sweep_lines[sl_i]; // the index of the original line segments
        line_j = m_sweep_lines[sl_j];

        check_intersect(line_i, line_j);
    }

    // check duplicate ends. i.e. the special case that line segments intersect
    // at their ends
    void proc_coincide_ends()
    {
        auto to_line_idx = [](Index point_idx) {
            return point_idx - (point_idx & 0x1);
        };

        for (auto it_lower = m_eventss.begin(); it_lower != m_eventss.end();)
        {
            Vector2d pnt = m_points.col(it_lower->second);

            double x = pnt(0);

            auto it_upper = std::next(it_lower);
            while (it_upper != m_eventss.end() && it_upper->first == x &&
                   m_points(1, it_upper->second) == pnt(1))
            {
                ++it_upper;
            }

            if (std::distance(it_lower, it_upper) <= 1)
            {
                // do nothing
            }
            else
            {
                for (auto it1 = it_lower; it1 != it_upper; ++it1)
                {
                    Index line_idx_1 = to_line_idx(it1->second);
                    for (auto it2 = std::next(it1); it2 != it_upper; ++it2)
                    {
                        Index line_idx_2 = to_line_idx(it2->second);
                        add_intersection(line_idx_1, line_idx_2, pnt);
                    }
                }
            }

            it_lower = it_upper;
        }
    }

    std::size_t add_vertical_sweep_line(Index new_line_idx)
    {
        Index idx = new_line_idx;

        double x = m_points(0, idx);

        double y1 = m_points(1, idx);
        double y2 = m_points(1, idx + 1);
        if (y1 > y2)
        {
            std::swap(y1, y2);
        }
        auto it1 = geo::lower_bound(
            m_sweep_lines.begin(), m_sweep_lines.end(),
            [&](Index i) { return int(get_y_at(i, x) < y1); });

        // add m_intersections
        for (auto it = it1; it != m_sweep_lines.end(); ++it)
        {
            Index line_idx = *it;

            if (!is_checked(new_line_idx, line_idx))
            {
                double y = get_y_at(line_idx, x);
                if (y <= y2)
                {
                    add_intersection(new_line_idx, line_idx, Vector2d{x, y});
                }
                else
                {
                    break;
                }
                // do not add intersection event
            }
        }

        // insert the sweep line
        auto it = m_sweep_lines.insert(it1, new_line_idx);
        std::size_t i = std::distance(m_sweep_lines.begin(), it);

        return i;
    }

    /*
     * there are two special cases need to be addressed:
     * 1. the new sweep line is vertical
     * 2. the start point is on other sweep lines
     */
    std::size_t add_sweep_line(Index new_line_idx)
    {
        Index idx = new_line_idx;
        Vector2d node_pnt = m_points.col(idx);
        double x = node_pnt(0);
        double y = node_pnt(1);

        if (m_points(0, idx + 1) == x)
        {
            return add_vertical_sweep_line(new_line_idx);
        }

        auto range = geo::equal_range(m_sweep_lines.begin(),
                                      m_sweep_lines.end(), [&](Index i) {
                                          double y_ = get_y_at(i, x);
                                          return (y_ > y) - (y_ < y);
                                      });

        std::size_t i;

        // add intersections
        /*
         * todo if there are more than one sweep lines in the range, that means
         * they intersect here and they should be reordered; in this case, after
         * adding the sweep line, there is no need to check intersection
         */
        for (auto it = range.first; it != range.second; ++it)
        {
            Index line_idx = *it;

            if (!is_checked(new_line_idx, line_idx))
            {
                add_intersection(new_line_idx, line_idx, Vector2d{x, y});
                // do not add intersection event
            }
        }

        // insert the sweep line in correct position
        // since this is rare situation, use sequential search
        bool added = false;
        for (auto it = range.first; it != range.second; ++it)
        {
            Index line_idx = *it;
            if (is_right(line_idx, m_points.col(idx + 1)))
            {
                it = m_sweep_lines.insert(it, new_line_idx);
                i = std::distance(m_sweep_lines.begin(), it);
                added = true;
                break;
            }
        }

        if (!added)
        {
            auto it = m_sweep_lines.insert(range.second, new_line_idx);
            i = std::distance(m_sweep_lines.begin(), it);
        }

        return i;
    }

    void proc_events()
    {
        for (auto it = m_eventss.begin(); it != m_eventss.end(); ++it)
        {
            auto& event = *it;
            double x = event.first;
            if (event.second >= 0)
            {
                Index node = event.second;
                if ((node & 0x1) == 0)
                {
                    // even index, the left end
                    Index seg_idx = node;
                    int idx = add_sweep_line(seg_idx);
                    // check intersection with SL below
                    check_sweep_line_intersect(idx, idx - 1);
                    // check intersection with SL above
                    check_sweep_line_intersect(idx, idx + 1);
                }
                else
                {
                    // odd index, the right end
                    Index seg_idx = node - 1;
                    int idx = find_sweep_line(seg_idx);
                    // check intersection with SL below
                    check_sweep_line_intersect(idx - 1, idx + 1);
                    m_sweep_lines.erase(m_sweep_lines.begin() + idx);
                }
            }
            else
            {
                // an intersection event
                Index intersection_idx = -1 - event.second;

                Index sl1, sl2;
                std::tie(sl1, sl2, std::ignore) =
                    m_intersections[intersection_idx];

                int idx1, idx2;
                idx1 = find_sweep_line(sl1);
                idx2 = idx1 + 1;
                if (idx2 >= (int)m_sweep_lines.size() ||
                    m_sweep_lines[idx2] != sl2)
                {
                    assert(!(idx1 <= 0 || m_sweep_lines[idx1 - 1] != sl2));
                    assert(idx1 > 0);
                    idx2 = idx1 - 1;
                    assert(m_sweep_lines[idx2] == sl2);
                    std::swap(idx1, idx2);
                }
                std::swap(m_sweep_lines[idx1], m_sweep_lines[idx2]);

                // check intersection with SL below
                check_sweep_line_intersect(idx1 - 1, idx1);
                // check intersection with SL above
                check_sweep_line_intersect(idx2 + 1, idx2);
            }
        }
    }
};

} // namespace geo
} // namespace ns

#endif // SWEEP_LINE_INTERSECT_SOLVER_H