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;
}
