// Routine to create a scene graph representing a dodecahedron
SoSeparator *
makePyramide()
{
SoSeparator *result = new SoSeparator;
result->ref();
// Define coordinates for vertices
SoCoordinate3 *myCoords = new SoCoordinate3;
myCoords->point.setValues(0, 5, pyramidVertexes);
result->addChild(myCoords);
// Define the IndexedFaceSet, with indices into the vertices:
SoIndexedFaceSet *myFaceSet = new SoIndexedFaceSet;
myFaceSet->coordIndex.setValues (0, 21, (const int32_t*)pyramidFaces);
result->addChild (myFaceSet);
result->unrefNoDelete();
return result;
}
SoSeparator *
createCameraObject (const float zoomFactor = 1.0)
{
vpDEBUG_TRACE (15, "# Entree.");
SoSeparator * cam = new SoSeparator;
cam->ref ();
SoMaterial *myMaterial = new SoMaterial;
myMaterial->diffuseColor.setValue(1.0, 0.0, 0.0);
myMaterial->emissiveColor.setValue(0.5, 0.0, 0.0);
SoScale *taille = new SoScale;
{
float zoom = 0.1f * zoomFactor;
taille->scaleFactor.setValue (zoom, zoom, zoom);
}
SoMaterial *couleurBlanc = new SoMaterial;
couleurBlanc->diffuseColor.setValue(1.0, 1.0, 1.0);
couleurBlanc->emissiveColor.setValue(1.0, 1.0, 1.0);
SoDrawStyle * filDeFer = new SoDrawStyle;
filDeFer->style.setValue (SoDrawStyle::LINES);
filDeFer->lineWidth.setValue (1);
SoSeparator * cone = new SoSeparator;
cone->ref();
cone->addChild (makePyramide());
cone->addChild (couleurBlanc);
cone->addChild (filDeFer);
cone->addChild (makePyramide());
cone->unrefNoDelete();
cam->addChild(myMaterial);
cam->addChild(taille);
cam->addChild(cone);
cam->addChild(createFrame(2.0f,0.1f,0.01f));
// cam->unref() ;
vpDEBUG_TRACE (15, "# Sortie.");
return cam;
}
VirtualRobot::VisualizationNodePtr CoinVisualizationNode::clone(bool deepCopy, float scaling)
{
THROW_VR_EXCEPTION_IF(scaling<=0,"Scaling must be >0");
SoSeparator* newModel = NULL;
if (visualization)
{
newModel = new SoSeparator;
newModel->ref();
if (scaling!=1.0)
{
SoScale *s = new SoScale;
s->scaleFactor.setValue(scaling,scaling,scaling);
newModel->addChild(s);
}
if (deepCopy)
{
newModel->addChild(visualization->copy(FALSE));
} else
newModel->addChild(visualization);
}
VisualizationNodePtr p(new CoinVisualizationNode(newModel));
if (newModel)
newModel->unrefNoDelete();
p->setUpdateVisualization(updateVisualization);
p->setGlobalPose(getGlobalPose());
p->setFilename(filename,boundingBox);
// clone attached visualizations
std::map< std::string, VisualizationNodePtr >::const_iterator i = attachedVisualizations.begin();
while (i!=attachedVisualizations.end())
{
VisualizationNodePtr attachedClone = i->second->clone(deepCopy,scaling);
p->attachVisualization(i->first, attachedClone);
i++;
}
return p;
}
int QilexDoc::doc_insert_kinematic_chain(Rchain *kineengine, SoSeparator *kinechain)
{
int error = 0;
int i;
SbVec3f joinax;
float joinangle;
SbName joints[] = {"joint1", "joint2", "joint3", "joint4","joint5", "joint6",
"joint7", "joint8", "joint9", "joint10","joint11", "joint12",
"joint13", "joint14", "joint15", "joint16","joint17", "joint18",
"joint19", "joint20", "joint21", "joint22","joint23", "joint24", };
SoEngineList compR(kineengine->dof);
SoNodeList Rots(kineengine->dof);
SoSearchAction lookingforjoints;
SoTransform *pjoint = new SoTransform;
// Identifie the rotations and assing the job
i = 0;
while (i < kineengine->dof && error == 0)
{
lookingforjoints.setName(joints[i]);
lookingforjoints.setType(SoTransform::getClassTypeId());
lookingforjoints.setInterest(SoSearchAction::FIRST);
lookingforjoints.apply(kinechain);
// assert(lookingforjoints.getPath() != NULL);
SoNode * pnode = lookingforjoints.getPath()->getTail();;
pjoint = (SoTransform *) pnode;
if(NULL != pjoint)
{
Rots.append((SoTransform *) pjoint);
compR.append(new SoQtComposeRotation);
// cal comprobar si l'articulació es de rotació o translació: arreglar...
((SoTransform *) Rots[i])->rotation.getValue(joinax, joinangle);
((SoQtComposeRotation *) compR[i])->axis.setValue(joinax);
((SoTransform *) Rots[i])->rotation.connectFrom(&((SoQtComposeRotation *) compR[i])->rotation);
}
else
{
error = 5; // not a valid Model3d file
}
i++ ;
}
if (error == 0)
{
SoSeparator *axisworld = new SoSeparator;
axisworld->unrefNoDelete();
SoCoordinateAxis *AxisW = new SoCoordinateAxis();
AxisW->fNDivision = 1;
AxisW->fDivisionLength = 200;
axisworld->addChild(AxisW);
//kinechain->insertChild(AxisW,1);
view->addNoColObject(axisworld);
/* lookingforjoints.setName("tool");
lookingforjoints.setType(SoSeparator::getClassTypeId());
lookingforjoints.setInterest(SoSearchAction::FIRST);
lookingforjoints.apply(kinechain);
if(lookingforjoints.getPath() != NULL)
{
SoNode * pnode = lookingforjoints.getPath()->getTail();;
SoCoordinateAxis *AxisT = new SoCoordinateAxis();
AxisT->fNDivision = 1;
AxisT->fDivisionLength = 200;
SoSeparator *axistool = new SoSeparator;
axistool->ref();
axistool = (SoSeparator *) pnode;
axistool->addChild(AxisT);
view->addNoColObject(axistool);
}
*/
panel_control *panel = new panel_control(0, "Panel", kineengine);
for (int i = 0; i < kineengine->dof; i++)
{
// connect(panel->ldial[i], SIGNAL(valueChange(double)),((SoQtComposeRotation *) compR[i]), SLOT(setValue_angle(double)));
connect(panel->ldial[i], SIGNAL(valueChange(double)),&panel->kinechain->list_plug[i], SLOT(setValue(double)));
connect(&panel->kinechain->list_plug[i], SIGNAL(valueChanged(double)),((SoQtComposeRotation *) compR[i]), SLOT(setValue_angle(double)));
connect(&panel->kinechain->list_plug[i], SIGNAL(valueChanged(double)),panel->ldial[i], SLOT(setValue(double)));
}
view->addRobotCell(kinechain);
panel->show();
panel->update_limits();
//panel->kinechain->setconsole_mode(true); Ja estava comentada
panel->kinechain->setconsole_mode(false);
panel->kinechain->do_ready();
connect(view, SIGNAL(pick_point(Rhmatrix)),panel, SLOT(move_pickpoint(Rhmatrix )));
//panel->kinechain->setconsole_mode(false); Ja estava comentada
}
SoSeparator* CoinConvexHullVisualization::createConvexHullVisualization(VirtualRobot::MathTools::ConvexHull6DPtr& convHull, bool buseFirst3Coords)
{
SoSeparator* result = new SoSeparator;
if (!convHull || convHull->vertices.size() == 0 || convHull->faces.size() == 0)
{
return result;
}
result->ref();
SoUnits* u = new SoUnits;
u->units = SoUnits::MILLIMETERS;
result->addChild(u);
SoScale* sc = new SoScale();
float fc = 400.0f;
sc->scaleFactor.setValue(fc, fc, fc);
result->addChild(sc);
int nFaces = (int)convHull->faces.size();
//Eigen::Vector3f v[6];
// project points to 3d, then create hull of these points to visualize it
std::vector<Eigen::Vector3f> vProjectedPoints;
for (int i = 0; i < nFaces; i++)
{
for (int j = 0; j < 6; j++)
{
if (buseFirst3Coords)
{
//v[j] = convHull->vertices.at(convHull->faces[i].id[j]).p;
vProjectedPoints.push_back(convHull->vertices[ convHull->faces[i].id[j] ].p);
}
else
{
//v[j] = convHull->vertices.at(convHull->faces[i].id[j]).n;
vProjectedPoints.push_back(convHull->vertices[ convHull->faces[i].id[j] ].n);
}
}
}
VirtualRobot::MathTools::ConvexHull3DPtr projectedHull = ConvexHullGenerator::CreateConvexHull(vProjectedPoints);
if (!projectedHull)
{
GRASPSTUDIO_ERROR << " Could not create hull of projected points, aborting..." << endl;
return result;
}
SoSeparator* hullV = createConvexHullVisualization(projectedHull);
if (!hullV)
{
GRASPSTUDIO_ERROR << " Could not create visualization of projected points, aborting..." << endl;
return result;
}
result->addChild(hullV);
result->unrefNoDelete();
return result;
}
