Merge branch 'add-line-and-path-to-arviz-example' into 'master'

Add line and path to arviz example

See merge request ArmarX/RobotAPI!263

source/RobotAPI/components/ArViz/Client/Elements.h

@@ -7,6 +7,7 @@
 
 #include "elements/Color.h"
 #include "elements/ElementOps.h"
+#include "elements/Line.h"
 #include "elements/Mesh.h"
 #include "elements/Path.h"
 #include "elements/PointCloud.h"

source/RobotAPI/components/ArViz/Example/ArVizExample.cpp

@@ -169,8 +169,85 @@ namespace armarx
 
             layer.add(arrow);
         }
+    }
+
+
+    void fillPathsAndLinesLayer(viz::Layer& layer)
+    {
+        Eigen::Vector3f offset = {-800, 0, 500};
+        std::vector<Eigen::Vector3f> pathPoints {
+            {    0,   0,  0 },
+            { -200,   0,  0 },
+            { -200, 200,  0 },
+        };
+        Eigen::Vector3f additionalPoint = {-200, 200, 200};
+
+        // Path: Connected sequence of lines.
+        {
+            viz::Path path = viz::Path("path")
+                    .position(offset)
+                    .points(pathPoints)
+                    .color(simox::Color::magenta()).width(10)
+                    ;
+
+            path.addPoint(additionalPoint);
+
+            layer.add(path);
+        }
+
+        pathPoints.push_back(additionalPoint);
+
+        // Line: Single line segments between 2 points.
+        {
+            offset = offset - 300 * Eigen::Vector3f::UnitX();
+
+            for (size_t i = 0; i < pathPoints.size() - 1; i += 2)
+            {
+                viz::Line line = viz::Line("line " + std::to_string(i) + " -> " + std::to_string(i+1))
+                        .fromTo(pathPoints.at(i) + offset, pathPoints.at(i+1) + offset)
+                        .color(simox::Color::lime()).lineWidth(10)
+                        ;
+
+                layer.add(line);
+            }
+        }
+
+        // Line and Path are drawn in pixel space. For a more consistent 3D look, use cylinders:
+        {
+            offset = offset - 300 * Eigen::Vector3f::UnitX();
+
+            for (size_t i = 0; i < pathPoints.size() - 1; ++i)
+            {
+                layer.add(viz::Cylinder("path cylinder " + std::to_string(i) + " -> " + std::to_string(i+1))
+                        .fromTo(pathPoints.at(i) + offset, pathPoints.at(i+1) + offset)
+                        .color(simox::Color::cyan()).radius(10)
+                          );
+            }
+        }
 
+        // Optional: Add spheres for an even nicer look.
+        {
+            offset = offset - 300 * Eigen::Vector3f::UnitX();
 
+            for (size_t i = 0; i < pathPoints.size(); ++i)
+            {
+                float radius = 10;
+                simox::Color color = simox::Color::azure();
+
+                if (i < pathPoints.size() - 1)
+                {
+                    layer.add(viz::Cylinder("path cylinder with spheres " + std::to_string(i) + " -> " + std::to_string(i+1))
+                            .fromTo(pathPoints.at(i) + offset, pathPoints.at(i+1) + offset)
+                            .color(color).radius(radius)
+                              );
+                }
+
+                layer.add(viz::Sphere("path sphere " + std::to_string(i))
+                        .position(pathPoints.at(i) + offset)
+                        .color(color).radius(radius)
+                          );
+            }
+        }
     }
 
 
@@ -556,6 +633,7 @@ namespace armarx
 
         viz::Layer testLayer = arviz.layer("Test");
         viz::Layer exampleLayer = arviz.layer("Example");
+        viz::Layer pathsAndLinesLayer = arviz.layer("Paths and Lines");
         viz::Layer pointsLayer = arviz.layer("Points");
         viz::Layer objectsLayer = arviz.layer("Objects");
         viz::Layer disAppearingLayer = arviz.layer("DisAppearing");
@@ -601,6 +679,8 @@ namespace armarx
             fillTestLayer(testLayer, timeInSeconds);
             exampleLayer.clear();
             fillExampleLayer(exampleLayer, timeInSeconds);
+            pathsAndLinesLayer.clear();
+            fillPathsAndLinesLayer(pathsAndLinesLayer);
             pointsLayer.clear();
             fillPointsLayer(pointsLayer, timeInSeconds);
             objectsLayer.clear();
@@ -620,6 +700,7 @@ namespace armarx
             // This is equivalent to calling add(layer) multiple times.
             stage.add({testLayer,
                        exampleLayer,
+                       pathsAndLinesLayer,
                        pointsLayer,
                        objectsLayer,
                        disAppearingLayer,