Add shape type and color options to PointCloudShape

CHANGELOG.md

@@ -21,6 +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)
 
 * Examples and Tutorials
 

dart/dynamics/PointCloudShape.cpp

@@ -32,6 +32,8 @@
 
 #include "dart/dynamics/PointCloudShape.hpp"
 
+#include "dart/common/Console.hpp"
+
 namespace dart {
 namespace dynamics {
 
@@ -51,7 +53,10 @@ Eigen::Vector3d toVector3d(const octomap::point3d& point)
 
 //==============================================================================
 PointCloudShape::PointCloudShape(double visualSize)
-  : Shape(), mVisualSize(visualSize)
+  : Shape(),
+    mPointShapeType(BOX),
+    mColorMode(USE_SHAPE_COLOR),
+    mVisualSize(visualSize)
 {
   // Do nothing
 }
@@ -142,10 +147,83 @@ void PointCloudShape::removeAllPoints()
   mPoints.clear();
 }
 
+//==============================================================================
+void PointCloudShape::setPointShapeType(PointCloudShape::PointShapeType type)
+{
+  if (mPointShapeType == type)
+    return;
+
+  mPointShapeType = type;
+  incrementVersion();
+}
+
+//==============================================================================
+PointCloudShape::PointShapeType PointCloudShape::getPointShapeType() const
+{
+  return mPointShapeType;
+}
+
+//==============================================================================
+void PointCloudShape::setColorMode(PointCloudShape::ColorMode mode)
+{
+  mColorMode = mode;
+}
+
+//==============================================================================
+PointCloudShape::ColorMode PointCloudShape::getColorMode() const
+{
+  return mColorMode;
+}
+
+//==============================================================================
+void PointCloudShape::setOverallColor(const Eigen::Vector4d& color)
+{
+  mColors.resize(1);
+  mColors[0] = color;
+}
+
+//==============================================================================
+Eigen::Vector4d PointCloudShape::getOverallColor() const
+{
+  if (mColors.empty())
+  {
+    dtwarn << "[PointCloudShape] Attempt to get the overall color when the "
+           << "color array is empty. Returning (RGBA: [0.5, 0.5, 0.5, 0.5]) "
+           << "color\n";
+    return Eigen::Vector4d(0.5, 0.5, 0.5, 0.5);
+  }
+
+  if (mColors.size() > 1)
+  {
+    dtwarn << "[PointCloudShape] Attempting to get the overal color when the "
+           << "color array contains more than one color. This is potentially "
+           << "an error. Returning the first color in the color array.\n";
+  }
+
+  return mColors[0];
+}
+
+//==============================================================================
+void PointCloudShape::setColors(
+    const std::vector<
+        Eigen::Vector4d,
+        Eigen::aligned_allocator<Eigen::Vector4d> >& colors)
+{
+  mColors = colors;
+}
+
+//==============================================================================
+const std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> >&
+PointCloudShape::getColors() const
+{
+  return mColors;
+}
+
 //==============================================================================
 void PointCloudShape::setVisualSize(double size)
 {
   mVisualSize = size;
+  incrementVersion();
 }
 
 //==============================================================================

dart/dynamics/PointCloudShape.hpp

@@ -46,6 +46,20 @@ namespace dynamics {
 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
+  };
+
+  enum PointShapeType
+  {
+    BOX = 0,          ///< 3D volumetric box
+    BILLBOARD_QUAD,   ///< 2D square always facing the screen
+    BILLBOARD_CIRCLE, ///< 2D circle always facing the screen
+  };
+
   /// Constructor
   ///
   /// \param[in] visualSize The size of cube that represents each point.
@@ -89,9 +103,41 @@ class PointCloudShape : public Shape
   /// Returns the number of points.
   std::size_t getNumPoints() const;
 
-  /// Remove all the points.
+  /// Removes all the points.
   void removeAllPoints();
 
+  /// Sets the point shape type.
+  void setPointShapeType(PointShapeType type);
+
+  /// Returns the point shape type.
+  PointShapeType getPointShapeType() const;
+
+  /// Sets the color mode.
+  void setColorMode(ColorMode mode);
+
+  /// Returns the color mode.
+  ColorMode getColorMode() const;
+
+  /// Sets the overall color.
+  ///
+  /// This function resizes the colors to one.
+  void setOverallColor(const Eigen::Vector4d& color);
+
+  /// Returns the overall color.
+  Eigen::Vector4d getOverallColor() const;
+
+  /// Sets the point cloud colors.
+  ///
+  /// The count of colors should be the same with points. It's undefined
+  /// behavior, otherwise.
+  void setColors(const std::vector<
+                 Eigen::Vector4d,
+                 Eigen::aligned_allocator<Eigen::Vector4d>>& colors);
+
+  /// Returns the point cloud colors.
+  const std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d>>&
+  getColors() const;
+
   /// Sets size of visual object that represents each point.
   void setVisualSize(double size);
 
@@ -111,6 +157,16 @@ class PointCloudShape : public Shape
   /// List of points
   std::vector<Eigen::Vector3d> mPoints;
 
+  /// The point shape type.
+  PointShapeType mPointShapeType;
+
+  /// The color mode
+  ColorMode mColorMode;
+
+  /// List of colors
+  std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d>>
+      mColors;
+
   /// The size of visual object that represents each point.
   double mVisualSize;
 };