Add heightmap support to OSG renderer (#1293)

CHANGELOG.md

@@ -10,6 +10,7 @@
 
 * GUI
 
+  * Added heightmap support to OSG renderer: [#1293](https://github.com/dartsim/dart/pull/1293)
   * Improved voxel grid and point cloud rendering performance: [#1294](https://github.com/dartsim/dart/pull/1294)
 
 #### Compilers Tested

dart/collision/bullet/BulletCollisionDetector.cpp

@@ -964,7 +964,7 @@ createBulletCollisionShapeFromHeightmap(
     const HeightmapShapeT* heightMap)
 {
   // get the heightmap parameters
-  const Eigen::Vector3d& scale = heightMap->getScale();
+  const auto& scale = heightMap->getScale();
   const auto minHeight = heightMap->getMinHeight();
   const auto maxHeight = heightMap->getMaxHeight();
 
@@ -978,12 +978,6 @@ createBulletCollisionShapeFromHeightmap(
     // scalarType = PHY_DOUBLE;
   }
 
-  // TODO take this out when testing is finished
-/*  dtdbg << "Creating height shape, heights w = "
-        << heightMap->getWidth() << ", h = " << heightMap->getDepth()
-        << " min/max = " << minHeight << "/" << maxHeight << " scale = "
-        << scale.x() <<", " << scale.y() << ", " << scale.z() << std::endl;*/
-
   // the y-values in the height field need to be flipped
   heightMap->flipY();
 

dart/collision/ode/detail/OdeHeightmap-impl.hpp

@@ -49,7 +49,7 @@ void setOdeHeightfieldDetails(
     const S* heights,
     const std::size_t& width,
     const std::size_t& height,
-    const Eigen::Vector3d& scale,
+    const Eigen::Matrix<S, 3, 1>& scale,
     typename std::enable_if<std::is_same<float, S>::value>::type* = 0)
 {
   assert(width >= 2);
@@ -82,7 +82,7 @@ void setOdeHeightfieldDetails(
     const S* heights,
     const std::size_t& width,
     const std::size_t& height,
-    const Eigen::Vector3d& scale,
+    const Eigen::Matrix<S, 3, 1>& scale,
     typename std::enable_if<std::is_same<double, S>::value>::type* = 0)
 {
   assert(width >= 2);
@@ -118,7 +118,7 @@ OdeHeightmap<S>::OdeHeightmap(
   assert(heightMap);
 
   // get the heightmap parameters
-  const Eigen::Vector3d& scale = heightMap->getScale();
+  const auto& scale = heightMap->getScale();
   const auto& heights = heightMap->getHeightField();
 
   // Create the ODE heightfield

dart/dynamics/HeightmapShape.hpp

@@ -48,6 +48,7 @@ class HeightmapShape : public Shape
 public:
   using S = S_;
 
+  using Vector3 = Eigen::Matrix<S, 3, 1>;
   using HeightField
       = Eigen::Matrix<S, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
 
@@ -72,38 +73,58 @@ class HeightmapShape : public Shape
 
   /// Sets scale of this heightmap.
   /// \param[in] scale Scale of the height map.
-  void setScale(const Eigen::Vector3d& scale);
+  void setScale(const Vector3& scale);
 
   /// Returns scale of this heightmap.
-  const Eigen::Vector3d& getScale() const;
+  const Vector3& getScale() const;
 
   /// Sets the height field.
   ///
-  /// The data in \e heights will be copied locally.
-  /// It would be nice to have the option to use the values in
-  /// \e heights directly instead of copying them locally to a vector in this
-  /// class (this would avoid any data being kept twice). However some
-  /// collision engine implementations may require to transform the height
-  /// values, e.g. bullet needs the y values flipped. Therefore,
-  /// a (mutable) copy of the height values passed in \e heights will be kept
-  /// in this class. The copied data can be modified via
+  /// The data in \e heights will be copied locally. It would be nice to have
+  /// the option to use the values in \e heights directly instead of copying
+  /// them locally to a vector in this class (this would avoid any data being
+  /// kept twice). However some collision engine implementations may require to
+  /// transform the height values, e.g. bullet needs the y values flipped.
+  /// Therefore, a (mutable) copy of the height values passed in \e heights will
+  /// be kept in this class. The copied data can be modified via
   /// getHeightFieldModifiable() and with flipY().
   ///
   /// \param[in] width Width of the field (x axis)
   /// \param[in] depth Depth of the field (-y axis)
-  /// \param[in] heights The height data of size \e width * \e depth.
-  /// The heights are interpreted as z values, while \e width goes in x
-  /// direction and \e depth in -y (it goes to -y because traditionally
-  /// images are read from top row to bottom row).
-  /// In the geometry which is to be generated from this shape, the min/max
-  /// height value is also the min/max z value (so if the minimum height
-  /// value is -100, the lowest terrain point will be -100, times the z
-  /// scale to be applied).
+  /// \param[in] heights The height data of size \e width * \e depth. The
+  /// heights are interpreted as z values, while \e width goes in x direction
+  /// and \e depth in -y (it goes to -y because traditionally images are read
+  /// from top row to bottom row). In the geometry which is to be generated from
+  /// this shape, the min/max height value is also the min/max z value (so if
+  /// the minimum height value is -100, the lowest terrain point will be -100,
+  /// times the z scale to be applied).
   void setHeightField(
       const std::size_t& width,
       const std::size_t& depth,
       const std::vector<S>& heights);
 
+  /// Sets the height field.
+  ///
+  /// The data in \e heights will be copied locally. It would be nice to have
+  /// the option to use the values in \e heights directly instead of copying
+  /// them locally to a vector in this class (this would avoid any data being
+  /// kept twice). However some collision engine implementations may require to
+  /// transform the height values, e.g. bullet needs the y values flipped.
+  /// Therefore, a (mutable) copy of the height values passed in \e heights will
+  /// be kept in this class. The copied data can be modified via
+  /// getHeightFieldModifiable() and with flipY().
+  ///
+  /// \param[in] heights The height data of size \e width * \e depth where
+  /// number of columns and number of rows are the width of the field (x axis)
+  /// and the depth of the field (-y axis), respectively. The heights are
+  /// interpreted as z values, while \e width goes in x direction and \e depth
+  /// in -y (it goes to -y because traditionally images are read from top row to
+  /// bottom row). In the geometry which is to be generated from this shape, the
+  /// min/max height value is also the min/max z value (so if the minimum height
+  /// value is -100, the lowest terrain point will be -100, times the z scale to
+  /// be applied).
+  void setHeightField(const HeightField& heights);
+
   /// Returns the height field.
   const HeightField& getHeightField() const;
 
@@ -125,6 +146,9 @@ class HeightmapShape : public Shape
   /// Returns the maximum height set by setHeightField()
   S getMaxHeight() const;
 
+  /// Set the color of this arrow
+  void notifyColorUpdated(const Eigen::Vector4d& color) override;
+
 protected:
   // Documentation inherited.
   void updateBoundingBox() const override;
@@ -143,7 +167,7 @@ class HeightmapShape : public Shape
 
 private:
   /// Scale of the heightmap
-  Eigen::Vector3d mScale;
+  Vector3 mScale;
 
   /// Height field
   mutable HeightField mHeights;

dart/dynamics/detail/HeightmapShape-impl.hpp

@@ -68,7 +68,7 @@ const std::string& HeightmapShape<S>::getStaticType()
 
 //==============================================================================
 template <typename S>
-void HeightmapShape<S>::setScale(const Eigen::Vector3d& scale)
+void HeightmapShape<S>::setScale(const Vector3& scale)
 {
   assert(scale[0] > 0.0);
   assert(scale[1] > 0.0);
@@ -82,7 +82,7 @@ void HeightmapShape<S>::setScale(const Eigen::Vector3d& scale)
 
 //==============================================================================
 template <typename S>
-const Eigen::Vector3d& HeightmapShape<S>::getScale() const
+auto HeightmapShape<S>::getScale() const -> const Vector3&
 {
   return mScale;
 }
@@ -97,34 +97,31 @@ void HeightmapShape<S>::setHeightField(
   assert(heights.size() == width * depth);
   if ((width * depth) != heights.size())
   {
-    dterr << "Size of height field needs to be width*depth=" << width * depth
-          << std::endl;
+    dterr << "[HeightmapShape] Size of height field needs to be width*depth="
+          << width * depth << "\n";
     return;
   }
   if (heights.empty())
   {
-    dtwarn << "Empty height field makes no sense." << std::endl;
+    dtwarn << "Empty height field makes no sense.\n";
     return;
   }
-  mHeights = HeightField(depth, width);
-  for (size_t r = 0; r < depth; ++r)
-  {
-    for (size_t c = 0; c < width; ++c)
-    {
-      mHeights(r, c) = heights[r * width + c];
-    }
-  }
 
-  // compute minimum and maximum height
-  mMinHeight = std::numeric_limits<S>::max();
-  mMaxHeight = -std::numeric_limits<S>::max();
-  for (auto it = heights.begin(); it != heights.end(); ++it)
-  {
-    if (*it < mMinHeight)
-      mMinHeight = *it;
-    if (*it > mMaxHeight)
-      mMaxHeight = *it;
-  }
+  // make heightmap data local copy
+  const Eigen::Map<const HeightField> data(heights.data(), depth, width);
+
+  setHeightField(data);
+}
+
+//==============================================================================
+template <typename S>
+void HeightmapShape<S>::setHeightField(const HeightField& heights)
+{
+  mHeights = heights;
+
+  mMinHeight = heights.minCoeff();
+  mMaxHeight = heights.maxCoeff();
+
   mIsBoundingBoxDirty = true;
   mIsVolumeDirty = true;
 
@@ -180,6 +177,13 @@ std::size_t HeightmapShape<S>::getDepth() const
   return mHeights.rows();
 }
 
+//==============================================================================
+template <typename S>
+void HeightmapShape<S>::notifyColorUpdated(const Eigen::Vector4d& /*color*/)
+{
+  incrementVersion();
+}
+
 //==============================================================================
 template <typename S>
 Eigen::Matrix3d HeightmapShape<S>::computeInertia(double mass) const

dart/gui/osg/CMakeLists.txt

@@ -106,4 +106,5 @@ dart_format_add(
   ImGuiHandler.cpp
   RealTimeWorldNode.hpp
   RealTimeWorldNode.cpp
+  Utils.hpp
 )

dart/gui/osg/DefaultEventHandler.cpp

@@ -155,8 +155,8 @@ void DefaultEventHandler::getNearAndFarPointUnderCursor(Eigen::Vector3d& near,
   invVPW.invert(VPW);
 
   double x = getWindowCursorX(), y = getWindowCursorY();
-  ::osg::Vec3 osgNear = ::osg::Vec3(x,y,0.0) * invVPW;
-  ::osg::Vec3 osgFar = ::osg::Vec3(x,y,distance) * invVPW;
+  auto osgNear = ::osg::Vec3d(x,y,0.0) * invVPW;
+  auto osgFar = ::osg::Vec3d(x,y,distance) * invVPW;
 
   near = osgToEigVec3(osgNear);
   far = osgToEigVec3(osgFar);
@@ -230,7 +230,7 @@ void DefaultEventHandler::pick(std::vector<PickInfo>& infoVector,
         PickInfo info;
         info.shape = shape->getShape();
         info.frame = shape->getParentShapeFrameNode()->getShapeFrame();
-        info.normal = osgToEigVec3(intersect.getWorldIntersectNormal());
+        info.normal = osgToEigVec3(intersect.getWorldIntersectNormal()).cast<double>();
         info.position = osgToEigVec3(intersect.getWorldIntersectPoint());
 
         infoVector.push_back(info);

dart/gui/osg/ShapeFrameNode.cpp

@@ -52,6 +52,7 @@
 #if HAVE_OCTOMAP
 #include "dart/gui/osg/render/VoxelGridShapeNode.hpp"
 #endif
+#include "dart/gui/osg/render/HeightmapShapeNode.hpp"
 #include "dart/gui/osg/render/WarningShapeNode.hpp"
 
 #include "dart/dynamics/Frame.hpp"
@@ -72,6 +73,7 @@
 #if HAVE_OCTOMAP
 #include "dart/dynamics/VoxelGridShape.hpp"
 #endif
+#include "dart/dynamics/HeightmapShape.hpp"
 #include "dart/dynamics/SimpleFrame.hpp"
 
 namespace dart {
@@ -313,6 +315,25 @@ void ShapeFrameNode::createShapeNode(
       warnAboutUnsuccessfulCast(shapeType, mShapeFrame->getName());
   }
 #endif // HAVE_OCTOMAP
+  else if(HeightmapShapef::getStaticType() == shapeType)
+  {
+    std::shared_ptr<HeightmapShapef> lss =
+        std::dynamic_pointer_cast<HeightmapShapef>(shape);
+    if(lss)
+      mRenderShapeNode = new render::HeightmapShapeNode<float>(lss, this);
+    else
+      warnAboutUnsuccessfulCast(shapeType, mShapeFrame->getName());
+  }
+  // OpenSceneGraph currently doesn't support double precision for Heightmap
+  // else if(HeightmapShaped::getStaticType() == shapeType)
+  // {
+  //   std::shared_ptr<HeightmapShaped> lss =
+  //       std::dynamic_pointer_cast<HeightmapShaped>(shape);
+  //   if(lss)
+  //     mRenderShapeNode = new render::HeightmapShapeNode<double>(lss, this);
+  //   else
+  //     warnAboutUnsuccessfulCast(shapeType, mShapeFrame->getName());
+  // }
   else
   {
     mRenderShapeNode = new render::WarningShapeNode(shape, this);