Added Ellipsoid and Capsule shapes to TPE

tpe/lib/src/Collision.cc

@@ -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);

tpe/lib/src/Collision_TEST.cc

@@ -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)
 {
@@ -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)
 {

tpe/lib/src/Shape.cc

@@ -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()
 {
@@ -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(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()
 {
@@ -245,5 +341,3 @@ void MeshShape::UpdateBoundingBox()
   this->bbox = math::AxisAlignedBox(
       this->scale * this->meshAABB.Min(), this->scale * this->meshAABB.Max());
 }
-
-

tpe/lib/src/Shape.hh

@@ -53,6 +53,12 @@ enum class IGNITION_PHYSICS_TPELIB_VISIBLE ShapeType
 
   /// \brief A mesh shape.
   MESH = 5,
+
+  /// \brief A capsule shape.
+  CAPSULE = 6,
+
+  /// \brief A ellipsoid shape.
+  ELLIPSOID = 7,
 };
 
 
@@ -120,6 +126,49 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE BoxShape : public Shape
   IGN_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING
 };
 
+/// \brief Cylinder geometry
+class IGNITION_PHYSICS_TPELIB_VISIBLE CapsuleShape : public Shape
+{
+  /// \brief Constructor
+  public: CapsuleShape();
+
+  /// \brief Copy Constructor
+  /// \param[in] _other shape to copy from
+  public: CapsuleShape(const CapsuleShape &_other);
+
+  /// \brief Destructor
+  public: ~CapsuleShape() = default;
+
+  /// \brief Assignment operator
+  /// \param[in] _other shape to copy from
+  public: Shape &operator=(const Shape &_other);
+
+  /// \brief Get capsule radius
+  /// \return capsule radius
+  public: double GetRadius() const;
+
+  /// \brief Set capsule radius
+  /// \param[in] _radius Cylinder radius
+  public: void SetRadius(double _radius);
+
+  /// \brief Get capsule length
+  /// \return Cylinder length
+  public: double GetLength() const;
+
+  /// \brief Set capsule length
+  /// \param[in] _length Cylinder length
+  public: void SetLength(double _length);
+
+  // Documentation inherited
+  protected: virtual void UpdateBoundingBox() override;
+
+  /// \brief Cylinder radius
+  private: double radius = 0.0;
+
+  /// \brief Cylinder length
+  private: double length = 0.0;
+};
+
 /// \brief Cylinder geometry
 class IGNITION_PHYSICS_TPELIB_VISIBLE CylinderShape : public Shape
 {
@@ -163,6 +212,38 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE CylinderShape : public Shape
   private: double length = 0.0;
 };
 
+/// \brief Ellipsoid geometry
+class IGNITION_PHYSICS_TPELIB_VISIBLE EllipsoidShape : public Shape
+{
+  /// \brief Constructor
+  public: EllipsoidShape();
+
+  /// \brief Copy Constructor
+  /// \param[in] _other shape to copy from
+  public: EllipsoidShape(const EllipsoidShape &_other);
+
+  /// \brief Destructor
+  public: ~EllipsoidShape() = default;
+
+  /// \brief Assignment operator
+  /// \param[in] _other shape to copy from
+  public: Shape &operator=(const Shape &_other);
+
+  /// \brief Get ellipsoid radius
+  /// \return ellipsoid radius
+  public: math::Vector3d GetRadii() const;
+
+  /// \brief Set ellipsoid radius
+  /// \param[in] _radius ellipsoid radius
+  public: void SetRadii(math::Vector3d _radii);
+
+  // Documentation inherited
+  protected: virtual void UpdateBoundingBox() override;
+
+  /// \brief ellipsoid radius
+  private: math::Vector3d radii = math::Vector3d::Zero;
+};
+
 /// \brief Sphere geometry
 class IGNITION_PHYSICS_TPELIB_VISIBLE SphereShape : public Shape
 {

tpe/lib/src/Shape_TEST.cc

@@ -45,6 +45,28 @@ TEST(Shape, BoxShape)
   EXPECT_EQ(math::Vector3d(0.6, 1.8, 2.9), bbox.Max());
 }
 
+/////////////////////////////////////////////////
+TEST(Shape, CapsuleShape)
+{
+  CapsuleShape shape;
+  EXPECT_EQ(ShapeType::CAPSULE, shape.GetType());
+  math::AxisAlignedBox empty = shape.GetBoundingBox();
+  EXPECT_EQ(math::Vector3d::Zero, empty.Center());
+  EXPECT_EQ(math::Vector3d::Zero, empty.Size());
+
+  double radius = 0.6;
+  double length = 2.8;
+  shape.SetRadius(radius);
+  EXPECT_NEAR(radius, shape.GetRadius(), 1e-6);
+  shape.SetLength(length);
+  EXPECT_NEAR(length, shape.GetLength(), 1e-6);
+  math::AxisAlignedBox bbox = shape.GetBoundingBox();
+  EXPECT_EQ(math::Vector3d::Zero, bbox.Center());
+  EXPECT_EQ(math::Vector3d(1.2, 1.2, 4.0), bbox.Size());
+  EXPECT_EQ(math::Vector3d(-0.6, -0.6, -2.0), bbox.Min());
+  EXPECT_EQ(math::Vector3d(0.6, 0.6, 2.0), bbox.Max());
+}
+
 /////////////////////////////////////////////////
 TEST(Shape, CylinderShape)
 {
@@ -67,6 +89,25 @@ TEST(Shape, CylinderShape)
   EXPECT_EQ(math::Vector3d(0.6, 0.6, 1.4), bbox.Max());
 }
 
+/////////////////////////////////////////////////
+TEST(Shape, EllipsoidShape)
+{
+  EllipsoidShape shape;
+  EXPECT_EQ(ShapeType::ELLIPSOID, shape.GetType());
+  math::AxisAlignedBox empty = shape.GetBoundingBox();
+  EXPECT_EQ(math::Vector3d::Zero, empty.Center());
+  EXPECT_EQ(math::Vector3d::Zero, empty.Size());
+
+  math::Vector3d radii(0.5, 0.6, 0.7);
+  shape.SetRadii(radii);
+  EXPECT_EQ(radii, shape.GetRadii());
+  math::AxisAlignedBox bbox = shape.GetBoundingBox();
+  EXPECT_EQ(math::Vector3d::Zero, bbox.Center());
+  EXPECT_EQ(math::Vector3d(1.0, 1.2, 1.4), bbox.Size());
+  EXPECT_EQ(math::Vector3d(-0.5, -0.6, -0.7), bbox.Min());
+  EXPECT_EQ(math::Vector3d(0.5, 0.6, 0.7), bbox.Max());
+}
+
 /////////////////////////////////////////////////
 TEST(Shape, SphereShape)
 {

tpe/plugin/src/SDFFeatures.cc

@@ -18,7 +18,9 @@
 #include "SDFFeatures.hh"
 
 #include <sdf/Box.hh>
+#include <sdf/Capsule.hh>
 #include <sdf/Cylinder.hh>
+#include <sdf/Ellipsoid.hh>
 #include <sdf/Sphere.hh>
 #include <sdf/Geometry.hh>
 #include <sdf/World.hh>
@@ -276,6 +278,14 @@ Identity SDFFeatures::ConstructSdfCollision(
     shape.SetSize(boxSdf->Size());
     collision->SetShape(shape);
   }
+  else if (geom->Type() == ::sdf::GeometryType::CAPSULE)
+  {
+    const auto capsuleSdf = geom->CapsuleShape();
+    tpelib::CapsuleShape shape;
+    shape.SetRadius(capsuleSdf->Radius());
+    shape.SetLength(capsuleSdf->Length());
+    collision->SetShape(shape);
+  }
   else if (geom->Type() == ::sdf::GeometryType::CYLINDER)
   {
     const auto cylinderSdf = geom->CylinderShape();
@@ -284,6 +294,13 @@ Identity SDFFeatures::ConstructSdfCollision(
     shape.SetLength(cylinderSdf->Length());
     collision->SetShape(shape);
   }
+  else if (geom->Type() == ::sdf::GeometryType::ELLIPSOID)
+  {
+    const auto ellipsoidSdf = geom->EllipsoidShape();
+    tpelib::EllipsoidShape shape;
+    shape.SetRadii(ellipsoidSdf->Radii());
+    collision->SetShape(shape);
+  }
   else if (geom->Type() == ::sdf::GeometryType::SPHERE)
   {
     const auto sphereSdf = geom->SphereShape();