Let ConstraintSolver set the colliding status of PointMass

dart/constraint/ConstraintSolver.cpp

@@ -398,8 +398,13 @@ void ConstraintSolver::updateConstraints()
     auto& ct = mCollisionResult.getContact(i);
 
     // Set colliding bodies
-    const_cast<dynamics::ShapeFrame*>(ct.collisionObject1->getShapeFrame())->asShapeNode()->getBodyNodePtr()->setColliding(true);
-    const_cast<dynamics::ShapeFrame*>(ct.collisionObject2->getShapeFrame())->asShapeNode()->getBodyNodePtr()->setColliding(true);
+    auto shapeFrame1 = const_cast<dynamics::ShapeFrame*>(
+          ct.collisionObject1->getShapeFrame());
+    auto shapeFrame2 = const_cast<dynamics::ShapeFrame*>(
+          ct.collisionObject2->getShapeFrame());
+
+    shapeFrame1->asShapeNode()->getBodyNodePtr()->setColliding(true);
+    shapeFrame2->asShapeNode()->getBodyNodePtr()->setColliding(true);
 
     if (isSoftContact(ct))
     {

dart/constraint/SoftContactConstraint.cpp

@@ -109,6 +109,7 @@ SoftContactConstraint::SoftContactConstraint(
     {
       mPointMass1 = selectCollidingPointMass(mSoftBodyNode1, contact.point,
                                              contact.triID1);
+      mPointMass1->setColliding(true);
     }
   }
   if (mSoftBodyNode2)
@@ -118,6 +119,7 @@ SoftContactConstraint::SoftContactConstraint(
     {
       mPointMass2 = selectCollidingPointMass(mSoftBodyNode2, contact.point,
                                              contact.triID2);
+      mPointMass2->setColliding(true);
     }
   }
 

dart/dynamics/BodyNode.cpp

@@ -180,6 +180,18 @@ BodyNode::~BodyNode()
   delete mParentJoint;
 }
 
+//==============================================================================
+SoftBodyNode* BodyNode::asSoftBodyNode()
+{
+  return nullptr;
+}
+
+//==============================================================================
+const SoftBodyNode* BodyNode::asSoftBodyNode() const
+{
+  return nullptr;
+}
+
 //==============================================================================
 void BodyNode::setProperties(const ExtendedProperties& _properties)
 {

dart/dynamics/BodyNode.h

@@ -103,6 +103,14 @@ class BodyNode :
   /// Destructor
   virtual ~BodyNode();
 
+  /// Convert 'this' into a SoftBodyNode pointer if this BodyNode is a
+  /// SoftBodyNode, otherwise return nullptr
+  virtual SoftBodyNode* asSoftBodyNode();
+
+  /// Convert 'const this' into a SoftBodyNode pointer if this BodyNode is a
+  /// SoftBodyNode, otherwise return nullptr
+  virtual const SoftBodyNode* asSoftBodyNode() const;
+
   /// Set the ExtendedProperties of this BodyNode
   void setProperties(const ExtendedProperties& _properties);
 
@@ -699,12 +707,13 @@ class BodyNode :
   /// Return the velocity change due to the constraint impulse
   const Eigen::Vector6d& getBodyVelocityChange() const;
 
-  /// Set whether this body node is colliding with others. This is called by
-  /// collision detector.
+  /// Set whether this body node is colliding with other objects. Note that
+  /// this status is set by the constraint solver during dynamics simulation but
+  /// not by collision detector.
   /// \param[in] True if this body node is colliding.
   void setColliding(bool _isColliding);
 
-  /// Return whether this body node is colliding with others
+  /// Return whether this body node is set to be colliding with other objects.
   /// \return True if this body node is colliding.
   bool isColliding();
 

dart/dynamics/PointMass.h

@@ -156,11 +156,13 @@ class PointMass : public common::Subject
   const PointMass* getConnectedPointMass(size_t _idx) const;
 
 
-  /// Set whether this point mass is colliding with others.
+  /// Set whether this point mass is colliding with other objects. Note that
+  /// this status is set by the constraint solver during dynamics simulation but
+  /// not by collision detector.
   /// \param[in] True if this point mass is colliding.
   void setColliding(bool _isColliding);
 
-  /// Get whether this point mass is colliding with others.
+  /// Return whether this point mass is set to be colliding with other objects.
   /// \return True if this point mass is colliding.
   bool isColliding();
 

dart/dynamics/ReferentialSkeleton.cpp

@@ -36,6 +36,7 @@
 
 #include "dart/dynamics/ReferentialSkeleton.h"
 #include "dart/dynamics/BodyNode.h"
+#include "dart/dynamics/SoftBodyNode.h"
 #include "dart/dynamics/Joint.h"
 #include "dart/dynamics/DegreeOfFreedom.h"
 
@@ -788,11 +789,19 @@ double ReferentialSkeleton::getPotentialEnergy() const
 //==============================================================================
 void ReferentialSkeleton::clearCollidingBodies()
 {
-  for(size_t i = 0; i < this->getNumBodyNodes(); i++)
+  for (auto i = 0u; i < getNumBodyNodes(); ++i)
   {
-    auto bd = this->getBodyNode(i);
-    if(bd)
-      bd->setColliding(false);
+    auto bodyNode = getBodyNode(i);
+    bodyNode->setColliding(false);
+
+    auto softBodyNode = bodyNode->asSoftBodyNode();
+    if (softBodyNode)
+    {
+      auto& pointMasses = softBodyNode->getPointMasses();
+
+      for (auto pointMass : pointMasses)
+        pointMass->setColliding(false);
+    }
   }
 }
 

dart/dynamics/Skeleton.cpp

@@ -3616,11 +3616,19 @@ double Skeleton::getPotentialEnergy() const
 //==============================================================================
 void Skeleton::clearCollidingBodies()
 {
-  for(size_t i = 0; i < this->getNumBodyNodes(); i++)
+  for (auto i = 0u; i < getNumBodyNodes(); ++i)
   {
-    auto bd = this->getBodyNode(i);
-    if(bd)
-      bd->setColliding(false);
+    auto bodyNode = getBodyNode(i);
+    bodyNode->setColliding(false);
+
+    auto softBodyNode = bodyNode->asSoftBodyNode();
+    if (softBodyNode)
+    {
+      auto& pointMasses = softBodyNode->getPointMasses();
+
+      for (auto pointMass : pointMasses)
+        pointMass->setColliding(false);
+    }
   }
 }
 

dart/dynamics/SoftBodyNode.cpp

@@ -127,6 +127,18 @@ SoftBodyNode::~SoftBodyNode()
   delete mNotifier;
 }
 
+//==============================================================================
+SoftBodyNode* SoftBodyNode::asSoftBodyNode()
+{
+  return this;
+}
+
+//==============================================================================
+const SoftBodyNode* SoftBodyNode::asSoftBodyNode() const
+{
+  return this;
+}
+
 //==============================================================================
 void SoftBodyNode::setProperties(const Properties& _properties)
 {

dart/dynamics/SoftBodyNode.h

@@ -121,6 +121,12 @@ class SoftBodyNode : public BodyNode
   /// \brief
   virtual ~SoftBodyNode();
 
+  // Documentation inherited
+  SoftBodyNode* asSoftBodyNode() override;
+
+  // Documentation inherited
+  const SoftBodyNode* asSoftBodyNode() const override;
+
   /// Set the Properties of this SoftBodyNode
   void setProperties(const Properties& _properties);