VoxelLine.cpp

Go to the documentation of this file.

#include "VoxelLine.h"
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
 
namespace visionx
{
 
    const Eigen::IOFormat VoxelLine::iof{3, 0, " ", " ", "", "", "[", "]"};
 
 
    VoxelLine::VoxelLine() : _finished(true)
    {}
 
    VoxelLine::VoxelLine(
        const Eigen::Vector3i& start, const Eigen::Vector3i& end,
        bool includeStart, bool includeEnd) :
        start(start), end(end), includeStart(includeStart), includeEnd(includeEnd)
    {
        init();
    }
 
 
    void VoxelLine::init()
    {
        delta = end - start;
 
        // Choose direction.
        for (Index i = 0; i < step.size(); ++i)
        {
            step(i) = delta(i) >= 0 ? 1 : -1;
        }
 
        delta = delta.cwiseAbs();
 
        // Choose the driving and non-driving axes.
        delta.maxCoeff(&x);
 
        y = x != 0 ? 0 : 1; // y = 0, except for x = 0, then y = 1 (and z = 2)
        z = x != 2 ? 2 : 1; // z = 2, except for x = 2, then z = 1 (and y = 0)
 
        py = 2 * delta(y) - delta(x);
        pz = 2 * delta(z) - delta(x);
 
        _finished = false;
        _next = start;
 
        // Skip start if excluded, but not if it is equal to end and end shall be included.
        if (!includeStart && !(includeEnd && start == end))
        {
            advance();
        }
    }
 
    void VoxelLine::advance()
    {
        if (_next(x) != end(x))
        {
            // Advance on the line.
            _next(x) += step(x);
 
            if (py >= 0)
            {
                _next(y) += step(y);
                py -= 2 * delta(x);
            }
            if (pz >= 0)
            {
                _next(z) += step(z);
                pz -= 2 * delta(x);
            }
            py += 2 * delta(y);
            pz += 2 * delta(z);
        }
        else
        {
            // The next will be end, after that the line is finished.
            _finished = true;
        }
 
        if (_next(x) == end(x) && !includeEnd)
        {
            // The next would be end, but that shall be excluded.
            _finished = true;
        }
    }
 
 
    bool VoxelLine::finished() const
    {
        return _finished;
    }
 
    Eigen::Vector3i VoxelLine::next()
    {
        if (_finished)
        {
            throw std::logic_error("No more voxels on the line. "
                                   "Check VoxelLine::finished() before calling VoxelLine::next().");
        }
 
        Eigen::Vector3i next = _next;
        advance();
        return next;
    }
 
 
    std::vector<Eigen::Vector3i> VoxelLine::getLineVoxels(
        const Eigen::Vector3i& start, const Eigen::Vector3i& end,
        bool includeStart, bool includeEnd)
    {
        VoxelLine line(start, end, includeStart, includeEnd);
        std::vector<Eigen::Vector3i> points;
 
        while (!line.finished())
        {
            points.push_back(line.next());
        }
 
        return points;
    }
 
}