BulletCylinderShape.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: Cylinder shape
18  * Author: Nate Koenig
19  * Date: 14 Oct 2009
20  */
21 
22 #ifndef _BULLETCYLINDERSHAPE_HH_
23 #define _BULLETCYLINDERSHAPE_HH_
24 
28 #include "gazebo/util/system.hh"
29 
30 namespace gazebo
31 {
32  namespace physics
33  {
37 
39  class GZ_PHYSICS_VISIBLE BulletCylinderShape : public CylinderShape
40  {
43  : CylinderShape(_parent) {}
44 
46  public: virtual ~BulletCylinderShape() {}
47 
51  public: void SetSize(double _radius, double _length)
52  {
53  if (_radius < 0)
54  {
55  gzerr << "Cylinder shape does not support negative radius\n";
56  return;
57  }
58  if (_length < 0)
59  {
60  gzerr << "Cylinder shape does not support negative length\n";
61  return;
62  }
63  if (math::equal(_radius, 0.0))
64  {
65  // Warn user, but still create shape with very small value
66  // otherwise later resize operations using setLocalScaling
67  // will not be possible
68  gzwarn << "Setting cylinder shape's radius to zero \n";
69  _radius = 1e-4;
70  }
71  if (math::equal(_length, 0.0))
72  {
73  gzwarn << "Setting cylinder shape's length to zero \n";
74  _length = 1e-4;
75  }
76 
77  CylinderShape::SetSize(_radius, _length);
78  BulletCollisionPtr bParent;
79  bParent = boost::dynamic_pointer_cast<BulletCollision>(
80  this->collisionParent);
81 
82  btCollisionShape *shape = bParent->GetCollisionShape();
83  if (!shape)
84  {
85  this->initialSize = math::Vector3(_radius, _radius, _length);
86  bParent->SetCollisionShape(new btCylinderShapeZ(
87  btVector3(_radius, _radius, _length * 0.5)));
88  }
89  else
90  {
91  btVector3 cylinderScale;
92  cylinderScale.setX(_radius / this->initialSize.x);
93  cylinderScale.setY(_radius / this->initialSize.y);
94  cylinderScale.setZ(_length / this->initialSize.z);
95 
96  shape->setLocalScaling(cylinderScale);
97 
98  // clear bullet cache and re-add the collision shape
99  // otherwise collisions won't work properly after scaling
100  BulletLinkPtr bLink =
101  boost::dynamic_pointer_cast<BulletLink>(
102  bParent->GetLink());
103  bLink->ClearCollisionCache();
104 
105  // remove and add the shape again
106  if (bLink->GetBulletLink()->getCollisionShape()->isCompound())
107  {
108  btCompoundShape *compoundShape =
109  dynamic_cast<btCompoundShape *>(
110  bLink->GetBulletLink()->getCollisionShape());
111 
112  compoundShape->removeChildShape(shape);
113  math::Pose relativePose =
114  this->collisionParent->GetRelativePose();
115  relativePose.pos -= bLink->GetInertial()->GetCoG();
116  compoundShape->addChildShape(
117  BulletTypes::ConvertPose(relativePose), shape);
118  }
119  }
120  }
121 
123  private: math::Vector3 initialSize;
124  };
126  }
127 }
128 #endif
Bullet collisions.
Definition: BulletCollision.hh:53
Forward declarations for the common classes.
Definition: Animation.hh:33
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
Cylinder collision.
Definition: CylinderShape.hh:43
boost::shared_ptr< BulletCollision > BulletCollisionPtr
Definition: BulletTypes.hh:40
Cylinder collision.
Definition: BulletCylinderShape.hh:39
#define gzwarn
Output a warning message.
Definition: Console.hh:47
btCollisionShape * GetCollisionShape() const
Get the bullet collision shape.
#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
virtual ~BulletCylinderShape()
Destructor.
Definition: BulletCylinderShape.hh:46
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 void SetSize(double _radius, double _length)
Set the size of the cylinder.
Vector3 pos
The position.
Definition: Pose.hh:252
void SetSize(double _radius, double _length)
Set the size of the cylinder.
Definition: BulletCylinderShape.hh:51
BulletCylinderShape(CollisionPtr _parent)
Constructor.
Definition: BulletCylinderShape.hh:42
boost::shared_ptr< Collision > CollisionPtr
Definition: PhysicsTypes.hh:104