VisualizationArrowCircle.h

Go to the documentation of this file.

#pragma once
 
#include <RobotAPI/interface/ArViz/Elements.h>
 
#include "ElementVisualizer.h"
#include <Inventor/nodes/SoCone.h>
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoCylinder.h>
#include <Inventor/nodes/SoIndexedFaceSet.h>
#include <Inventor/nodes/SoShapeHints.h>
#include <Inventor/nodes/SoSphere.h>
#include <Inventor/nodes/SoTransform.h>
#include <Inventor/nodes/SoTranslation.h>
 
namespace armarx::viz::coin
{
    struct VisualizationArrowCircle : TypedElementVisualization<SoSeparator>
    {
        using ElementType = data::ElementArrowCircle;
 
        SoCoordinate3* coords;
        SoIndexedFaceSet* torusFaceSet;
        SoSeparator* torus;
        SoSeparator* coneSep;
        SoTransform* coneTransform;
        SoTransform* coneSignRotation;
        SoCone* cone;
 
        std::vector<SbVec3f> vertexPositions;
        std::vector<int32_t> faces;
        std::vector<int32_t> matInx;
 
        static const int RINGS = 32;
        static const int SIDES = 8;
 
        VisualizationArrowCircle()
        {
            SoMaterialBinding* torusBinding = new SoMaterialBinding;
            torusBinding->value = SoMaterialBinding::PER_VERTEX_INDEXED;
 
            coords = new SoCoordinate3;
 
            SoShapeHints* torusHints = new SoShapeHints;
            // Disable back culling and enable two-sided lighting
            torusHints->vertexOrdering = SoShapeHints::VertexOrdering::COUNTERCLOCKWISE;
            torusHints->shapeType = SoShapeHints::ShapeType::UNKNOWN_SHAPE_TYPE;
 
            torusFaceSet = new SoIndexedFaceSet();
            torus = new SoSeparator;
 
            torus->addChild(torusBinding);
            torus->addChild(coords);
            torus->addChild(torusHints);
            torus->addChild(torusFaceSet);
 
            coneSep = new SoSeparator();
            coneTransform = new SoTransform;
 
            node->addChild(torus);
            node->addChild(coneSep);
 
            coneSignRotation = new SoTransform;
            cone = new SoCone;
 
            coneSep->addChild(coneTransform);
            coneSep->addChild(coneSignRotation);
            coneSep->addChild(cone);
 
            int numVerticesPerRow = SIDES + 1;
            int numVerticesPerColumn = RINGS + 1;
            vertexPositions.resize(numVerticesPerRow * numVerticesPerColumn);
 
            int numFaces = RINGS * SIDES * 2;
            faces.resize(numFaces * 4);
            matInx.resize(numFaces * 4);
        }
 
        bool
        update(ElementType const& element)
        {
            float completion = std::min<float>(1.0f, std::max(-1.0f, element.completion));
            int sign = completion >= 0 ? 1 : -1;
            float torusCompletion = completion - 1.0f / RINGS * sign;
            if (torusCompletion * sign < 0)
            {
                torusCompletion = 0;
            }
 
            const float TWO_PI = 2.0f * (float)M_PI;
 
            {
                // Create a torus mesh (for the completion circle
                float radius = element.radius;
                float tubeRadius = element.width;
                float completion = torusCompletion;
 
                int numVerticesPerRow = SIDES + 1;
                int numVerticesPerColumn = RINGS + 1;
 
 
                float theta = 0.0f;
                float phi = 0.0f;
                float verticalAngularStride = TWO_PI / RINGS;
                float horizontalAngularStride = TWO_PI / SIDES;
 
                int numVertices = numVerticesPerColumn * numVerticesPerRow;
                for (int verticalIt = 0; verticalIt < numVerticesPerColumn; verticalIt++)
                {
                    theta = verticalAngularStride * verticalIt * completion;
 
                    for (int horizontalIt = 0; horizontalIt < numVerticesPerRow; horizontalIt++)
                    {
                        phi = horizontalAngularStride * horizontalIt;
 
                        // position
                        float x = std::cos(theta) * (radius + tubeRadius * std::cos(phi));
                        float y = std::sin(theta) * (radius + tubeRadius * std::cos(phi));
                        float z = tubeRadius * std::sin(phi);
 
                        int vertexIndex = verticalIt * numVerticesPerRow + horizontalIt;
                        vertexPositions[vertexIndex].setValue(x, y, z);
                    }
                }
                coords->point.setValuesPointer(numVertices, vertexPositions.data());
 
                for (int verticalIt = 0; verticalIt < RINGS; verticalIt++)
                {
                    for (int horizontalIt = 0; horizontalIt < SIDES; horizontalIt++)
                    {
                        int faceIndex = (verticalIt * SIDES + horizontalIt) * 2;
 
                        short lt = (short)(horizontalIt + verticalIt * (numVerticesPerRow));
                        short rt = (short)((horizontalIt + 1) + verticalIt * (numVerticesPerRow));
 
                        short lb = (short)(horizontalIt + (verticalIt + 1) * (numVerticesPerRow));
                        short rb =
                            (short)((horizontalIt + 1) + (verticalIt + 1) * (numVerticesPerRow));
 
 
                        faces[faceIndex * 4 + 0] = lt;
                        faces[faceIndex * 4 + 1] = rt;
                        faces[faceIndex * 4 + 2] = lb;
                        faces[faceIndex * 4 + 3] = SO_END_FACE_INDEX;
 
                        matInx[faceIndex * 4 + 0] = 0;
                        matInx[faceIndex * 4 + 1] = 0;
                        matInx[faceIndex * 4 + 2] = 0;
                        matInx[faceIndex * 4 + 3] = SO_END_FACE_INDEX;
 
                        faceIndex += 1;
 
                        faces[faceIndex * 4 + 0] = rt;
                        faces[faceIndex * 4 + 1] = rb;
                        faces[faceIndex * 4 + 2] = lb;
                        faces[faceIndex * 4 + 3] = SO_END_FACE_INDEX;
 
                        matInx[faceIndex * 4 + 0] = 0;
                        matInx[faceIndex * 4 + 1] = 0;
                        matInx[faceIndex * 4 + 2] = 0;
                        matInx[faceIndex * 4 + 3] = SO_END_FACE_INDEX;
                    }
                }
 
                torusFaceSet->coordIndex.setValuesPointer(faces.size(), faces.data());
                torusFaceSet->materialIndex.setValuesPointer(matInx.size(), matInx.data());
            }
 
            {
                // Create a cone to make the arrow for the completion circle
                float angle0 = (RINGS - 1.0f) / RINGS * TWO_PI * completion;
                float x0 = element.radius * std::cos(angle0);
                float y0 = element.radius * std::sin(angle0);
                float angle1 = (RINGS - 0.5f) / RINGS * TWO_PI * completion;
 
                coneTransform->translation.setValue(x0, y0, 0);
 
                coneTransform->rotation.setValue(SbVec3f(0, 0, 1), angle1);
 
                float coneHeight = element.width * 6.0f;
                float coneBottomRadius = element.width * 2.5f;
 
                SbVec3f axis(0.0f, 0.0f, 1.0f);
                float angle = sign > 0.0f ? 0.0f : (float)M_PI;
                coneSignRotation->rotation.setValue(axis, angle);
 
                cone->bottomRadius.setValue(coneBottomRadius);
                cone->height.setValue(coneHeight);
            }
 
            return true;
        }
    };
} // namespace armarx::viz::coin