bool ConstraintChecker::HasEnvCollision(Robot& robot,Terrain& env,const Stance& stance, const vector<int>& ignoreList)
{
robot.UpdateGeometry();
robot.InitMeshCollision(*env.geometry);
vector<bool> fixed(robot.links.size(), false);
for (Stance::const_iterator i = stance.begin(); i != stance.end(); i++)
fixed[i->first] = true;
for (int i = 0; i < ignoreList.size(); i++)
fixed[ignoreList[i]] = true;
for (size_t i = 0; i < robot.links.size(); i++) {
if (!fixed[i]) {
if (robot.MeshCollision(i)) {
return true;
}
}
}
return false;
}
bool ConstraintChecker::HasEnvCollision(Robot& robot,Terrain& env,const vector<IKGoal>& constraints, const vector<int>& ignoreList)
{
robot.UpdateGeometry();
robot.InitMeshCollision(*env.geometry);
vector<bool> fixed(robot.links.size(), false);
for (size_t i=0;i<constraints.size();i++) {
if(constraints[i].destLink == -1)
fixed[constraints[i].link] = true;
}
for (int i = 0; i < ignoreList.size(); i++)
fixed[ignoreList[i]] = true;
for (size_t i = 0; i < robot.links.size(); i++) {
if (!fixed[i]) {
if (robot.MeshCollision(i)) {
printf("Collision between robot link %s and env\n",robot.linkNames[i].c_str());
return true;
}
}
}
return false;
}
bool ConstraintChecker::HasEnvCollision(Robot& robot,Terrain& env,const vector<IKGoal>& constraints)
{
robot.UpdateGeometry();
robot.InitMeshCollision(*env.geometry);
vector<bool> fixed(robot.links.size(), false);
for ( int i = 0;i < constraints.size(); i++)
{
int linkIndex = constraints[i].link;
fixed[ linkIndex] = true;
}
for (size_t i = 0; i < robot.links.size(); i++) {
if (!fixed[i]) {
cout << "Checking link " << i << ":" << endl;
if (robot.MeshCollision(i)) {
cout << " Link " << i << " is in collision!" << endl;
return true;
}
else{
cout << " Link " << i << " is not in collision!" << endl;
}
}
}
return false;
}
bool ConstraintChecker::HasEnvCollision(Robot& robot,Terrain& env)
{
robot.UpdateGeometry();
return robot.MeshCollision(*env.geometry);
}
