##

BulletCylinderShape.hh
Go to the documentation of this file.
/*
* Copyright (C) 2012 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef GAZEBO_PHYSICS_BULLET_BULLETCYLINDERSHAPE_HH_
#define GAZEBO_PHYSICS_BULLET_BULLETCYLINDERSHAPE_HH_
#include "gazebo/physics/bullet/BulletPhysics.hh"
#include "gazebo/physics/bullet/BulletLink.hh"
#include "gazebo/physics/CylinderShape.hh"
#include "gazebo/util/system.hh"
namespace gazebo
{
namespace physics
{
class GZ_PHYSICS_VISIBLE BulletCylinderShape : public CylinderShape
{
public: explicit BulletCylinderShape(CollisionPtr _parent)
: CylinderShape(_parent) {}
public: virtual ~BulletCylinderShape() {}
public: void SetSize(double _radius, double _length)
{
if (_radius < 0)
{
gzerr << "Cylinder shape does not support negative radius\n";
return;
}
if (_length < 0)
{
gzerr << "Cylinder shape does not support negative length\n";
return;
}
if (ignition::math::equal(_radius, 0.0))
{
// Warn user, but still create shape with very small value
// otherwise later resize operations using setLocalScaling
// will not be possible
gzwarn << "Setting cylinder shape's radius to zero \n";
_radius = 1e-4;
}
if (ignition::math::equal(_length, 0.0))
{
gzwarn << "Setting cylinder shape's length to zero \n";
_length = 1e-4;
}
CylinderShape::SetSize(_radius, _length);
BulletCollisionPtr bParent;
bParent = boost::dynamic_pointer_cast<BulletCollision>(
this->collisionParent);
btCollisionShape *shape = bParent->GetCollisionShape();
if (!shape)
{
this->initialSize =
ignition::math::Vector3d(_radius, _radius, _length);
bParent->SetCollisionShape(new btCylinderShapeZ(
btVector3(_radius, _radius, _length * 0.5)));
}
else
{
btVector3 cylinderScale;
cylinderScale.setX(_radius / this->initialSize.X());
cylinderScale.setY(_radius / this->initialSize.Y());
cylinderScale.setZ(_length / this->initialSize.Z());
shape->setLocalScaling(cylinderScale);
// clear bullet cache and re-add the collision shape
// otherwise collisions won't work properly after scaling
BulletLinkPtr bLink =
boost::dynamic_pointer_cast<BulletLink>(
bParent->GetLink());
bLink->ClearCollisionCache();
// remove and add the shape again
if (bLink->GetBulletLink()->getCollisionShape()->isCompound())
{
btCompoundShape *compoundShape =
dynamic_cast<btCompoundShape *>(
bLink->GetBulletLink()->getCollisionShape());
compoundShape->removeChildShape(shape);
ignition::math::Pose3d relativePose =
this->collisionParent->RelativePose()
- bLink->GetInertial()->Pose();
compoundShape->addChildShape(
BulletTypes::ConvertPose(relativePose), shape);
}
}
}
private: ignition::math::Vector3d initialSize;
};
}
}
#endif
gazebo::physics::CylinderShape::SetSize
virtual void SetSize(double _radius, double _length)
Set the size of the cylinder.
gazebo::physics::BulletCylinderShape::BulletCylinderShape
BulletCylinderShape(CollisionPtr _parent)
Constructor.
Definition: BulletCylinderShape.hh:37
gazebo
Forward declarations for the common classes.
Definition: Animation.hh:26
gazebo::physics::BulletCollisionPtr
boost::shared_ptr< BulletCollision > BulletCollisionPtr
Definition: BulletTypes.hh:39
gazebo::physics::BulletLinkPtr
boost::shared_ptr< BulletLink > BulletLinkPtr
Definition: BulletTypes.hh:42
gazebo::physics::BulletTypes::ConvertPose
static btTransform ConvertPose(const ignition::math::Pose3d &_pose)
Convert an ignition math pose to a bullet transform.
Definition: BulletTypes.hh:111
gazebo::physics::CylinderShape
Cylinder collision.
Definition: CylinderShape.hh:32
gzwarn
#define gzwarn
Output a warning message.
Definition: Console.hh:47
system.hh
gzerr
#define gzerr
Output an error message.
Definition: Console.hh:50
gazebo::physics::BulletCylinderShape
Cylinder collision.
Definition: BulletCylinderShape.hh:34
gazebo::physics::Shape::collisionParent
CollisionPtr collisionParent
This shape's collision parent.
Definition: Shape.hh:73
BulletPhysics.hh
CylinderShape.hh
BulletLink.hh
gazebo::physics::BulletCylinderShape::~BulletCylinderShape
virtual ~BulletCylinderShape()
Destructor.
Definition: BulletCylinderShape.hh:41
gazebo::physics::CollisionPtr
boost::shared_ptr< Collision > CollisionPtr
Definition: PhysicsTypes.hh:113
gazebo::physics::BulletCylinderShape::SetSize
void SetSize(double _radius, double _length)
Set the size of the cylinder.
Definition: BulletCylinderShape.hh:46