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
25namespace 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
Cylinder collision.
Definition BulletCylinderShape.hh:35
virtual ~BulletCylinderShape()
Destructor.
Definition BulletCylinderShape.hh:41
BulletCylinderShape(CollisionPtr _parent)
Constructor.
Definition BulletCylinderShape.hh:37
void SetSize(double _radius, double _length)
Set the size of the cylinder.
Definition BulletCylinderShape.hh:46
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:33
virtual void SetSize(double _radius, double _length)
Set the size of the cylinder.
CollisionPtr collisionParent
This shape's collision parent.
Definition Shape.hh:73
#define gzerr
Output an error message.
Definition Console.hh:50
#define gzwarn
Output a warning message.
Definition Console.hh:47
boost::shared_ptr< BulletCollision > BulletCollisionPtr
Definition BulletTypes.hh:41
boost::shared_ptr< BulletLink > BulletLinkPtr
Definition BulletTypes.hh:42
boost::shared_ptr< Collision > CollisionPtr
Definition PhysicsTypes.hh:113
Forward declarations for the common classes.
Definition Animation.hh:27