BulletElement::BulletElement(const Matrix4d& T_elem_to_link, Shape shape,
const vector<double>& params)
: T_elem_to_link(T_elem_to_link),shape(shape)
{
//DEBUG
//std::cout << "BulletElement::BulletElement: START" << std::endl;
//END_DEBUG
btCollisionShape* bt_shape;
switch (shape) {
case BOX:
//DEBUG
//std::cout << "BulletElement::BulletElement: Create BOX ..." << std::endl;
//END_DEBUG
bt_shape = new btBoxShape( btVector3(params[0]/2,params[1]/2,params[2]/2) );
bt_shape->setMargin(0.0);
//DEBUG
//std::cout << "BulletElement::BulletElement: Created BOX" << std::endl;
//END_DEBUG
break;
case SPHERE:
if (true || params[0] != 0) {
//DEBUG
//std::cout << "BulletElement::BulletElement: Create SPHERE ..." << std::endl;
//END_DEBUG
bt_shape = new btSphereShape(params[0]) ;
//DEBUG
//std::cout << "BulletElement::BulletElement: Created SPHERE" << std::endl;
//END_DEBUG
} else {
//DEBUG
//std::cout << "BulletElement::BulletElement: THROW" << std::endl;
//END_DEBUG
throw zeroRadiusSphereException();
}
break;
case CYLINDER:
//DEBUG
//std::cout << "BulletElement::BulletElement: Create CYLINDER ..." << std::endl;
//END_DEBUG
bt_shape = new btCylinderShapeZ( btVector3(params[0],params[0],params[1]/2) );
//DEBUG
//std::cout << "BulletElement::BulletElement: Created CYLINDER ..." << std::endl;
//END_DEBUG
break;
case MESH:
//DEBUG
//std::cout << "BulletElement::BulletElement: Create MESH ..." << std::endl;
//END_DEBUG
//bt_shape = new btConvexHullShape( (btScalar*) params.data(),
//params.size()/3,
//(int) 3*sizeof(double) );
bt_shape = new btConvexHullShape();
bt_shape->setMargin(0.05);
for (int i=0; i<params.size(); i+=3){
//DEBUG
//std::cout << "BulletElement::BulletElement: Adding point " << i/3 + 1 << std::endl;
//END_DEBUG
dynamic_cast<btConvexHullShape*>(bt_shape)->addPoint(btVector3(params[i],params[i+1],params[i+2]));
}
//DEBUG
//std::cout << "BulletElement::BulletElement: Created MESH ..." << std::endl;
//END_DEBUG
break;
case CAPSULE:
bt_shape = new btConvexHullShape();
dynamic_cast<btConvexHullShape*>(bt_shape)->addPoint(btVector3(0,0,-params[1]/2));
dynamic_cast<btConvexHullShape*>(bt_shape)->addPoint(btVector3(0,0,params[1]/2));
bt_shape->setMargin(params[0]);
break;
default:
cerr << "Warning: Collision element has an unknown type " << shape << endl;
//DEBUG
//std::cout << "BulletElement::BulletElement: THROW" << std::endl;
//END_DEBUG
throw unknownShapeException(shape);
break;
}
//DEBUG
//cout << "BulletElement::BulletElement: Creating btCollisionObject" << endl;
//END_DEBUG
bt_obj = make_shared<btCollisionObject>();
//DEBUG
//cout << "BulletElement::BulletElement: Setting bt_shape for bt_ob" << endl;
//END_DEBUG
bt_obj->setCollisionShape(bt_shape);
//DEBUG
//cout << "BulletElement::BulletElement: Setting world transform for bt_ob" << endl;
//END_DEBUG
setWorldTransform(Matrix4d::Identity());
//DEBUG
//cout << "BulletElement::BulletElement: END" << std::endl;
//END_DEBUG
}
ElementId BulletModel::addElement(const Element& element)
{
ElementId id = Model::addElement(element);
if (id != 0) {
unique_ptr<btCollisionShape> bt_shape;
unique_ptr<btCollisionShape> bt_shape_no_margin;
switch (elements[id]->getShape()) {
case DrakeShapes::BOX:
{
const auto box = static_cast<const DrakeShapes::Box&>(elements[id]->getGeometry());
bt_shape = newBulletBoxShape(box, true);
bt_shape_no_margin = newBulletBoxShape(box, false);
}
break;
case DrakeShapes::SPHERE:
{
const auto sphere = static_cast<const DrakeShapes::Sphere&>(elements[id]->getGeometry());
bt_shape = newBulletSphereShape(sphere, true);
bt_shape_no_margin = newBulletSphereShape(sphere, false);
}
break;
case DrakeShapes::CYLINDER:
{
const auto cylinder = static_cast<const DrakeShapes::Cylinder&>(elements[id]->getGeometry());
bt_shape = newBulletCylinderShape(cylinder, true);
bt_shape_no_margin = newBulletCylinderShape(cylinder, false);
}
break;
case DrakeShapes::MESH:
{
const auto mesh = static_cast<const DrakeShapes::Mesh&>(elements[id]->getGeometry());
bt_shape = newBulletMeshShape(mesh, true);
bt_shape_no_margin = newBulletMeshShape(mesh, false);
}
break;
case DrakeShapes::MESH_POINTS:
{
const auto mesh = static_cast<const DrakeShapes::MeshPoints&>(elements[id]->getGeometry());
bt_shape = newBulletMeshPointsShape(mesh, true);
bt_shape_no_margin = newBulletMeshPointsShape(mesh, false);
}
break;
case DrakeShapes::CAPSULE:
{
const auto capsule = static_cast<const DrakeShapes::Capsule&>(elements[id]->getGeometry());
bt_shape = newBulletCapsuleShape(capsule, true);
bt_shape_no_margin = newBulletCapsuleShape(capsule, false);
}
break;
default:
cerr << "Warning: Collision elements[id] has an unknown type " << elements[id]->getShape() << endl;
throw unknownShapeException(elements[id]->getShape());
break;
}
if (bt_shape) {
// Create the collision objects
unique_ptr<btCollisionObject> bt_obj(new btCollisionObject());
unique_ptr<btCollisionObject> bt_obj_no_margin(new btCollisionObject());
bt_obj->setCollisionShape(bt_shape.get());
bt_obj_no_margin->setCollisionShape(bt_shape_no_margin.get());
bt_obj->setUserPointer(elements[id].get());
bt_obj_no_margin->setUserPointer(elements[id].get());
// Add the collision objects to the collision worlds
bullet_world.bt_collision_world->addCollisionObject(bt_obj.get());
bullet_world_no_margin.bt_collision_world->addCollisionObject(bt_obj_no_margin.get());
if (elements[id]->isStatic()) {
bt_obj->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
bt_obj->activate();
bt_obj_no_margin->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
bt_obj_no_margin->activate();
} else {
bt_obj->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);
bt_obj_no_margin->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);
}
// Store the Bullet collision objects
bullet_world.bt_collision_objects.insert(make_pair(id, move(bt_obj)));
bullet_world_no_margin.bt_collision_objects.insert(make_pair(id, move(bt_obj_no_margin)));
// Store the Bullet collision shapes too, because Bullet does no cleanup
bt_collision_shapes.push_back(move(bt_shape));
bt_collision_shapes.push_back(move(bt_shape_no_margin));
}
}
return id;
}
BulletElement::BulletElement(const Matrix4d& T_elem_to_link, Shape shape,
const vector<double>& params,
const string& group_name,
bool use_margins)
: T_elem_to_link(T_elem_to_link),shape(shape)
{
setGroupName(group_name);
//DEBUG
//std::cout << "BulletElement::BulletElement: START" << std::endl;
//END_DEBUG
btCollisionShape* bt_shape;
double small_margin = 1e-9;
switch (shape) {
case BOX:
{
//DEBUG
//std::cout << "BulletElement::BulletElement: Create BOX ..." << std::endl;
//END_DEBUG
btBoxShape bt_box( btVector3(params[0]/2,params[1]/2,params[2]/2) );
/* Strange things happen to the collision-normals when we use the
* convex interface to the btBoxShape. Instead, we'll explicitly create
* a btConvexHullShape.
*/
bt_shape = new btConvexHullShape();
if (use_margins)
bt_shape->setMargin(0.05);
else
bt_shape->setMargin(small_margin);
for (int i=0; i<8; ++i){
btVector3 vtx;
bt_box.getVertex(i,vtx);
dynamic_cast<btConvexHullShape*>(bt_shape)->addPoint(vtx);
}
//DEBUG
//std::cout << "BulletElement::BulletElement: Created BOX" << std::endl;
//END_DEBUG
}
break;
case SPHERE:
if (true || params[0] != 0) {
//DEBUG
//std::cout << "BulletElement::BulletElement: Create SPHERE ..." << std::endl;
//END_DEBUG
bt_shape = new btSphereShape(params[0]) ;
//DEBUG
//std::cout << "BulletElement::BulletElement: Created SPHERE" << std::endl;
//END_DEBUG
} else {
//DEBUG
//std::cout << "BulletElement::BulletElement: THROW" << std::endl;
//END_DEBUG
throw zeroRadiusSphereException();
}
break;
case CYLINDER:
//DEBUG
//std::cout << "BulletElement::BulletElement: Create CYLINDER ..." << std::endl;
//END_DEBUG
bt_shape = new btCylinderShapeZ( btVector3(params[0],params[0],params[1]/2) );
//DEBUG
//std::cout << "BulletElement::BulletElement: Created CYLINDER ..." << std::endl;
//END_DEBUG
break;
case MESH:
//DEBUG
//std::cout << "BulletElement::BulletElement: Create MESH ..." << std::endl;
//END_DEBUG
//bt_shape = new btConvexHullShape( (btScalar*) params.data(),
//params.size()/3,
//(int) 3*sizeof(double) );
bt_shape = new btConvexHullShape();
if (use_margins)
bt_shape->setMargin(0.05);
else
bt_shape->setMargin(small_margin);
for (int i=0; i<params.size(); i+=3){
//DEBUG
//std::cout << "BulletElement::BulletElement: Adding point " << i/3 + 1 << std::endl;
//END_DEBUG
dynamic_cast<btConvexHullShape*>(bt_shape)->addPoint(btVector3(params[i],params[i+1],params[i+2]));
}
//DEBUG
//std::cout << "BulletElement::BulletElement: Created MESH ..." << std::endl;
//END_DEBUG
break;
case CAPSULE:
bt_shape = new btConvexHullShape();
dynamic_cast<btConvexHullShape*>(bt_shape)->addPoint(btVector3(0,0,-params[1]/2));
dynamic_cast<btConvexHullShape*>(bt_shape)->addPoint(btVector3(0,0,params[1]/2));
bt_shape->setMargin(params[0]);
break;
default:
cerr << "Warning: Collision element has an unknown type " << shape << endl;
//DEBUG
//std::cout << "BulletElement::BulletElement: THROW" << std::endl;
//END_DEBUG
throw unknownShapeException(shape);
break;
}
//DEBUG
//cout << "BulletElement::BulletElement: Creating btCollisionObject" << endl;
//END_DEBUG
bt_obj = make_shared<btCollisionObject>();
//DEBUG
//cout << "BulletElement::BulletElement: Setting bt_shape for bt_ob" << endl;
//END_DEBUG
bt_obj->setCollisionShape(bt_shape);
//DEBUG
//cout << "BulletElement::BulletElement: Setting world transform for bt_ob" << endl;
//END_DEBUG
setWorldTransform(Matrix4d::Identity());
//DEBUG
//cout << "BulletElement::BulletElement: END" << std::endl;
//END_DEBUG
}
