BalanceConstraint: Removed constructor delegation

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

VirtualRobot/IK/constraints/BalanceConstraint.cpp

@@ -17,15 +17,30 @@ using namespace VirtualRobot;
 
 BalanceConstraint::BalanceConstraint(const RobotPtr &robot, const RobotNodeSetPtr &joints, const RobotNodeSetPtr &bodies, const SceneObjectSetPtr &contactNodes,
                                      float tolerance, float minimumStability, float maxSupportDistance) :
-    Constraint(joints),
-    joints(joints),
-    bodies(bodies),
-    minimumStability(minimumStability)
+    Constraint(joints)
 {
+    initialize(robot, joints, bodies, contactNodes, tolerance, minimumStability, maxSupportDistance);
+}
+
+BalanceConstraint::BalanceConstraint(const RobotPtr &robot, const RobotNodeSetPtr &joints, const RobotNodeSetPtr &bodies, const SupportPolygonPtr &supportPolygon,
+                                     float tolerance, float minimumStability, float maxSupportDistance) :
+    Constraint(joints)
+{
+    initialize(robot, joints, bodies, supportPolygon->getContactModels(), tolerance, minimumStability, maxSupportDistance);
+}
+
+void BalanceConstraint::initialize(const RobotPtr &robot, const RobotNodeSetPtr &joints, const RobotNodeSetPtr &bodies, const SceneObjectSetPtr &contactNodes, float tolerance, float minimumStability, float maxSupportDistance)
+{
+    this->joints = joints;
+    this->bodies = bodies;
+    this->minimumStability = minimumStability;
+
     supportPolygon.reset(new SupportPolygon(contactNodes));
     supportPolygon->updateSupportPolygon(maxSupportDistance);
 
     MathTools::ConvexHull2DPtr convexHull = supportPolygon->getSupportPolygon2D();
+    THROW_VR_EXCEPTION_IF(!convexHull, "Empty support polygon for balance constraint");
+
     Eigen::Vector2f supportPolygonCenter = MathTools::getConvexHullCenter(convexHull);
 
     comIK.reset(new CoMIK(joints, bodies));
@@ -34,12 +49,6 @@ BalanceConstraint::BalanceConstraint(const RobotPtr &robot, const RobotNodeSetPt
     initialized = true;
 }
 
-BalanceConstraint::BalanceConstraint(const RobotPtr &robot, const RobotNodeSetPtr &joints, const RobotNodeSetPtr &bodies, const SupportPolygonPtr &supportPolygon,
-                                     float tolerance, float minimumStability, float maxSupportDistance) :
-    BalanceConstraint(robot, joints, bodies, supportPolygon->getContactModels(), tolerance, minimumStability, maxSupportDistance)
-{
-}
-
 Eigen::MatrixXf BalanceConstraint::getJacobianMatrix()
 {
     return comIK->getJacobianMatrix();
@@ -156,3 +165,4 @@ std::string BalanceConstraint::getConstraintType()
     return "Balance(" + joints->getName() + ")";
 }
 
+

VirtualRobot/IK/constraints/BalanceConstraint.h

@@ -55,6 +55,10 @@ namespace VirtualRobot
 
             std::string getConstraintType();
 
+        protected:
+            void initialize(const RobotPtr &robot, const RobotNodeSetPtr &joints, const RobotNodeSetPtr &bodies, const SceneObjectSetPtr &contactNodes,
+                            float tolerance, float minimumStability, float maxSupportDistance);
+
         protected:
             CoMIKPtr comIK;
             SupportPolygonPtr supportPolygon;