Skip to content

Commit

Permalink
Added Ellipsoid and Capsule shapes to TPE (#203)
Browse files Browse the repository at this point in the history 
Signed-off-by: ahcorde <[email protected]>

Co-authored-by: Louise Poubel <[email protected]>
  • Loading branch information
@ahcorde @chapulina
ahcorde and chapulina authored Jun 25, 2021
1 parent 0860af0 commit a20dbba
Show file tree
Hide file tree
Showing 11 changed files with 755 additions and 12 deletions.
12 changes: 12 additions & 0 deletions tpe/lib/src/Collision.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,12 +75,24 @@ void Collision::SetShape(const Shape &_shape)
const BoxShape *typedShape = static_cast<const BoxShape *>(&_shape);
this->dataPtr->shape.reset(new BoxShape(*typedShape));
}
else if (_shape.GetType() == ShapeType::CAPSULE)
{
const CapsuleShape *typedShape =
static_cast<const CapsuleShape *>(&_shape);
this->dataPtr->shape.reset(new CapsuleShape(*typedShape));
}
else if (_shape.GetType() == ShapeType::CYLINDER)
{
const CylinderShape *typedShape =
static_cast<const CylinderShape *>(&_shape);
this->dataPtr->shape.reset(new CylinderShape(*typedShape));
}
else if (_shape.GetType() == ShapeType::ELLIPSOID)
{
const EllipsoidShape *typedShape =
static_cast<const EllipsoidShape *>(&_shape);
this->dataPtr->shape.reset(new EllipsoidShape(*typedShape));
}
else if (_shape.GetType() == ShapeType::SPHERE)
{
const SphereShape *typedShape = dynamic_cast<const SphereShape *>(&_shape);
Expand Down
126 changes: 124 additions & 2 deletions tpe/lib/src/CollisionDetector_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -113,6 +113,26 @@ TEST(CollisionDetector, CheckCollisions)
cylinderShapeC.SetLength(4);
collisionC->SetShape(cylinderShapeC);

// model D
std::shared_ptr<Model> modelD(new Model);
Entity &linkDEnt = modelD->AddLink();
Link *linkD = static_cast<Link *>(&linkDEnt);
Entity &collisionDEnt = linkD->AddCollision();
Collision *collisionD = static_cast<Collision *>(&collisionDEnt);
CapsuleShape capsuleShapeD;
capsuleShapeD.SetRadius(0.2);
capsuleShapeD.SetLength(0.6);
collisionD->SetShape(capsuleShapeD);

// model E
std::shared_ptr<Model> modelE(new Model);
Entity &linkEEnt = modelE->AddLink();
Link *linkE = static_cast<Link *>(&linkEEnt);
Entity &collisionEEnt = linkE->AddCollision();
Collision *collisionE = static_cast<Collision *>(&collisionEEnt);
EllipsoidShape ellipsoidShapeE;
ellipsoidShapeE.SetRadii({2, 2, 0.5});
collisionE->SetShape(ellipsoidShapeE);

// check collisions
CollisionDetector cd;
Expand All @@ -121,14 +141,18 @@ TEST(CollisionDetector, CheckCollisions)
modelA->SetPose(math::Pose3d(100, 0, 0, 0, 0, 0));
modelB->SetPose(math::Pose3d(0, 0, 0, 0, 0, 0));
modelC->SetPose(math::Pose3d(-100, 0, 0, 0, 0, 0));
modelD->SetPose(math::Pose3d(-200, 0, 0, 0, 0, 0));
modelE->SetPose(math::Pose3d(200, 0, 0, 0, 0, 0));
entities[modelA->GetId()] = modelA;
entities[modelB->GetId()] = modelB;
entities[modelC->GetId()] = modelC;
entities[modelD->GetId()] = modelD;
entities[modelE->GetId()] = modelE;

std::vector<Contact> contacts = cd.CheckCollisions(entities);
EXPECT_TRUE(contacts.empty());

// collision between model A and B but not model C
// collision between model A and B but not model C, D and E
modelA->SetPose(math::Pose3d(2, 2, 2, 0, 0, 0));
modelB->SetPose(math::Pose3d(0, 0, 0, 0, 0, 0));
modelC->SetPose(math::Pose3d(100, 0, 0, 0, 0, 0));
Expand All @@ -140,6 +164,8 @@ TEST(CollisionDetector, CheckCollisions)
EXPECT_TRUE(c.entity1 == modelA->GetId() || c.entity2 == modelA->GetId());
EXPECT_TRUE(c.entity1 == modelB->GetId() || c.entity2 == modelB->GetId());
EXPECT_TRUE(c.entity1 != modelC->GetId() && c.entity2 != modelC->GetId());
EXPECT_TRUE(c.entity1 != modelD->GetId() && c.entity2 != modelD->GetId());
EXPECT_TRUE(c.entity1 != modelE->GetId() && c.entity2 != modelE->GetId());
EXPECT_NE(c.entity1, c.entity2);
}
// check single contact point
Expand All @@ -151,8 +177,12 @@ TEST(CollisionDetector, CheckCollisions)
(contacts[0].entity2 == modelB->GetId()));
EXPECT_TRUE((contacts[0].entity1 != modelC->GetId()) &&
(contacts[0].entity2 != modelC->GetId()));
EXPECT_TRUE((contacts[0].entity1 != modelD->GetId()) &&
(contacts[0].entity2 != modelD->GetId()));
EXPECT_TRUE((contacts[0].entity1 != modelE->GetId()) &&
(contacts[0].entity2 != modelE->GetId()));

// collision between model A and C but not model B
// collision between model A and C but not model B, D and E
modelA->SetPose(math::Pose3d(2, 1, 2, 0, 0, 0));
modelB->SetPose(math::Pose3d(100, 0, 0, 0, 0, 0));
modelC->SetPose(math::Pose3d(2, 3, 4, 0, 0, 0));
Expand All @@ -164,6 +194,8 @@ TEST(CollisionDetector, CheckCollisions)
EXPECT_TRUE(c.entity1 == modelA->GetId() || c.entity2 == modelA->GetId());
EXPECT_TRUE(c.entity1 != modelB->GetId() && c.entity2 != modelB->GetId());
EXPECT_TRUE(c.entity1 == modelC->GetId() || c.entity2 == modelC->GetId());
EXPECT_TRUE(c.entity1 != modelD->GetId() && c.entity2 != modelD->GetId());
EXPECT_TRUE(c.entity1 != modelE->GetId() && c.entity2 != modelE->GetId());
EXPECT_NE(c.entity1, c.entity2);
}
// check single contact point
Expand All @@ -175,6 +207,10 @@ TEST(CollisionDetector, CheckCollisions)
(contacts[0].entity2 != modelB->GetId()));
EXPECT_TRUE((contacts[0].entity1 == modelC->GetId()) ||
(contacts[0].entity2 == modelC->GetId()));
EXPECT_TRUE((contacts[0].entity1 != modelD->GetId()) &&
(contacts[0].entity2 != modelD->GetId()));
EXPECT_TRUE((contacts[0].entity1 != modelE->GetId()) &&
(contacts[0].entity2 != modelE->GetId()));

// collision between model A and B, and B and C, but not A and C
modelA->SetPose(math::Pose3d(-2, -2, -2, 0, 0, 0));
Expand All @@ -196,6 +232,27 @@ TEST(CollisionDetector, CheckCollisions)
else if ((c.entity1 == modelA->GetId() || c.entity2 == modelA->GetId()) &&
(c.entity1 == modelC->GetId() || c.entity2 == modelC->GetId()))
FAIL() << "There should be no contacts between model A and C";
else if ((c.entity1 == modelA->GetId() || c.entity2 == modelA->GetId()) &&
(c.entity1 == modelD->GetId() || c.entity2 == modelD->GetId()))
FAIL() << "There should be no contacts between model A and D";
else if ((c.entity1 == modelA->GetId() || c.entity2 == modelA->GetId()) &&
(c.entity1 == modelE->GetId() || c.entity2 == modelE->GetId()))
FAIL() << "There should be no contacts between model A and E";
else if ((c.entity1 == modelB->GetId() || c.entity2 == modelB->GetId()) &&
(c.entity1 == modelD->GetId() || c.entity2 == modelD->GetId()))
FAIL() << "There should be no contacts between model B and D";
else if ((c.entity1 == modelB->GetId() || c.entity2 == modelB->GetId()) &&
(c.entity1 == modelE->GetId() || c.entity2 == modelE->GetId()))
FAIL() << "There should be no contacts between model B and E";
else if ((c.entity1 == modelC->GetId() || c.entity2 == modelC->GetId()) &&
(c.entity1 == modelD->GetId() || c.entity2 == modelD->GetId()))
FAIL() << "There should be no contacts between model C and D";
else if ((c.entity1 == modelC->GetId() || c.entity2 == modelC->GetId()) &&
(c.entity1 == modelE->GetId() || c.entity2 == modelE->GetId()))
FAIL() << "There should be no contacts between model C and E";
else if ((c.entity1 == modelD->GetId() || c.entity2 == modelD->GetId()) &&
(c.entity1 == modelE->GetId() || c.entity2 == modelE->GetId()))
FAIL() << "There should be no contacts between model D and E";
}
EXPECT_EQ(8u, contactAB);
EXPECT_EQ(8u, contactBC);
Expand Down Expand Up @@ -240,6 +297,71 @@ TEST(CollisionDetector, CheckCollisions)
EXPECT_EQ(8u, contacts.size());
contacts = cd.CheckCollisions(entities, true);
EXPECT_EQ(1u, contacts.size());

entities[modelC->GetId()] = modelC;
// collision between model E and B but not model A, C, D
modelA->SetPose(math::Pose3d(100, 0, 0, 0, 0, 0));
modelB->SetPose(math::Pose3d(0, 0, 0, 0, 0, 0));
modelC->SetPose(math::Pose3d(-100, 0, 0, 0, 0, 0));
modelD->SetPose(math::Pose3d(-200, 0, 0, 0, 0, 0));
modelE->SetPose(math::Pose3d(5, 0, 0, 0, 0, 0));
contacts = cd.CheckCollisions(entities);
EXPECT_EQ(8u, contacts.size());

for (const auto &c : contacts)
{
EXPECT_TRUE(c.entity1 == modelE->GetId() || c.entity2 == modelE->GetId());
EXPECT_TRUE(c.entity1 == modelB->GetId() || c.entity2 == modelB->GetId());
EXPECT_TRUE(c.entity1 != modelA->GetId() && c.entity2 != modelA->GetId());
EXPECT_TRUE(c.entity1 != modelC->GetId() && c.entity2 != modelC->GetId());
EXPECT_TRUE(c.entity1 != modelD->GetId() && c.entity2 != modelD->GetId());
EXPECT_NE(c.entity1, c.entity2);
}
// check single contact point
contacts = cd.CheckCollisions(entities, true);
EXPECT_EQ(1u, contacts.size());
EXPECT_TRUE((contacts[0].entity1 != modelA->GetId()) &&
(contacts[0].entity2 != modelA->GetId()));
EXPECT_TRUE((contacts[0].entity1 == modelB->GetId()) ||
(contacts[0].entity2 == modelB->GetId()));
EXPECT_TRUE((contacts[0].entity1 != modelC->GetId()) &&
(contacts[0].entity2 != modelC->GetId()));
EXPECT_TRUE((contacts[0].entity1 != modelD->GetId()) &&
(contacts[0].entity2 != modelD->GetId()));
EXPECT_TRUE((contacts[0].entity1 == modelE->GetId()) ||
(contacts[0].entity2 == modelE->GetId()));

// collision between model D and B but not model A, C, E
modelA->SetPose(math::Pose3d(100, 0, 0, 0, 0, 0));
modelB->SetPose(math::Pose3d(0, 0, 0, 0, 0, 0));
modelC->SetPose(math::Pose3d(-100, 0, 0, 0, 0, 0));
modelD->SetPose(math::Pose3d(5, 0, 0, 0, 0, 0));
modelE->SetPose(math::Pose3d(200, 0, 0, 0, 0, 0));
contacts = cd.CheckCollisions(entities);
EXPECT_EQ(8u, contacts.size());

for (const auto &c : contacts)
{
EXPECT_TRUE(c.entity1 != modelA->GetId() && c.entity2 != modelA->GetId());
EXPECT_TRUE(c.entity1 == modelB->GetId() || c.entity2 == modelB->GetId());
EXPECT_TRUE(c.entity1 != modelC->GetId() && c.entity2 != modelC->GetId());
EXPECT_TRUE(c.entity1 == modelD->GetId() || c.entity2 == modelD->GetId());
EXPECT_TRUE(c.entity1 != modelE->GetId() && c.entity2 != modelE->GetId());
EXPECT_NE(c.entity1, c.entity2);
}
// check single contact point
contacts = cd.CheckCollisions(entities, true);
EXPECT_EQ(1u, contacts.size());
EXPECT_TRUE((contacts[0].entity1 != modelA->GetId()) &&
(contacts[0].entity2 != modelA->GetId()));
EXPECT_TRUE((contacts[0].entity1 == modelB->GetId()) ||
(contacts[0].entity2 == modelB->GetId()));
EXPECT_TRUE((contacts[0].entity1 != modelC->GetId()) &&
(contacts[0].entity2 != modelC->GetId()));
EXPECT_TRUE((contacts[0].entity1 == modelD->GetId()) ||
(contacts[0].entity2 == modelD->GetId()));
EXPECT_TRUE((contacts[0].entity1 != modelE->GetId()) &&
(contacts[0].entity2 != modelE->GetId()));
}

/////////////////////////////////////////////////
Expand Down
26 changes: 26 additions & 0 deletions tpe/lib/src/Collision_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,20 @@ TEST(Collision, BoxShape)
result->GetBoundingBox().Max());
}

/////////////////////////////////////////////////
TEST(Collision, CapsuleShape)
{
Collision collision;
CapsuleShape CapsuleShape;
CapsuleShape.SetRadius(2.0);
CapsuleShape.SetLength(3.0);
collision.SetShape(CapsuleShape);
auto result = collision.GetShape();
ASSERT_NE(nullptr, result);
EXPECT_EQ(ignition::math::Vector3d(2.0, 2.0, 3.5),
result->GetBoundingBox().Max());
}

/////////////////////////////////////////////////
TEST(Collision, CylinderShape)
{
Expand All @@ -82,6 +96,18 @@ TEST(Collision, CylinderShape)
result->GetBoundingBox().Max());
}

/////////////////////////////////////////////////
TEST(Collision, EllipsoidShape)
{
Collision collision;
EllipsoidShape EllipsoidShape;
EllipsoidShape.SetRadii({1.0, 2.0, 1.3});
collision.SetShape(EllipsoidShape);
auto result = collision.GetShape();
ASSERT_NE(nullptr, result);
EXPECT_EQ(ignition::math::Vector3d(1.0, 2.0, 1.3),
result->GetBoundingBox().Max());}

/////////////////////////////////////////////////
TEST(Collision, SphereShape)
{
Expand Down
98 changes: 96 additions & 2 deletions tpe/lib/src/Shape.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,6 +97,61 @@ void BoxShape::UpdateBoundingBox()
this->bbox = math::AxisAlignedBox(-halfSize, halfSize);
}

//////////////////////////////////////////////////
CapsuleShape::CapsuleShape() : Shape()
{
this->type = ShapeType::CAPSULE;
}

//////////////////////////////////////////////////
CapsuleShape::CapsuleShape(const CapsuleShape &_other)
: Shape()
{
*this = _other;
}

//////////////////////////////////////////////////
Shape &CapsuleShape::operator=(const Shape &_other)
{
auto other = static_cast<const CapsuleShape *>(&_other);
this->radius = other->radius;
this->length = other->length;
return *this;
}

//////////////////////////////////////////////////
double CapsuleShape::GetRadius() const
{
return this->radius;
}

//////////////////////////////////////////////////
void CapsuleShape::SetRadius(double _radius)
{
this->radius = _radius;
this->dirty = true;
}

//////////////////////////////////////////////////
double CapsuleShape::GetLength() const
{
return this->length;
}

//////////////////////////////////////////////////
void CapsuleShape::SetLength(double _length)
{
this->length = _length;
this->dirty = true;
}

//////////////////////////////////////////////////
void CapsuleShape::UpdateBoundingBox()
{
math::Vector3d halfSize(this->radius, this->radius, this->length*0.5 + this->radius);
this->bbox = math::AxisAlignedBox(-halfSize, halfSize);
}

//////////////////////////////////////////////////
CylinderShape::CylinderShape() : Shape()
{
Expand Down Expand Up @@ -152,6 +207,47 @@ void CylinderShape::UpdateBoundingBox()
this->bbox = math::AxisAlignedBox(-halfSize, halfSize);
}

//////////////////////////////////////////////////
EllipsoidShape::EllipsoidShape() : Shape()
{
this->type = ShapeType::ELLIPSOID;
}

//////////////////////////////////////////////////
EllipsoidShape::EllipsoidShape(const EllipsoidShape &_other)
: Shape()
{
*this = _other;
}

//////////////////////////////////////////////////
Shape &EllipsoidShape::operator=(const Shape &_other)
{
auto other = static_cast<const EllipsoidShape *>(&_other);
this->radii = other->radii;
return *this;
}

//////////////////////////////////////////////////
math::Vector3d EllipsoidShape::GetRadii() const
{
return this->radii;
}

//////////////////////////////////////////////////
void EllipsoidShape::SetRadii(const math::Vector3d &_radii)
{
this->radii = _radii;
this->dirty = true;
}

//////////////////////////////////////////////////
void EllipsoidShape::UpdateBoundingBox()
{
math::Vector3d halfSize(this->radii.X(), this->radii.Y(), this->radii.Z());
this->bbox = math::AxisAlignedBox(-halfSize, halfSize);
}

//////////////////////////////////////////////////
SphereShape::SphereShape() : Shape()
{
Expand Down Expand Up @@ -245,5 +341,3 @@ void MeshShape::UpdateBoundingBox()
this->bbox = math::AxisAlignedBox(
this->scale * this->meshAABB.Min(), this->scale * this->meshAABB.Max());
}


Loading

0 comments on commit a20dbba

Please sign in to comment.