Use btStaticPlaneShape for CDynamics3DFloorPlugin

Testing shows that btStaticPlaneShape performs better (i.e., it is more stable at lower resolutions) than btBoxShape for the floor.
darkobozhinoski · May 6, 2019 · 835767c · 835767c
1 parent 58fb400
commit 835767c
Show file tree

Hide file tree

Showing 8 changed files with 12 additions and 30 deletions.
diff --git a/src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_floor_plugin.cpp b/src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_floor_plugin.cpp
@@ -12,26 +12,12 @@ namespace argos {
    /****************************************/
 
    void CDynamics3DFloorPlugin::Init(TConfigurationNode& t_tree) {
-      /* Get the floor height */
-      Real fHeight = 0.0f;
-      GetNodeAttributeOrDefault(t_tree, "height", fHeight, fHeight);
-      /* Get the arena center and size from the arena node */
-      CVector3 cArenaCenter(0.0f, 0.0f, 0.0f);
-      CVector3 cArenaSize(0.0f, 0.0f, 0.0f);
-      TConfigurationNode& tArena = GetNode(CSimulator::GetInstance().GetConfigurationRoot(), "arena");
-      GetNodeAttribute(tArena, "size", cArenaSize);
-      GetNodeAttributeOrDefault(tArena, "center", cArenaCenter, cArenaCenter);
-      /* Configure the floor */
-      m_cFloorExtents = btVector3(cArenaSize.GetX(), fHeight, cArenaSize.GetY());
-      m_cFloorOrigin = btVector3(cArenaCenter.GetX(), -fHeight * 0.5f, -cArenaCenter.GetY());
       /* Get the friction of the floor */
       m_fFriction = m_pcEngine->GetDefaultFriction();
       GetNodeAttributeOrDefault(t_tree, "friction", m_fFriction, m_fFriction);
-      /* Call the destructors for the floor components */
+      /* Call the destructor for the floor body */
       m_cFloor.~btRigidBody();
-      m_cFloorShape.~btBoxShape();
-      /* Call the constructors for the floor components */
-      new (&m_cFloorShape) btBoxShape(m_cFloorExtents * 0.5f);
+      /* Call the constructor for the floor body */
       btRigidBody::btRigidBodyConstructionInfo sConstructionInfo(0, nullptr, &m_cFloorShape);
       sConstructionInfo.m_friction = m_fFriction;
       /* Create the floor */
@@ -48,11 +34,9 @@ namespace argos {
    void CDynamics3DFloorPlugin::Reset() {
       /* Remove floor from world */
       m_pcEngine->GetWorld().removeRigidBody(&m_cFloor);
-      /* Call the destructors for the floor components */ 
+      /* Call the destructor for the floor body */ 
       m_cFloor.~btRigidBody();
-      m_cFloorShape.~btBoxShape();
-      /* Call the constructors for the floor components */
-      new (&m_cFloorShape) btBoxShape(m_cFloorExtents * 0.5f);
+      /* Call the constructor for the floor body */
       btRigidBody::btRigidBodyConstructionInfo sConstructionInfo(0, nullptr, &m_cFloorShape);
       sConstructionInfo.m_friction = m_fFriction;
       /* Create the floor */

diff --git a/src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_floor_plugin.h b/src/plugins/simulator/physics_engines/dynamics3d/dynamics3d_floor_plugin.h
@@ -19,9 +19,8 @@ namespace argos {
    class CDynamics3DFloorPlugin : public CDynamics3DPlugin {
    public:
       CDynamics3DFloorPlugin() :
-         m_cFloorExtents(0.0f, 0.0f, 0.0f),
          m_cFloorOrigin(0.0f, 0.0f, 0.0f),
-         m_cFloorShape(btVector3(0.0f, 0.0f, 0.0f)),
+         m_cFloorShape(btVector3(0.0f, 1.0f, 0.0f), 0.0f),
          m_cFloor(0.0f, nullptr, nullptr) {}
 
       ~CDynamics3DFloorPlugin() {}
@@ -41,9 +40,8 @@ namespace argos {
    private:
 
       btScalar m_fFriction;
-      btVector3 m_cFloorExtents;
       btVector3 m_cFloorOrigin;
-      btBoxShape m_cFloorShape;
+      btStaticPlaneShape m_cFloorShape;
       btRigidBody m_cFloor;
    };
 

diff --git a/src/testing/examples/dynamics3d_magnetism_test.argos b/src/testing/examples/dynamics3d_magnetism_test.argos
@@ -57,7 +57,7 @@
   <physics_engines>
     <dynamics3d id="dyn3d" iterations="20" default_friction="0.25">
       <gravity g="9.8" />
-      <floor height="0.01"/>
+      <floor />
       <magnetism max_distance="0.08"/>
     </dynamics3d>
   </physics_engines>

diff --git a/src/testing/examples/dynamics3d_multi_body_test.argos b/src/testing/examples/dynamics3d_multi_body_test.argos
@@ -91,7 +91,7 @@
   <!-- ******************* -->
   <physics_engines>
     <dynamics3d id="dyn3d" iterations="20">
-      <floor height="0.01"/>
+      <floor />
       <gravity g="9.8" />
     </dynamics3d>
   </physics_engines>

diff --git a/src/testing/examples/dynamics3d_single_body_test.argos b/src/testing/examples/dynamics3d_single_body_test.argos
@@ -50,7 +50,7 @@
   <!-- ******************* -->
   <physics_engines>
     <dynamics3d id="dyn3d" iterations="20">
-      <floor height="0.01"/>
+      <floor />
       <gravity g="9.8" />
     </dynamics3d>
   </physics_engines>

diff --git a/src/testing/examples/footbot_camera_prototype_example.argos b/src/testing/examples/footbot_camera_prototype_example.argos
@@ -90,7 +90,7 @@
     <dynamics2d id="dyn2d" iterations="20" />
     <dynamics3d id="dyn3d" iterations="20">
       <gravity g="9.8" />
-      <floor height="0.01"/>
+      <floor />
     </dynamics3d>
   </physics_engines>
 

diff --git a/src/testing/examples/prototype_radio_test.argos b/src/testing/examples/prototype_radio_test.argos
@@ -119,7 +119,7 @@
   <physics_engines>
     <dynamics3d id="dyn3d" iterations="20">
       <gravity g="9.8" />
-      <floor height="0.01"/>
+      <floor />
     </dynamics3d>
   </physics_engines>
 

diff --git a/src/testing/examples/prototype_srocs_system.argos b/src/testing/examples/prototype_srocs_system.argos
@@ -352,7 +352,7 @@
   <physics_engines>
     <dynamics3d id="dyn3d" iterations="20">
       <gravity g="9.8" />
-      <floor height="0.01"/>
+      <floor />
       <magnetism max_distance="0.04"/>
     </dynamics3d>
   </physics_engines>