[Playerstage-commit] SF.net SVN: playerstage:[6921] code/gazebo/trunk

[<na...@us...>]

Revision: 6921
          http://playerstage.svn.sourceforge.net/playerstage/?rev=6921&view=rev
Author:   natepak
Date:     2008-07-24 16:22:28 +0000 (Thu, 24 Jul 2008)

Log Message:
-----------
Fixed placement of static  geometries

Modified Paths:
--------------
    code/gazebo/trunk/server/physics/Body.cc
    code/gazebo/trunk/server/physics/Geom.cc
    code/gazebo/trunk/server/physics/MapGeom.cc
    code/gazebo/trunk/server/physics/SphereGeom.cc
    code/gazebo/trunk/worlds/map.world

Modified: code/gazebo/trunk/server/physics/Body.cc
===================================================================
--- code/gazebo/trunk/server/physics/Body.cc	2008-07-24 15:27:54 UTC (rev 6920)
+++ code/gazebo/trunk/server/physics/Body.cc	2008-07-24 16:22:28 UTC (rev 6921)
@@ -202,10 +202,6 @@
        geomIter!=this->geoms.end(); geomIter++)
   {
     (*geomIter)->Update();
-    if ((*geomIter)->GetName() == "sphere1_geom")
-    {
-      std::cout << "Geom pose[" << (*geomIter)->GetPose() << "]\n";
-    }
   }
 
   for (sensorIter=this->sensors.begin();
@@ -250,12 +246,6 @@
     //this->SetPosition(this->staticPose.pos);
     //this->SetRotation(this->staticPose.rot);
    
-    if (this->GetName() == "map_body")
-    {
-      std::cout << "Set new pose[" << pose << "]\n";
-      std::cout << "Old Pose[ " << oldPose << "]\n";
-    }
-
     for (iter = this->geoms.begin(); iter != this->geoms.end(); iter++)
     {
       newPose = (*iter)->GetPose() - oldPose;

Modified: code/gazebo/trunk/server/physics/Geom.cc
===================================================================
--- code/gazebo/trunk/server/physics/Geom.cc	2008-07-24 15:27:54 UTC (rev 6920)
+++ code/gazebo/trunk/server/physics/Geom.cc	2008-07-24 16:22:28 UTC (rev 6921)
@@ -188,7 +188,7 @@
   this->geomId = geomId;
   this->transId = NULL;
 
-  if (this->placeable)
+  if (this->placeable && !this->IsStatic())
   {
     if (dGeomGetClass(geomId) != dTriMeshClass)
     {
@@ -198,8 +198,11 @@
       assert(dGeomGetSpace(this->geomId) == 0);
     }
   }
-  else
+  else if ( dGeomGetSpace(this->geomId) == 0 )
+  {
+    dSpaceAdd(this->spaceId, this->geomId);
     assert(dGeomGetSpace(this->geomId) != 0);
+  }
 
   dGeomSetData(this->geomId, this);
 
@@ -271,17 +274,13 @@
     q[2] = localPose.rot.y;
     q[3] = localPose.rot.z;
 
-    this->visualNode->SetPose(pose);
 
     // Set the pose of the encapsulated geom; this is always relative
     // to the CoM
     dGeomSetPosition(this->geomId, localPose.pos.x, localPose.pos.y, localPose.pos.z);
     dGeomSetQuaternion(this->geomId, q);
 
-    if (this->GetName() == "sphere1_geom")
-    {
-      std::cout << "Geom set pose[" << this->GetPose() << "]\n";
-    }
+    this->visualNode->SetPose(localPose);
 
     if (updateCoM)
     {

Modified: code/gazebo/trunk/server/physics/MapGeom.cc
===================================================================
--- code/gazebo/trunk/server/physics/MapGeom.cc	2008-07-24 15:27:54 UTC (rev 6920)
+++ code/gazebo/trunk/server/physics/MapGeom.cc	2008-07-24 16:22:28 UTC (rev 6921)
@@ -110,50 +110,14 @@
     this->ReduceTree(this->root);
   }
 
-  //this->CreateBoxes(this->root);
-  this->CreateBox();
+  this->CreateBoxes(this->root);
 
   this->visualNode->MakeStatic();
 }
 
-void MapGeom::CreateBox()
-{
-  std::ostringstream stream;
 
-  // Create the box geometry
-  BoxGeom* newBox = new BoxGeom( this->body );
-
-  XMLConfig *boxConfig = new XMLConfig();
-
-  stream << "<gazebo:world xmlns:gazebo=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#gz\" xmlns:geom=\"http://playerstage.sourceforge.net/gazebo/xmlschema/#geom\">"; 
-
-  float x = 2;
-  float y = 2;
-  float z = 0.5;
-  float xSize = 1.0;
-  float ySize = 1.0;
-  float zSize = 1.0;
-
-  stream << "<geom:box name='map_geom_box'>";
-  stream << "  <mass>0.0</mass>";
-  stream << "  <xyz>" << x << " " << y << " " << z << "</xyz>";
-  stream << "  <rpy>0 0 0</rpy>";
-  stream << "  <size>" << xSize << " " << ySize << " " << zSize << "</size>";
-  stream << "  <mass>0.01</mass>";
-  stream << "  <visual>";
-  stream << "    <mesh>unit_box</mesh>";
-  stream << "    <material>" << this->material << "</material>";
-  stream << "  <size>" << xSize << " " << ySize << " " << zSize << "</size>";
-  stream << "  </visual>";
-  stream << "</geom:box>";
-  stream << "</gazebo:world>";
-
-  boxConfig->LoadString( stream.str() );
-
-  newBox->Load( boxConfig->GetRootNode()->GetChild() );
-  delete boxConfig;
-}
-
+//////////////////////////////////////////////////////////////////////////////
+// Create the ODE boxes
 void MapGeom::CreateBoxes(QuadNode *node)
 {
   if (node->leaf)
@@ -205,6 +169,8 @@
   }
 }
 
+//////////////////////////////////////////////////////////////////////////////
+// Reduce the size of the quad tree
 void MapGeom::ReduceTree(QuadNode *node)
 {
   std::deque<QuadNode*>::iterator iter;
@@ -262,6 +228,8 @@
   }
 }
 
+//////////////////////////////////////////////////////////////////////////////
+// Merege quad tree cells
 void MapGeom::Merge(QuadNode *nodeA, QuadNode *nodeB)
 {
   std::deque<QuadNode*>::iterator iter;
@@ -310,6 +278,8 @@
 }
 
 
+//////////////////////////////////////////////////////////////////////////////
+// Construct the quad tree
 void MapGeom::BuildTree(QuadNode *node)
 {
   QuadNode *newNode = NULL;
@@ -382,6 +352,8 @@
 
 }
 
+//////////////////////////////////////////////////////////////////////////////
+// Get a count of pixels within a given area
 void MapGeom::GetPixelCount(unsigned int xStart, unsigned int yStart, 
                                  unsigned int width, unsigned int height, 
                                  unsigned int &freePixels, 

Modified: code/gazebo/trunk/server/physics/SphereGeom.cc
===================================================================
--- code/gazebo/trunk/server/physics/SphereGeom.cc	2008-07-24 15:27:54 UTC (rev 6920)
+++ code/gazebo/trunk/server/physics/SphereGeom.cc	2008-07-24 16:22:28 UTC (rev 6921)
@@ -56,7 +56,7 @@
   dMassSetSphereTotal(&this->mass, this->dblMass, radius);
 
   // Create the sphere geometry
-  this->SetGeom(dCreateSphere(0, radius ), true);
+  this->SetGeom( dCreateSphere(0, radius ), true);
 
   //to be able to show physics
   /*  this->visualNode->AttachMesh("unit_sphere"); // unit_sphere radius=1 diameter=2

Modified: code/gazebo/trunk/worlds/map.world
===================================================================
--- code/gazebo/trunk/worlds/map.world	2008-07-24 15:27:54 UTC (rev 6920)
+++ code/gazebo/trunk/worlds/map.world	2008-07-24 16:22:28 UTC (rev 6921)
@@ -19,7 +19,7 @@
   <verbosity>5</verbosity>
 
   <physics:ode>
-    <stepTime>0.03</stepTime>
+    <stepTime>0.3</stepTime>
     <gravity>0 0 -9.8</gravity>
     <cfm>10e-5</cfm>
     <erp>0.3</erp>


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.