BulletBoxShape.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_BULLETBOXSHAPE_HH_
18 #define GAZEBO_PHYSICS_BULLET_BULLETBOXSHAPE_HH_
19 
24 #include "gazebo/physics/World.hh"
26 #include "gazebo/util/system.hh"
27 
28 namespace gazebo
29 {
30  namespace physics
31  {
35 
37  class GZ_PHYSICS_VISIBLE BulletBoxShape : public BoxShape
38  {
40  public: explicit BulletBoxShape(CollisionPtr _parent)
41  : BoxShape(_parent) {}
42 
44  public: virtual ~BulletBoxShape() {}
45 
47  public: void SetSize(const ignition::math::Vector3d &_size)
48  {
49  if (_size.X() < 0 || _size.Y() < 0 || _size.Z() < 0)
50  {
51  gzerr << "Box shape does not support negative size\n";
52  return;
53  }
54  ignition::math::Vector3d size = _size;
55  if (ignition::math::equal(size.X(), 0.0))
56  {
57  // Warn user, but still create shape with very small value
58  // otherwise later resize operations using setLocalScaling
59  // will not be possible
60  gzwarn << "Setting box shape's x to zero \n";
61  size.X() = 1e-4;
62  }
63  if (ignition::math::equal(size.Y(), 0.0))
64  {
65  gzwarn << "Setting box shape's y to zero \n";
66  size.Y() = 1e-4;
67  }
68  if (ignition::math::equal(size.Z(), 0.0))
69  {
70  gzwarn << "Setting box shape's z to zero \n";
71  size.Z() = 1e-4;
72  }
73 
74  BoxShape::SetSize(size);
75  BulletCollisionPtr bParent;
76  bParent = boost::dynamic_pointer_cast<BulletCollision>(
77  this->collisionParent);
78 
80  btCollisionShape *shape = bParent->GetCollisionShape();
81  if (!shape)
82  {
83  this->initialSize = size;
84  bParent->SetCollisionShape(new btBoxShape(
85  btVector3(size.X()*0.5, size.Y()*0.5, size.Z()*0.5)));
86  }
87  else
88  {
89  btVector3 boxScale = shape->getLocalScaling();
90  boxScale.setX(size.X() / this->initialSize.X());
91  boxScale.setY(size.Y() / this->initialSize.Y());
92  boxScale.setZ(size.Z() / this->initialSize.Z());
93 
94  shape->setLocalScaling(boxScale);
95 
96  // clear bullet cache and re-add the collision shape
97  // otherwise collisions won't work properly after scaling
98  BulletLinkPtr bLink =
99  boost::dynamic_pointer_cast<BulletLink>(
100  bParent->GetLink());
101  bLink->ClearCollisionCache();
102 
103  // remove and add the shape again
104  if (bLink->GetBulletLink()->getCollisionShape()->isCompound())
105  {
106  btCompoundShape *compoundShape =
107  dynamic_cast<btCompoundShape *>(
108  bLink->GetBulletLink()->getCollisionShape());
109 
110  compoundShape->removeChildShape(shape);
111  ignition::math::Pose3d relativePose =
112  this->collisionParent->RelativePose()
113  - bLink->GetInertial()->Pose();
114  compoundShape->addChildShape(
115  BulletTypes::ConvertPose(relativePose), shape);
116  }
117  }
118  }
119 
121  private: ignition::math::Vector3d initialSize;
122  };
124  }
125 }
126 #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
BulletBoxShape(CollisionPtr _parent)
Constructor.
Definition: BulletBoxShape.hh:40
boost::shared_ptr< BulletCollision > BulletCollisionPtr
Definition: BulletTypes.hh:39
#define gzwarn
Output a warning message.
Definition: Console.hh:47
#define gzerr
Output an error message.
Definition: Console.hh:50
Bullet wrapper forward declarations and typedefs.
boost::shared_ptr< BulletLink > BulletLinkPtr
Definition: BulletTypes.hh:42
virtual ~BulletBoxShape()
Destructor.
Definition: BulletBoxShape.hh:44
void SetSize(const ignition::math::Vector3d &_size)
Set the size of the box.
Definition: BulletBoxShape.hh:47
Box geometry primitive.
Definition: BoxShape.hh:34
btCollisionShape * GetCollisionShape() const
Get the bullet collision shape.
virtual void SetSize(const ignition::math::Vector3d &_size)
Set the size of the box.
Bullet box collision.
Definition: BulletBoxShape.hh:37
boost::shared_ptr< Collision > CollisionPtr
Definition: PhysicsTypes.hh:113