VisualizationPose.h

Go to the documentation of this file.

#pragma once
 
#include "ElementVisualizer.h"
 
#include <RobotAPI/interface/ArViz/Elements.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <Inventor/nodes/SoAsciiText.h>
#include <Inventor/nodes/SoCube.h>
#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/nodes/SoTransform.h>
#include <Inventor/nodes/SoTranslation.h>
 
namespace armarx::viz::coin
{
    struct VisualizationPose : TypedElementVisualization<SoSeparator>
    {
        using ElementType = data::ElementPose;
 
        static const int NumberOfBlocks = 3;
 
        VisualizationPose()
        {
            for (int i = 0; i < 3; i++)
            {
                SoTransform* t = new SoTransform();
                SoMaterial* m = new SoMaterial();
                t_[i] = t;
                m_[i] = m;
 
                SoCube* c = new SoCube();
                SoCube* c2 = new SoCube();
                SoTransform* t2 = new SoTransform();
                c_[i] = c;
                c2_[i] = c2;
                t2_[i] = t2;
 
                SoSeparator* tmp1 = new SoSeparator();
                tmp1->addChild(m);
                tmp1->addChild(t);
                tmp1->addChild(c);
                node->addChild(tmp1);
 
                SoSeparator* tmp2 = new SoSeparator();
                SoMaterial* m2 = new SoMaterial();
                m2->diffuseColor.setValue(1.0f, 1.0f, 1.0f);
                tmp2->addChild(m2);
 
                for (int j = 0; j < NumberOfBlocks; j++)
                {
                    tmp2->addChild(t2);
                    tmp2->addChild(c2);
                }
 
                node->addChild(tmp2);
            }
 
            SoSeparator* textSep = new SoSeparator();
            SoTranslation* moveT = new SoTranslation();
            moveT->translation.setValue(2.0f, 2.0f, 0.0f);
            textSep->addChild(moveT);
            textNode = new SoAsciiText();
            textSep->addChild(textNode);
 
            node->addChild(textSep);
        }
 
        bool update(ElementType const& element)
        {
            const float axisSize = 3.0f;
            const float axisLength = 100.0f;
            const int numberOfBlocks = NumberOfBlocks;
 
            float scaling = 1.0f;
 
            float blockSize = axisSize + 0.5f;
            float blockWidth = 0.1f;
 
            if (axisSize > 10.0f)
            {
                blockSize += axisSize / 10.0f;
                blockWidth += axisSize / 10.0f;
            }
 
            float axisBlockTranslation = axisLength / numberOfBlocks;
 
            for (int i = 0; i < 3; i++)
            {
                SoTransform* t = t_[i];
                SoMaterial* m = m_[i];
 
                float translation = (axisLength / 2.0f + axisSize / 2.0f) * scaling;
                if (i == 0)
                {
                    m->diffuseColor.setValue(1.0f, 0, 0);
                    t->translation.setValue(translation, 0, 0);
                }
                else if (i == 1)
                {
                    m->diffuseColor.setValue(0, 1.0f, 0);
                    t->translation.setValue(0, translation, 0);
                }
                else
                {
                    m->diffuseColor.setValue(0, 0, 1.0f);
                    t->translation.setValue(0, 0, translation);
                }
 
                SoCube* c = c_[i];
                SoCube* c2 = c2_[i];
                SoTransform* t2 = t2_[i];
 
                if (i == 0)
                {
                    c->width = axisLength * scaling;
                    c->height = axisSize * scaling;
                    c->depth = axisSize * scaling;
                    c2->width = blockWidth * scaling;
                    c2->height = blockSize * scaling;
                    c2->depth = blockSize * scaling;
                    t2->translation.setValue(axisBlockTranslation * scaling, 0, 0);
                }
                else if (i == 1)
                {
                    c->height = axisLength * scaling;
                    c->width = axisSize * scaling;
                    c->depth = axisSize * scaling;
                    c2->width = blockSize * scaling;
                    c2->height = blockWidth * scaling;
                    c2->depth = blockSize * scaling;
                    t2->translation.setValue(0, axisBlockTranslation * scaling, 0);
                }
                else
                {
                    c->depth = axisLength * scaling;
                    c->height = axisSize * scaling;
                    c->width = axisSize * scaling;
                    c2->width = blockSize * scaling;
                    c2->height = blockSize * scaling;
                    c2->depth = blockWidth * scaling;
                    t2->translation.setValue(0, 0, axisBlockTranslation * scaling);
                }
            }
 
            textNode->string.setValue(element.id.c_str());
 
            return true;
        }
 
        SoAsciiText* textNode;
        std::array<SoTransform*, 3> t_;
        std::array<SoMaterial*, 3> m_;
        std::array<SoCube*, 3> c_;
        std::array<SoCube*, 3> c2_;
        std::array<SoTransform*, 3> t2_;
    };
}