//============================================================================== btTransform convertTransform(const Eigen::Isometry3d& _T) { btTransform trans; trans.setOrigin(convertVector3(_T.translation())); trans.setBasis(convertMatrix3x3(_T.linear())); return trans; }
//============================================================================== fcl::Transform3f FCLTypes::convertTransform(const Eigen::Isometry3d& _T) { fcl::Transform3f trans; trans.setTranslation(convertVector3(_T.translation())); trans.setRotation(convertMatrix3x3(_T.linear())); return trans; }
//============================================================================== dart::collision::fcl::Transform3 FCLTypes::convertTransform(const Eigen::Isometry3d& _T) { #if FCL_VERSION_AT_LEAST(0,6,0) return _T; #else dart::collision::fcl::Transform3 trans; trans.setTranslation(convertVector3(_T.translation())); trans.setRotation(convertMatrix3x3(_T.linear())); return trans; #endif }
//============================================================================== BulletCollisionNode::BulletCollisionNode(dynamics::BodyNode* _bodyNode) : CollisionNode(_bodyNode) { for (int i = 0; i < _bodyNode->getNumCollisionShapes(); i++) { dynamics::Shape* shape = _bodyNode->getCollisionShape(i); switch (shape->getShapeType()) { case dynamics::Shape::BOX: { dynamics::BoxShape* box = static_cast<dynamics::BoxShape*>(shape); btBoxShape* btBox = new btBoxShape(btVector3(box->getSize()[0]*0.5, box->getSize()[1]*0.5, box->getSize()[2]*0.5)); btBox->setMargin(0.0); btCollisionObject* btCollObj = new btCollisionObject(); btCollObj->setCollisionShape(btBox); BulletUserData* userData = new BulletUserData; userData->bodyNode = _bodyNode; userData->shape = shape; userData->btCollNode = this; btCollObj->setUserPointer(userData); mbtCollsionObjects.push_back(btCollObj); break; } case dynamics::Shape::ELLIPSOID: { dynamics::EllipsoidShape* ellipsoid = static_cast<dynamics::EllipsoidShape*>(shape); if (ellipsoid->isSphere()) { btSphereShape* btSphere = new btSphereShape(ellipsoid->getSize()[0] * 0.5); btSphere->setMargin(0.0); btCollisionObject* btCollObj = new btCollisionObject(); btCollObj->setCollisionShape(btSphere); BulletUserData* userData = new BulletUserData; userData->bodyNode = _bodyNode; userData->shape = shape; userData->btCollNode = this; btCollObj->setUserPointer(userData); mbtCollsionObjects.push_back(btCollObj); } else { // TODO(JS): Add mesh for ellipsoid } break; } case dynamics::Shape::CYLINDER: { dynamics::CylinderShape* cylinder = static_cast<dynamics::CylinderShape*>(shape); btCylinderShapeZ* btCylinder = new btCylinderShapeZ(btVector3(cylinder->getRadius(), cylinder->getRadius(), cylinder->getHeight() * 0.5)); btCylinder->setMargin(0.0); btCollisionObject* btCollObj = new btCollisionObject(); btCollObj->setCollisionShape(btCylinder); BulletUserData* userData = new BulletUserData; userData->bodyNode = _bodyNode; userData->shape = shape; userData->btCollNode = this; btCollObj->setUserPointer(userData); mbtCollsionObjects.push_back(btCollObj); break; } case dynamics::Shape::PLANE: { dynamics::PlaneShape* plane = static_cast<dynamics::PlaneShape*>(shape); btScalar d = plane->getNormal().dot(plane->getPoint()) / plane->getNormal().squaredNorm(); btStaticPlaneShape* btStaticPlane = new btStaticPlaneShape(convertVector3(plane->getNormal()), d); btStaticPlane->setMargin(0.0); btCollisionObject* btCollObj = new btCollisionObject(); btCollObj->setCollisionShape(btStaticPlane); BulletUserData* userData = new BulletUserData; userData->bodyNode = _bodyNode; userData->shape = shape; userData->btCollNode = this; btCollObj->setUserPointer(userData); mbtCollsionObjects.push_back(btCollObj); break; } case dynamics::Shape::MESH: { dynamics::MeshShape* shapeMesh = static_cast<dynamics::MeshShape*>(shape); btConvexTriangleMeshShape* btMesh = _createMesh(shapeMesh->getScale(), shapeMesh->getMesh()); btMesh->setMargin(0.0); btCollisionObject* btCollObj = new btCollisionObject(); // Add user data btCollObj->setCollisionShape(btMesh); BulletUserData* userData = new BulletUserData; userData->bodyNode = _bodyNode; userData->shape = shape; userData->btCollNode = this; btCollObj->setUserPointer(userData); // mbtCollsionObjects.push_back(btCollObj); break; } default: { std::cout << "ERROR: Collision checking does not support " << _bodyNode->getName() << "'s Shape type\n"; break; } } } }