BulletBoxShape.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: Box shape
18  * Author: Nate Koenig
19  * Date: 14 Oct 2009
20  */
21 
22 #ifndef _BULLETBOXSHAPE_HH_
23 #define _BULLETBOXSHAPE_HH_
24 
26 #include "gazebo/physics/World.hh"
28 #include "gazebo/util/system.hh"
29 
30 namespace gazebo
31 {
32  namespace physics
33  {
37 
40  {
42  public: BulletBoxShape(CollisionPtr _parent) : BoxShape(_parent) {}
43 
45  public: virtual ~BulletBoxShape() {}
46 
48  public: void SetSize(const math::Vector3 &_size)
49  {
50  if (_size.x < 0 || _size.y < 0 || _size.z < 0)
51  {
52  gzerr << "Box shape does not support negative size\n";
53  return;
54  }
55  math::Vector3 size = _size;
56  if (math::equal(size.x, 0.0))
57  {
58  // Warn user, but still create shape with very small value
59  // otherwise later resize operations using setLocalScaling
60  // will not be possible
61  gzwarn << "Setting box shape's x to zero \n";
62  size.x = 1e-4;
63  }
64  if (math::equal(size.y, 0.0))
65  {
66  gzwarn << "Setting box shape's y to zero \n";
67  size.y = 1e-4;
68  }
69  if (math::equal(size.z, 0.0))
70  {
71  gzwarn << "Setting box shape's z to zero \n";
72  size.z = 1e-4;
73  }
74 
75  BoxShape::SetSize(size);
76  BulletCollisionPtr bParent;
77  bParent = boost::dynamic_pointer_cast<BulletCollision>(
78  this->collisionParent);
79 
81  btCollisionShape *shape = bParent->GetCollisionShape();
82  if (!shape)
83  {
84  this->initialSize = size;
85  bParent->SetCollisionShape(new btBoxShape(
86  btVector3(size.x*0.5, size.y*0.5, size.z*0.5)));
87  }
88  else
89  {
90  btVector3 boxScale = shape->getLocalScaling();
91  boxScale.setX(size.x / this->initialSize.x);
92  boxScale.setY(size.y / this->initialSize.y);
93  boxScale.setZ(size.z / this->initialSize.z);
94 
95  shape->setLocalScaling(boxScale);
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
virtual ~BulletBoxShape()
Destructor.
Definition: BulletBoxShape.hh:45
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
btCollisionShape * GetCollisionShape() const
Get the bullet collision shape.
virtual void SetSize(const math::Vector3 &_size)
Set the size of the box.
double x
X location.
Definition: Vector3.hh:302
#define gzwarn
Output a warning message.
Definition: Console.hh:46
double z
Z location.
Definition: Vector3.hh:308
#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
void SetSize(const math::Vector3 &_size)
Set the size of the box.
Definition: BulletBoxShape.hh:48
Box geometry primitive.
Definition: BoxShape.hh:37
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
Bullet box collision.
Definition: BulletBoxShape.hh:39
BulletBoxShape(CollisionPtr _parent)
Constructor.
Definition: BulletBoxShape.hh:42
#define GAZEBO_VISIBLE
Use to represent "symbol visible" if supported.
Definition: system.hh:48
double y
Y location.
Definition: Vector3.hh:305