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