int ConfigFile::processVisual(boost::shared_ptr<const urdf::Visual> visual,Link &link, int nmat, std::vector<Material> &materials){
processGeometry(visual->geometry.get(),link.geom.get());
processPose(visual->origin,link.position,link.rpy,link.quat);
//Material
//Search if it's a new materia
int found=0;
if(visual->material!=NULL){
for(int i=0;i<nmat && !found;i++){
if(visual->material_name==materials[i].name){
link.material=i;
found=1;
}
}
if(!found){
materials[nmat].name=visual->material_name;
materials[nmat].r=visual->material->color.r;
materials[nmat].g=visual->material->color.g;
materials[nmat].b=visual->material->color.b;
materials[nmat].a=visual->material->color.a;
link.material=nmat;
nmat++;
}
}
else
link.material=-1;
return nmat;
}
int ConfigFile::processLink(boost::shared_ptr<const urdf::Link> link,Vehicle &vehicle,int nlink,int njoint,int nmat, std::vector<Material> &materials){
vehicle.links[nlink].name=link->name;
vehicle.links[nlink].geom.reset( new Geometry);
if(link->visual)
nmat= processVisual(link->visual,vehicle.links[nlink],nmat, materials);
else{
vehicle.links[nlink].geom->type=4;
vehicle.links[nlink].material=-1;
memset(vehicle.links[nlink].position,0,3*sizeof(double));
memset(vehicle.links[nlink].rpy,0,3*sizeof(double));
memset(vehicle.links[nlink].quat,0,4*sizeof(double));
vehicle.links[nlink].quat[3]=1;
}
if(link->collision){
vehicle.links[nlink].cs.reset( new Geometry);
processGeometry(link->collision->geometry.get(),vehicle.links[nlink].cs.get());
}
else
vehicle.links[nlink].cs.reset();
int linkNumber=nlink;
for(uint i=0;i<link->child_joints.size();i++){
processJoint(link->child_joints[i],vehicle.joints[linkNumber],nlink,linkNumber+1);
linkNumber=processLink(link->child_links[i],vehicle,linkNumber+1,linkNumber+1,nmat,materials);
}
return linkNumber;
}
//--------------------------------------------------------------
void testApp::updateRenderer(ofVideoPlayer& fromPlayer){
if (!temporalAlignmentMode) {
if(alignmentScrubber.getPairSequence().isSequenceTimebased()){
long movieMillis = fromPlayer.getPosition() * fromPlayer.getDuration()*1000;
currentDepthFrame = alignmentScrubber.getPairSequence().getDepthFrameForVideoFrame(movieMillis);
depthSequence.selectTime(currentDepthFrame);
}
else {
currentDepthFrame = alignmentScrubber.getPairSequence().getDepthFrameForVideoFrame(fromPlayer.getCurrentFrame());
depthSequence.selectFrame(currentDepthFrame);
}
renderer.setDepthImage(depthPixelDecodeBuffer);
}
processDepthFrame();
renderer.update();
processGeometry();
if(!drawPointcloud && !drawWireframe && !drawMesh){
drawPointcloud = true;
}
currentDepthFrame = depthSequence.getSelectedFrame();
}
void OBJWriterNodeVisitor::apply(osg::Geometry& geometry)
{
osg::Matrix m = osg::computeLocalToWorld(getNodePath());
pushStateSet(geometry.getStateSet());
processGeometry(&geometry,m);
popStateSet(geometry.getStateSet());
}
void DXFWriterNodeVisitor::apply( osg::Geode &node )
{
pushStateSet(node.getStateSet());
osg::Matrix m = osg::computeLocalToWorld(getNodePath());
unsigned int count = node.getNumDrawables();
for ( unsigned int i = 0; i < count; i++ )
{
osg::Geometry *g = node.getDrawable( i )->asGeometry();
if ( g != NULL )
{
pushStateSet(g->getStateSet());
processGeometry(g,m);
popStateSet(g->getStateSet());
}
}
popStateSet(node.getStateSet());
}
void QgsMapToolSimplify::updateSimplificationPreview()
{
QgsVectorLayer *vl = currentVectorLayer();
double layerTolerance = QgsTolerance::toleranceInMapUnits( mTolerance, vl, mCanvas->mapSettings(), mToleranceUnits );
mReducedHasErrors = false;
mReducedVertexCount = 0;
int i = 0;
Q_FOREACH ( const QgsFeature &fSel, mSelectedFeatures )
{
QgsGeometry g = processGeometry( fSel.geometry(), layerTolerance );
if ( !g.isNull() )
{
mReducedVertexCount += g.constGet()->nCoordinates();
mRubberBands.at( i )->setToGeometry( g, vl );
}
else
mReducedHasErrors = true;
++i;
}
static void processGeometry( Obj *obj, Scene *scene,
const mmap& materials, TransformNode *transform )
{
string name;
Obj *child;
if( obj->getTypeName() == "id" ) {
name = obj->getID();
child = NULL;
} else if( obj->getTypeName() == "named" ) {
name = obj->getName();
child = obj->getChild();
} else {
ostrstream oss;
oss << "Unknown input object ";
obj->printOn( oss );
throw ParseError( string( oss.str() ) );
}
processGeometry( name, child, scene, materials, transform );
}
void Terrain::initGL()
{
processGeometry();
// Generate And Bind The Vertex Buffer
glGenBuffers( 1, &vbuffer ); // Get A Valid Name
glBindBuffer( GL_ARRAY_BUFFER, vbuffer ); // Bind The Buffer
// Load the vertex data
glBufferData( GL_ARRAY_BUFFER, vertex.size()*3*sizeof(float), &vertex[0][0], GL_STATIC_DRAW );
glGenBuffers( 1, &nbuffer ); // Get A Valid Name
glBindBuffer( GL_ARRAY_BUFFER, nbuffer ); // Bind The Buffer
// Load the normal data
glBufferData( GL_ARRAY_BUFFER, normal.size()*3*sizeof(float), &normal[0][0], GL_STATIC_DRAW );
glBindBuffer(GL_ARRAY_BUFFER, 0);
glGenBuffers(1, &ibuffer);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, ibuffer);
// Load the face data
glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(GLuint) * face.size() * 3, &face[0][0], GL_STATIC_DRAW);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
}
static void processObject( Obj *obj, Scene *scene, mmap& materials )
{
// Assume the object is named.
string name;
Obj *child;
if( obj->getTypeName() == "id" ) {
name = obj->getID();
child = NULL;
} else if( obj->getTypeName() == "named" ) {
name = obj->getName();
child = obj->getChild();
} else {
ostrstream oss;
oss << "Unknown input object ";
obj->printOn( oss );
throw ParseError( string( oss.str() ) );
}
if( name == "directional_light" ) {
if( child == NULL ) {
throw ParseError( "No info for directional_light" );
}
scene->add( new DirectionalLight( scene,
tupleToVec( getField( child, "direction" ) ).normalize(),
tupleToVec( getColorField( child ) ) ) );
} else if( name == "point_light" ) {
if( child == NULL ) {
throw ParseError( "No info for point_light" );
}
PointLight* pointLight = new PointLight(scene,
tupleToVec(getField(child, "position")),
tupleToVec(getColorField(child)));
scene->add(pointLight);
if (hasField(child, "constant_attenuation_coeff") &&
hasField(child, "linear_attenuation_coeff") &&
hasField(child, "quadratic_attenuation_coeff"))
{
pointLight->setAttenuationCoefficients(
getField(child, "constant_attenuation_coeff")->getScalar(),
getField(child, "linear_attenuation_coeff")->getScalar(),
getField(child, "quadratic_attenuation_coeff")->getScalar());
}
}
else if (name == "ambient_light") {
if (child == NULL) {
throw ParseError("No info for ambient_light");
}
AmbientLight* ambientLight = new AmbientLight(scene,
tupleToVec(getColorField(child)));
scene->add(ambientLight);
} else if( name == "sphere" ||
name == "box" ||
name == "cylinder" ||
name == "cone" ||
name == "square" ||
name == "translate" ||
name == "rotate" ||
name == "scale" ||
name == "transform" ||
name == "trimesh" ||
name == "polymesh") { // polymesh is for backwards compatibility.
processGeometry( name, child, scene, materials, &scene->transformRoot);
//scene->add( geo );
} else if( name == "material" ) {
processMaterial( child, &materials );
} else if( name == "camera" ) {
processCamera( child, scene );
} else {
throw ParseError( string( "Unrecognized object: " ) + name );
}
}
static void processGeometry( string name, Obj *child, Scene *scene,
const mmap& materials, TransformNode *transform )
{
if( name == "translate" ) {
const mytuple& tup = child->getTuple();
verifyTuple( tup, 4 );
processGeometry( tup[3],
scene,
materials,
transform->createChild(mat4f::translate( vec3f(tup[0]->getScalar(),
tup[1]->getScalar(),
tup[2]->getScalar() ) ) ) );
} else if( name == "rotate" ) {
const mytuple& tup = child->getTuple();
verifyTuple( tup, 5 );
processGeometry( tup[4],
scene,
materials,
transform->createChild(mat4f::rotate( vec3f(tup[0]->getScalar(),
tup[1]->getScalar(),
tup[2]->getScalar() ),
tup[3]->getScalar() ) ) );
} else if( name == "scale" ) {
const mytuple& tup = child->getTuple();
if( tup.size() == 2 ) {
double sc = tup[0]->getScalar();
processGeometry( tup[1],
scene,
materials,
transform->createChild(mat4f::scale( vec3f( sc, sc, sc ) ) ) );
} else {
verifyTuple( tup, 4 );
processGeometry( tup[3],
scene,
materials,
transform->createChild(mat4f::scale( vec3f(tup[0]->getScalar(),
tup[1]->getScalar(),
tup[2]->getScalar() ) ) ) );
}
} else if( name == "transform" ) {
const mytuple& tup = child->getTuple();
verifyTuple( tup, 5 );
const mytuple& l1 = tup[0]->getTuple();
const mytuple& l2 = tup[1]->getTuple();
const mytuple& l3 = tup[2]->getTuple();
const mytuple& l4 = tup[3]->getTuple();
verifyTuple( l1, 4 );
verifyTuple( l2, 4 );
verifyTuple( l3, 4 );
verifyTuple( l4, 4 );
processGeometry( tup[4],
scene,
materials,
transform->createChild(mat4f(vec4f( l1[0]->getScalar(),
l1[1]->getScalar(),
l1[2]->getScalar(),
l1[3]->getScalar() ),
vec4f( l2[0]->getScalar(),
l2[1]->getScalar(),
l2[2]->getScalar(),
l2[3]->getScalar() ),
vec4f( l3[0]->getScalar(),
l3[1]->getScalar(),
l3[2]->getScalar(),
l3[3]->getScalar() ),
vec4f( l4[0]->getScalar(),
l4[1]->getScalar(),
l4[2]->getScalar(),
l4[3]->getScalar() ) ) ) );
} else if( name == "trimesh" || name == "polymesh" ) { // 'polymesh' is for backwards compatibility
processTrimesh( name, child, scene, materials, transform);
} else {
SceneObject *obj = NULL;
Material *mat;
//if( hasField( child, "material" ) )
mat = getMaterial(getField( child, "material" ), materials );
//else
// mat = new Material();
if( name == "sphere" ) {
obj = new Sphere( scene, mat );
} else if( name == "box" ) {
obj = new Box( scene, mat );
}
else if (name == "cylinder") {
bool capped = true;
maybeExtractField(child, "capped", capped);
obj = new Cylinder(scene, mat, capped);
} else if( name == "cone" ) {
double height = 1.0;
double bottom_radius = 1.0;
double top_radius = 0.0;
bool capped = true;
maybeExtractField( child, "height", height );
maybeExtractField( child, "bottom_radius", bottom_radius );
maybeExtractField( child, "top_radius", top_radius );
maybeExtractField( child, "capped", capped );
obj = new Cone( scene, mat, height, bottom_radius, top_radius, capped );
} else if( name == "square" ) {
obj = new Square( scene, mat );
}
obj->setTransform(transform);
scene->add(obj);
}
}