BulletSphereShape.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2016 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 /* Desc: Sphere collisionetry
18  * Author: Nate Koenig
19  * Date: 21 May 2009
20  */
21 
22 #ifndef _BULLETSPHERESHAPE_HH_
23 #define _BULLETSPHERESHAPE_HH_
24 
28 #include "gazebo/physics/World.hh"
30 #include "gazebo/util/system.hh"
31 
32 namespace gazebo
33 {
34  namespace physics
35  {
39 
41  class GZ_PHYSICS_VISIBLE BulletSphereShape : public SphereShape
42  {
44  public: BulletSphereShape(CollisionPtr _parent) : SphereShape(_parent) {}
45 
47  public: virtual ~BulletSphereShape() {}
48 
51  public: void SetRadius(double _radius)
52  {
53  if (_radius < 0)
54  {
55  gzerr << "Sphere shape does not support negative radius\n";
56  return;
57  }
58  if (math::equal(_radius, 0.0))
59  {
60  // Warn user, but still create shape with very small value
61  // otherwise later resize operations using setLocalScaling
62  // will not be possible
63  gzwarn << "Setting sphere shape's radius to zero \n";
64  _radius = 1e-4;
65  }
66 
67  SphereShape::SetRadius(_radius);
68  BulletCollisionPtr bParent;
69  bParent = boost::dynamic_pointer_cast<BulletCollision>(
70  this->collisionParent);
71 
72  btCollisionShape *shape = bParent->GetCollisionShape();
73  if (!shape)
74  {
75  this->initialSize = math::Vector3(_radius, _radius, _radius);
76  bParent->SetCollisionShape(new btSphereShape(_radius));
77  }
78  else
79  {
80  btVector3 sphereScale;
81  sphereScale.setX(_radius / this->initialSize.x);
82  sphereScale.setY(_radius / this->initialSize.y);
83  sphereScale.setZ(_radius / this->initialSize.z);
84 
85  shape->setLocalScaling(sphereScale);
86 
87  // clear bullet cache and re-add the collision shape
88  // otherwise collisions won't work properly after scaling
89  BulletLinkPtr bLink =
90  boost::dynamic_pointer_cast<BulletLink>(
91  bParent->GetLink());
92  bLink->ClearCollisionCache();
93 
94  // remove and add the shape again
95  if (bLink->GetBulletLink()->getCollisionShape()->isCompound())
96  {
97  btCompoundShape *compoundShape =
98  dynamic_cast<btCompoundShape *>(
99  bLink->GetBulletLink()->getCollisionShape());
100 
101  compoundShape->removeChildShape(shape);
102  math::Pose relativePose =
103  this->collisionParent->GetRelativePose();
104  relativePose.pos -= bLink->GetInertial()->GetCoG();
105  compoundShape->addChildShape(
106  BulletTypes::ConvertPose(relativePose), shape);
107  }
108  }
109  }
110 
112  private: math::Vector3 initialSize;
113  };
115  }
116 }
117 #endif
Bullet collisions.
Definition: BulletCollision.hh:53
Encapsulates a position and rotation in three space.
Definition: Pose.hh:37
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:39
boost::shared_ptr< BulletCollision > BulletCollisionPtr
Definition: BulletTypes.hh:40
#define gzwarn
Output a warning message.
Definition: Console.hh:47
#define gzerr
Output an error message.
Definition: Console.hh:50
bool equal(const T &_a, const T &_b, const T &_epsilon=1e-6)
check if two values are equal, within a tolerance
Definition: Helpers.hh:171
BulletSphereShape(CollisionPtr _parent)
Constructor.
Definition: BulletSphereShape.hh:44
virtual void SetRadius(double _radius)
Set the size.
static math::Pose ConvertPose(const btTransform &_bt)
Convert a bullet transform to a gazebo pose.
Definition: BulletTypes.hh:92
boost::shared_ptr< BulletLink > BulletLinkPtr
Definition: BulletTypes.hh:43
virtual ~BulletSphereShape()
Destructor.
Definition: BulletSphereShape.hh:47
btCollisionShape * GetCollisionShape() const
Get the bullet collision shape.
Vector3 pos
The position.
Definition: Pose.hh:252
void SetRadius(double _radius)
Set the radius.
Definition: BulletSphereShape.hh:51
Bullet sphere collision.
Definition: BulletSphereShape.hh:41
boost::shared_ptr< Collision > CollisionPtr
Definition: PhysicsTypes.hh:113
Sphere collision shape.
Definition: SphereShape.hh:44