Fix handling color mode of PointCloudShape in OSG

CHANGELOG.md

@@ -21,7 +21,7 @@
   * Fixed incorrect alpha value update of InteractiveFrame: [#1297](https://github.com/dartsim/dart/pull/1297)
   * Fixed dereferencing a dangling pointer in WorldNode: [#1311](https://github.com/dartsim/dart/pull/1311)
   * Removed warning of ImGuiViewer + OSG shadow: [#1312](https://github.com/dartsim/dart/pull/1312)
-  * Added shape type and color options to PointCloudShape: [#1314](https://github.com/dartsim/dart/pull/1314)
+  * Added shape type and color options to PointCloudShape: [#1314](https://github.com/dartsim/dart/pull/1314), [#1316](https://github.com/dartsim/dart/pull/1316)
   * Fixed incorrect transparency of MeshShape for MATERIAL_COLOR mode: [#1315](https://github.com/dartsim/dart/pull/1315)
 
 * Examples and Tutorials

dart/dynamics/PointCloudShape.hpp

@@ -48,9 +48,14 @@ class PointCloudShape : public Shape
 public:
   enum ColorMode
   {
-    BIND_OVERALL = 0, ///< Use one color for all the points
-    BIND_PER_POINT,   ///< Use one color per point
-    USE_SHAPE_COLOR,  ///< Use the color specified by the ShapeAspect
+    /// Use the color specified by the ShapeAspect. This is the default.
+    USE_SHAPE_COLOR = 0,
+
+    /// Use one color for all the points
+    BIND_OVERALL,
+
+    /// Use one color per point
+    BIND_PER_POINT,
   };
 
   enum PointShapeType

dart/gui/osg/render/PointCloudShapeNode.cpp

@@ -296,6 +296,50 @@ void PointCloudShapeNode::refresh()
   mPointCloudVersion = mPointCloudShape->getVersion();
 }
 
+//==============================================================================
+bool shouldUseVisualAspectColor(
+    const std::vector<Eigen::Vector3d>& points,
+    const std::vector<
+        Eigen::Vector4d,
+        Eigen::aligned_allocator<Eigen::Vector4d>>& colors,
+    dynamics::PointCloudShape::ColorMode colorMode)
+{
+  if (colorMode == dynamics::PointCloudShape::USE_SHAPE_COLOR)
+  {
+    return true;
+  }
+  else if (colorMode == dynamics::PointCloudShape::BIND_OVERALL)
+  {
+    if (colors.empty())
+    {
+      dtwarn << "[PointCloudShapeNode] The color array in PointCloudShape "
+             << "is empty while the color mode is BIND_OVERALL, which "
+             << "requires at least on color in the color array. "
+             << "Using visual aspect color instead.\n";
+      return true;
+    }
+  }
+  else if (colorMode == dynamics::PointCloudShape::BIND_PER_POINT)
+  {
+    if (colors.size() != points.size())
+    {
+      dtwarn << "[PointCloudShapeNode] The color array in PointCloudShape "
+             << "has different size from the point array while the color mode "
+             << "is BIND_PER_POINT, which requires the same number of colors. "
+             << "Using visual aspect color instead.\n";
+      return true;
+    }
+  }
+  else
+  {
+    dtwarn << "[PointCloudShapeNode] The current color mode '" << colorMode
+           << "' is not supported by OSG renderer.\n";
+    return true;
+  }
+
+  return false;
+}
+
 //==============================================================================
 void PointCloudShapeNode::extractData(bool firstTime)
 {
@@ -313,6 +357,10 @@ void PointCloudShapeNode::extractData(bool firstTime)
   const auto visualSize = mPointCloudShape->getVisualSize();
   const auto& points = mPointCloudShape->getPoints();
   const auto& colors = mPointCloudShape->getColors();
+  const auto colorMode = mPointCloudShape->getColorMode();
+
+  const bool useVisualAspectColor
+      = shouldUseVisualAspectColor(points, colors, colorMode);
 
   // Pre-allocate for the case that the size of new points are greater than
   // previous update
@@ -325,16 +373,41 @@ void PointCloudShapeNode::extractData(bool firstTime)
   {
     mPointNodes[i]->updateCenter(points[i]);
     mPointNodes[i]->updateSize(visualSize);
-    mPointNodes[i]->updateColor(colors[i]);
+    if (useVisualAspectColor
+        || colorMode == dynamics::PointCloudShape::USE_SHAPE_COLOR)
+    {
+      mPointNodes[i]->updateColor(mVisualAspect->getRGBA());
+    }
+    else if (colorMode == dynamics::PointCloudShape::BIND_OVERALL)
+    {
+      mPointNodes[i]->updateColor(colors[0]);
+    }
+    else if (colorMode == dynamics::PointCloudShape::BIND_PER_POINT)
+    {
+      mPointNodes[i]->updateColor(colors[i]);
+    }
   }
 
   // If the number of new points is greater than cache box, then create new
   // boxes that many.
   for (auto i = mPointNodes.size(); i < points.size(); ++i)
   {
-    auto osgSphere = createPointNode(points[i], visualSize, colors[i]);
-    assert(osgSphere);
-    mPointNodes.emplace_back(osgSphere);
+    ::osg::ref_ptr<PointNode> pointNode;
+    if (useVisualAspectColor
+        || colorMode == dynamics::PointCloudShape::USE_SHAPE_COLOR)
+    {
+      pointNode
+          = createPointNode(points[i], visualSize, mVisualAspect->getRGBA());
+    }
+    else if (colorMode == dynamics::PointCloudShape::BIND_OVERALL)
+    {
+      pointNode = createPointNode(points[i], visualSize, colors[0]);
+    }
+    else if (colorMode == dynamics::PointCloudShape::BIND_PER_POINT)
+    {
+      pointNode = createPointNode(points[i], visualSize, colors[i]);
+    }
+    mPointNodes.emplace_back(pointNode);
     addChild(mPointNodes.back());
   }
 

examples/point_cloud/main.cpp

@@ -399,25 +399,27 @@ class PointCloudWidget : public dart::gui::osg::ImGuiWidget
         }
 
         auto pointCloudShape = mNode->getPointCloudShape();
-        int pointShapeType;
-        if (pointCloudShape->getPointShapeType()
-            == dynamics::PointCloudShape::BOX)
+
+        ImGui::Text("Color Mode:");
+        int colorMode = pointCloudShape->getColorMode();
+        if (ImGui::RadioButton("Use shape color", &colorMode, 0))
         {
-          pointShapeType = 0;
+          pointCloudShape->setColorMode(
+              dynamics::PointCloudShape::USE_SHAPE_COLOR);
         }
-        else if (
-            pointCloudShape->getPointShapeType()
-            == dynamics::PointCloudShape::BILLBOARD_QUAD)
+        else if (ImGui::RadioButton("Bind overall", &colorMode, 1))
         {
-          pointShapeType = 1;
+          pointCloudShape->setColorMode(
+              dynamics::PointCloudShape::BIND_OVERALL);
         }
-        else if (
-            pointCloudShape->getPointShapeType()
-            == dynamics::PointCloudShape::BILLBOARD_CIRCLE)
+        else if (ImGui::RadioButton("Bind per point", &colorMode, 2))
         {
-          pointShapeType = 2;
+          pointCloudShape->setColorMode(
+              dynamics::PointCloudShape::BIND_PER_POINT);
         }
 
+        ImGui::Text("Point Shape Type:");
+        int pointShapeType = pointCloudShape->getPointShapeType();
         if (ImGui::RadioButton("Box", &pointShapeType, 0))
         {
           pointCloudShape->setPointShapeType(dynamics::PointCloudShape::BOX);