Draw line primitives

在栅格图中找到两点间的栅格(栅格图坐标), 这些点是两个点构成的线段经过的栅格, 使用算法:
Bresenham's_line_algorithm.

See also:

• https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
• https://yuiwong.org/gitlab/math/geometry/blob/master/yuiwonggeometry/include/yuiwong/segment.hpp.
• https://yuiwong.org/gitlab/math/geometrydoc/blob/master/draw-line-primitives.md

---

Implementation:

NOTE: Algorithm and implementation for integer arithmetic.

/**
 * Draw the segment with a Brush instance.
 * Implements in Bresenham's line algorithm: is a line drawing algorithm
 * that determines the points of an n-dimensional raster that should be
 * selected in order to form a close approximation to a straight line
 * between two points.
 * It is commonly used to draw line primitives in a bitmap image
 * (e.g. on a computer screen), as it uses only integer addition,
 * subtraction and bit shifting, all of which are very cheap operations
 * in standard computer architectures.
 * @tparam Brush the brush to draw, operator()(x, y) requried.
 * @param brush the brush instance.
 * @note
 * - Scalar should be integer!
 * - n should >= 2, this method for 2D, only use x and y.
 * - operator()(x, y) for Brush requried.
 * @return drawed count.
 * @sa
 * - https://en.wikipedia.org/wiki/Bresenham's_line_algorithm
 * - https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm#All_cases
 * - costmap_2d/include/costmap_2d/costmap_2d.h
 */
template<typename Brush>
uint32_t draw2D(const Brush& brush) const noexcept
{
    // Tparams check
    static_assert(2 == n || 3 == n, "n should be 2 or 3");
    static_assert(
        std::is_integral<typename std::decay<Scalar>::type>::value,
        "Scalar should be integer");
    // Draw
    uint32_t ndrawed = 0;
    // Cover gradients between 0 and -1 by checking whether y needs to
    // increase or decrease (i.e. dy < 0).
    const auto draw2DLow = [&brush, &ndrawed](const P& a, const P& b) {
        Scalar dx = b(0) - a(0);
        Scalar dy = b(1) - a(1);
        Scalar yi;
        if (dy < 0) {
            yi = -1;
            dy = -dy;
        } else {
            yi = 1;
        }
        const Scalar ddx = 2 * dx;
        const Scalar ddy = 2 * dy;
        Scalar D = ddy  - dx;
        Scalar y = a(1);
        for (const Scalar& x : Range<Scalar>{ a(0), b(0) }) {
            brush(x, y);
            ++ndrawed;
            if (D > 0) {
                y += yi;
                D -= ddx;
            }
            D += ddy;
        }
    };
    // By switching the x and y axis an implementation for positive or negative
    // steep gradients can be written as:
    const auto draw2DHigh = [&brush, &ndrawed](const P& a, const P& b) {
        Scalar dx = b(0) - a(0);
        Scalar dy = b(1) - a(1);
        Scalar xi;
        if (dx < 0) {
            xi = -1;
            dx = -dx;
        } else {
            xi = 1;
        }
        const Scalar ddx = 2 * dx;
        const Scalar ddy = 2 * dy;
        Scalar D = ddx  - dy;
        Scalar x = a(0);
        for (const Scalar& y : Range<Scalar>{ a(1), b(1) }) {
            brush(x, y);
            ++ndrawed;
            if (D > 0) {
                x += xi;
                D -= ddy;
            }
            D += ddx;
        }
    };
    // Need to detect whether x1 > x0 or y1 > y0 and reverse the input
    // coordinates before drawing
    if (std::abs(this->b(1) - this->a(1)) < std::abs(this->b(0) - this->a(0))) {
        if (this->a(0) > this->b(0)) {
            draw2DLow(this->b, this->a);
        } else {
            draw2DLow(this->a, this->b);
        }
    } else {
        if (this->a(1) > this->b(1)) {
            draw2DHigh(this->b, this->a);
        } else {
            draw2DHigh(this->a, this->b);
        }
    }
    return ndrawed;
}

Test and example:

TEST(Segment, draw2D)
{
    Segment<int32_t, 2> segment{ Eigen::Vector2i{ 3, 1 }, Eigen::Vector2i{ -6, -4 } };
    const std::vector<Eigen::Vector2i> expect{
        Eigen::Vector2i{ -6, -4 },
        Eigen::Vector2i{ -5, -3 },
        Eigen::Vector2i{ -4, -3 },
        Eigen::Vector2i{ -3, -2 },
        Eigen::Vector2i{ -2, -2 },
        Eigen::Vector2i{ -1, -1 },
        Eigen::Vector2i{ 0,  -1 },
        Eigen::Vector2i{ 1,  0 },
        Eigen::Vector2i{ 2,  0 },
        Eigen::Vector2i{ 3,  1 } };
    std::vector<Eigen::Vector2i> ps;
    const auto brush = [&ps](const int32_t x, const int32_t y) {
        std::cout << x << ' ' << y << '\n';
        ps.emplace_back(Eigen::Vector2i{ x, y });
    };
    ps.clear();
    EXPECT_EQ(expect.size(), segment.draw2D(brush));
    EXPECT_EQ(expect.size(), ps.size());
    if (expect.size() == ps.size()) {
        for (uint32_t i = 0; i < ps.size(); ++i) {
            EXPECT_EQ(expect[i], ps[i]);
        }
    }
    //
    Segment<int32_t, 3> s3;
    ps.clear();
    s3.draw2D(brush);
    EXPECT_EQ(1u, ps.size());
    if (1u == ps.size()) {
        EXPECT_EQ((Eigen::Vector2i{ 0, 0 }), ps[0]);
    }
    // BAD test:
    //Segment<double, 3> badS3;
    //badS3.draw2D(brush);
}