Bresenham.h

Go to the documentation of this file.

/*
 * Copyright (C) 2012 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_BRESENHAM_H_
#define _MIRA_BRESENHAM_H_

#include <cstdlib>
#include <algorithm>
#include <geometry/Point.h>

namespace mira {


template<typename Visitor>
void bresenham(int x0, int y0, int x1, int y1, Visitor&& visitor);


template<typename Visitor>
void bresenham(Point2i p0, Point2i p1, Visitor&& visitor)
{
    bresenham(p0[0], p0[1], p1[0], p1[1], visitor);
}


template<typename Visitor>
void bresenhamRunSlice(int x0, int y0, int x1, int y1, Visitor&& visitor);


template<typename Visitor>
void bresenhamRunSlice(Point2i p0, Point2i p1, Visitor&& visitor)
{
    bresenhamRunSlice(p0[0], p0[1], p1[0], p1[1], visitor);
}

// implementation section: BresenhamLineIterator class definition

class BresenhamLineIterator
{
public:
    BresenhamLineIterator(int x0, int y0, int x1, int y1,
                          int xInc=1, int yInc=1);

    bool hasNext() const;

    const BresenhamLineIterator& operator++();

    const BresenhamLineIterator& operator--();

    int x() const { return mX; }

    int y() const { return mY; }

    bool isMajorX() const { return (mDeltaX >= mDeltaY); }

    bool isMajorY() const { return !isMajorX(); }

    int pos() const { return mI; }

    int length() const { return mDen; }

    BresenhamLineIterator orthogonal();

private:
    BresenhamLineIterator(int mx, int my,
                          int xInc1, int xInc2, int yInc1, int yInc2,
                          int num, int den, int numInc,
                          int deltaX, int deltaY);

private:
    int mI;
    int mX,mY;
    int mXInc1, mXInc2, mYInc1, mYInc2, mNum, mDen, mNumInc;
    int mDeltaX, mDeltaY;
};

// implementation section: GeneralBresenhamLineIterator class definition

class GeneralBresenhamLineIteratorCommonBase
{
protected:
    struct AxisBase
    {
        int current;
        int step;
        int end;
        int64_t distance;
        int64_t error;
    };
};

template <int Drive, bool DrivenByLongestAxis = (Drive < 0)>
class GeneralBresenhamLineIteratorBase : public GeneralBresenhamLineIteratorCommonBase
{
protected:
    inline void checkForDrivingAxis(int index, const int64_t& dist, int64_t& maxd)
    {
        if (dist > maxd)
        {
            maxd = dist;
            mDrivingAxis = index;
        }
    }

    inline void step(AxisBase& axis, const AxisBase& drive)
    {
        if (axis.error >= 0)
        {
            axis.current += axis.step;
            axis.error   += axis.distance - drive.distance;
        }
        else
            axis.error += axis.distance;
    }

public:
    inline int drivingAxis() const { return mDrivingAxis; }

protected:
    int mDrivingAxis;    
};

template <int Drive>
class GeneralBresenhamLineIteratorBase<Drive, false> : public GeneralBresenhamLineIteratorCommonBase
{
protected:
    inline void checkForDrivingAxis(int index, const int64_t& dist, int64_t& maxd) {} // do nothing

    inline void step(AxisBase& axis, const AxisBase& drive)
    {
        if (axis.error >= 0)
        {
            int steps = axis.error / drive.distance + 1;
            axis.current += steps * axis.step;
            axis.error   -= steps * drive.distance;
        }
        axis.error += axis.distance;
    }

public:
    inline int drivingAxis() const { return Drive; }
};


template <int D, typename T = float, int Drive = -1, int Res = 1000>
class GeneralBresenhamLineIterator : public GeneralBresenhamLineIteratorBase<Drive>
{
    static_assert(Drive < D, "Driving axis Drive must not be >= number of dimensions D in GeneralBresenhamLineIterator instantiation!");
public:
    struct Axis : public GeneralBresenhamLineIteratorCommonBase::AxisBase
    {
        T startExact, endExact;
    };

public:
    GeneralBresenhamLineIterator(Point<T, D> start = Point<T, D>(),
                                 Point<T, D> end = Point<T, D>());

public:
    void init(Point<T, D> start = Point<T, D>(),
              Point<T, D> end = Point<T, D>());

    inline bool hasNext() const
        { return mAxes[this->drivingAxis()].current != mAxes[this->drivingAxis()].end; }

    const GeneralBresenhamLineIterator& operator++();

    Point<int, D> pos() const;

    inline const Axis& axis(uint32 d) const { return mAxes[d]; }

protected:

    Axis mAxes[D];       ///< the axes variables
};

// implementation section: BresenhamLineIterator class implementation

inline BresenhamLineIterator::BresenhamLineIterator(int x0, int y0,
                                                    int x1, int y1,
                                                    int xInc, int yInc)
{
    // prepare the bresenham algo
    mDeltaX = std::abs(x1 - x0);
    mDeltaY = std::abs(y1 - y0);

    mX = x0;
    mY = y0;

    if (x1 >= x0) {mXInc1 =  xInc; mXInc2 =  xInc; }
             else {mXInc1 = -xInc; mXInc2 = -xInc; }

    if (y1 >= y0) {mYInc1 =  yInc; mYInc2 =  yInc; }
             else {mYInc1 = -yInc; mYInc2 = -yInc; }

    if (mDeltaX >= mDeltaY) {
        mXInc1  = 0;
        mYInc2  = 0;
        mDen    = mDeltaX;
        mNumInc = mDeltaY;
    }
    else {
        mXInc2  = 0;
        mYInc1  = 0;
        mDen    = mDeltaY;
        mNumInc = mDeltaX;
    }

    mNum = mDen >> 1;
    mI=0;
}


inline BresenhamLineIterator::BresenhamLineIterator(int mx, int my,
                                                    int xInc1, int xInc2,
                                                    int yInc1, int yInc2,
                                                    int num, int den, int numInc,
                                                    int deltaX, int deltaY) :
    mI(0),
    mX(mx), mY(my),
    mXInc1(xInc1), mXInc2(xInc2), mYInc1(yInc1), mYInc2(yInc2),
    mNum(num), mDen(den), mNumInc(numInc),
    mDeltaX(deltaX), mDeltaY(deltaY)
{
    // this constructor is for internal use only
}


inline const BresenhamLineIterator& BresenhamLineIterator::operator++()
{
    mNum += mNumInc;
    if (mNum >= mDen)
    {
        mNum -= mDen;
        mX += mXInc1;
        mY += mYInc1;
    }
    mX += mXInc2;
    mY += mYInc2;

    ++mI;

    return *this;
}


inline const BresenhamLineIterator& BresenhamLineIterator::operator--()
{
    mNum -= mNumInc;
    if (mNum < 0)
    {
        mNum += mDen;
        mX -= mXInc1;
        mY -= mYInc1;
    }
    mX -= mXInc2;
    mY -= mYInc2;

    --mI;

    return *this;
}


inline bool BresenhamLineIterator::hasNext() const
{
    if(mI<=mDen)
        return true;
    else
        return false;
}


// normal line has tangent of dY/dX -> orthogonal line has tangent of -dX/dY.
// So we have to switch and negate the increments and switch the deltas. The
// line itself still starts at (mX,mY).
inline BresenhamLineIterator BresenhamLineIterator::orthogonal()
{
    return BresenhamLineIterator(mX, mY, mYInc1, mYInc2, -mXInc1, -mXInc2,
                                 mNum, mDen, mNumInc, mDeltaY, mDeltaX);
}

// interface implementation

template<typename Visitor>
inline void bresenham(int x0, int y0, int x1, int y1, Visitor&& visitor)
{
    for(BresenhamLineIterator l(x0,y0,x1,y1); l.hasNext(); ++l)
        visitor(l.x(), l.y());

}



template<typename Visitor>
inline void bresenhamRunSlice(int x0, int y0, int x1, int y1, Visitor&& visitor)
{
    // make sure we always go from top to bottom
    if (y1 < y0) {
        std::swap(y0,y1);
        std::swap(x0,x1);
    }

    int dx = x1 - x0;
    int dy = y1 - y0;

    int x, y;
    x = x0;
    y = y0;

    int adjUp, adjDown, error;
    int wholeStep, initialLength, finalLength, runLength;

    // are we going from right to left ?
    if (dx < 0)
        dx = -dx;

    // special case: horizontal line
    if(dy==0) {
        if(x0<=x1)
            visitor(x0, y0, dx+1);
        else
            visitor(x1, y0, dx+1);
        return;
    }

    // use normal bresenham for lines with major y-axis
    if(dx<=dy) {
        for(BresenhamLineIterator l(x0,y0,x1,y1); l.hasNext(); ++l)
            visitor(l.x(), l.y(), 1);
        return;
    }

    // use run-slice algorithm for lines with major x-axis:

    // minimum number of pixels in a run in this line
    wholeStep = dx / dy;

    // Error term adjust each time Y steps by 1; used to tell when one
    // extra pixel should be drawn as part of a run, to account for
    // fractional steps along the X axis per 1-pixel steps along Y
    adjUp = (dx % dy) * 2;

    // Error term adjust when the error term turns over, used to factor
    // out the X step made at that time
    adjDown = dy * 2;

    // Initial error term; reflects an initial step of 0.5 along the Y
    // axis
    error = (dx % dy) - (dy * 2);

    // The initial and last runs are partial, because Y advances only 0.5
    // for these runs, rather than 1. Divide one full run, plus the
    // initial pixel, between the initial and last runs
    initialLength = (wholeStep / 2) + 1;
    finalLength = initialLength;

    // If the basic run length is even and there's no fractional
    // advance, we have one pixel that could go to either the initial
    // or last partial run, which we'll arbitrarily allocate to the
    // last run
    if ((adjUp == 0) && ((wholeStep & 0x01) == 0)) {
        initialLength--;
    }

    // If there're an odd number of pixels per run, we have 1 pixel that can't
    // be allocated to either the initial or last partial run, so we'll add 0.5
    // to error term so this pixel will be handled by the normal full-run loop
    if ((wholeStep & 0x01) != 0) {
        error += dy;
    }

    if (x1 >= x0) {
        // Draw the first, partial run of pixels
        visitor(x, y, initialLength);
        x += initialLength;
        ++y;

        // process full runs
        for (int i = 0; i < (dy - 1); i++) {
            runLength = wholeStep; // run is at least this long
            // advance the error term and add an extra pixel if the error
            // term so indicates
            if ((error += adjUp) > 0) {
                runLength++;
                error -= adjDown;
            }

            // call the visitor
            visitor(x, y, runLength);
            x += runLength;
            ++y;
        }

        // the final run of pixels
        visitor(x, y, finalLength);
    } else {
        ++x;

        // Draw the first, partial run of pixels
        x -= initialLength;
        visitor(x, y, initialLength);
        ++y;


        // process full runs
        for (int i = 0; i < (dy - 1); i++) {
            runLength = wholeStep; // run is at least this long
            // advance the error term and add an extra pixel if the error
            // term so indicates
            if ((error += adjUp) > 0) {
                runLength++;
                error -= adjDown;
            }

            // call the visitor
            x -= runLength;
            visitor(x, y, runLength);
            ++y;
        }

        // the final run of pixels
        x -= finalLength;
        visitor(x, y, finalLength);
    }
}

// implementation section: GeneralBresenhamLineIterator class implementation
template <int D, typename T, int Drive, int Res>
inline GeneralBresenhamLineIterator<D, T, Drive, Res>::
GeneralBresenhamLineIterator(Point<T, D> start,
                             Point<T, D> end)
{
    init(start, end);
}

template <int D, typename T, int Drive, int Res>
inline void GeneralBresenhamLineIterator<D, T, Drive, Res>::
init(Point<T, D> start, Point<T, D> end)
{
    int64_t maxd = -1;

    for (int n = 0; n < D; ++n)
    {
        Axis& axis = mAxes[n];
        axis.startExact = start(n);
        axis.endExact   = end(n);
        axis.distance   = std::abs(Res * (axis.endExact - axis.startExact));
        axis.current    = floor(axis.startExact);
        axis.end        = floor(axis.endExact);
        axis.step       = (axis.endExact >= axis.startExact) ? 1 : -1;

        this->checkForDrivingAxis(n, axis.distance, maxd);
    }

    Axis& drive = mAxes[this->drivingAxis()];
    drive.distance   = std::ceil(std::abs(Res * (drive.endExact - drive.startExact)));

    T driveOffset = drive.startExact - drive.current;
    if (drive.step < 0)
        driveOffset = 1 - driveOffset;

    for (int n = 0; n < D; ++n)
    {
        Axis& axis  = mAxes[n];

        if (n == this->drivingAxis())
        {
            axis.error = 0;
            continue;
        }

        T axisOffset = axis.startExact -  axis.current;
        if (axis.step < 0)
            axisOffset = 1 - axisOffset;

        axis.error = floor(  axis.distance * (1 - driveOffset)
                            -drive.distance * (1 - axisOffset) );
    }
}

template <int D, typename T, int Drive, int Res>
inline const GeneralBresenhamLineIterator<D, T, Drive, Res>&
GeneralBresenhamLineIterator<D, T, Drive, Res>::operator++()
{
    for (int n = 0; n < D; ++n)
    {
        Axis& axis = mAxes[n];

        if (n == this->drivingAxis())
            axis.current += axis.step;
        else
            this->step(axis, mAxes[this->drivingAxis()]);
    }

    return *this;
}

template <int D, typename T, int Drive, int Res>
inline Point<int, D>
GeneralBresenhamLineIterator<D, T, Drive, Res>::pos() const
{
    Point<int, D> p;
    for (int n = 0; n < D; ++n)
        p(n) = mAxes[n].current;

    return p;
}

}

#endif