RasterPolygonUtils.h

Go to the documentation of this file.

/*
 * Copyright (C) by
 *   MetraLabs GmbH (MLAB), GERMANY
 * and
 *   Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
 * All rights reserved.
 *
 * Contact: info@mira-project.org
 *
 * Commercial Usage:
 *   Licensees holding valid commercial licenses may use this file in
 *   accordance with the commercial license agreement provided with the
 *   software or, alternatively, in accordance with the terms contained in
 *   a written agreement between you and MLAB or NICR.
 *
 * GNU General Public License Usage:
 *   Alternatively, this file may be used under the terms of the GNU
 *   General Public License version 3.0 as published by the Free Software
 *   Foundation and appearing in the file LICENSE.GPL3 included in the
 *   packaging of this file. Please review the following information to
 *   ensure the GNU General Public License version 3.0 requirements will be
 *   met: http://www.gnu.org/copyleft/gpl.html.
 *   Alternatively you may (at your option) use any later version of the GNU
 *   General Public License if such license has been publicly approved by
 *   MLAB and NICR (or its successors, if any).
 *
 * IN NO EVENT SHALL "MLAB" OR "NICR" BE LIABLE TO ANY PARTY FOR DIRECT,
 * INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF
 * THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "MLAB" OR
 * "NICR" HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * "MLAB" AND "NICR" SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND "MLAB" AND "NICR" HAVE NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
 */

#ifndef _MIRA_BASE_INCLUDE_GEOMETRY_RASTERPOLYGONUTILS_H_
#define _MIRA_BASE_INCLUDE_GEOMETRY_RASTERPOLYGONUTILS_H_

#include <algorithm>
#include <geometry/Polygon.h>

namespace mira { namespace Private {


class EdgeCuts
{
    // Class that generates cuts of an edge with a grid
public:
    EdgeCuts(Point2f start, Point2f end)
    {
        if (start.y() < end.y()) {
            std::swap(start, end);
        }
        // y is descending from start to end

        // x = m*y + n
        const auto m = (end.x() - start.x()) / (end.y() - start.y());
        const auto n = start.x() - m * start.y();

        auto startY = (int)std::round(start.y());
        auto endY = (int)std::round(end.y());

        const float currentGrid = startY - 0.5;
        mCurrentCut = m * currentGrid + n;

        mNumberOfCutsWithGrid = startY - endY;
        // m*y + n = x
        // m*(y - 1) + n = m*y + n - m = x - m
        mDeltaToNextCut = -m;
        mMaxY = startY - 1;
    }

    inline int getMaxY() const
    {
        return mMaxY;
    }

    inline int getNumberOfCutsWithGrid() const
    {
        return mNumberOfCutsWithGrid;
    }

    inline bool hasCuts() const
    {
        return mNumberOfCutsWithGrid > 0;
    }

    inline float getCurrentCut() const
    {
        return mCurrentCut;
    }

    inline void advanceToNextCut()
    {
        mCurrentCut += mDeltaToNextCut;
        mNumberOfCutsWithGrid--;
    }

protected:
    int mMaxY;
    int mNumberOfCutsWithGrid;
    float mDeltaToNextCut;
    float mCurrentCut;
};

inline Point2f roundAwayFromGrid(Point2f point)
{
    constexpr float epsilon = 0.001;
    const auto diffy = std::round(point.y() - 0.5) - (point.y() - 0.5);
    if (std::abs(diffy) < epsilon)
        point.y() += diffy - std::copysign(epsilon, diffy);

    return point;
}

inline void processEdge(const Point2f& current, const Point2f& prev, std::vector<EdgeCuts>& cuts)
{
    if (prev.y() != current.y()) {
        auto edge = EdgeCuts(current, prev);
        if (edge.hasCuts()) {
            cuts.push_back(std::move(edge));
        }
    }
}

template<class Transformation>
std::vector<EdgeCuts> createEdgeList(const Polygon2f& polygon, Transformation&& F)
{
    assert(polygon.size() > 1);

    std::vector<EdgeCuts> cuts;
    cuts.reserve(polygon.size());

    const Point2f lastPoint = roundAwayFromGrid(F(polygon.back()));
    Point2f prevPoint = lastPoint;
    for (auto it = polygon.cbegin(); std::next(it) != polygon.cend(); ++it) {
        const Point2f currentPoint = roundAwayFromGrid(F(*it));

        processEdge(currentPoint, prevPoint, cuts);
        prevPoint = currentPoint;
    }

    // in the final "iteration", just use the previously calculated lastPoint
    // -> saving extra computation of F
    processEdge(lastPoint, prevPoint, cuts);

    std::sort(cuts.begin(), cuts.end(),
              [](const EdgeCuts& a, const EdgeCuts& b) { return a.getMaxY() > b.getMaxY(); });

    return cuts;
}

struct ActiveEdgeCuts
{
    using iterator = std::vector<EdgeCuts>::iterator;
    using reverse_iterator = std::reverse_iterator<iterator>;

    ActiveEdgeCuts(iterator first)
    {
        begin = first;
        end = std::next(begin, 2);
        currentY = first->getMaxY();
    }

    reverse_iterator rbegin()
    {
        return reverse_iterator(end);
    }

    reverse_iterator rend()
    {
        return reverse_iterator(begin);
    }

    int currentY;
    iterator begin;
    iterator end;
};

struct Interval
{
    float start;
    float end;
};

inline bool intervalsOverlap(const Interval& interval1, const Interval& interval2)
{
    assert(interval1.end >= interval1.start);
    assert(interval2.end >= interval2.start);

    return !(interval2.end < interval1.start || interval2.start > interval1.end);
}

class Intervals
{
public:
    void reserve(std::size_t size)
    {
        mIntervals.reserve(size);
    }

    void clear()
    {
        mIntervals.clear();
    }

    bool empty() const
    {
        return mIntervals.empty();
    }

    void addOrMerge(Interval interval)
    {
        // sort all overlapping intervals to end of container
        const auto it = std::remove_if(mIntervals.begin(), mIntervals.end(), [&](const Interval& tInterval) {
            const auto overlaps = intervalsOverlap(tInterval, interval);
            if (overlaps) {
                interval.start = std::min(interval.start, tInterval.start);
                interval.end = std::max(interval.end, tInterval.end);
            }
            return overlaps;
        });

        // erase overlapping intervals
        mIntervals.erase(it, mIntervals.end());
        // insert interval
        mIntervals.push_back(std::move(interval));
    }

    const std::vector<Interval>& getIntervals() const
    {
        return mIntervals;
    }

private:
    std::vector<Interval> mIntervals;
};

inline int scaleDown(int numToRound, int multiple)
{
    int isNegative = (int)(numToRound < 0);
    return ((numToRound - isNegative * (multiple - 1)) / multiple);
}

inline bool compareCurrentCut(const EdgeCuts& a, const EdgeCuts& b)
{
    return a.getCurrentCut() < b.getCurrentCut();
}

inline void removeEmptyEdges(ActiveEdgeCuts& activeEdges)
{
    auto it = std::remove_if(activeEdges.rbegin(), activeEdges.rend(), [](EdgeCuts& a) {
        return !a.hasCuts();
    });
    activeEdges.begin = it.base();
}

inline void advanceEdgeCuts(ActiveEdgeCuts& activeEdges)
{
    // the standard requires that the predicate is called exactly once for every element
    auto it = std::remove_if(activeEdges.rbegin(), activeEdges.rend(), [](EdgeCuts& a) {
        a.advanceToNextCut();
        return !a.hasCuts();
    });
    activeEdges.begin = it.base();
    --activeEdges.currentY;
}

template<class Visitor>
bool visitInterval(int x_start, int x_end, int y, Visitor& visitor)
{
    for (int x = x_start; x <= x_end; ++x) {
        if (visitor(x, y))
            return true;
    }
    return false;
}


}} // namespace mira::Private

#endif