Merge branch 'feature/Arviz-Path-Visu-update' into 'master'

Feature/arviz path visu update

See merge request ArmarX/RobotAPI!168

source/RobotAPI/components/ArViz/Client/elements/Path.cpp

 #include "Path.h"
 
+#include <iterator>
+
+#include <ArmarXCore/interface/core/BasicVectorTypes.h>
+#include <ArmarXCore/interface/core/BasicVectorTypesHelpers.h>
+
 namespace armarx::viz
 {
 
     Path& Path::clear()
     {
         data_->points.clear();
-        return *this;
-    }
-
-    Path& Path::lineColor(Color color)
-    {
-        data_->lineColor = color;
 
         return *this;
     }
 
-    Path& Path::lineColorGlasbeyLUT(std::size_t id, int alpha)
-    {
-        return lineColor(Color::fromRGBA(simox::color::GlasbeyLUT::at(id, alpha)));
-    }
-
-    Path& Path::lineWidth(float w)
+    Path& Path::width(float w)
     {
         data_->lineWidth = w;
 
@@ -33,17 +27,21 @@ namespace armarx::viz
         auto& points = data_->points;
         points.clear();
         points.reserve(ps.size());
-        for (auto& p : ps)
+
+        std::transform(ps.begin(),
+                       ps.end(),
+                       std::back_inserter(points),
+                       [](const auto & e)
         {
-            points.push_back(armarx::Vector3f{p.x(), p.y(), p.z()});
-        }
+            return ToBasicVectorType(e);
+        });
 
         return *this;
     }
 
     Path& Path::addPoint(Eigen::Vector3f p)
     {
-        data_->points.push_back(armarx::Vector3f{p.x(), p.y(), p.z()});
+        data_->points.emplace_back(ToBasicVectorType(p));
 
         return *this;
     }

source/RobotAPI/components/ArViz/Client/elements/Path.h

@@ -35,17 +35,7 @@ namespace armarx::viz
 
         Path& clear();
 
-        Path& lineColor(Color color);
-
-        template<class...Ts>
-        Path& lineColor(Ts&& ...ts)
-        {
-            return lineColor({std::forward<Ts>(ts)...});
-        }
-
-        Path& lineColorGlasbeyLUT(std::size_t id, int alpha = 255);
-
-        Path& lineWidth(float w);
+        Path& width(float w);
 
         Path& points(std::vector<Eigen::Vector3f> const& ps);
 

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

 #include "VisualizationPath.h"
 
+#include <algorithm>
+
 #include <Inventor/SbVec3f.h>
 #include <Inventor/nodes/SoCoordinate3.h>
 #include <Inventor/nodes/SoDrawStyle.h>
 #include <Inventor/nodes/SoLineSet.h>
+#include <Inventor/nodes/SoPointSet.h>
 
 namespace armarx::viz::coin
 {
@@ -27,19 +30,36 @@ namespace armarx::viz::coin
 
         node->addChild(coordinate3);
         node->addChild(lineSep);
+
+        // points (use the following, if the points should be drawn in a different color)
+
+        // pclMat = new SoMaterial;
+
+        // SoMaterialBinding* pclMatBind = new SoMaterialBinding;
+        // pclMatBind->value = SoMaterialBinding::PER_PART;
+
+        pclCoords = new SoCoordinate3;
+        pclStyle = new SoDrawStyle;
+
+        // node->addChild(pclMat);
+        // node->addChild(pclMatBind);
+        node->addChild(pclCoords);
+        node->addChild(pclStyle);
+        node->addChild(new SoPointSet);
     }
 
     bool VisualizationPath::update(ElementType const& element)
     {
         // set position
-        coordinate3->point.setValuesPointer(element.points.size(), reinterpret_cast<const float*>(element.points.data()));
+        coordinate3->point.setValuesPointer(element.points.size(),
+                                            reinterpret_cast<const float*>(element.points.data()));
 
         // set color
-        const auto lineColor = element.lineColor;
+        const auto lineColor = element.color;
 
         constexpr float toUnit = 1.0F / 255.0F;
 
-        const auto color = SbVec3f(lineColor.r, lineColor.g, lineColor.b) * toUnit;
+        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);
@@ -58,6 +78,20 @@ namespace armarx::viz::coin
         const int pointSize = static_cast<int>(element.points.size());
         lineSet->numVertices.set1Value(0, pointSize);
 
+        // points at each node
+        // const std::vector<SbVec3f> colorData(element.points.size(), color);
+
+        // pclMat->diffuseColor.setValuesPointer(colorData.size(),
+        //                                       reinterpret_cast<const float*>(colorData.data()));
+        // pclMat->ambientColor.setValuesPointer(colorData.size(),
+        //                                       reinterpret_cast<const float*>(colorData.data()));
+        // pclMat->transparency = transparency;
+
+        pclCoords->point.setValuesPointer(element.points.size(),
+                                          reinterpret_cast<const float*>(element.points.data()));
+
+        pclStyle->pointSize = element.lineWidth + 5;
+
         return true;
     }
 } // namespace armarx::viz::coin
\ No newline at end of file

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

@@ -39,9 +39,17 @@ namespace armarx::viz::coin
 
         bool update(ElementType const& element);
 
+        /// lines
         SoCoordinate3* coordinate3;
         SoDrawStyle* lineStyle;
         SoLineSet* lineSet;
         SoMaterial* lineMaterial;
+
+
+        /// points
+        // SoMaterial* pclMat;
+        SoCoordinate3* pclCoords;
+        SoDrawStyle* pclStyle;
+
     };
 }  // namespace armarx::viz::coin

source/RobotAPI/interface/ArViz/Elements.ice

@@ -104,7 +104,6 @@ module data
     {
         Vector3fSeq points;
 
-        Color lineColor;
         float lineWidth = 10.0f;
     };