diff --git a/CHANGELOG.md b/CHANGELOG.md index 620c3bb2fc672..bebee85ce54ce 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/dart/dynamics/PointCloudShape.hpp b/dart/dynamics/PointCloudShape.hpp index 292753becc64f..49614f0c486cb 100644 --- a/dart/dynamics/PointCloudShape.hpp +++ b/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 diff --git a/dart/gui/osg/render/PointCloudShapeNode.cpp b/dart/gui/osg/render/PointCloudShapeNode.cpp index e04e1410d6955..b0c08e9be5a49 100644 --- a/dart/gui/osg/render/PointCloudShapeNode.cpp +++ b/dart/gui/osg/render/PointCloudShapeNode.cpp @@ -296,6 +296,50 @@ void PointCloudShapeNode::refresh() mPointCloudVersion = mPointCloudShape->getVersion(); } +//============================================================================== +bool shouldUseVisualAspectColor( + const std::vector& points, + const std::vector< + Eigen::Vector4d, + Eigen::aligned_allocator>& 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; + 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()); } diff --git a/examples/point_cloud/main.cpp b/examples/point_cloud/main.cpp index 6d2fd252661d7..c9754baca4018 100644 --- a/examples/point_cloud/main.cpp +++ b/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);