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.
-------------------------------------------------------------------------
This SF.Net email is sponsored by the Moblin Your Move Developer's challenge
Build the coolest Linux based applications with Moblin SDK & win great prizes
Grand prize is a trip for two to an Open Source event anywhere in the world
http://moblin-contest.org/redirect.php?banner_id=100&url=/
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
