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