BulletCylinderShape.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_BULLETCYLINDERSHAPE_HH_
18 #define GAZEBO_PHYSICS_BULLET_BULLETCYLINDERSHAPE_HH_
19 
23 #include "gazebo/util/system.hh"
24 
25 namespace gazebo
26 {
27  namespace physics
28  {
32 
34  class GZ_PHYSICS_VISIBLE BulletCylinderShape : public CylinderShape
35  {
37  public: explicit BulletCylinderShape(CollisionPtr _parent)
38  : CylinderShape(_parent) {}
39 
41  public: virtual ~BulletCylinderShape() {}
42 
46  public: void SetSize(double _radius, double _length)
47  {
48  if (_radius < 0)
49  {
50  gzerr << "Cylinder shape does not support negative radius\n";
51  return;
52  }
53  if (_length < 0)
54  {
55  gzerr << "Cylinder shape does not support negative length\n";
56  return;
57  }
58  if (ignition::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 cylinder shape's radius to zero \n";
64  _radius = 1e-4;
65  }
66  if (ignition::math::equal(_length, 0.0))
67  {
68  gzwarn << "Setting cylinder shape's length to zero \n";
69  _length = 1e-4;
70  }
71 
72  CylinderShape::SetSize(_radius, _length);
73  BulletCollisionPtr bParent;
74  bParent = boost::dynamic_pointer_cast<BulletCollision>(
75  this->collisionParent);
76 
77  btCollisionShape *shape = bParent->GetCollisionShape();
78  if (!shape)
79  {
80  this->initialSize =
81  ignition::math::Vector3d(_radius, _radius, _length);
82  bParent->SetCollisionShape(new btCylinderShapeZ(
83  btVector3(_radius, _radius, _length * 0.5)));
84  }
85  else
86  {
87  btVector3 cylinderScale;
88  cylinderScale.setX(_radius / this->initialSize.X());
89  cylinderScale.setY(_radius / this->initialSize.Y());
90  cylinderScale.setZ(_length / this->initialSize.Z());
91 
92  shape->setLocalScaling(cylinderScale);
93 
94  // clear bullet cache and re-add the collision shape
95  // otherwise collisions won't work properly after scaling
96  BulletLinkPtr bLink =
97  boost::dynamic_pointer_cast<BulletLink>(
98  bParent->GetLink());
99  bLink->ClearCollisionCache();
100 
101  // remove and add the shape again
102  if (bLink->GetBulletLink()->getCollisionShape()->isCompound())
103  {
104  btCompoundShape *compoundShape =
105  dynamic_cast<btCompoundShape *>(
106  bLink->GetBulletLink()->getCollisionShape());
107 
108  compoundShape->removeChildShape(shape);
109  ignition::math::Pose3d relativePose =
110  this->collisionParent->RelativePose()
111  - bLink->GetInertial()->Pose();
112  compoundShape->addChildShape(
113  BulletTypes::ConvertPose(relativePose), shape);
114  }
115  }
116  }
117 
119  private: ignition::math::Vector3d initialSize;
120  };
122  }
123 }
124 #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
Cylinder collision.
Definition: CylinderShape.hh:38
boost::shared_ptr< BulletCollision > BulletCollisionPtr
Definition: BulletTypes.hh:39
Cylinder collision.
Definition: BulletCylinderShape.hh:34
#define gzwarn
Output a warning message.
Definition: Console.hh:47
#define gzerr
Output an error message.
Definition: Console.hh:50
virtual ~BulletCylinderShape()
Destructor.
Definition: BulletCylinderShape.hh:41
boost::shared_ptr< BulletLink > BulletLinkPtr
Definition: BulletTypes.hh:42
virtual void SetSize(double _radius, double _length)
Set the size of the cylinder.
btCollisionShape * GetCollisionShape() const
Get the bullet collision shape.
void SetSize(double _radius, double _length)
Set the size of the cylinder.
Definition: BulletCylinderShape.hh:46
BulletCylinderShape(CollisionPtr _parent)
Constructor.
Definition: BulletCylinderShape.hh:37
boost::shared_ptr< Collision > CollisionPtr
Definition: PhysicsTypes.hh:113