Revision: 8605
http://playerstage.svn.sourceforge.net/playerstage/?rev=8605&view=rev
Author: natepak
Date: 2010-04-02 15:06:14 +0000 (Fri, 02 Apr 2010)
Log Message:
-----------
Improved ODE performance, and fixed some boost::signals bugs
Modified Paths:
--------------
code/gazebo/trunk/Media/materials/scripts/Gazebo.material
code/gazebo/trunk/server/Model.cc
code/gazebo/trunk/server/Simulator.cc
code/gazebo/trunk/server/World.cc
code/gazebo/trunk/server/World.hh
code/gazebo/trunk/server/physics/Body.cc
code/gazebo/trunk/server/physics/Body.hh
code/gazebo/trunk/server/physics/Geom.cc
code/gazebo/trunk/server/physics/Geom.hh
code/gazebo/trunk/server/physics/Joint.cc
code/gazebo/trunk/server/physics/PhysicsEngine.cc
code/gazebo/trunk/server/physics/bullet/BulletBody.hh
code/gazebo/trunk/server/physics/ode/ODEBody.cc
code/gazebo/trunk/server/physics/ode/ODEBody.hh
code/gazebo/trunk/server/physics/ode/ODEGeom.cc
code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
code/gazebo/trunk/server/physics/ode/ODEPhysics.hh
code/gazebo/trunk/server/rendering/OgreVisual.cc
code/gazebo/trunk/server/rendering/OgreVisual.hh
code/gazebo/trunk/worlds/empty.world
code/gazebo/trunk/worlds/map.world
code/gazebo/trunk/worlds/models/sicklms200.model
code/gazebo/trunk/worlds/simpleshapes.world
Modified: code/gazebo/trunk/Media/materials/scripts/Gazebo.material
===================================================================
--- code/gazebo/trunk/Media/materials/scripts/Gazebo.material 2010-03-31
22:43:07 UTC (rev 8604)
+++ code/gazebo/trunk/Media/materials/scripts/Gazebo.material 2010-04-02
15:06:14 UTC (rev 8605)
@@ -944,7 +944,7 @@
}
}
-material Gazebo/TransparentTest
+material Gazebo/RedTransparent
{
technique
{
@@ -965,6 +965,27 @@
}
}
+material Gazebo/GreenTransparent
+{
+ technique
+ {
+ pass
+ {
+ scene_blend alpha_blend
+ depth_write off
+
+ ambient 0.0 1.0 0.0 1
+ diffuse 0.0 1.0 0.0 1
+
+ texture_unit
+ {
+ colour_op_ex source1 src_current src_current 0 1 0
+ alpha_op_ex source1 src_manual src_current 0.1
+ }
+ }
+ }
+}
+
material Gazebo/DepthMap
{
technique
Modified: code/gazebo/trunk/server/Model.cc
===================================================================
--- code/gazebo/trunk/server/Model.cc 2010-03-31 22:43:07 UTC (rev 8604)
+++ code/gazebo/trunk/server/Model.cc 2010-04-02 15:06:14 UTC (rev 8605)
@@ -411,16 +411,10 @@
//DiagnosticTimer timer("Model[" + this->GetName() + "] Update ");
-#ifdef USE_THREADPOOL
- World::Instance()->GetPhysicsEngine()->InitForThread();
-#endif
-
std::map<std::string, Body* >::iterator bodyIter;
std::map<std::string, Controller* >::iterator contIter;
JointContainer::iterator jointIter;
- Pose3d bodyPose, newPose, oldPose;
-
//this->updateSignal();
{
@@ -474,31 +468,31 @@
}
// Call the model's python update function, if one exists
- /*if (this->pFuncUpdate)
- {
- boost::python::call<void>(this->pFuncUpdate, this);
- }*/
+ //if (this->pFuncUpdate)
+ //{
+ // boost::python::call<void>(this->pFuncUpdate, this);
+ //}
// BULLET:
- /*if (!this->canonicalBodyNameP->GetValue().empty())
- {
- /// model pose is the canonical body pose of the body + a transform from
body frame to model frame
- /// the tranform is defined by initModelOffset in body frame,
+ //if (!this->canonicalBodyNameP->GetValue().empty())
+ //{
+ // /// model pose is the canonical body pose of the body + a transform from
body frame to model frame
+ // /// the tranform is defined by initModelOffset in body frame,
- /// recover the transform in inertial frame based on body pose
- this->pose = this->bodies[**this->canonicalBodyNameP]->GetPose();
- Quatern body_rot = this->pose.rot;
- Pose3d offset_transform =
this->bodies[**this->canonicalBodyNameP]->initModelOffset;
- Vector3 xyz_offset =
(offset_transform.RotatePositionAboutOrigin(body_rot.GetInverse())).pos;
- Quatern q_offset = offset_transform.rot;
+ // /// recover the transform in inertial frame based on body pose
+ // this->pose = this->bodies[**this->canonicalBodyNameP]->GetPose();
+ // Quatern body_rot = this->pose.rot;
+ // Pose3d offset_transform =
this->bodies[**this->canonicalBodyNameP]->initModelOffset;
+ // Vector3 xyz_offset =
(offset_transform.RotatePositionAboutOrigin(body_rot.GetInverse())).pos;
+ // Quatern q_offset = offset_transform.rot;
- // apply transform to get model pose
- this->pose.pos = this->pose.pos + xyz_offset;
- this->pose.rot = this->pose.CoordRotationAdd(q_offset);
+ // // apply transform to get model pose
+ // this->pose.pos = this->pose.pos + xyz_offset;
+ // this->pose.rot = this->pose.CoordRotationAdd(q_offset);
- this->xyzP->SetValue(this->pose.pos);
- this->rpyP->SetValue(this->pose.rot);
- }*/
+ // this->xyzP->SetValue(this->pose.pos);
+ // this->rpyP->SetValue(this->pose.rot);
+ //}
{
//DiagnosticTimer timer("Model[" + this->GetName() + "] Children Update ");
Modified: code/gazebo/trunk/server/Simulator.cc
===================================================================
--- code/gazebo/trunk/server/Simulator.cc 2010-03-31 22:43:07 UTC (rev
8604)
+++ code/gazebo/trunk/server/Simulator.cc 2010-04-02 15:06:14 UTC (rev
8605)
@@ -361,10 +361,10 @@
this->gui->Update();
if (this->renderEngineEnabled)
+ {
OgreAdaptor::Instance()->UpdateCameras();
-
- if (this->renderEngineEnabled)
World::Instance()->GraphicsUpdate();
+ }
currTime = this->GetWallTime();
Modified: code/gazebo/trunk/server/World.cc
===================================================================
--- code/gazebo/trunk/server/World.cc 2010-03-31 22:43:07 UTC (rev 8604)
+++ code/gazebo/trunk/server/World.cc 2010-04-02 15:06:14 UTC (rev 8605)
@@ -264,6 +264,11 @@
// Initialize the world
void World::Init()
{
+#ifdef USE_THREADPOOL
+ // If calling Body::Update in threadpool
+ this->physicsEngine->InitForThread();
+#endif
+
std::vector< Model* >::iterator miter;
this->simPauseTime = 0;
Modified: code/gazebo/trunk/server/World.hh
===================================================================
--- code/gazebo/trunk/server/World.hh 2010-03-31 22:43:07 UTC (rev 8604)
+++ code/gazebo/trunk/server/World.hh 2010-04-02 15:06:14 UTC (rev 8605)
@@ -36,6 +36,8 @@
#include <boost/tuple/tuple.hpp>
#include <boost/signal.hpp>
+#include "Global.hh"
+
#ifdef USE_THREADPOOL
#include "boost/threadpool.hpp"
#include "boost/thread/mutex.hpp"
@@ -45,7 +47,6 @@
#include "Vector3.hh"
#include "Pose3d.hh"
#include "Entity.hh"
-#include "Global.hh"
#include "Timer.hh"
namespace gazebo
@@ -264,59 +265,74 @@
/// \brief Connect a boost::slot the the add entity signal
public: template<typename T>
- void ConnectAddEntitySignal( T subscriber )
- {
- addEntitySignal.connect(subscriber);
- }
+ boost::signals::connection ConnectAddEntitySignal( T subscriber )
+ { return addEntitySignal.connect(subscriber); }
+ public: template<typename T>
+ void DisconnectAddEntitySignal( T subscriber)
+ { addEntitySignal.disconnect(subscriber); }
+
/// \brief Connect a boost::slot the the show light source signal
public: template<typename T>
- void ConnectShowLightsSignal( T subscriber )
- {
- showLightsSignal.connect(subscriber);
- }
+ boost::signals::connection ConnectShowLightsSignal( T subscriber )
+ { return showLightsSignal.connect(subscriber); }
+ public: template<typename T>
+ void DisconnectShowLightsSignal( T subscriber )
+ { showLightsSignal.disconnect(subscriber); }
+
/// \brief Connect a boost::slot the the show camera source signal
public: template<typename T>
- void ConnectShowCamerasSignal( T subscriber )
- {
- showCamerasSignal.connect(subscriber);
- }
+ boost::signals::connection ConnectShowCamerasSignal( T subscriber )
+ { return showCamerasSignal.connect(subscriber); }
+ public: template<typename T>
+ void DisconnectShowCamerasSignal( T subscriber )
+ { showCamerasSignal.disconnect(subscriber); }
/// \brief Connect a boost::slot the the show contacts signal
public: template<typename T>
- void ConnectShowContactsSignal( T subscriber )
- {
- showContactsSignal.connect(subscriber);
- }
+ boost::signals::connection ConnectShowContactsSignal( T subscriber )
+ { return showContactsSignal.connect(subscriber); }
+ public: template<typename T>
+ void DisconnectShowContactsSignal( T subscriber )
+ { showContactsSignal.disconnect(subscriber); }
+
/// \brief Connect a boost::slot the the show wireframe signal
public: template<typename T>
- void ConnectShowWireframeSignal( T subscriber )
- {
- wireframeSignal.connect(subscriber);
- }
+ boost::signals::connection ConnectShowWireframeSignal( T subscriber )
+ { return wireframeSignal.connect(subscriber); }
+ public: template<typename T>
+ void DisconnectShowWireframeSignal( T subscriber )
+ { wireframeSignal.disconnect(subscriber); }
+
/// \brief Connect a boost::slot the the show physics signal
public: template<typename T>
- void ConnectShowPhysicsSignal( T subscriber )
- {
- showPhysicsSignal.connect(subscriber);
- }
+ boost::signals::connection ConnectShowPhysicsSignal( T subscriber )
+ { return showPhysicsSignal.connect(subscriber); }
+ public: template<typename T>
+ void DisconnectShowPhysicsSignal( T subscriber )
+ { showPhysicsSignal.disconnect(subscriber); }
+
/// \brief Connect a boost::slot the the show joints signal
public: template<typename T>
- void ConnectShowJointsSignal( T subscriber )
- {
- showJointsSignal.connect(subscriber);
- }
+ boost::signals::connection ConnectShowJointsSignal( T subscriber )
+ { return showJointsSignal.connect(subscriber); }
+ public: template<typename T>
+ void DisconnectShowJointsSignal( T subscriber )
+ { showJointsSignal.disconnect(subscriber); }
/// \brief Connect a boost::slot the the show bounding boxes signal
public: template<typename T>
- void ConnectShowBoundingBoxesSignal( T subscriber )
- {
- showBoundingBoxesSignal.connect(subscriber);
- }
+ boost::signals::connection ConnectShowBoundingBoxesSignal( T
subscriber )
+ { return showBoundingBoxesSignal.connect(subscriber); }
+ public: template<typename T>
+ void DisconnectShowBoundingBoxesSignal( T subscriber )
+ { showBoundingBoxesSignal.disconnect(subscriber); }
+
+
/// \brief Get the names of interfaces defined in the tree of a model
private: void GetInterfaceNames(Entity* m, std::vector<std::string>& list);
Modified: code/gazebo/trunk/server/physics/Body.cc
===================================================================
--- code/gazebo/trunk/server/physics/Body.cc 2010-03-31 22:43:07 UTC (rev
8604)
+++ code/gazebo/trunk/server/physics/Body.cc 2010-04-02 15:06:14 UTC (rev
8605)
@@ -407,11 +407,7 @@
void Body::Update()
{
//DiagnosticTimer timer("Body[" + this->GetName() +"] Update");
-
-#ifdef USE_THREADPOOL
- // If calling Body::Update in threadpool
- World::Instance()->GetPhysicsEngine()->InitForThread();
-#endif
+
std::map< std::string, Geom* >::iterator geomIter;
Vector3 vel;
Vector3 avel;
@@ -429,7 +425,7 @@
geomIter!=this->geoms.end(); geomIter++)
{
#ifdef USE_THREADPOOL
- World::Instance()->threadpool->schedule(boost::bind(&Geom::Update,
(geomIter->second)));
+ World::Instance()->threadPool->schedule(boost::bind(&Geom::Update,
(geomIter->second)));
#else
geomIter->second->Update();
#endif
Modified: code/gazebo/trunk/server/physics/Body.hh
===================================================================
--- code/gazebo/trunk/server/physics/Body.hh 2010-03-31 22:43:07 UTC (rev
8604)
+++ code/gazebo/trunk/server/physics/Body.hh 2010-04-02 15:06:14 UTC (rev
8605)
@@ -91,6 +91,9 @@
/// \brief Set whether this body is enabled
public: virtual void SetEnabled(bool enable) const = 0;
+ /// \brief Get whether this body is enabled in the physics engine
+ public: virtual bool GetEnabled() const = 0;
+
/// \brief Set whether this entity has been selected by the user
/// through the gui
public: virtual bool SetSelected( bool s );
Modified: code/gazebo/trunk/server/physics/Geom.cc
===================================================================
--- code/gazebo/trunk/server/physics/Geom.cc 2010-03-31 22:43:07 UTC (rev
8604)
+++ code/gazebo/trunk/server/physics/Geom.cc 2010-04-02 15:06:14 UTC (rev
8605)
@@ -86,6 +86,10 @@
// Destructor
Geom::~Geom()
{
+ World::Instance()->DisconnectShowPhysicsSignal(
boost::bind(&Geom::ShowPhysics, this, _1) );
+ World::Instance()->DisconnectShowJointsSignal(
boost::bind(&Geom::ShowJoints, this, _1) );
+ World::Instance()->DisconnectShowBoundingBoxesSignal(
boost::bind(&Geom::ShowBoundingBox, this, _1) );
+
for (std::vector<OgreVisual*>::iterator iter = this->visuals.begin(); iter
!= this->visuals.end(); iter++)
{
if (*iter)
@@ -194,6 +198,7 @@
{
this->bbVisual->SetCastShadows(false);
this->bbVisual->AttachBoundingBox(min,max);
+ this->bbVisual->SetVisible( World::Instance()->GetShowBoundingBoxes() );
}
}
}
@@ -260,6 +265,14 @@
// Update
void Geom::Update()
{
+ if (this->body && this->bbVisual)
+ {
+ if (this->body->GetEnabled())
+ this->bbVisual->SetBoundingBoxMaterial("Gazebo/GreenTransparent");
+ else
+ this->bbVisual->SetBoundingBoxMaterial("Gazebo/RedTransparent");
+ }
+
this->ClearContacts();
}
Modified: code/gazebo/trunk/server/physics/Geom.hh
===================================================================
--- code/gazebo/trunk/server/physics/Geom.hh 2010-03-31 22:43:07 UTC (rev
8604)
+++ code/gazebo/trunk/server/physics/Geom.hh 2010-04-02 15:06:14 UTC (rev
8605)
@@ -62,7 +62,7 @@
public: virtual ~Geom();
/// \brief Load the geom
- public: void Load(XMLConfigNode *node);
+ public: virtual void Load(XMLConfigNode *node);
/// \brief Load the geom
public: void Save(std::string &prefix, std::ostream &stream);
Modified: code/gazebo/trunk/server/physics/Joint.cc
===================================================================
--- code/gazebo/trunk/server/physics/Joint.cc 2010-03-31 22:43:07 UTC (rev
8604)
+++ code/gazebo/trunk/server/physics/Joint.cc 2010-04-02 15:06:14 UTC (rev
8605)
@@ -73,6 +73,9 @@
// Desctructor
Joint::~Joint()
{
+ World::Instance()->DisconnectShowJointsSignal(
+ boost::bind(&Joint::ShowJoints, this, _1) );
+
if (this->visual)
{
OgreCreator::Instance()->DeleteVisual( this->visual );
Modified: code/gazebo/trunk/server/physics/PhysicsEngine.cc
===================================================================
--- code/gazebo/trunk/server/physics/PhysicsEngine.cc 2010-03-31 22:43:07 UTC
(rev 8604)
+++ code/gazebo/trunk/server/physics/PhysicsEngine.cc 2010-04-02 15:06:14 UTC
(rev 8605)
@@ -86,6 +86,8 @@
// Destructor
PhysicsEngine::~PhysicsEngine()
{
+ World::Instance()->DisconnectShowContactsSignal(
boost::bind(&PhysicsEngine::ShowVisual, this, _1) );
+
if (this->visual)
{
OgreCreator::Instance()->DeleteVisual( this->visual );
Modified: code/gazebo/trunk/server/physics/bullet/BulletBody.hh
===================================================================
--- code/gazebo/trunk/server/physics/bullet/BulletBody.hh 2010-03-31
22:43:07 UTC (rev 8604)
+++ code/gazebo/trunk/server/physics/bullet/BulletBody.hh 2010-04-02
15:06:14 UTC (rev 8605)
@@ -85,6 +85,10 @@
/// \brief Set whether this body is enabled
public: virtual void SetEnabled(bool enable) const;
+ /// \brief Get whether this body is enabled in the physics engine
+ public: virtual bool GetEnabled() const {return true;}
+
+
/// \brief Update the center of mass
public: virtual void UpdateCoM();
Modified: code/gazebo/trunk/server/physics/ode/ODEBody.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEBody.cc 2010-03-31 22:43:07 UTC
(rev 8604)
+++ code/gazebo/trunk/server/physics/ode/ODEBody.cc 2010-04-02 15:06:14 UTC
(rev 8605)
@@ -55,6 +55,7 @@
{
this->bodyId = dBodyCreate(this->odePhysics->GetWorldId());
dBodySetData(this->bodyId, this);
+ dBodySetAutoDisableDefaults(this->bodyId);
}
else
this->bodyId = NULL;
@@ -78,11 +79,9 @@
// Update the Center of Mass.
this->UpdateCoM();
-
}
-
////////////////////////////////////////////////////////////////////////////////
// Init the ODE body
void ODEBody::Init()
@@ -312,6 +311,18 @@
}
/////////////////////////////////////////////////////////////////////
+/// Get whether this body is enabled in the physics engine
+bool ODEBody::GetEnabled() const
+{
+ bool result = false;
+
+ if (this->bodyId)
+ result = dBodyIsEnabled(this->bodyId);
+
+ return result;
+}
+
+/////////////////////////////////////////////////////////////////////
// Update the CoM and mass matrix
/*
What's going on here? In ODE the CoM of a body corresponds to the
Modified: code/gazebo/trunk/server/physics/ode/ODEBody.hh
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEBody.hh 2010-03-31 22:43:07 UTC
(rev 8604)
+++ code/gazebo/trunk/server/physics/ode/ODEBody.hh 2010-04-02 15:06:14 UTC
(rev 8605)
@@ -87,6 +87,9 @@
/// \brief Set whether this body is enabled
public: virtual void SetEnabled(bool enable) const;
+ /// \brief Get whether this body is enabled in the physics engine
+ public: virtual bool GetEnabled() const;
+
/// \brief Update the center of mass
public: virtual void UpdateCoM();
Modified: code/gazebo/trunk/server/physics/ode/ODEGeom.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEGeom.cc 2010-03-31 22:43:07 UTC
(rev 8604)
+++ code/gazebo/trunk/server/physics/ode/ODEGeom.cc 2010-04-02 15:06:14 UTC
(rev 8605)
@@ -67,7 +67,7 @@
{
Geom::Load(node);
- if (this->geomId && this->placeable)
+ /*if (this->geomId && this->placeable)
{
Pose3d localPose;
dQuaternion q;
@@ -85,8 +85,14 @@
dGeomSetOffsetPosition(this->geomId, localPose.pos.x, localPose.pos.y,
localPose.pos.z);
dGeomSetOffsetQuaternion(this->geomId, q);
+ }*/
+
+
+ if (this->IsStatic())
+ {
+ this->SetCategoryBits(GZ_FIXED_COLLIDE);
+ this->SetCollideBits(~GZ_FIXED_COLLIDE);
}
-
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2010-03-31 22:43:07 UTC
(rev 8604)
+++ code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2010-04-02 15:06:14 UTC
(rev 8605)
@@ -82,10 +82,10 @@
// If auto-disable is active, then user interaction with the joints
// doesn't behave properly
dWorldSetAutoDisableFlag(this->worldId, 1);
- dWorldSetAutoDisableTime(this->worldId, 2.0);
- dWorldSetAutoDisableLinearThreshold(this->worldId, 0.001);
- dWorldSetAutoDisableAngularThreshold(this->worldId, 0.001);
- dWorldSetAutoDisableSteps(this->worldId, 20);
+ dWorldSetAutoDisableTime(this->worldId, 1.0);
+ dWorldSetAutoDisableLinearThreshold(this->worldId, 0.01);
+ dWorldSetAutoDisableAngularThreshold(this->worldId, 0.01);
+ dWorldSetAutoDisableSteps(this->worldId, 10);
Param::Begin(&this->parameters);
this->globalCFMP = new ParamT<double>("cfm", 10e-5, 0);
@@ -196,15 +196,15 @@
// Update the ODE collisions, create joints
void ODEPhysics::UpdateCollision()
{
- //DiagnosticTimer timer("ODEPhysics Collision Update");
std::vector<ContactFeedback>::iterator iter;
std::vector<dJointFeedback>::iterator jiter;
- //timer.Start();
-
// Do collision detection; this will add contacts to the contact group
this->LockMutex();
- dSpaceCollide( this->spaceId, this, CollisionCallback );
+ {
+ //DiagnosticTimer timer("ODEPhysics Collision Update");
+ dSpaceCollide( this->spaceId, this, CollisionCallback );
+ }
this->UnlockMutex();
// Process all the contacts, get the feedback info, and call the geom
@@ -418,25 +418,16 @@
// Handle a collision
void ODEPhysics::CollisionCallback( void *data, dGeomID o1, dGeomID o2)
{
- int maxContacts = 10000;
- ODEPhysics *self;
- ODEGeom *geom1 = NULL;
- ODEGeom *geom2 = NULL;
- int i;
- int numc = 0;
- dContactGeom contactGeoms[maxContacts];
- dContact contact;
-
- self = (ODEPhysics*) data;
-
- // exit without doing anything if the two bodies are connected by a joint
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
-
+ // exit without doing anything if the two bodies are connected by a joint
if (b1 && b2 && dAreConnectedExcluding(b1,b2,dJointTypeContact))
return;
+ ODEPhysics *self;
+ self = (ODEPhysics*) data;
+
// Check if either are spaces
if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
{
@@ -444,9 +435,24 @@
}
else
{
- // We should never test two geoms in the same space
- assert(dGeomGetSpace(o1) != dGeomGetSpace(o2));
+ ODEGeom *geom1 = NULL;
+ ODEGeom *geom2 = NULL;
+ /*if (b1 && b2)
+ {
+ ODEBody *odeBody1 = (ODEBody*)dBodyGetData(b1);
+ ODEBody *odeBody2 = (ODEBody*)dBodyGetData(b1);
+ if (odeBody1->IsStatic() && odeBody2->IsStatic())
+ return;
+ }*/
+ // Exit if both bodies are not enabled
+ if ( (b1 && b2 && !dBodyIsEnabled(b1) && !dBodyIsEnabled(b2)) ||
+ (!b2 && b1 && !dBodyIsEnabled(b1)) ||
+ (!b1 && b2 && !dBodyIsEnabled(b2)) )
+ {
+ return;
+ }
+
// Get pointers to the underlying geoms
if (dGeomGetClass(o1) == dGeomTransformClass)
geom1 = (ODEGeom*) dGeomGetData(dGeomTransformGetGeom(o1));
@@ -458,12 +464,18 @@
else
geom2 = (ODEGeom*) dGeomGetData(o2);
+
+ int maxContacts = 1000;
int numContacts = 100;
+ int i;
+ int numc = 0;
+ dContact contact;
if (geom1->GetType() == Shape::TRIMESH && geom2->GetType()==Shape::TRIMESH)
numContacts = maxContacts;
- numc = dCollide(o1,o2,numContacts, contactGeoms, sizeof(contactGeoms[0]));
+ numc = dCollide(o1,o2,numContacts, self->contactGeoms,
+ sizeof(self->contactGeoms[0]));
if (numc != 0)
{
@@ -472,15 +484,14 @@
(*self->contactFeedbackIter).contact.geom2 = geom2;
(*self->contactFeedbackIter).feedbacks.resize(numc);
+ double h, kp, kd;
for (i=0; i<numc; i++)
{
- double h, kp, kd;
-
// skip negative depth contacts
- if(contactGeoms[i].depth < 0)
+ if(self->contactGeoms[i].depth < 0)
continue;
- contact.geom = contactGeoms[i];
+ contact.geom = self->contactGeoms[i];
//contact.surface.mode = dContactSlip1 | dContactSlip2 |
// dContactSoftERP | dContactSoftCFM |
// dContactBounce | dContactMu2 |
dContactApprox1;
Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.hh
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEPhysics.hh 2010-03-31 22:43:07 UTC
(rev 8604)
+++ code/gazebo/trunk/server/physics/ode/ODEPhysics.hh 2010-04-02 15:06:14 UTC
(rev 8605)
@@ -170,6 +170,8 @@
private: std::vector<ContactFeedback>::iterator contactFeedbackIter;
private: std::map<std::string, dSpaceID> spaces;
+
+ private: dContactGeom contactGeoms[1000];
};
/** \}*/
Modified: code/gazebo/trunk/server/rendering/OgreVisual.cc
===================================================================
--- code/gazebo/trunk/server/rendering/OgreVisual.cc 2010-03-31 22:43:07 UTC
(rev 8604)
+++ code/gazebo/trunk/server/rendering/OgreVisual.cc 2010-04-02 15:06:14 UTC
(rev 8605)
@@ -894,15 +894,46 @@
simple = dynamic_cast<Ogre::SimpleRenderable*>(odeObj);
if (ent)
- ent->setMaterialName("Gazebo/TransparentTest");
+ ent->setMaterialName("Gazebo/GreenTransparent");
else if (simple)
- simple->setMaterial("Gazebo/TransparentTest");
+ simple->setMaterial("Gazebo/GreenTransparent");
this->boundingBoxNode->setVisible(false);
}
////////////////////////////////////////////////////////////////////////////////
+// Set the material of the bounding box
+void OgreVisual::SetBoundingBoxMaterial(const std::string &materialName )
+{
+ boost::recursive_mutex::scoped_lock lock(*this->mutex);
+
+ // Stop here if the rendering engine has been disabled
+ if (!Simulator::Instance()->GetRenderEngineEnabled())
+ return;
+
+ if (materialName.empty())
+ return;
+
+ try
+ {
+ for (int i=0; i < this->boundingBoxNode->numAttachedObjects(); i++)
+ {
+ Ogre::MovableObject *obj = this->boundingBoxNode->getAttachedObject(i);
+
+ if (dynamic_cast<Ogre::Entity*>(obj))
+ ((Ogre::Entity*)obj)->setMaterialName(materialName);
+ else
+ ((Ogre::SimpleRenderable*)obj)->setMaterial(materialName);
+ }
+ }
+ catch (Ogre::Exception e)
+ {
+ gzmsg(0) << "Unable to set BoundingBoxMaterial[" << materialName << "]["
<< e.getFullDescription() << "]\n";
+ }
+}
+
+////////////////////////////////////////////////////////////////////////////////
/// Get the entity that manages this visual
Entity *OgreVisual::GetOwner() const
{
Modified: code/gazebo/trunk/server/rendering/OgreVisual.hh
===================================================================
--- code/gazebo/trunk/server/rendering/OgreVisual.hh 2010-03-31 22:43:07 UTC
(rev 8604)
+++ code/gazebo/trunk/server/rendering/OgreVisual.hh 2010-04-02 15:06:14 UTC
(rev 8605)
@@ -136,6 +136,9 @@
/// \brief Create a bounding box for this visual
public: void AttachBoundingBox(const Vector3 &min, const Vector3 &max);
+ /// \brief Set the material of the bounding box
+ public: void SetBoundingBoxMaterial(const std::string &materialName );
+
/// \brief Make the visual objects static renderables
public: void MakeStatic();
Modified: code/gazebo/trunk/worlds/empty.world
===================================================================
--- code/gazebo/trunk/worlds/empty.world 2010-03-31 22:43:07 UTC (rev
8604)
+++ code/gazebo/trunk/worlds/empty.world 2010-04-02 15:06:14 UTC (rev
8605)
@@ -17,13 +17,19 @@
<physics:ode>
<stepTime>0.001</stepTime>
<gravity>0 0 -9.8</gravity>
- <cfm>10e-2</cfm>
+ <cfm>0.0000000001</cfm>
<erp>0.2</erp>
+ <quickStep>true</quickStep>
+ <quickStepIters>10</quickStepIters>
+ <quickStepW>1.3</quickStepW>
+ <contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
+ <contactSurfaceLayer>0.001</contactSurfaceLayer>
+
<!-- updateRate: <0 == throttle simTime to match realTime.
0 == No throttling
>0 == Frequency at which to throttle the sim -->
- <updateRate>0</updateRate>
+ <!--<updateRate>0</updateRate>-->
</physics:ode>
<rendering:gui>
@@ -34,7 +40,7 @@
<rendering:ogre>
<ambient>1 1 1 1</ambient>
- <shadowTechnique>stencilAdditive</shadowTechnique>
+ <shadowTechnique>stencilModulative</shadowTechnique>
<grid>false</grid>
</rendering:ogre>
Modified: code/gazebo/trunk/worlds/map.world
===================================================================
--- code/gazebo/trunk/worlds/map.world 2010-03-31 22:43:07 UTC (rev 8604)
+++ code/gazebo/trunk/worlds/map.world 2010-04-02 15:06:14 UTC (rev 8605)
@@ -33,9 +33,6 @@
<rendering:ogre>
<ambient>0.5 0.5 0.5 1.0</ambient>
- <sky>
- <material>Gazebo/CloudySky</material>
- </sky>
<fog>
<color>1.0 1.0 1.0</color>
@@ -43,6 +40,8 @@
<linearEnd>100</linearEnd>
</fog>
<grid>false</grid>
+
+ <shadowTechnique>stencilModulative</shadowTechnique>
</rendering:ogre>
<!-- Ground Plane -->
@@ -68,7 +67,7 @@
<geom:map name="map_geom">
<image>willowMap.png</image>
<threshold>200</threshold>
- <granularity>2</granularity>
+ <granularity>10</granularity>
<negative>false</negative>
<scale>0.1</scale>
<material>Gazebo/Rocky</material>
@@ -76,6 +75,7 @@
</body:map>
</model:physical>
+ <!--
<model:physical name="pioneer2dx_model1">
<xyz>0 0 0.145</xyz>
<rpy>0.0 0.0 0.0</rpy>
@@ -97,6 +97,7 @@
<xi:include href="models/pioneer2dx.model" />
</include>
</model:physical>
+ -->
<!-- White Point light -->
<model:renderable name="point_white">
Modified: code/gazebo/trunk/worlds/models/sicklms200.model
===================================================================
--- code/gazebo/trunk/worlds/models/sicklms200.model 2010-03-31 22:43:07 UTC
(rev 8604)
+++ code/gazebo/trunk/worlds/models/sicklms200.model 2010-04-02 15:06:14 UTC
(rev 8605)
@@ -53,12 +53,10 @@
<maxRange>8</maxRange>
<resRange>.1</resRange>
- <!--
<controller:sicklms200_laser name="laser_controller_1">
<interface:laser name="laser_iface_0"/>
<interface:fiducial name="fiducial_iface_0"/>
</controller:sicklms200_laser>
- -->
</sensor:ray>
</body:box>
Modified: code/gazebo/trunk/worlds/simpleshapes.world
===================================================================
--- code/gazebo/trunk/worlds/simpleshapes.world 2010-03-31 22:43:07 UTC (rev
8604)
+++ code/gazebo/trunk/worlds/simpleshapes.world 2010-04-02 15:06:14 UTC (rev
8605)
@@ -14,7 +14,7 @@
<verbosity>5</verbosity>
- <physics:bullet>
+ <physics:ode>
<stepTime>0.001</stepTime>
<gravity>0 0 -9.8</gravity>
<cfm>10e-10</cfm>
@@ -29,7 +29,7 @@
0 == No throttling
>0 == Frequency at which to throttle the sim -->
<updateRate>0</updateRate>
- </physics:bullet>
+ </physics:ode>
<rendering:gui>
<type>fltk</type>
@@ -68,7 +68,7 @@
</model:physical>
<!-- Ground Plane -->
- <model:physical name="plane1_model">
+ <model:physical name="plane1_model">
<xyz>0 0 0</xyz>
<rpy>0 0 0</rpy>
<static>true</static>
@@ -97,7 +97,6 @@
<specularColor>.1 .1 .1</specularColor>
<range>20</range>
- <!-- Constant(0-1) Linear(0-1) Quadratic -->
<attenuation>0.1 0.01 0.001</attenuation>
</light>
</model:renderable>
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Download Intel® Parallel Studio Eval
Try the new software tools for yourself. Speed compiling, find bugs
proactively, and fine-tune applications for parallel performance.
See why Intel Parallel Studio got high marks during beta.
http://p.sf.net/sfu/intel-sw-dev
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
