arviz: coin implementation for path

source/RobotAPI/components/ArViz/CMakeLists.txt

@@ -21,6 +21,7 @@ set(SOURCES
     Coin/ElementVisualizer.cpp
 
     Coin/VisualizationRobot.cpp
+    Coin/VisualizationPath.cpp
     Coin/VisualizationObject.cpp
 
     Coin/Visualizer.cpp
@@ -49,6 +50,7 @@ set(HEADERS
     Coin/VisualizationEllipsoid.h
     Coin/VisualizationSphere.h
     Coin/VisualizationPose.h
+    Coin/VisualizationPath.h
     Coin/VisualizationLine.h
     Coin/VisualizationText.h
     Coin/VisualizationArrow.h

source/RobotAPI/components/ArViz/Coin/RegisterVisualizationTypes.cpp

@@ -15,13 +15,14 @@
 #include "VisualizationMesh.h"
 #include "VisualizationRobot.h"
 #include "VisualizationObject.h"
+#include "VisualizationPath.h"
 
 
 void armarx::viz::CoinVisualizer::registerVisualizationTypes()
 {
     using namespace armarx::viz::coin;
 
-    elementVisualizers.reserve(15);
+    elementVisualizers.reserve(16);
 
     registerVisualizerFor<VisualizationBox>();
     registerVisualizerFor<VisualizationCylinder>();
@@ -38,4 +39,5 @@ void armarx::viz::CoinVisualizer::registerVisualizationTypes()
     registerVisualizerFor<VisualizationMesh>();
     registerVisualizerFor<VisualizationRobot>();
     registerVisualizerFor<VisualizationObject>();
+    registerVisualizerFor<VisualizationPath>();
 }

source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp

+#include "VisualizationPath.h"
+
+#include <Inventor/SbVec3f.h>
+#include <Inventor/nodes/SoCoordinate3.h>
+#include <Inventor/nodes/SoDrawStyle.h>
+#include <Inventor/nodes/SoLineSet.h>
+
+namespace armarx::viz::coin
+{
+
+    VisualizationPath::VisualizationPath()
+    {
+        coordinate3 = new SoCoordinate3;
+
+        // create line around polygon
+        SoSeparator* lineSep = new SoSeparator;
+
+        lineMaterial = new SoMaterial;
+        lineSep->addChild(lineMaterial);
+        lineSep->addChild(coordinate3);
+
+        lineStyle = new SoDrawStyle();
+        lineSep->addChild(lineStyle);
+
+        lineSet = new SoLineSet;
+        lineSep->addChild(lineSet);
+
+        node->addChild(coordinate3);
+        node->addChild(lineSep);
+    }
+
+    bool VisualizationPath::update(ElementType const& element)
+    {
+        // set position
+        coordinate3->point.setValuesPointer(element.points.size(), reinterpret_cast<const float*>(element.points.data()));
+
+        // set color
+        const auto lineColor = element.lineColor;
+
+        constexpr float toUnit = 1.0F / 255.0F;
+
+        const auto color = SbVec3f(lineColor.r, lineColor.g, lineColor.b) * toUnit;
+        const float transparency = 1.0F - static_cast<float>(lineColor.a) * toUnit;
+
+        lineMaterial->diffuseColor.setValue(color);
+        lineMaterial->ambientColor.setValue(color);
+        lineMaterial->transparency.setValue(transparency);
+
+        if (element.lineWidth > 0.0F)
+        {
+            lineStyle->lineWidth.setValue(element.lineWidth);
+        }
+        else
+        {
+            lineStyle->style = SoDrawStyleElement::INVISIBLE;
+        }
+
+        const int pointSize = static_cast<int>(element.points.size());
+        lineSet->numVertices.set1Value(0, pointSize);
+
+        return true;
+    }
+} // namespace armarx::viz::coin
\ No newline at end of file

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

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include "ElementVisualizer.h"
+
+#include <RobotAPI/interface/ArViz/Elements.h>
+
+class SoCoordinate3;
+class SoDrawStyle;
+class SoLineSet;
+
+namespace armarx::viz::coin
+{
+    struct VisualizationPath : TypedElementVisualization<SoSeparator>
+    {
+        using ElementType = data::ElementPath;
+
+        VisualizationPath();
+
+        bool update(ElementType const& element);
+
+        SoCoordinate3* coordinate3;
+        SoDrawStyle* lineStyle;
+        SoLineSet* lineSet;
+        SoMaterial* lineMaterial;
+    };
+}  // namespace armarx::viz::coin