//Write void urdfLink::createLink(bool hideCollisionLinks,bool convexDecomposeNonConvexCollidables,bool createVisualIfNone,bool& showConvexDecompositionDlg) { std::string txt("Creating link '"+name+"'..."); printToConsole(txt.c_str()); //visuals.clear(); // Visuals for (int i=0; i<visuals.size(); i++) { urdfElement &visual = visuals[i]; if(!visual.meshFilename.empty()) { std::string fname(visual.meshFilename); bool exists=true; bool useAlt=false; if (!simDoesFileExist(fname.c_str())) { fname=visual.meshFilename_alt; exists=simDoesFileExist(fname.c_str()); useAlt=true; } if (!exists) printToConsole("ERROR: the mesh file could not be found."); else visual.n = simImportShape(visual.meshExtension,fname.c_str(),0,0.0001f,1.0); if (!visual.n) { if (!useAlt) txt="ERROR: failed to create the mesh '"+visual.meshFilename+"' with extension type "+boost::lexical_cast<std::string>(visual.meshExtension); else txt="ERROR: failed to create the mesh '"+visual.meshFilename+"' or '"+visual.meshFilename_alt+"' with extension type "+boost::lexical_cast<std::string>(visual.meshExtension); printToConsole(txt.c_str()); } else visual.n = scaleShapeIfRequired(visual.n,visual.mesh_scaling); } else if (!isArrayEmpty(visual.sphere_size)) visual.n = simCreatePureShape( 1,1+2+16, visual.sphere_size, mass, NULL); else if (!isArrayEmpty(visual.cylinder_size)) visual.n = simCreatePureShape( 2,1+2+16, visual.cylinder_size, mass, NULL); else if (!isArrayEmpty(visual.box_size)) visual.n = simCreatePureShape( 0,1+2+16, visual.box_size, mass, NULL); } //collisions.clear(); //mass=0.1; //collision for (int i=0; i<collisions.size(); i++) { urdfElement &collision = collisions[i]; if(!collision.meshFilename.empty()) { std::string fname(collision.meshFilename); bool exists=true; bool useAlt=false; if (!simDoesFileExist(fname.c_str())) { fname=collision.meshFilename_alt; exists=simDoesFileExist(fname.c_str()); useAlt=true; } if (!exists) printToConsole("ERROR: the mesh file could not be found"); else collision.n = simImportShape(collision.meshExtension,fname.c_str(),0,0.0001f,1.0); if (collision.n == -1) { if (!useAlt) txt="ERROR: failed to create the mesh '"+collision.meshFilename+"' with extension type "+boost::lexical_cast<std::string>(collision.meshExtension); else txt="ERROR: failed to create the mesh '"+collision.meshFilename+"' or '"+collision.meshFilename_alt+"' with extension type "+boost::lexical_cast<std::string>(collision.meshExtension); printToConsole(txt.c_str()); } else { collision.n=scaleShapeIfRequired(collision.n,collision.mesh_scaling); if (createVisualIfNone&&(visuals.size()==0)) { // We create a visual from the collision shape (before it gets morphed hereafter): simRemoveObjectFromSelection(sim_handle_all,-1); simAddObjectToSelection(sim_handle_single,collision.n); simCopyPasteSelectedObjects(); addVisual(); currentVisual().n = simGetObjectLastSelection(); } int p; int convInts[5]={1,500,200,0,0}; // 3rd value from 100 to 500 on 5/2/2014 float convFloats[5]={100.0f,30.0f,0.25f,0.0f,0.0f}; if ( convexDecomposeNonConvexCollidables&&(simGetObjectIntParameter(collision.n,3017,&p)>0)&&(p==0) ) { int aux=1+4+8+16+64; if (showConvexDecompositionDlg) aux=1+2+8+16+64; showConvexDecompositionDlg=false; simConvexDecompose(collision.n,aux,convInts,convFloats); // we generate convex shapes! } simSetObjectIntParameter(collision.n,3003,!inertiaPresent); // we make it non-static if there is an inertia simSetObjectIntParameter(collision.n,3004,1); // we make it respondable since it is a collision object } } else if (!isArrayEmpty(collision.sphere_size)) collision.n = simCreatePureShape( 1,1+2+4+8+16*(!inertiaPresent), collision.sphere_size, mass, NULL); else if (!isArrayEmpty(collision.cylinder_size)) collision.n = simCreatePureShape( 2,1+2+4+8+16*(!inertiaPresent), collision.cylinder_size, mass, NULL); else if (!isArrayEmpty(collision.box_size)) collision.n = simCreatePureShape( 0,1+2+4+8+16*(!inertiaPresent), collision.box_size, mass, NULL); } // Hack to draw COM in the collision layer /* addCollision(); currentCollision().xyz[0] = inertial_xyz[0]; currentCollision().xyz[1] = inertial_xyz[1]; currentCollision().xyz[0] = inertial_xyz[2]; currentCollision().rpy[0] = 1.5; float dummySize[3]={0.01f,0.01f,0.01f}; currentCollision().n = simCreatePureShape( 1,1+2+16, dummySize, mass, NULL); */ // Grouping collisions shapes nLinkCollision = groupShapes(collisions); // Inertia if (inertiaPresent) { C3Vector euler; if (nLinkCollision==-1) { // we do not have a collision object. Let's create a dummy collision object, since inertias can't exist on their own in V-REP: float dummySize[3]={0.01f,0.01f,0.01f}; //nLinkCollision = simCreatePureShape( 1,1+2+4, dummySize, mass, NULL); // we make it non-respondable! nLinkCollision = simCreatePureShape( 1,1+2+16, dummySize, mass, NULL); } C7Vector inertiaFrame; inertiaFrame.X.set(inertial_xyz); inertiaFrame.Q=getQuaternionFromRpy(inertial_rpy); //simSetObjectPosition(nLinkCollision,-1,inertiaFrame.X.data); //C7Vector collisionFrame; //collisionFrame.X.set(collision_xyz); //collisionFrame.Q=getQuaternionFromRpy(collision_rpy); C7Vector collisionFrame; simGetObjectPosition(nLinkCollision,-1,collisionFrame.X.data); simGetObjectOrientation(nLinkCollision,-1,euler.data); collisionFrame.Q.setEulerAngles(euler); //C4X4Matrix x((collisionFrame.getInverse()*inertiaFrame).getMatrix()); C4X4Matrix x(inertiaFrame.getMatrix()); float i[12]={x.M(0,0),x.M(0,1),x.M(0,2),x.X(0),x.M(1,0),x.M(1,1),x.M(1,2),x.X(1),x.M(2,0),x.M(2,1),x.M(2,2),x.X(2)}; simSetShapeMassAndInertia(nLinkCollision,mass,inertia,C3Vector::zeroVector.data,i); //std::cout << "Mass: " << mass << std::endl; } else { if (nLinkCollision!=-1) { std::string txt("ERROR: found a collision object without inertia data for link '"+ name+"'. Is that link meant to be static?"); printToConsole(txt.c_str()); } } if (createVisualIfNone&&(visuals.size()==0)&&(nLinkCollision!=-1)) { // We create a visual from the collision shape (meshes were handled earlier): addVisual(); urdfElement &visual = currentVisual(); simRemoveObjectFromSelection(sim_handle_all,-1); simAddObjectToSelection(sim_handle_single,nLinkCollision); simCopyPasteSelectedObjects(); visual.n=simGetObjectLastSelection(); simSetObjectIntParameter(visual.n,3003,1); // we make it static since only visual simSetObjectIntParameter(visual.n,3004,0); // we make it non-respondable since only visual } // Set the respondable mask: if (nLinkCollision!=-1) simSetObjectIntParameter(nLinkCollision,3019,0xff00); // colliding with everything except with other objects in that tree hierarchy // Grouping shapes nLinkVisual = groupShapes(visuals); // Set the names, visibility, etc.: if (nLinkVisual!=-1) { setVrepObjectName(nLinkVisual,std::string(name+"_visual").c_str()); const float specularDiffuse[3]={0.3f,0.3f,0.3f}; if (nLinkCollision!=-1) { // if we have a collision object, we attach the visual object to it, then forget the visual object C7Vector collisionFrame; C3Vector euler; simGetObjectPosition(nLinkCollision,-1,collisionFrame.X.data); simGetObjectOrientation(nLinkCollision,-1,euler.data); collisionFrame.Q.setEulerAngles(euler); C7Vector visualFrame; simGetObjectPosition(nLinkVisual,-1,visualFrame.X.data); simGetObjectOrientation(nLinkVisual,-1,euler.data); visualFrame.Q.setEulerAngles(euler); C7Vector x(collisionFrame.getInverse()*visualFrame); simSetObjectPosition(nLinkVisual,-1,x.X.data); simSetObjectOrientation(nLinkVisual,-1,x.Q.getEulerAngles().data); simSetObjectParent(nLinkVisual,nLinkCollision,0); } } if (nLinkCollision!=-1) { setVrepObjectName(nLinkCollision,std::string(name+"_respondable").c_str()); if (hideCollisionLinks) simSetObjectIntParameter(nLinkCollision,10,256); // we "hide" that object in layer 9 } }
void edTerrainNode::updateVisual() { osg::ref_ptr<osgUtil::SmoothingVisitor> sv= new osgUtil::SmoothingVisitor(); // double cellDim= 1000.0; CellCoords coords; osg::Vec3d center; Terrain terrain; if (selectedVisuals.empty()) { double maxX= -1000000.0; double minX= 1000000.0; double maxY= -1000000.0; double minY= 1000000.0; for (unsigned int i=0; i<triangles.size(); i++) { center= triangles[i].getCenter(); if (center.x()<minX) minX= center.x(); if (center.x()>maxX) maxX= center.x(); if (center.y()<minY) minY= center.y(); if (center.y()>maxY) maxY= center.y(); coords.first= Editor::lastInstance()->getCellX(center.x());//Editor::cellDim; coords.second= Editor::lastInstance()->getCellY(center.y());//Editor::cellDim; // coords.second= center.y()/Editor::cellDim; terrain[coords].push_back(&triangles[i]); } pos.set(minX*0.5+maxX*0.5,minY*0.5+maxY*0.5,0); osg::Geometry *geom; osg::Texture2D *tex= NULL; osg::Material *mater= NULL; osg::StateSet *dstate= NULL; osg::Geode *geode= NULL; osg::TexEnvCombine *envComb= NULL; osg::Vec3Array *v; osg::Vec3Array *norm; osg::Vec2Array *t; osg::Vec2Array *t1; osg::Vec2Array *t2; osg::Vec4Array *col; osg::Matrixd mat; osg::Vec3d origin,vert,texc; TriList::iterator tri_iter; Terrain::iterator it; // char buf[256]; setNumVisuals(terrain.size()); for (it=terrain.begin(); it!=terrain.end(); ++it) { osg::MatrixTransform *trans= Editor::lastInstance()->getOrCreateCell(it->first.first,it->first.second); // trans->setNodeMask(Editor::nm_SolidTerrain); origin= trans->getMatrix().getTrans(); // origin.set(it->first.first*Editor::cellDim,it->first.second*Editor::cellDim,0); // nameFromCoords5(buf,it->first.first,it->first.second); // for (unsigned int i=0; i<terrainRoot->getNumChildren(); i++) // if (terrainRoot->getChild(i)->getName().compare(buf)==0) // { // trans= dynamic_cast<osg::MatrixTransform*>(terrainRoot->getChild(i)); // break; // } // if (trans==NULL) // { // mat.makeTranslate(origin); // trans= new osg::MatrixTransform(mat); // trans->setName(buf); // terrainRoot->addChild(trans); // } unsigned int n= it->second.size(); if (n>0) { if (!material.valid()) { // DoubleMaterialTris singleMatTris; DoubleMaterialTris dblMatTris; TrippleMaterialTris trplMatTris; TerrainMaterial* mats[]= { NULL,NULL,NULL }; MaterialPair matPair; for (tri_iter= it->second.begin(); tri_iter!=it->second.end(); ++tri_iter) { for (unsigned int i=0; i<3; i++) { // if ((*tri_iter)->v[i]->getTerrainOwnersList().size()>0) mats[i]= (*tri_iter)->getValidMaterial(i);//(*tri_iter)->v[i]->getTerrainOwnersList().front()->material.get(); // else // mats[i]= NULL; } if (mats[0]==NULL || mats[1]==NULL || mats[2]==NULL) continue; if (mats[0]!=mats[1] && mats[0]!=mats[2] && mats[1]!=mats[2]) { trplMatTris.push_back(*tri_iter); continue; } matPair.first= mats[0]; if (mats[1]!=matPair.first) { matPair.second= mats[1]; } else matPair.second= mats[2]; if (matPair.first>matPair.second) std::swap(matPair.second,matPair.first); // if (matPair.first!=matPair.second) dblMatTris[matPair].push_back(*tri_iter); } DoubleMaterialTris::iterator dmt_iter; for (dmt_iter=dblMatTris.begin() ;dmt_iter!=dblMatTris.end(); ++dmt_iter) { unsigned int n= dmt_iter->second.size(); if (n>0) { v = new osg::Vec3Array; norm = new osg::Vec3Array; t1 = new osg::Vec2Array; t2 = new osg::Vec2Array; col = new osg::Vec4Array; v->reserve(n*3); norm->reserve(n*3); t1->reserve(n*3); t2->reserve(n*3); col->reserve(n*3); for (tri_iter= dmt_iter->second.begin(); tri_iter!=dmt_iter->second.end(); ++tri_iter) { for (unsigned int i=0; i<3; i++) { vert= (*tri_iter)->v[i]->getPosition()-origin; v->push_back(vert); norm->push_back((*tri_iter)->getNormal()); t1->push_back(dmt_iter->first.first->getTexCoord(vert)); t2->push_back(dmt_iter->first.second->getTexCoord(vert)); if ((*tri_iter)->hasMaterial(i,dmt_iter->first.first)) col->push_back(osg::Vec4(1,1,1,0)); else col->push_back(osg::Vec4(1,1,1,1)); /* edPoint::OwnersList::iterator ow_it; edPoint::OwnersList &ol((*tri_iter)->v[i]->getTerrainOwnersList()); for (ow_it=ol.begin(); ow_it!=ol.end(); ++ow_it) if ((*ow_it)->material.get()==dmt_iter->first.first) { col->push_back(osg::Vec4(1,1,1,0)); break; } if (ow_it==ol.end()) col->push_back(osg::Vec4(1,1,1,1));*/ } } geom = new osg::Geometry; geom->setVertexArray( v ); geom->setNormalArray( norm ); geom->setNormalBinding( osg::Geometry::BIND_PER_VERTEX ); fixupTexCoords(*t1); geom->setTexCoordArray( 0, t1 ); fixupTexCoords(*t2); geom->setTexCoordArray( 1, t2 ); geom->setColorArray( col ); geom->setColorBinding( osg::Geometry::BIND_PER_VERTEX ); geom->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::TRIANGLES,0,n*3)); // geom->setStateSet( material->dstate.get() ); geode = new osg::Geode; geode->addDrawable( geom ); geode->setUserData(this); geode->setNodeMask(getNodeMask()); // geode->setStateSet( material->dstate.get() ); dstate= new osg::StateSet; tex = new osg::Texture2D; tex->setWrap(osg::Texture::WRAP_S, osg::Texture::REPEAT); tex->setWrap(osg::Texture::WRAP_T, osg::Texture::REPEAT); tex->setImage(osgDB::readImageFile(dmt_iter->first.first->tex)); dstate->setTextureAttributeAndModes(0, tex, osg::StateAttribute::ON ); tex = new osg::Texture2D; tex->setWrap(osg::Texture::WRAP_S, osg::Texture::REPEAT); tex->setWrap(osg::Texture::WRAP_T, osg::Texture::REPEAT); printf("tex0: %s ; tex1: %s\n",dmt_iter->first.first->tex.c_str(),dmt_iter->first.second->tex.c_str()); tex->setImage(osgDB::readImageFile(dmt_iter->first.second->tex)); dstate->setTextureAttributeAndModes(1, tex, osg::StateAttribute::ON ); envComb= new osg::TexEnvCombine; envComb->setSource0_RGB(osg::TexEnvCombine::TEXTURE1); envComb->setSource1_RGB(osg::TexEnvCombine::TEXTURE0); envComb->setSource2_RGB(osg::TexEnvCombine::PRIMARY_COLOR); envComb->setCombine_RGB(osg::TexEnvCombine::INTERPOLATE); dstate->setTextureAttribute(0, envComb); envComb= new osg::TexEnvCombine; envComb->setSource0_RGB(osg::TexEnvCombine::PREVIOUS); envComb->setSource1_RGB(osg::TexEnvCombine::PRIMARY_COLOR); envComb->setCombine_RGB(osg::TexEnvCombine::MODULATE); dstate->setTextureAttribute(1, envComb); dstate->setMode( GL_ALPHA_TEST, osg::StateAttribute::OFF ); geode->setStateSet( dstate ); sv->reset(); geode->accept(*sv.get()); // addVisual(geode); trans->addChild(geode); addVisual(geode); // printf("geode->referenceCount(): %d\n",geode->referenceCount()); geode = new osg::Geode; geode->addDrawable( geom ); geode->setUserData(this); geode->setNodeMask(Editor::nm_SelectedGeometry); // dstate= new osg::StateSet(); // dstate->setRenderBinDetails(10,"RenderBin"); // dstate->setAttributeAndModes(new osg::Depth(osg::Depth::ALWAYS)); // osg::PolygonMode *pm= new osg::PolygonMode(); // pm->setMode(osg::PolygonMode::FRONT_AND_BACK, osg::PolygonMode::LINE); // dstate->setAttributeAndModes(pm); // dstate->setAttributeAndModes(new osg::LineWidth(3)); // dstate->setMode(GL_DEPTH_TEST,osg::StateAttribute::OFF); //dstate->setRenderBinDetails(20,"RenderBin"); // mater= new osg::Material(); // mater->setAmbient(osg::Material::Face::FRONT_AND_BACK, osg::Vec4(1.0,0.0,0.0,1.0)); // mater->setDiffuse(osg::Material::Face::FRONT_AND_BACK, osg::Vec4(1.0,0.0,0.0,1.0)); // mater->setEmission(osg::Material::Face::FRONT_AND_BACK, osg::Vec4(1.0,0.0,1.0,1.0)); // dstate->setAttributeAndModes(mater); // geode->setStateSet( dstate ); geode->setStateSet( Editor::instance()->selectedNodesState.get() ); trans->addChild(geode); addSelectedVisual(geode); } } if (dblMatTris.empty()) material= Editor::instance()->getDefaultMaterial(); // getTerrainOwnersList() } if (material.valid()) { v = new osg::Vec3Array; norm = new osg::Vec3Array; t = new osg::Vec2Array; col = new osg::Vec4Array(1); v->reserve(n*3); norm->reserve(n*3); t->reserve(n*3); (*col)[0].set(1,1,1,1); for (tri_iter= it->second.begin(); tri_iter!=it->second.end(); ++tri_iter) { for (unsigned int i=0; i<3; i++) { vert= (*tri_iter)->v[i]->getPosition()-origin; v->push_back(vert); norm->push_back(( material->type==1 || !material->reflectionTex.empty() ? osg::Vec3f(0,0,1) : (*tri_iter)->getNormal() )); // texc= texMat*vert; // t->push_back(osg::Vec2(texc.x()/material->texDim.x(),texc.y()/material->texDim.y())); t->push_back(material->getTexCoord(vert)); } /* vert= tri_iter->getPt2(out)-origin; v->push_back(vert); norm->push_back(tri_iter->getNormal(out)); texc= texMat*vert; t->push_back(osg::Vec2(texc.x()/material->texDim.x(),texc.y()/material->texDim.x())); vert= tri_iter->getPt3(out)-origin; v->push_back(vert); norm->push_back(tri_iter->getNormal(out)); texc= texMat*vert; t->push_back(osg::Vec2(texc.x()/material->texDim.x(),texc.y()/material->texDim.x()));*/ } geom = new osg::Geometry; if (material->drawCallback.valid()) { geom->setDrawCallback(material->drawCallback.get()); geom->setUseDisplayList(false); } geom->setVertexArray( v ); geom->setNormalArray( norm ); geom->setNormalBinding( osg::Geometry::BIND_PER_VERTEX ); fixupTexCoords(*t); geom->setTexCoordArray( 0, t ); geom->setColorArray( col ); geom->setColorBinding( osg::Geometry::BIND_OVERALL ); geom->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::TRIANGLES,0,n*3)); // geom->setStateSet( material->dstate.get() ); geode = new osg::Geode; geode->addDrawable( geom ); geode->setUserData(this); geode->setNodeMask(getNodeMask()); geode->setStateSet( material->dstate.get() ); // addVisual(geode); sv->reset(); geode->accept(*sv.get()); trans->addChild(geode); addVisual(geode); printf("geode->referenceCount(): %d\n",geode->referenceCount()); geode = new osg::Geode; geode->addDrawable( geom ); geode->setUserData(this); geode->setNodeMask(Editor::nm_SelectedGeometry); /* dstate= new osg::StateSet(); osg::PolygonMode *pm= new osg::PolygonMode(); pm->setMode(osg::PolygonMode::FRONT_AND_BACK, osg::PolygonMode::LINE); dstate->setAttributeAndModes(pm); dstate->setAttributeAndModes(new osg::LineWidth(1)); dstate->setMode(GL_DEPTH_TEST,osg::StateAttribute::OFF); dstate->setRenderBinDetails(20,"RenderBin"); mater= new osg::Material(); mater->setAmbient(osg::Material::Face::FRONT_AND_BACK, osg::Vec4(1.0,0.0,0.0,1.0)); mater->setDiffuse(osg::Material::Face::FRONT_AND_BACK, osg::Vec4(1.0,0.0,0.0,1.0)); // mater->setEmission(osg::Material::Face::FRONT_AND_BACK, osg::Vec4(1.0,0.0,1.0,1.0)); dstate->setAttributeAndModes(mater); geode->setStateSet( dstate );*/ geode->setStateSet( Editor::instance()->selectedNodesState.get() ); trans->addChild(geode); addSelectedVisual(geode); } } } } }