Update armem/dev to master

Revert "Make RobotStateComponent more efficient" until we have successfully tested it thoroughly during the demo

This reverts commit 69a37d8e.

source/RobotAPI/components/ArViz/Coin/VisualizationArrowCircle.h

@@ -3,13 +3,15 @@
 #include "ElementVisualizer.h"
 
 #include <RobotAPI/interface/ArViz/Elements.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/SoTranslation.h>
 
-#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
-#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
 
 namespace armarx::viz::coin
 {
@@ -17,50 +19,172 @@ namespace armarx::viz::coin
     {
         using ElementType = data::ElementArrowCircle;
 
-        bool update(ElementType const& element)
+        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()
         {
-            int rings = 32;
+            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;
+            float torusCompletion = completion - 1.0f / RINGS * sign;
             if (torusCompletion * sign < 0)
             {
                 torusCompletion = 0;
             }
-            auto color = element.color;
-            const float conv = 1.0f / 255.0f;
-            float r = color.r * conv;
-            float g = color.g * conv;
-            float b = color.b * conv;
-            float a = color.a * conv;
 
-            auto torusNode = VirtualRobot::CoinVisualizationFactory().createTorus(
-                                 element.radius, element.width, torusCompletion,
-                                 r, g, b, 1.0f - a,
-                                 8, rings);
-            SoNode* torus = dynamic_cast<VirtualRobot::CoinVisualizationNode&>(*torusNode).getCoinVisualization();
+            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;
 
-            float angle0 = (float)(rings - 2) / rings * 2 * M_PI * completion;
-            float x0 = element.radius * cos(angle0);
-            float y0 = element.radius * sin(angle0);
-            float angle1 = (float)(rings - 1) / rings * 2 * M_PI * completion;
+                int numVerticesPerRow = SIDES + 1;
+                int numVerticesPerColumn = RINGS + 1;
 
-            SoSeparator* subSep = new SoSeparator();
-            SoTransform* tr = new SoTransform;
-            tr->translation.setValue(x0, y0, 0);
 
-            tr->rotation.setValue(SbVec3f(0, 0, 1), angle1);
-            subSep->addChild(tr);
+                float theta = 0.0f;
+                float phi = 0.0f;
+                float verticalAngularStride = TWO_PI / RINGS;
+                float horizontalAngularStride = TWO_PI / SIDES;
 
-            subSep->addChild(VirtualRobot::CoinVisualizationFactory::CreateArrow(
-                                 Eigen::Vector3f::UnitY()*sign, 0, element.width,
-                                 VirtualRobot::VisualizationFactory::Color(r, g, b, 1.0f - a)));
+                int numVertices = numVerticesPerColumn * numVerticesPerRow;
+                for (int verticalIt = 0; verticalIt < numVerticesPerColumn; verticalIt++)
+                {
+                    theta = verticalAngularStride * verticalIt * completion;
 
-            node->removeAllChildren();
-            node->addChild(torus);
-            node->addChild(subSep);
+                    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;
         }