Add VoxelGridShape support to OSG renderer (#1276)

![image](https://user-images.githubusercontent.com/4038467/56428754-5cda0b00-6275-11e9-9de9-683c9078d2bf.png)

This PR depends on #1275 

***

**Before creating a pull request**

- [x] Document new methods and classes
- [x] Format new code files using `clang-format`

**Before merging a pull request**

- [x] Set version target by selecting a milestone on the right side
- [x] Summarize this change in `CHANGELOG.md`
- [x] Add unit test(s) for this change (N/A)

CHANGELOG.md

@@ -38,6 +38,7 @@
 * GUI
 
   * Updated ImGui to 1.69: [#1274](https://github.com/dartsim/dart/pull/1274)
+  * Add VoxelGridShape support to OSG renderer: [#1276](https://github.com/dartsim/dart/pull/1276)
 
 * Build System
 

dart/dynamics/CMakeLists.txt

@@ -28,17 +28,14 @@ install(
   COMPONENT headers
 )
 
-dart_format_add(
-  TranslationalJoint2D.hpp
-  TranslationalJoint2D.cpp
-  detail/TranslationalJoint2DAspect.hpp
-  detail/TranslationalJoint2DAspect.cpp
-)
-
 dart_format_add(
   HeightmapShape.hpp
   VoxelGridShape.hpp
   VoxelGridShape.cpp
   SmartPointer.hpp
+  TranslationalJoint2D.hpp
+  TranslationalJoint2D.cpp
   detail/HeightmapShape-impl.hpp
+  detail/TranslationalJoint2DAspect.hpp
+  detail/TranslationalJoint2DAspect.cpp
 )

dart/dynamics/VoxelGridShape.cpp

@@ -43,23 +43,35 @@ namespace dynamics {
 namespace {
 
 //==============================================================================
-octomap::point3d toPoint3d(const Eigen::Vector3d& point)
+octomap::point3d toPoint3f(const Eigen::Vector3f& point)
 {
   return octomap::point3d(point.x(), point.y(), point.z());
 }
 
 //==============================================================================
-octomath::Quaternion toQuaternion(const Eigen::Matrix3d& rotation)
+octomap::point3d toPoint3d(const Eigen::Vector3d& point)
+{
+  return toPoint3f(point.cast<float>());
+}
+
+//==============================================================================
+octomath::Quaternion toQuaternionf(const Eigen::Matrix3f& rotation)
 {
-  Eigen::Quaterniond quat(rotation);
+  Eigen::Quaternionf quat(rotation);
   return octomath::Quaternion(quat.w(), quat.x(), quat.y(), quat.z());
 }
 
+//==============================================================================
+octomath::Quaternion toQuaterniond(const Eigen::Matrix3d& rotation)
+{
+  return toQuaternionf(rotation.cast<float>());
+}
+
 //==============================================================================
 octomap::pose6d toPose6d(const Eigen::Isometry3d& frame)
 {
   return octomap::pose6d(
-      toPoint3d(frame.translation()), toQuaternion(frame.linear()));
+      toPoint3d(frame.translation()), toQuaterniond(frame.linear()));
 }
 
 } // namespace
@@ -68,6 +80,8 @@ octomap::pose6d toPose6d(const Eigen::Isometry3d& frame)
 VoxelGridShape::VoxelGridShape(double resolution) : Shape()
 {
   setOctree(fcl_make_shared<octomap::OcTree>(resolution));
+
+  mVariance = DYNAMIC_ELEMENTS;
 }
 
 //==============================================================================
@@ -82,6 +96,8 @@ VoxelGridShape::VoxelGridShape(fcl_shared_ptr<octomap::OcTree> octree) : Shape()
   }
 
   setOctree(std::move(octree));
+
+  mVariance = DYNAMIC_ELEMENTS;
 }
 
 //==============================================================================

dart/gui/osg/ShapeFrameNode.cpp

@@ -48,6 +48,9 @@
 #include "dart/gui/osg/render/MeshShapeNode.hpp"
 #include "dart/gui/osg/render/SoftMeshShapeNode.hpp"
 #include "dart/gui/osg/render/LineSegmentShapeNode.hpp"
+#if HAVE_OCTOMAP
+#include "dart/gui/osg/render/VoxelGridShapeNode.hpp"
+#endif
 #include "dart/gui/osg/render/WarningShapeNode.hpp"
 
 #include "dart/dynamics/Frame.hpp"
@@ -64,6 +67,9 @@
 #include "dart/dynamics/MeshShape.hpp"
 #include "dart/dynamics/SoftMeshShape.hpp"
 #include "dart/dynamics/LineSegmentShape.hpp"
+#if HAVE_OCTOMAP
+#include "dart/dynamics/VoxelGridShape.hpp"
+#endif
 #include "dart/dynamics/SimpleFrame.hpp"
 
 namespace dart {
@@ -285,6 +291,17 @@ void ShapeFrameNode::createShapeNode(
     else
       warnAboutUnsuccessfulCast(shapeType, mShapeFrame->getName());
   }
+#if HAVE_OCTOMAP
+  else if(VoxelGridShape::getStaticType() == shapeType)
+  {
+    std::shared_ptr<VoxelGridShape> lss =
+        std::dynamic_pointer_cast<VoxelGridShape>(shape);
+    if(lss)
+      mRenderShapeNode = new render::VoxelGridShapeNode(lss, this);
+    else
+      warnAboutUnsuccessfulCast(shapeType, mShapeFrame->getName());
+  }
+#endif // HAVE_OCTOMAP
   else
   {
     mRenderShapeNode = new render::WarningShapeNode(shape, this);

dart/gui/osg/ShapeFrameNode.hpp

@@ -35,7 +35,10 @@
 
 #include <map>
 #include <memory>
+
 #include <osg/MatrixTransform>
+
+#include "dart/config.hpp"
 #include "dart/dynamics/SmartPointer.hpp"
 
 namespace dart {

dart/gui/osg/render/CMakeLists.txt

@@ -18,3 +18,8 @@ install(
   DESTINATION include/dart/gui/osg/render
   COMPONENT headers
 )
+
+dart_format_add(
+  VoxelGridShapeNode.hpp
+  VoxelGridShapeNode.cpp
+)

dart/gui/osg/render/ShapeNode.cpp

@@ -42,7 +42,7 @@ namespace render {
 ShapeNode::ShapeNode(std::shared_ptr<dart::dynamics::Shape> shape,
                      ShapeFrameNode* parentNode,
                      ::osg::Node* node)
-  : mShape(shape),
+  : mShape(std::move(shape)),
     mParentShapeFrameNode(parentNode),
     mNode(node),
     mUtilized(true)