Improved SDF GetElement. Added SDF2POV tool. Fixed RecalculateNormals…

… to make objects smooth.
gazebosim · Nov 14, 2011 · ea54b56 · ea54b56
1 parent 9eaf0a2
commit ea54b56
Show file tree

Hide file tree

Showing 14 changed files with 474 additions and 34 deletions.
diff --git a/sdf/worlds/pr2.world b/sdf/worlds/pr2.world
@@ -33,7 +33,8 @@
       </link>
     </model>
 
-    <include filename="pr2.model"/>
+    <!--<include filename="pr2.model" />-->
+    <include filename="pr2.model"  model_pose="0 0 0 0 0 3.1415"/> 
 
     <light type="directional" name="sun" cast_shadows="true">
       <origin pose="0 0 10 0 0 0"/>

diff --git a/src/Server.cc b/src/Server.cc
@@ -93,7 +93,7 @@ bool Server::Load(const std::string &_filename)
       gzthrow("Failed to load the World\n"  << e);
     }
 
-    worldElem = sdf->root->GetNextElement("world", worldElem);
+    worldElem = worldElem->GetNextElement();
   }
 
   return true;

diff --git a/src/common/Mesh.cc b/src/common/Mesh.cc
@@ -669,9 +669,16 @@ void SubMesh::RecalculateNormals()
     math::Vector3 v2 = this->vertices[this->indices[i+1]];
     math::Vector3 v3 = this->vertices[this->indices[i+2]];
     math::Vector3 n = math::Vector3::GetNormal(v1, v2, v3);
-    this->normals[this->indices[i]] += n;
-    this->normals[this->indices[i+1]] += n;
-    this->normals[this->indices[i+2]] += n;
+
+    for (unsigned int j=0; j< this->vertices.size(); j++)
+    {
+      if (this->vertices[j] == v1 || 
+          this->vertices[j] == v2 ||
+          this->vertices[j] == v3)
+      {
+        this->normals[j] += n;
+      }
+    }
   }
 
   // Normalize the results

diff --git a/src/gui/InsertModelWidget.cc b/src/gui/InsertModelWidget.cc
@@ -186,10 +186,10 @@ void InsertModelWidget::OnModelSelection(QTreeWidgetItem *_item, int /*_column*/
         this->visuals.push_back(visVisual);
 
 
-        visualElem = linkElem->GetNextElement("visual", visualElem);
+        visualElem = visualElem->GetNextElement();
       }
 
-      linkElem = modelElem->GetNextElement("link", linkElem);
+      linkElem = linkElem->GetNextElement();
     }
 
     msgs::Selection selectMsg;

diff --git a/src/physics/Link.cc b/src/physics/Link.cc
@@ -123,7 +123,7 @@ void Link::Load( sdf::ElementPtr &_sdf )
       this->visPub->Publish(msg);
       this->visuals.push_back(msg.name());
 
-      visualElem = this->sdf->GetNextElement("visual", visualElem); 
+      visualElem = visualElem->GetNextElement(); 
     }
   }
 
@@ -135,7 +135,7 @@ void Link::Load( sdf::ElementPtr &_sdf )
     {
       // Create and Load a collision, which will belong to this body.
       this->LoadCollision(collisionElem);
-      collisionElem = this->sdf->GetNextElement("collision", collisionElem); 
+      collisionElem = collisionElem->GetNextElement(); 
     }
   }
 
@@ -145,7 +145,7 @@ void Link::Load( sdf::ElementPtr &_sdf )
     while (sensorElem)
     {
       this->LoadSensor(sensorElem);
-      sensorElem = this->sdf->GetNextElement("sensor", sensorElem); 
+      sensorElem = sensorElem->GetNextElement(); 
     }
   }
 }
@@ -294,7 +294,7 @@ void Link::UpdateParameters( sdf::ElementPtr &_sdf )
 
       this->visPub->Publish(msg);
 
-      visualElem = this->sdf->GetNextElement("visual", visualElem); 
+      visualElem = visualElem->GetNextElement(); 
     }
   }
 
@@ -308,7 +308,7 @@ void Link::UpdateParameters( sdf::ElementPtr &_sdf )
 
       if (collision)
         collision->UpdateParameters(collisionElem);
-      collisionElem = this->sdf->GetNextElement("collision", collisionElem);
+      collisionElem = collisionElem->GetNextElement();
     }
   }
 }

diff --git a/src/physics/Model.cc b/src/physics/Model.cc
@@ -111,7 +111,7 @@ void Model::Load( sdf::ElementPtr &_sdf )
       // bodies collisionetries
       link->Load(linkElem);
 
-      linkElem = _sdf->GetNextElement("link", linkElem);
+      linkElem = linkElem->GetNextElement();
     }
   }
 
@@ -122,7 +122,7 @@ void Model::Load( sdf::ElementPtr &_sdf )
     while (jointElem)
     {
       this->LoadJoint(jointElem);
-      jointElem = _sdf->GetNextElement("joint", jointElem);
+      jointElem = jointElem->GetNextElement();
     }
   }
 
@@ -133,7 +133,7 @@ void Model::Load( sdf::ElementPtr &_sdf )
     while (pluginElem)
     {
       this->LoadPlugin(pluginElem);
-      pluginElem = _sdf->GetNextElement("plugin", pluginElem);
+      pluginElem = pluginElem->GetNextElement();
     }
   }
 }
@@ -304,7 +304,7 @@ void Model::UpdateParameters( sdf::ElementPtr &_sdf )
       LinkPtr link = boost::shared_dynamic_cast<Link>(
           this->GetChild(linkElem->GetValueString("name")));
       link->UpdateParameters(linkElem);
-      linkElem = _sdf->GetNextElement("link", linkElem);
+      linkElem = linkElem->GetNextElement();
     }
   }
   /*
@@ -316,7 +316,7 @@ void Model::UpdateParameters( sdf::ElementPtr &_sdf )
     {
       JointPtr joint = boost::shared_dynamic_cast<Joint>(this->GetChild(jointElem->GetValueString("name")));
       joint->UpdateParameters(jointElem);
-      jointElem = _sdf->GetNextElement("joint", jointElem);
+      jointElem = jointElem->GetNextElement();
     }
   }
   */

diff --git a/src/physics/World.cc b/src/physics/World.cc
@@ -438,7 +438,7 @@ void World::LoadEntities( sdf::ElementPtr &_sdf, BasePtr _parent )
       msgs::Light *lm = this->sceneMsg.add_light();
       lm->CopyFrom( msgs::LightFromSDF(childElem) );
 
-      childElem = _sdf->GetNextElement("light", childElem);
+      childElem = childElem->GetNextElement();
     }
   }
 
@@ -453,7 +453,7 @@ void World::LoadEntities( sdf::ElementPtr &_sdf, BasePtr _parent )
       // TODO : Put back in the ability to nest models. We should do this
       // without requiring a joint.
 
-      childElem = _sdf->GetNextElement("model", childElem);
+      childElem = childElem->GetNextElement();
     }
   }
 
@@ -464,7 +464,7 @@ void World::LoadEntities( sdf::ElementPtr &_sdf, BasePtr _parent )
     while (pluginElem)
     {
       this->LoadPlugin(pluginElem);
-      pluginElem = _sdf->GetNextElement("plugin", pluginElem);
+      pluginElem = pluginElem->GetNextElement();
     }
   }
 }

diff --git a/src/physics/server.cc b/src/physics/server.cc
@@ -114,7 +114,7 @@ void Load()
       gzthrow("Failed to load the World\n"  << e);
     }
 
-    worldElem = sdf->root->GetNextElement("world", worldElem);
+    worldElem = worldElem->GetNextElement();
   }
 
   gazebo::physics::init_worlds();

diff --git a/src/sdf/interface/SDF.cc b/src/sdf/interface/SDF.cc
@@ -428,21 +428,33 @@ ElementPtr Element::GetFirstElement() const
   return this->elements.front();
 }
 
-ElementPtr Element::GetNextElement(const std::string &_name, 
-                          const ElementPtr &_elem) const
+ElementPtr Element::GetNextElement(const std::string &_name) const
 {
-  ElementPtr_V::const_iterator iter;
-  iter = std::find(this->elements.begin(), this->elements.end(), _elem);
-
-  for (iter++; iter != this->elements.end(); iter++)
+  if (this->parent)
   {
-    if ( (*iter)->GetName() == _name )
-      return (*iter);
+    ElementPtr_V::const_iterator iter;
+    iter = std::find(this->parent->elements.begin(), 
+        this->parent->elements.end(), shared_from_this());
+
+    if (iter == this->parent->elements.end())
+      return ElementPtr();
+
+    for (iter++; iter != this->parent->elements.end(); iter++)
+    {
+      if ( (*iter)->GetName() == _name )
+        return (*iter);
+    }
   }
 
   return ElementPtr();
 }
 
+ElementPtr Element::GetNextElement() const
+{
+  return this->GetNextElement(this->name);
+}
+
+
 boost::shared_ptr<Element> Element::GetOrCreateElement(const std::string &_name)
 {
   if (this->HasElement(_name))

diff --git a/src/sdf/interface/SDF.hh b/src/sdf/interface/SDF.hh
@@ -106,8 +106,10 @@ namespace sdf
 
     public: ElementPtr GetElement(const std::string &_name) const;
     public: ElementPtr GetFirstElement() const;
-    public: ElementPtr GetNextElement(const std::string &_name, 
-                                      const ElementPtr &_elem) const;
+
+    public: ElementPtr GetNextElement(const std::string &_name) const;
+    public: ElementPtr GetNextElement() const;
+
 
     public: ElementPtr GetOrCreateElement(const std::string &_name);
     public: ElementPtr AddElement(const std::string &_name);

diff --git a/src/sensors/ContactSensor.cc b/src/sensors/ContactSensor.cc
@@ -110,8 +110,7 @@ void ContactSensor::Load()
       this->connections.push_back(collision->ConnectContact(
             boost::bind(&ContactSensor::OnContact, this, _1, _2)));
     }
-    collisionElem = this->sdf->GetElement("contact")->GetNextElement(
-        "collision", collisionElem);
+    collisionElem = collisionElem->GetNextElement();
   }
 }
 

diff --git a/src/sensors/Sensor.cc b/src/sensors/Sensor.cc
@@ -102,7 +102,7 @@ void Sensor::Init()
     while (pluginElem)
     {
       this->LoadPlugin(pluginElem);
-      pluginElem = this->sdf->GetNextElement("plugin", pluginElem);
+      pluginElem = pluginElem->GetNextElement();
     }
   }
   //SensorManager::Instance()->AddSensor(shared_from_this());

diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt
@@ -20,3 +20,7 @@ add_executable( gzstats gzstats.cc )
 target_link_libraries(gzstats gazebo_msgs gazebo_common gazebo_transport )
 INSTALL_EXECUTABLE( gzstats )
 
+add_executable( sdf2pov sdf2pov.cc )
+target_link_libraries(sdf2pov gazebo_common gazebo_sdf_parser)
+INSTALL_EXECUTABLE( sdf2pov )
+