VisualizationGrid.cpp

Go to the documentation of this file.

#include "VisualizationGrid.h"
 
#include <array>
#include <cstddef>
#include <cstdint>
#include <limits>
#include <vector>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include "RobotAPI/components/ArViz/Coin/ElementVisualizer.h"
 
#include <Inventor/SbVec2s.h>
#include <Inventor/SbVec3f.h>
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoFaceSet.h>
#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/nodes/SoTexture2.h>
#include <Inventor/nodes/SoTextureCoordinate2.h>
#include <Inventor/nodes/SoTextureCoordinateBinding.h>
 
namespace armarx::viz::coin
{
 
    VisualizationGrid::VisualizationGrid()
    {
        // Note: the element's pose (origin) and the millimeter units are already
        // applied by the base ElementVisualization (separator -> units -> transform).
        // We must not apply the pose again here, otherwise it is doubled.
        SoSeparator* pGrid = new SoSeparator;
 
        pImagePlaneSoCoordinate3 = new SoCoordinate3;
 
        SoFaceSet* pSoFaceSet = new SoFaceSet;
        pSoFaceSet->numVertices.set1Value(0, 4);
 
        // Texture coordinates mapping the grid texture onto the quad. The grid's
        // X/Y axes are swapped relative to the texture (see extents below), so the
        // mapping is: s along the quad's Y extent (texture width = sizeX), t along
        // the quad's X extent (texture height = sizeY). Cell (0,0) is at UV (0,0),
        // which is the (0,0) corner of the quad (vertex 2). No vertical flip is
        // needed since SoSFImage stores data lower-left first.
        SoTextureCoordinate2* pSoTextureCoordinate2 = new SoTextureCoordinate2;
        pSoTextureCoordinate2->point.set1Value(0, SbVec2f(1.0f, 1.0f)); // vertex (X, Y)
        pSoTextureCoordinate2->point.set1Value(1, SbVec2f(1.0f, 0.0f)); // vertex (0, Y)
        pSoTextureCoordinate2->point.set1Value(2, SbVec2f(0.0f, 0.0f)); // vertex (0, 0)
        pSoTextureCoordinate2->point.set1Value(3, SbVec2f(0.0f, 1.0f)); // vertex (X, 0)
 
        SoTextureCoordinateBinding* pSoTextureCoordinateBinding = new SoTextureCoordinateBinding;
        pSoTextureCoordinateBinding->value.setValue(SoTextureCoordinateBinding::PER_VERTEX);
 
        pSoTexture2 = new SoTexture2;
 
        pSoTexture2->wrapS = pSoTexture2->wrapT = SoTexture2::REPEAT;
        pGrid->addChild(pSoTextureCoordinate2);
        pGrid->addChild(pSoTextureCoordinateBinding);
        pGrid->addChild(pSoTexture2);
        pGrid->addChild(pImagePlaneSoCoordinate3);
        pGrid->addChild(pSoFaceSet);
        // pGrid->unrefNoDelete();
 
        node->addChild(pGrid);
    }
 
    bool
    VisualizationGrid::update(ElementType const& element)
    {
        // The element's pose (origin) is applied by the base ElementVisualization,
        // so it must not be set here. The X/Y axis swap between the grid and the
        // texture is baked into the quad extents and texture coordinates below.
 
        // Validate the element before touching any scene graph data. An invalid
        // grid (e.g. coming over the wire from a remote component) must never
        // crash the visualizer, so we bail out gracefully instead.
 
        // The grid dimensions must be strictly positive.
        if (element.sizeX <= 0 || element.sizeY <= 0)
        {
            ARMARX_WARNING << deactivateSpam(1) << "Ignoring grid element with non-positive size ("
                           << element.sizeX << " x " << element.sizeY << ").";
            return false;
        }
 
        // The dimensions are written into an SbVec2s (signed 16-bit) below, so
        // they must fit into that range to avoid silent overflow / corruption.
        constexpr int maxDimension = std::numeric_limits<std::int16_t>::max();
        if (element.sizeX > maxDimension || element.sizeY > maxDimension)
        {
            ARMARX_WARNING << deactivateSpam(1) << "Ignoring grid element that is too large ("
                           << element.sizeX << " x " << element.sizeY << ", max is " << maxDimension
                           << " per dimension).";
            return false;
        }
 
        // Compute the expected number of cells in 64-bit to avoid int overflow,
        // then make sure the provided color data actually matches it.
        const std::size_t sizeX = static_cast<std::size_t>(element.sizeX);
        const std::size_t sizeY = static_cast<std::size_t>(element.sizeY);
        const std::size_t expectedCellCount = sizeX * sizeY;
 
        if (element.colors.size() != expectedCellCount)
        {
            ARMARX_WARNING << deactivateSpam(1)
                           << "Ignoring grid element with mismatched color data: "
                           << "expected " << expectedCellCount << " colors (" << element.sizeX
                           << " x " << element.sizeY << ") but got " << element.colors.size()
                           << ".";
            return false;
        }
 
        // update grid extents
        {
            // intentionally swapped X and Y
            const float X = element.sizeY * element.resolution;
            const float Y = element.sizeX * element.resolution;
 
            pImagePlaneSoCoordinate3->point.set1Value(0, SbVec3f(X, Y, 0.0f));
            pImagePlaneSoCoordinate3->point.set1Value(1, SbVec3f(0, Y, 0.0f));
            pImagePlaneSoCoordinate3->point.set1Value(2, SbVec3f(0, 0, 0.0f));
            pImagePlaneSoCoordinate3->point.set1Value(3, SbVec3f(X, 0, 0.0f));
        }
 
 
        // update texture (grid values)
        {
            using Color = std::array<unsigned char, 4>; // RGBA
 
            std::vector<Color> pixels(expectedCellCount);
 
            for (std::size_t x = 0; x < sizeX; x++)
            {
                for (std::size_t y = 0; y < sizeY; y++)
                {
                    const std::size_t idx = (x * sizeY) + y;
 
                    const auto& color = element.colors[idx];
                    pixels[idx] = Color{color.r, color.g, color.b, color.a};
                }
            }
 
 
            pSoTexture2->image.setValue(SbVec2s{static_cast<std::int16_t>(element.sizeX),
                                                static_cast<std::int16_t>(element.sizeY)},
                                        4,
                                        reinterpret_cast<const unsigned char*>(pixels.data()));
        }
 
 
        return true;
    }
} // namespace armarx::viz::coin