//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);
}
}
}
}
}
