ConstrainedIK: Added constraint removal functionality

git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@838 042f3d55-54a8-47e9-b7fb-15903f145c44

VirtualRobot/IK/ConstrainedIK.cpp

@@ -21,6 +21,18 @@ void ConstrainedIK::addConstraint(const ConstraintPtr &constraint, int priority,
     std::sort(constraints.begin(), constraints.end(), [this](const ConstraintPtr& lhs, const ConstraintPtr& rhs){return priorities[lhs] > priorities[rhs];});
 }
 
+void ConstrainedIK::removeConstraint(const ConstraintPtr &constraint)
+{
+    auto position = std::find(constraints.begin(), constraints.end(), constraint);
+
+    if(position != constraints.end())
+    {
+        constraints.erase(position);
+        priorities.erase(constraint);
+        hardConstraints.erase(constraint);
+    }
+}
+
 std::vector<ConstraintPtr> ConstrainedIK::getConstraints()
 {
     return constraints;

VirtualRobot/IK/ConstrainedIK.h

@@ -38,6 +38,7 @@ namespace VirtualRobot
             ConstrainedIK(RobotPtr &robot, int maxIterations = 1000, float stall_epsilon = 0.0001, float raise_epsilon = 0.8);
 
             void addConstraint(const ConstraintPtr &constraint, int priority=0, bool hard_constraint=true);
+            void removeConstraint(const ConstraintPtr &constraint);
             std::vector<ConstraintPtr> getConstraints();
 
             virtual bool initialize();