Merge pull request #586 from dartsim/constraint_missing_implementation

Implement missing implementation in ConstrainedGroup
dartsim · Jan 16, 2016 · c3bfb60 · c3bfb60
2 parents 0c91dca + 8c994dc
commit c3bfb60
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 34 deletions.
diff --git a/dart/constraint/ConstrainedGroup.cpp b/dart/constraint/ConstrainedGroup.cpp
@@ -36,6 +36,7 @@
 
 #include "dart/constraint/ConstrainedGroup.h"
 
+#include <algorithm>
 #include <iostream>
 #include <vector>
 
@@ -59,9 +60,9 @@ ConstrainedGroup::~ConstrainedGroup()
 //==============================================================================
 void ConstrainedGroup::addConstraint(const ConstraintBasePtr& _constraint)
 {
-  assert(_constraint != nullptr && "Null constraint pointer is now allowed.");
-  assert(containConstraint(_constraint) == false
-         && "Don't try to add same constraint multiple times into Community.");
+  assert(_constraint != nullptr && "Attempted to add nullptr.");
+  assert(!containConstraint(_constraint)
+         && "Attempted to add a duplicate constraint.");
   assert(_constraint->isActive());
 
   mConstraints.push_back(_constraint);
@@ -83,9 +84,9 @@ ConstraintBasePtr ConstrainedGroup::getConstraint(size_t _index) const
 //==============================================================================
 void ConstrainedGroup::removeConstraint(const ConstraintBasePtr& _constraint)
 {
-  assert(_constraint != nullptr && "Null constraint pointer is now allowed.");
-  assert(containConstraint(_constraint) == true
-         && "Don't try to remove a constraint not contained in Community.");
+  assert(_constraint != nullptr && "Attempted to add nullptr.");
+  assert(containConstraint(_constraint)
+         && "Attempted to remove not existing constraint.");
 
   mConstraints.erase(
         remove(mConstraints.begin(), mConstraints.end(), _constraint),
@@ -95,39 +96,18 @@ void ConstrainedGroup::removeConstraint(const ConstraintBasePtr& _constraint)
 //==============================================================================
 void ConstrainedGroup::removeAllConstraints()
 {
-//  dtwarn << "ConstrainedGroup::removeAllConstraints(): "
-//         << "Not implemented." << std::endl;
-
-  // TODO(JS): Temporary implementation
-//  for (size_t i = 0; i < mConstraints.size(); ++i)
-//  {
-//    delete mConstraints[i];
-//  }
-
   mConstraints.clear();
 }
 
 //==============================================================================
+#ifndef NDEBUG
 bool ConstrainedGroup::containConstraint(
     const ConstConstraintBasePtr& _constraint) const
 {
-//  std::cout << "CommunityTEST::_containConstraint(): Not implemented."
-//            << std::endl;
-
-  // TODO(MXG): Is there any reason these functions are not implemented yet?
-
-  return false;
-}
-
-//==============================================================================
-bool ConstrainedGroup::checkAndAddConstraint(
-    const ConstraintBasePtr& _constraint)
-{
-  std::cout << "CommunityTEST::_checkAndAddConstraint(): Not implemented."
-            << std::endl;
-
-  return false;
+  return std::find(mConstraints.begin(), mConstraints.end(), _constraint)
+      != mConstraints.end();
 }
+#endif
 
 //==============================================================================
 size_t ConstrainedGroup::getTotalDimension() const

diff --git a/dart/constraint/ConstrainedGroup.h b/dart/constraint/ConstrainedGroup.h
@@ -100,11 +100,10 @@ class ConstrainedGroup
   friend class ConstraintSolver;
 
 private:
+#ifndef NDEBUG
   /// Return true if _constraint is contained
   bool containConstraint(const ConstConstraintBasePtr& _constraint) const;
-
-  /// Return true and add the constraint if _constraint is contained
-  bool checkAndAddConstraint(const ConstraintBasePtr& _constraint);
+#endif
 
   /// List of constraints
   std::vector<ConstraintBasePtr> mConstraints;