bool ConstraintChecker::HasSupportPolygon(const Robot& robot,const Stance& stance,const Vector3& gravity,int numFCEdges)
{
vector<ContactPoint> cps(NumContactPoints(stance));
int k=0;
for(Stance::const_iterator i=stance.begin();i!=stance.end();i++) {
const Hold& h=i->second;
for(size_t j=0;j<h.contacts.size();j++,k++)
cps[k]=h.contacts[j];
}
Vector3 com = robot.GetCOM();
vector<Vector3> f;
return TestCOMEquilibrium(cps,gravity,numFCEdges,com,f);
}
bool ConstraintChecker::HasSupportPolygon_Robust(const Robot& robot,const Stance& stance,const Vector3& gravity,Real robustnessFactor,int numFCEdges)
{
vector<ContactPoint> cps(NumContactPoints(stance));
int k=0;
for(Stance::const_iterator i=stance.begin();i!=stance.end();i++) {
const Hold& h=i->second;
for(size_t j=0;j<h.contacts.size();j++,k++)
cps[k]=h.contacts[j];
}
Vector3 com = robot.GetCOM();
EquilibriumTester eq;
eq.Setup(cps,gravity,numFCEdges,com);
eq.SetRobustnessFactor(robustnessFactor);
return eq.TestCurrent();
}
