Commits

Ian Chen committed c53d546

Support bullet shape resize

Comments (0)

Files changed (6)

gazebo/physics/Link.cc

 //////////////////////////////////////////////////
 void Link::Init()
 {
-  this->SetKinematic(this->sdf->GetValueBool("kinematic"));
-  this->SetGravityMode(this->sdf->GetValueBool("gravity"));
-
-  this->SetLinearDamping(this->GetLinearDamping());
-  this->SetAngularDamping(this->GetAngularDamping());
-
   this->linearAccel.Set(0, 0, 0);
   this->angularAccel.Set(0, 0, 0);
 

gazebo/physics/bullet/BulletBoxShape.hh

       /// \brief Destructor
       public: virtual ~BulletBoxShape() {}
 
-      /// \brief Load shape
-      /// \param[in] _sdf SDF description of shape geometry
-      public: void Load(sdf::ElementPtr _sdf)
+      /// \brief Set the size of the box
+      public: void SetSize(const math::Vector3 &_size)
               {
-                Base::Load(_sdf);
+                if (_size.x < 0 || _size.y < 0 || _size.z < 0)
+                {
+                    gzerr << "Box shape does not support negative"
+                          << " size\n";
+                    return;
+                }
+
+                BoxShape::SetSize(_size);
                 BulletCollisionPtr bParent;
                 bParent = boost::shared_dynamic_cast<BulletCollision>(
                     this->collisionParent);
 
-                math::Vector3 size = this->sdf->GetValueVector3("size");
-                bParent->SetCollisionShape(new btBoxShape(
-                    btVector3(size.x*0.5, size.y*0.5, size.z*0.5)));
-              }
-
-      /// \brief Set the size of the box
-      public: void SetSize(const math::Vector3 &_size)
-              {
-                BoxShape::SetSize(_size);
-                /*BulletCollisionPtr bParent;
-                bParent = boost::shared_dynamic_cast<BulletCollision>(
-                    this->collisionParent);
-
                 /// Bullet requires the half-extents of the box
-                bParent->SetCollisionShape(new btBoxShape(
-                    btVector3(_size.x*0.5, _size.y*0.5, _size.z*0.5)));*/
+                btCollisionShape *shape = bParent->GetCollisionShape();
+                if (!shape)
+                {
+                  bParent->SetCollisionShape(new btBoxShape(
+                      btVector3(_size.x*0.5, _size.y*0.5, _size.z*0.5)));
+                }
+                else
+                {
+                  btVector3 scale = shape->getLocalScaling();
+                  math::Vector3 boxSize = this->GetSize();
+                  if (boxSize.x > 0)
+                    scale.setX(_size.x / boxSize.x);
+                  if (boxSize.y > 0)
+                    scale.setY(_size.y / boxSize.y);
+                  if (boxSize.z > 0)
+                    scale.setZ(_size.z / boxSize.z);
+                  shape->setLocalScaling(scale);
+                }
               }
     };
     /// \}

gazebo/physics/bullet/BulletCylinderShape.hh

       public: virtual ~BulletCylinderShape() {}
 
       /// \brief Set the size of the cylinder
+      /// \param[in] _radius Cylinder radius
+      /// \param[in] _length Cylinder length
       public: void SetSize(double _radius, double _length)
               {
+                if (_radius < 0)
+                {
+                    gzerr << "Cylinder shape does not support negative"
+                          << " radius\n";
+                    return;
+                }
+                if (_length < 0)
+                {
+                    gzerr << "Cylinder shape does not support negative"
+                          << " length\n";
+                    return;
+                }
+
                 CylinderShape::SetSize(_radius, _length);
                 BulletCollisionPtr bParent;
                 bParent = boost::shared_dynamic_cast<BulletCollision>(
                     this->collisionParent);
 
-                bParent->SetCollisionShape(new btCylinderShapeZ(
-                    btVector3(_radius, _radius, _length * 0.5)));
+                btCollisionShape *shape = bParent->GetCollisionShape();
+                if (!shape)
+                {
+                  bParent->SetCollisionShape(new btCylinderShapeZ(
+                      btVector3(_radius, _radius, _length * 0.5)));
+                }
+                else
+                {
+                  btVector3 scale = shape->getLocalScaling();
+                  double cylinderRadius = this->GetRadius();
+                  double cylinderLength = this->GetLength();
+                  if (cylinderRadius > 0)
+                  {
+                    scale.setX(_radius / cylinderRadius);
+                    scale.setY(scale.x());
+                  }
+                  if (cylinderLength > 0)
+                    scale.setZ(_length / cylinderLength);
+                  shape->setLocalScaling(scale);
+                }
               }
     };
     /// \}

gazebo/physics/bullet/BulletLink.cc

 //////////////////////////////////////////////////
 void BulletLink::Init()
 {
+
+  Link::Init();
+
   GZ_ASSERT(this->inertial != NULL, "Inertial pointer is NULL");
   btScalar mass = this->inertial->GetMass();
   // The bullet dynamics solver checks for zero mass to identify static and
 
   btDynamicsWorld *wd = this->bulletPhysics->GetDynamicsWorld();
   wd->addRigidBody(this->rigidLink);
+  // this->rigidLink->setSleepingThresholds(0,0);
 
-  Link::Init();
-  // this->rigidLink->setSleepingThresholds(0,0);
+  this->SetKinematic(this->sdf->GetValueBool("kinematic"));
+  this->SetGravityMode(this->sdf->GetValueBool("gravity"));
+
+  this->SetLinearDamping(this->GetLinearDamping());
+  this->SetAngularDamping(this->GetAngularDamping());
 }
 
 //////////////////////////////////////////////////

gazebo/physics/bullet/BulletSphereShape.hh

       /// \brief Destructor
       public: virtual ~BulletSphereShape() {}
 
-      /// \brief Load shape
-      /// \param[in] _sdf SDF description of shape geometry
-      public: void Load(sdf::ElementPtr _sdf)
+      /// \brief Set the radius
+      /// \param[in] _radius Sphere radius
+      public: void SetRadius(double _radius)
               {
-                Base::Load(_sdf);
+                if (_radius < 0)
+                {
+                    gzerr << "Sphere shape does not support negative"
+                          << " radius\n";
+                    return;
+                }
+
+                SphereShape::SetRadius(_radius);
                 BulletCollisionPtr bParent;
                 bParent = boost::shared_dynamic_cast<BulletCollision>(
                     this->collisionParent);
 
-                bParent->SetCollisionShape(
-                    new btSphereShape(this->sdf->GetValueDouble("radius")));
-              }
-
-      /// \brief Set the radius
-      public: void SetRadius(double _radius)
-              {
-                SphereShape::SetRadius(_radius);
-                /*BulletCollisionPtr bParent;
-                bParent = boost::shared_dynamic_cast<BulletCollision>(
-                    this->collisionParent);
-
-                bParent->SetCollisionShape(new btSphereShape(_radius));*/
+                btCollisionShape *shape = bParent->GetCollisionShape();
+                if (!shape)
+                {
+                  bParent->SetCollisionShape(new btSphereShape(_radius));
+                }
+                else
+                {
+                  double sphereRadius = this->GetRadius();
+                  if (sphereRadius > 0)
+                  {
+                    double scale = _radius / sphereRadius;
+                    shape->setLocalScaling(btVector3(scale, scale, scale));
+                  }
+                }
               }
     };
     /// \}

gazebo/physics/ode/ODELink.cc

 //////////////////////////////////////////////////
 void ODELink::Init()
 {
+
+
   if (!this->IsStatic())
   {
     this->linkId = dBodyCreate(this->odePhysics->GetWorldId());
     }
   }
 
+  this->SetKinematic(this->sdf->GetValueBool("kinematic"));
+  this->SetGravityMode(this->sdf->GetValueBool("gravity"));
+
+  this->SetLinearDamping(this->GetLinearDamping());
+  this->SetAngularDamping(this->GetAngularDamping());
+
   Link::Init();
 
   if (this->linkId)