int collide(dynamics::ConstShapePtr _shape0, const Eigen::Isometry3d& _T0, dynamics::ConstShapePtr _shape1, const Eigen::Isometry3d& _T1, std::vector<Contact>* _result) { dynamics::Shape::ShapeType LeftType = _shape0->getShapeType(); dynamics::Shape::ShapeType RightType = _shape1->getShapeType(); switch(LeftType) { case dynamics::Shape::BOX: { const dynamics::BoxShape* box0 = static_cast<const dynamics::BoxShape*>(_shape0.get()); switch(RightType) { case dynamics::Shape::BOX: { const dynamics::BoxShape* box1 = static_cast<const dynamics::BoxShape*>(_shape1.get()); return collideBoxBox(box0->getSize(), _T0, box1->getSize(), _T1, _result); } case dynamics::Shape::ELLIPSOID: { const dynamics::EllipsoidShape* ellipsoid1 = static_cast<const dynamics::EllipsoidShape*>(_shape1.get()); return collideBoxSphere(box0->getSize(), _T0, ellipsoid1->getSize()[0] * 0.5, _T1, _result); } case dynamics::Shape::CYLINDER: { //---------------------------------------------------------- // NOT SUPPORT CYLINDER //---------------------------------------------------------- const dynamics::CylinderShape* cylinder1 = static_cast<const dynamics::CylinderShape*>(_shape1.get()); Eigen::Vector3d dimTemp(cylinder1->getRadius() * sqrt(2.0), cylinder1->getRadius() * sqrt(2.0), cylinder1->getHeight()); return collideBoxBox(box0->getSize(), _T0, dimTemp, _T1, _result); } default: return false; break; } break; } case dynamics::Shape::ELLIPSOID: { const dynamics::EllipsoidShape* ellipsoid0 = static_cast<const dynamics::EllipsoidShape*>(_shape0.get()); switch(RightType) { case dynamics::Shape::BOX: { const dynamics::BoxShape* box1 = static_cast<const dynamics::BoxShape*>(_shape1.get()); return collideSphereBox(ellipsoid0->getSize()[0] * 0.5, _T0, box1->getSize(), _T1, _result); } case dynamics::Shape::ELLIPSOID: { const dynamics::EllipsoidShape* ellipsoid1 = static_cast<const dynamics::EllipsoidShape*>(_shape1.get()); return collideSphereSphere(ellipsoid0->getSize()[0] * 0.5, _T0, ellipsoid1->getSize()[0] * 0.5, _T1, _result); } case dynamics::Shape::CYLINDER: { //---------------------------------------------------------- // NOT SUPPORT CYLINDER //---------------------------------------------------------- const dynamics::CylinderShape* cylinder1 = static_cast<const dynamics::CylinderShape*>(_shape1.get()); Eigen::Vector3d dimTemp1(cylinder1->getRadius() * sqrt(2.0), cylinder1->getRadius() * sqrt(2.0), cylinder1->getHeight()); return collideSphereBox( ellipsoid0->getSize()[0] * 0.5, _T0, dimTemp1, _T1, _result); } default: return false; break; } break; } case dynamics::Shape::CYLINDER: { //---------------------------------------------------------- // NOT SUPPORT CYLINDER //---------------------------------------------------------- const dynamics::CylinderShape* cylinder0 = static_cast<const dynamics::CylinderShape*>(_shape0.get()); Eigen::Vector3d dimTemp0(cylinder0->getRadius() * sqrt(2.0), cylinder0->getRadius() * sqrt(2.0), cylinder0->getHeight()); switch(RightType) { case dynamics::Shape::BOX: { const dynamics::BoxShape* box1 = static_cast<const dynamics::BoxShape*>(_shape1.get()); return collideBoxBox(dimTemp0, _T0, box1->getSize(), _T1, _result); } case dynamics::Shape::ELLIPSOID: { const dynamics::EllipsoidShape* ellipsoid1 = static_cast<const dynamics::EllipsoidShape*>(_shape1.get()); return collideBoxSphere(dimTemp0, _T0, ellipsoid1->getSize()[0] * 0.5, _T1, _result); } case dynamics::Shape::CYLINDER: { //---------------------------------------------------------- // NOT SUPPORT CYLINDER //---------------------------------------------------------- const dynamics::CylinderShape* cylinder1 = static_cast<const dynamics::CylinderShape*>(_shape1.get()); Eigen::Vector3d dimTemp1(cylinder1->getRadius() * sqrt(2.0), cylinder1->getRadius() * sqrt(2.0), cylinder1->getHeight()); return collideBoxBox(dimTemp0, _T0, dimTemp1, _T1, _result); } default: { return false; } } break; } default: return false; break; } }
//============================================================================== int collide(CollisionObject* o1, CollisionObject* o2, CollisionResult& result) { // TODO(JS): We could make the contact point computation as optional for // the case that we want only binary check. const auto& shape1 = o1->getShape(); const auto& shape2 = o2->getShape(); const auto& shapeType1 = shape1->getType(); const auto& shapeType2 = shape2->getType(); const Eigen::Isometry3d& T1 = o1->getTransform(); const Eigen::Isometry3d& T2 = o2->getTransform(); if (dynamics::SphereShape::getStaticType() == shapeType1) { const auto* sphere0 = static_cast<const dynamics::SphereShape*>(shape1.get()); if (dynamics::SphereShape::getStaticType() == shapeType2) { const auto* sphere1 = static_cast<const dynamics::SphereShape*>(shape2.get()); return collideSphereSphere( o1, o2, sphere0->getRadius(), T1, sphere1->getRadius(), T2, result); } else if (dynamics::BoxShape::getStaticType() == shapeType2) { const auto* box1 = static_cast<const dynamics::BoxShape*>(shape2.get()); return collideSphereBox( o1, o2, sphere0->getRadius(), T1, box1->getSize(), T2, result); } else if (dynamics::EllipsoidShape::getStaticType() == shapeType2) { const auto* ellipsoid1 = static_cast<const dynamics::EllipsoidShape*>(shape2.get()); return collideSphereSphere(o1, o2, sphere0->getRadius(), T1, ellipsoid1->getRadii()[0], T2, result); } } else if (dynamics::BoxShape::getStaticType() == shapeType1) { const auto* box0 = static_cast<const dynamics::BoxShape*>(shape1.get()); if (dynamics::SphereShape::getStaticType() == shapeType2) { const auto* sphere1 = static_cast<const dynamics::SphereShape*>(shape2.get()); return collideBoxSphere( o1, o2, box0->getSize(), T1, sphere1->getRadius(), T2, result); } else if (dynamics::BoxShape::getStaticType() == shapeType2) { const auto* box1 = static_cast<const dynamics::BoxShape*>(shape2.get()); return collideBoxBox(o1, o2, box0->getSize(), T1, box1->getSize(), T2, result); } else if (dynamics::EllipsoidShape::getStaticType() == shapeType2) { const auto* ellipsoid1 = static_cast<const dynamics::EllipsoidShape*>(shape2.get()); return collideBoxSphere(o1, o2, box0->getSize(), T1, ellipsoid1->getRadii()[0], T2, result); } } else if (dynamics::EllipsoidShape::getStaticType() == shapeType1) { const auto* ellipsoid0 = static_cast<const dynamics::EllipsoidShape*>(shape1.get()); if (dynamics::SphereShape::getStaticType() == shapeType2) { const auto* sphere1 = static_cast<const dynamics::SphereShape*>(shape2.get()); return collideSphereSphere(o1, o2, ellipsoid0->getRadii()[0], T1, sphere1->getRadius(), T2, result); } else if (dynamics::BoxShape::getStaticType() == shapeType2) { const auto* box1 = static_cast<const dynamics::BoxShape*>(shape2.get()); return collideSphereBox(o1, o2, ellipsoid0->getRadii()[0], T1, box1->getSize(), T2, result); } else if (dynamics::EllipsoidShape::getStaticType() == shapeType2) { const auto* ellipsoid1 = static_cast<const dynamics::EllipsoidShape*>(shape2.get()); return collideSphereSphere(o1, o2, ellipsoid0->getRadii()[0], T1, ellipsoid1->getRadii()[0], T2, result); } } dterr << "[DARTCollisionDetector] Attempting to check for an " << "unsupported shape pair: [" << shape1->getType() << "] - [" << shape2->getType() << "]. Returning false.\n"; return false; }