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