From fcb3fba29f532f0d7a5b28d71b091dfcfacef956 Mon Sep 17 00:00:00 2001
From: Konstantinos Chatzilygeroudis <costashatz@gmail.com>
Date: Thu, 14 Apr 2016 15:05:23 +0200
Subject: [PATCH] Fixes #666 - quick n dirty

---
 dart/constraint/ConstraintSolver.cpp  |  7 +++++++
 dart/dynamics/MetaSkeleton.h          |  3 +++
 dart/dynamics/ReferentialSkeleton.cpp | 11 +++++++++++
 dart/dynamics/ReferentialSkeleton.h   |  3 +++
 dart/dynamics/Skeleton.cpp            | 11 +++++++++++
 dart/dynamics/Skeleton.h              |  3 +++
 6 files changed, 38 insertions(+)

diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp
index 6d2aaa5624d49..07a59484a128d 100644
--- a/dart/constraint/ConstraintSolver.cpp
+++ b/dart/constraint/ConstraintSolver.cpp
@@ -291,7 +291,10 @@ LCPSolver* ConstraintSolver::getLCPSolver() const
 void ConstraintSolver::solve()
 {
   for (size_t i = 0; i < mSkeletons.size(); ++i)
+  {
     mSkeletons[i]->clearConstraintImpulses();
+    mSkeletons[i]->clearCollidingBodies();
+  }
 
   // Update constraints and collect active constraints
   updateConstraints();
@@ -394,6 +397,10 @@ 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);
+
     if (isSoftContact(ct))
     {
       mSoftContactConstraints.push_back(
diff --git a/dart/dynamics/MetaSkeleton.h b/dart/dynamics/MetaSkeleton.h
index 7d6041ea28ada..0b8c816fcc4d6 100644
--- a/dart/dynamics/MetaSkeleton.h
+++ b/dart/dynamics/MetaSkeleton.h
@@ -537,6 +537,9 @@ class MetaSkeleton : public common::Subject
   /// Get the potential energy of this MetaSkeleton
   virtual double getPotentialEnergy() const = 0;
 
+  /// Clear collision flags of the BodyNodes in this MetaSkeleton
+  virtual void clearCollidingBodies() = 0;
+
   /// \}
 
   //----------------------------------------------------------------------------
diff --git a/dart/dynamics/ReferentialSkeleton.cpp b/dart/dynamics/ReferentialSkeleton.cpp
index daf6a25665e9e..1de7b7eff6fd7 100644
--- a/dart/dynamics/ReferentialSkeleton.cpp
+++ b/dart/dynamics/ReferentialSkeleton.cpp
@@ -785,6 +785,17 @@ double ReferentialSkeleton::getPotentialEnergy() const
   return PE;
 }
 
+//==============================================================================
+void ReferentialSkeleton::clearCollidingBodies()
+{
+  for(size_t i = 0; i < this->getNumBodyNodes(); i++)
+  {
+    auto bd = this->getBodyNode(i);
+    if(bd)
+      bd->setColliding(false);
+  }
+}
+
 //==============================================================================
 Eigen::Vector3d ReferentialSkeleton::getCOM(const Frame* _withRespectTo) const
 {
diff --git a/dart/dynamics/ReferentialSkeleton.h b/dart/dynamics/ReferentialSkeleton.h
index eda06c81ed04d..d7a44d24ed891 100644
--- a/dart/dynamics/ReferentialSkeleton.h
+++ b/dart/dynamics/ReferentialSkeleton.h
@@ -273,6 +273,9 @@ class ReferentialSkeleton : public MetaSkeleton
   // Documentation inherited
   double getPotentialEnergy() const override;
 
+  // Documentation inherited
+  void clearCollidingBodies() override;
+
   /// \}
 
   //----------------------------------------------------------------------------
diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp
index bf51486babab8..243ee8518d153 100644
--- a/dart/dynamics/Skeleton.cpp
+++ b/dart/dynamics/Skeleton.cpp
@@ -3613,6 +3613,17 @@ double Skeleton::getPotentialEnergy() const
   return PE;
 }
 
+//==============================================================================
+void Skeleton::clearCollidingBodies()
+{
+  for(size_t i = 0; i < this->getNumBodyNodes(); i++)
+  {
+    auto bd = this->getBodyNode(i);
+    if(bd)
+      bd->setColliding(false);
+  }
+}
+
 //==============================================================================
 Eigen::Vector3d Skeleton::getCOM(const Frame* _withRespectTo) const
 {
diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h
index 569a1fb1b259e..2bf283e515019 100644
--- a/dart/dynamics/Skeleton.h
+++ b/dart/dynamics/Skeleton.h
@@ -878,6 +878,9 @@ class Skeleton :
   // Documentation inherited
   double getPotentialEnergy() const override;
 
+  // Documentation inherited
+  void clearCollidingBodies() override;
+
   /// \}
 
   //----------------------------------------------------------------------------