Merge pull request #692 from dartsim/grey/markers

Change Markers into Nodes

dart/collision/dart/DARTCollide.cpp

@@ -43,6 +43,7 @@
 #include "dart/dynamics/EllipsoidShape.h"
 #include "dart/dynamics/CylinderShape.h"
 #include "dart/dynamics/BodyNode.h"
+#include "dart/math/Helpers.h"
 
 namespace dart {
 namespace collision {

dart/constraint/ContactConstraint.cpp

@@ -43,6 +43,7 @@
 #include "dart/dynamics/Skeleton.h"
 #include "dart/collision/CollisionObject.h"
 #include "dart/lcpsolver/lcp.h"
+#include "dart/math/Helpers.h"
 
 #define DART_EPSILON 1e-6
 #define DART_ERROR_ALLOWANCE 0.0

dart/dynamics/BodyNode.cpp

@@ -243,13 +243,6 @@ BodyNode::~BodyNode()
   mNodeMap.clear();
   mNodeDestructors.clear();
 
-  // Release markers
-  for (std::vector<Marker*>::const_iterator it = mMarkers.begin();
-       it != mMarkers.end(); ++it)
-  {
-    delete (*it);
-  }
-
   delete mParentJoint;
 }
 
@@ -329,17 +322,6 @@ void BodyNode::setAspectProperties(const AspectProperties& properties)
   setGravityMode(properties.mGravityMode);
   setFrictionCoeff(properties.mFrictionCoeff);
   setRestitutionCoeff(properties.mRestitutionCoeff);
-
-  // TODO(MXG): Make Markers into Nodes before DART 6.0
-  mAspectProperties.mMarkerProperties = properties.mMarkerProperties;
-  // Remove current markers
-  for(Marker* marker : mMarkers)
-    delete marker;
-
-  // Create new markers
-  mMarkers.clear();
-  for(const Marker::Properties& marker : mAspectProperties.mMarkerProperties)
-    addMarker(new Marker(marker, this));
 }
 
 //==============================================================================
@@ -899,6 +881,9 @@ const Joint* BodyNode::getChildJoint(std::size_t _index) const
   return const_cast<BodyNode*>(this)->getChildJoint(_index);
 }
 
+//==============================================================================
+DART_BAKE_SPECIALIZED_NODE_DEFINITIONS( BodyNode, ShapeNode )
+
 //==============================================================================
 ShapeNode* BodyNode::createShapeNode(const ShapePtr& shape)
 {
@@ -925,24 +910,6 @@ ShapeNode* BodyNode::createShapeNode(const ShapePtr& shape, const char* name)
   return createShapeNode(shape, std::string(name));
 }
 
-//==============================================================================
-std::size_t BodyNode::getNumShapeNodes() const
-{
-  return getNumNodes<ShapeNode>();
-}
-
-//==============================================================================
-ShapeNode* BodyNode::getShapeNode(std::size_t index)
-{
-  return getNode<ShapeNode>(index);
-}
-
-//==============================================================================
-const ShapeNode* BodyNode::getShapeNode(std::size_t index) const
-{
-  return getNode<ShapeNode>(index);
-}
-
 //==============================================================================
 const std::vector<ShapeNode*> BodyNode::getShapeNodes()
 {
@@ -977,6 +944,16 @@ void BodyNode::removeAllShapeNodes()
     shapeNode->remove();
 }
 
+//==============================================================================
+DART_BAKE_SPECIALIZED_NODE_DEFINITIONS( BodyNode, EndEffector )
+
+//==============================================================================
+EndEffector* BodyNode::createEndEffector(
+    const EndEffector::BasicProperties& _properties)
+{
+  return createNode<EndEffector>(_properties);
+}
+
 //==============================================================================
 EndEffector* BodyNode::createEndEffector(const std::string& _name)
 {
@@ -993,30 +970,25 @@ EndEffector* BodyNode::createEndEffector(const char* _name)
 }
 
 //==============================================================================
-void BodyNode::addMarker(Marker* _marker)
-{
-  mMarkers.push_back(_marker);
-  const SkeletonPtr& skel = getSkeleton();
-  if(skel)
-    skel->addEntryToMarkerNameMgr(_marker);
-}
+DART_BAKE_SPECIALIZED_NODE_DEFINITIONS( BodyNode, Marker )
 
 //==============================================================================
-std::size_t BodyNode::getNumMarkers() const
+Marker* BodyNode::createMarker(const std::string& name,
+                               const Eigen::Vector3d& position,
+                               const Eigen::Vector4d& color)
 {
-  return mMarkers.size();
-}
+  Marker::BasicProperties properties;
+  properties.mName = name;
+  properties.mRelativeTf.translation() = position;
+  properties.mColor = color;
 
-//==============================================================================
-Marker* BodyNode::getMarker(std::size_t _index)
-{
-  return common::getVectorObjectIfAvailable<Marker*>(_index, mMarkers);
+  return createNode<Marker>(properties);
 }
 
 //==============================================================================
-const Marker* BodyNode::getMarker(std::size_t _index) const
+Marker* BodyNode::createMarker(const Marker::BasicProperties& properties)
 {
-  return common::getVectorObjectIfAvailable<Marker*>(_index, mMarkers);
+  return createNode<Marker>(properties);
 }
 
 //==============================================================================

dart/dynamics/BodyNode.h

@@ -76,7 +76,7 @@ class Marker;
 /// BodyNode of the BodyNode.
 class BodyNode :
     public detail::BodyNodeCompositeBase,
-    public virtual BodyNodeSpecializedFor<ShapeNode, EndEffector>,
+    public virtual BodyNodeSpecializedFor<ShapeNode, EndEffector, Marker>,
     public SkeletonRefCountingBase,
     public TemplatedJacobianNode<BodyNode>
 {
@@ -535,6 +535,8 @@ class BodyNode :
   template <class NodeType, typename ...Args>
   NodeType* createNode(Args&&... args);
 
+  DART_BAKE_SPECIALIZED_NODE_DECLARATIONS( ShapeNode )
+
   /// Create an ShapeNode attached to this BodyNode. Pass a
   /// ShapeNode::Properties argument into its constructor. If automaticName is
   /// true, then the mName field of properties will be ignored, and the
@@ -555,15 +557,6 @@ class BodyNode :
   /// Create an ShapeNode with the specified name
   ShapeNode* createShapeNode(const ShapePtr& shape, const char* name);
 
-  /// Return the number of all the ShapeNodes in this BodyNode
-  std::size_t getNumShapeNodes() const;
-
-  /// Return the index-th ShapeNode
-  ShapeNode* getShapeNode(std::size_t index);
-
-  /// Return the index-th (const) ShapeNode
-  const ShapeNode* getShapeNode(std::size_t index) const;
-
   /// Return the list of ShapeNodes
   const std::vector<ShapeNode*> getShapeNodes();
 
@@ -599,28 +592,29 @@ class BodyNode :
   template <class Aspect>
   void removeAllShapeNodesWith();
 
+  DART_BAKE_SPECIALIZED_NODE_DECLARATIONS( EndEffector )
+
   /// Create an EndEffector attached to this BodyNode. Pass an
   /// EndEffector::Properties argument into this function.
-  template <class EndEffectorProperties>
-  EndEffector* createEndEffector(const EndEffectorProperties& _properties);
+  EndEffector* createEndEffector(
+      const EndEffector::BasicProperties& _properties);
 
   /// Create an EndEffector with the specified name
   EndEffector* createEndEffector(const std::string& _name = "EndEffector");
 
   /// Create an EndEffector with the specified name
   EndEffector* createEndEffector(const char* _name);
 
-  /// Add a marker into the bodynode
-  void addMarker(Marker* _marker);
-
-  /// Return the number of markers of the bodynode
-  std::size_t getNumMarkers() const;
+  DART_BAKE_SPECIALIZED_NODE_DECLARATIONS( Marker )
 
-  /// Return _index-th marker of the bodynode
-  Marker* getMarker(std::size_t _index);
+  /// Create a Marker with the given fields
+  Marker* createMarker(
+      const std::string& name = "marker",
+      const Eigen::Vector3d& position = Eigen::Vector3d::Zero(),
+      const Eigen::Vector4d& color = Eigen::Vector4d::Constant(1.0));
 
-  /// Return (const) _index-th marker of the bodynode
-  const Marker* getMarker(std::size_t _index) const;
+  /// Create a Marker given its basic properties
+  Marker* createMarker(const Marker::BasicProperties& properties);
 
   // Documentation inherited
   bool dependsOn(std::size_t _genCoordIndex) const override;
@@ -1071,9 +1065,6 @@ class BodyNode :
   /// allows some performance optimizations.
   std::set<Entity*> mNonBodyNodeEntities;
 
-  /// List of markers associated
-  std::vector<Marker*> mMarkers;
-
   /// A increasingly sorted list of dependent dof indices.
   std::vector<std::size_t> mDependentGenCoordIndices;
 

dart/dynamics/EndEffector.h

@@ -158,9 +158,8 @@ class EndEffector final :
   /// Constructor used by the Skeleton class
   explicit EndEffector(BodyNode* parent, const BasicProperties& properties);
 
-  /// Create a clone of this BodyNode. This may only be called by the Skeleton
-  /// class.
-  virtual Node* cloneNode(BodyNode* _parent) const override;
+  // Documentation inherited
+  Node* cloneNode(BodyNode* _parent) const override;
 
 public: