Add mesh visualizer

source/RobotAPI/components/ArViz/CMakeLists.txt

@@ -29,6 +29,7 @@ Coin/VisualizerArrow.h
 Coin/VisualizerArrowCircle.h
 Coin/VisualizerPointCloud.h
 Coin/VisualizerPolygon.h
+Coin/VisualizerMesh.h
 
 Coin/Visualizer.h
 )

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

@@ -10,6 +10,7 @@
 #include "VisualizerArrowCircle.h"
 #include "VisualizerPointCloud.h"
 #include "VisualizerPolygon.h"
+#include "VisualizerMesh.h"
 
 #include <ArmarXCore/core/logging/Logging.h>
 
@@ -59,6 +60,7 @@ namespace armarx
             registerVisualizer<VisualizerArrowCircle>();
             registerVisualizer<VisualizerPointCloud>();
             registerVisualizer<VisualizerPolygon>();
+            registerVisualizer<VisualizerMesh>();
 
             root = new SoSeparator;
 

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

+#pragma once
+
+#include "ElementVisualizer.h"
+
+#include <RobotAPI/interface/ArViz/Elements.h>
+
+#include <Inventor/nodes/SoCoordinate3.h>
+#include <Inventor/nodes/SoIndexedFaceSet.h>
+#include <Inventor/nodes/SoMaterialBinding.h>
+#include <Inventor/nodes/SoNormal.h>
+#include <Inventor/nodes/SoShapeHints.h>
+
+namespace armarx::viz::coin
+{
+    struct VisuDataMesh : TypedVisuData<SoSeparator>
+    {
+        VisuDataMesh()
+        {
+            SoMaterialBinding* myBinding = new SoMaterialBinding;
+            myBinding->value = SoMaterialBinding::PER_VERTEX_INDEXED;
+
+            myMaterials = new SoMaterial;
+            myCoords = new SoCoordinate3;
+            normals = new SoNormal;
+
+            SoNormalBinding* normBinding = new SoNormalBinding;
+            normBinding->value = SoNormalBinding::PER_VERTEX_INDEXED;
+
+            SoShapeHints* hints = new SoShapeHints;
+            // Disable back culling and enable two-sided lighting
+            hints->vertexOrdering = SoShapeHints::VertexOrdering::COUNTERCLOCKWISE;
+            hints->shapeType = SoShapeHints::ShapeType::UNKNOWN_SHAPE_TYPE;
+
+            myFaceSet = new SoIndexedFaceSet;
+
+            auto* sep = node();
+            sep->addChild(myBinding);
+            sep->addChild(myMaterials);
+            sep->addChild(myCoords);
+            sep->addChild(normals);
+            sep->addChild(normBinding);
+            sep->addChild(hints);
+            sep->addChild(myFaceSet);
+        }
+
+        SoMaterial* myMaterials;
+        SoCoordinate3* myCoords;
+        SoNormal* normals;
+        SoIndexedFaceSet* myFaceSet;
+    };
+
+    struct VisualizerMesh: TypedElementVisualizer<VisualizerMesh, ElementMesh, VisuDataMesh>
+    {
+        void updateElement(ElementType const& element, DataType* data)
+        {
+            int colorSize = (int)element.colors.size();
+            bool noColorsArray = colorSize == 0;
+            if (colorSize == 0)
+            {
+                colorSize = 1;
+            }
+            // TODO: Optimize these temporary arrays away
+            std::vector<SbColor> matAmb(colorSize);
+            std::vector<SbColor> matDif(colorSize);
+            std::vector<float> transp(colorSize);
+
+            const float conv = 1.0f / 255.0f;
+            if (noColorsArray)
+            {
+                auto color = element.color;
+                float r = color.r * conv;
+                float g = color.g * conv;
+                float b = color.b * conv;
+                float a = color.a * conv;
+                matAmb[0].setValue(r, g, b);
+                matDif[0].setValue(r, g, b);
+                transp[0] = 1.0f - a;
+            }
+            else
+            {
+                for (int i = 0; i < colorSize; i++)
+                {
+                    auto color = element.colors[i];
+                    float r = color.r * conv;
+                    float g = color.g * conv;
+                    float b = color.b * conv;
+                    float a = color.a * conv;
+                    matAmb[i].setValue(r, g, b);
+                    matDif[i].setValue(r, g, b);
+                    transp[i] = 1.0f - a;
+                }
+            }
+
+            // Define colors for the faces
+            SoMaterial* myMaterials = data->myMaterials;
+            myMaterials->diffuseColor.setValues(0, colorSize, matDif.data());
+            myMaterials->ambientColor.setValues(0, colorSize, matAmb.data());
+            myMaterials->transparency.setValues(0, colorSize, transp.data());
+
+            // define vertex array
+            int vertexSize = (int)element.vertices.size();
+            std::vector<SbVec3f> vertexPositions(vertexSize);
+            for (int i = 0; i < vertexSize; i++)
+            {
+                auto v = element.vertices[i];
+                vertexPositions[i].setValue(v.e0, v.e1, v.e2);
+            }
+
+            // Define coordinates for vertices
+            data->myCoords->point.setValues(0, vertexSize, vertexPositions.data());
+
+            // define nomals array
+            int normalSize = (int)element.normals.size();
+            // per vertex normals
+            std::vector<SbVec3f> normalsArray(normalSize);
+            for (int i = 0; i < normalSize; i++)
+            {
+                auto n = element.normals[i];
+                normalsArray[i].setValue(n.e0, n.e1, n.e2);
+            }
+
+            // Define coordinates for vertices
+            data->normals->vector.setValues(0, normalSize, normalsArray.data());
+
+            // define faces and normals
+            int facesSize = (int)element.faces.size();
+            std::vector<int32_t> faces(facesSize * 4);
+            std::vector<int32_t> normalIndx(facesSize * 4);
+            std::vector<int32_t> matInx(facesSize * 4);
+
+            for (int i = 0; i < facesSize; i++)
+            {
+                auto& face = element.faces[i];
+
+                faces[i * 4 + 0] = face.v0;
+                faces[i * 4 + 1] = face.v1;
+                faces[i * 4 + 2] = face.v2;
+                faces[i * 4 + 3] = SO_END_FACE_INDEX;
+
+                normalIndx[i * 4 + 0] = face.n0;
+                normalIndx[i * 4 + 1] = face.n1;
+                normalIndx[i * 4 + 2] = face.n2;
+                normalIndx[i * 4 + 3] = SO_END_FACE_INDEX;
+
+                matInx[i * 4 + 0] = face.c0;
+                matInx[i * 4 + 1] = face.c1;
+                matInx[i * 4 + 2] = face.c2;
+                matInx[i * 4 + 3] = SO_END_FACE_INDEX;
+            }
+
+            SoIndexedFaceSet* myFaceSet = data->myFaceSet;
+            myFaceSet->coordIndex.setValues(0, faces.size(), faces.data());
+            myFaceSet->normalIndex.setValues(0, normalIndx.size(), normalIndx.data());
+            myFaceSet->materialIndex.setValues(0, matInx.size(), matInx.data());
+        }
+    };
+}

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

@@ -80,7 +80,14 @@ namespace armarx::viz::coin
             data->lineMaterial->ambientColor.setValue(r, g, b);
             data->lineMaterial->transparency.setValue(1.0f - a);
 
-            data->lineStyle->lineWidth.setValue(element.lineWidth);
+            if (element.lineWidth > 0.0f)
+            {
+                data->lineStyle->lineWidth.setValue(element.lineWidth);
+            }
+            else
+            {
+                data->lineStyle->style = SoDrawStyleElement::INVISIBLE;
+            }
 
             data->lineSet->numVertices.set1Value(0, pointSize + 1);
         }

source/RobotAPI/components/ArVizExample/ArVizExample.cpp

@@ -222,6 +222,80 @@ void ArVizExample::update()
 
             layer.elements.push_back(poly);
         }
+        {
+            viz::ElementPolygonPtr poly(new viz::ElementPolygon);
+            poly->id = "poly2";
+            poly->action = viz::Element_UPDATE;
+
+            poly->color = armarx::viz::Color{128, 255, 128, 0};
+            poly->lineColor = {255, 255, 0, 0};
+            poly->lineWidth = 0.0f;
+
+            float t = 1.0f + std::sin(timeInSeconds);
+            float offset = 20.0f * t;
+            poly->points.push_back(armarx::Vector3f{-100.0f - offset, -100.0f - offset, 0.0f});
+            poly->points.push_back(armarx::Vector3f{-100.0f, +100.0f, 0.0f});
+            poly->points.push_back(armarx::Vector3f{+100.0f + offset, +100.0f + offset, 0.0f});
+            poly->points.push_back(armarx::Vector3f{+100.0f, -100.0f, 0.0f});
+
+            Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+            pos.z() = +1500.0f;
+            poly->pose = makeGlobalPose(pos, Eigen::Quaternionf::Identity());
+
+            layer.elements.push_back(poly);
+        }
+        {
+            viz::ElementMeshPtr mesh(new viz::ElementMesh);
+            mesh->id = "mesh";
+            mesh->action = viz::Element_UPDATE;
+
+            armarx::Vector3f vertices[] =
+            {
+                {-100.0f, -100.0f, 0.0f},
+                {-100.0f, +100.0f, 0.0f},
+                {+100.0f, +100.0f, 0.0f},
+                {+100.0f, +100.0f, 200.0f},
+            };
+            armarx::viz::Color colors[] =
+            {
+                {255, 255, 0, 0},
+                {255, 0, 255, 0},
+                {255, 0, 0, 255},
+            };
+            armarx::Vector3f normals[] =
+            {
+                {0.0f, 0.0f, 1.0f},
+                {0.0f, 1.0f, 0.0f},
+            };
+            armarx::viz::Face faces[] =
+            {
+                {
+                    2, 1, 0,
+                    0, 1, 2,
+                    0, 0, 0
+                },
+                {
+                    1, 2, 3,
+                    0, 1, 2,
+                    1, 1, 1
+                },
+            };
+            std::size_t verticesSize = sizeof(vertices) / sizeof(vertices[0]);
+            mesh->vertices.assign(vertices, vertices + verticesSize);
+            std::size_t colorsSize = sizeof(colors) / sizeof(colors[0]);
+            mesh->colors.assign(colors, colors + colorsSize);
+            std::size_t normalsSize = sizeof(normals) / sizeof(normals[0]);
+            mesh->normals.assign(normals, normals + normalsSize);
+            std::size_t facesSize = sizeof(faces) / sizeof(faces[0]);
+            mesh->faces.assign(faces, faces + facesSize);
+
+            Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+            pos.z() = +1000.0f;
+            pos.x() = -500.0f;
+            mesh->pose = makeGlobalPose(pos, Eigen::Quaternionf::Identity());
+
+            layer.elements.push_back(mesh);
+        }
 
         topic->updateLayers({layer});
 

source/RobotAPI/interface/ArViz/Elements.ice

@@ -114,8 +114,30 @@ module viz
         float transparency = 0.0f;
     };
 
-    // TODO: TriMesh: Use format like Simox
+    sequence<Color> ColorSeq;
 
+    struct Face
+    {
+        int v0 = 0;
+        int v1 = 0;
+        int v2 = 0;
+        int c0 = 0;
+        int c1 = 0;
+        int c2 = 0;
+        int n0 = 0;
+        int n1 = 0;
+        int n2 = 0;
+    };
+
+    sequence<Face> FaceSeq;
+
+    class ElementMesh extends Element
+    {
+        Vector3fSeq vertices;
+        ColorSeq colors;
+        Vector3fSeq normals;
+        FaceSeq faces;
+    };
 
 
 };