VirtualRobot::MathTools::ConvexHull2DPtr SupportPolygon::updateSupportPolygon(float maxFloorDist)
{
CollisionCheckerPtr colChecker = colModels[0]->getCollisionChecker();
std::vector< CollisionModelPtr >::iterator i = colModels.begin();
std::vector< MathTools::ContactPoint > points;
while (i != colModels.end())
{
colChecker->getContacts(floor, *i, points, maxFloorDist);
i++;
}
currentContactPoints2D.clear();
for (size_t u = 0; u < points.size(); u++)
{
Eigen::Vector2f pt2d = MathTools::projectPointToPlane2D(points[u].p, floor);
currentContactPoints2D.push_back(pt2d);
}
if (currentContactPoints2D.size() < 3)
{
suportPolygonFloor.reset();
}
else
{
suportPolygonFloor = MathTools::createConvexHull2D(currentContactPoints2D);
}
return suportPolygonFloor;
}
void stabilityWindow::updateSupportVisu()
{
supportVisu->removeAllChildren();
if (UI.checkBoxSupportPolygon->isChecked())
{
/*SoMaterial *material = new SoMaterial;
material->diffuseColor.setValue(1.0f,0.2f,0.2f);
supportVisu->addChild(material);*/
MathTools::Plane p = MathTools::getFloorPlane();
//supportVisu->addChild(CoinVisualizationFactory::CreatePlaneVisualization(p.p,p.n,100000.0f,0.5f));
std::vector< CollisionModelPtr > colModels = robot->getCollisionModels();
CollisionCheckerPtr colChecker = CollisionChecker::getGlobalCollisionChecker();
std::vector< CollisionModelPtr >::iterator i = colModels.begin();
std::vector< MathTools::ContactPoint > points;
while (i != colModels.end())
{
colChecker->getContacts(p, *i, points, 5.0f);
i++;
}
std::vector< Eigen::Vector2f > points2D;
//MathTools::Plane plane2(Eigen::Vector3f(0,0,0),Eigen::Vector3f(0,1.0f,0));
for (size_t u = 0; u < points.size(); u++)
{
Eigen::Vector2f pt2d = MathTools::projectPointToPlane2D(points[u].p, p);
points2D.push_back(pt2d);
}
MathTools::ConvexHull2DPtr cv = MathTools::createConvexHull2D(points2D);
SoSeparator* visu = CoinVisualizationFactory::CreateConvexHull2DVisualization(cv, p, VisualizationFactory::Color::Blue(), VisualizationFactory::Color::Black(), 6.0f, Eigen::Vector3f(0, 0, 2.0f));
supportVisu->addChild(visu);
}
}