BulletSphereShape.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 #ifndef GAZEBO_PHYSICS_BULLET_BULLETSPHERESHAPE_HH_
18 #define GAZEBO_PHYSICS_BULLET_BULLETSPHERESHAPE_HH_
19 
23 #include "gazebo/physics/World.hh"
25 #include "gazebo/util/system.hh"
26 
27 namespace gazebo
28 {
29  namespace physics
30  {
34 
36  class GZ_PHYSICS_VISIBLE BulletSphereShape : public SphereShape
37  {
39  public: explicit BulletSphereShape(CollisionPtr _parent)
40  : SphereShape(_parent) {}
41 
43  public: virtual ~BulletSphereShape() {}
44 
47  public: void SetRadius(double _radius)
48  {
49  if (_radius < 0)
50  {
51  gzerr << "Sphere shape does not support negative radius\n";
52  return;
53  }
54  if (ignition::math::equal(_radius, 0.0))
55  {
56  // Warn user, but still create shape with very small value
57  // otherwise later resize operations using setLocalScaling
58  // will not be possible
59  gzwarn << "Setting sphere shape's radius to zero \n";
60  _radius = 1e-4;
61  }
62 
63  SphereShape::SetRadius(_radius);
64  BulletCollisionPtr bParent;
65  bParent = boost::dynamic_pointer_cast<BulletCollision>(
66  this->collisionParent);
67 
68  btCollisionShape *shape = bParent->GetCollisionShape();
69  if (!shape)
70  {
71  this->initialSize =
72  ignition::math::Vector3d(_radius, _radius, _radius);
73  bParent->SetCollisionShape(new btSphereShape(_radius));
74  }
75  else
76  {
77  btVector3 sphereScale;
78  sphereScale.setX(_radius / this->initialSize.X());
79  sphereScale.setY(_radius / this->initialSize.Y());
80  sphereScale.setZ(_radius / this->initialSize.Z());
81 
82  shape->setLocalScaling(sphereScale);
83 
84  // clear bullet cache and re-add the collision shape
85  // otherwise collisions won't work properly after scaling
86  BulletLinkPtr bLink =
87  boost::dynamic_pointer_cast<BulletLink>(
88  bParent->GetLink());
89  bLink->ClearCollisionCache();
90 
91  // remove and add the shape again
92  if (bLink->GetBulletLink()->getCollisionShape()->isCompound())
93  {
94  btCompoundShape *compoundShape =
95  dynamic_cast<btCompoundShape *>(
96  bLink->GetBulletLink()->getCollisionShape());
97 
98  compoundShape->removeChildShape(shape);
99  ignition::math::Pose3d relativePose =
100  this->collisionParent->RelativePose()
101  - bLink->GetInertial()->Pose();
102  compoundShape->addChildShape(
103  BulletTypes::ConvertPose(relativePose), shape);
104  }
105  }
106  }
107 
109  private: ignition::math::Vector3d initialSize;
110  };
112  }
113 }
114 #endif
Bullet collisions.
Definition: BulletCollision.hh:43
static btTransform ConvertPose(const ignition::math::Pose3d &_pose)
Convert an ignition math pose to a bullet transform.
Definition: BulletTypes.hh:111
boost::shared_ptr< BulletCollision > BulletCollisionPtr
Definition: BulletTypes.hh:39
#define gzwarn
Output a warning message.
Definition: Console.hh:47
#define gzerr
Output an error message.
Definition: Console.hh:50
BulletSphereShape(CollisionPtr _parent)
Constructor.
Definition: BulletSphereShape.hh:39
virtual void SetRadius(double _radius)
Set the size.
boost::shared_ptr< BulletLink > BulletLinkPtr
Definition: BulletTypes.hh:42
virtual ~BulletSphereShape()
Destructor.
Definition: BulletSphereShape.hh:43
btCollisionShape * GetCollisionShape() const
Get the bullet collision shape.
void SetRadius(double _radius)
Set the radius.
Definition: BulletSphereShape.hh:47
Bullet sphere collision.
Definition: BulletSphereShape.hh:36
boost::shared_ptr< Collision > CollisionPtr
Definition: PhysicsTypes.hh:113
Sphere collision shape.
Definition: SphereShape.hh:39