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: BulletSphereShape(CollisionPtr _parent) : SphereShape(_parent) {}
40 
42  public: virtual ~BulletSphereShape() {}
43 
46  public: void SetRadius(double _radius)
47  {
48  if (_radius < 0)
49  {
50  gzerr << "Sphere shape does not support negative radius\n";
51  return;
52  }
53  if (ignition::math::equal(_radius, 0.0))
54  {
55  // Warn user, but still create shape with very small value
56  // otherwise later resize operations using setLocalScaling
57  // will not be possible
58  gzwarn << "Setting sphere shape's radius to zero \n";
59  _radius = 1e-4;
60  }
61 
62  SphereShape::SetRadius(_radius);
63  BulletCollisionPtr bParent;
64  bParent = boost::dynamic_pointer_cast<BulletCollision>(
65  this->collisionParent);
66 
67  btCollisionShape *shape = bParent->GetCollisionShape();
68  if (!shape)
69  {
70  this->initialSize =
71  ignition::math::Vector3d(_radius, _radius, _radius);
72  bParent->SetCollisionShape(new btSphereShape(_radius));
73  }
74  else
75  {
76  btVector3 sphereScale;
77  sphereScale.setX(_radius / this->initialSize.X());
78  sphereScale.setY(_radius / this->initialSize.Y());
79  sphereScale.setZ(_radius / this->initialSize.Z());
80 
81  shape->setLocalScaling(sphereScale);
82 
83  // clear bullet cache and re-add the collision shape
84  // otherwise collisions won't work properly after scaling
85  BulletLinkPtr bLink =
86  boost::dynamic_pointer_cast<BulletLink>(
87  bParent->GetLink());
88  bLink->ClearCollisionCache();
89 
90  // remove and add the shape again
91  if (bLink->GetBulletLink()->getCollisionShape()->isCompound())
92  {
93  btCompoundShape *compoundShape =
94  dynamic_cast<btCompoundShape *>(
95  bLink->GetBulletLink()->getCollisionShape());
96 
97  compoundShape->removeChildShape(shape);
98  ignition::math::Pose3d relativePose =
99  this->collisionParent->RelativePose();
100  relativePose.Pos() -= bLink->GetInertial()->CoG();
101  compoundShape->addChildShape(
102  BulletTypes::ConvertPose(relativePose), shape);
103  }
104  }
105  }
106 
108  private: ignition::math::Vector3d initialSize;
109  };
111  }
112 }
113 #endif
Bullet collisions.
Definition: BulletCollision.hh:43
Forward declarations for the common classes.
Definition: Animation.hh:33
boost::shared_ptr< BulletCollision > BulletCollisionPtr
Definition: BulletTypes.hh:42
#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:45
virtual ~BulletSphereShape()
Destructor.
Definition: BulletSphereShape.hh:42
btCollisionShape * GetCollisionShape() const
Get the bullet collision shape.
static math::Pose ConvertPose(const btTransform &_bt) GAZEBO_DEPRECATED(8.0)
Convert a bullet transform to a gazebo pose.
Definition: BulletTypes.hh:183
void SetRadius(double _radius)
Set the radius.
Definition: BulletSphereShape.hh:46
Bullet sphere collision.
Definition: BulletSphereShape.hh:36
boost::shared_ptr< Collision > CollisionPtr
Definition: PhysicsTypes.hh:113
Sphere collision shape.
Definition: SphereShape.hh:39