diff --git a/include/sdf/Surface.hh b/include/sdf/Surface.hh index b7ea1b802..e2d1d906a 100644 --- a/include/sdf/Surface.hh +++ b/include/sdf/Surface.hh @@ -17,6 +17,9 @@ #ifndef SDF_SURFACE_HH_ #define SDF_SURFACE_HH_ +#include +#include + #include #include #include "sdf/Element.hh" @@ -53,8 +56,17 @@ namespace sdf public: uint16_t CollideBitmask() const; /// \brief Set the collide bitmask parameter. + /// \param[in] _bitmask Category bitmask to set public: void SetCollideBitmask(const uint16_t _bitmask); + /// \brief Get the category bitmask parameter. + /// \return The category bitmask parameter. + public: std::optional CategoryBitmask() const; + + /// \brief Set the category bitmask parameter. + /// \param[in] _bitmask Category bitmask to set + public: void SetCategoryBitmask(const uint16_t _bitmask); + /// \brief Private data pointer. GZ_UTILS_IMPL_PTR(dataPtr) }; diff --git a/python/src/sdf/pySurface.cc b/python/src/sdf/pySurface.cc index ac296efea..efa1960bd 100644 --- a/python/src/sdf/pySurface.cc +++ b/python/src/sdf/pySurface.cc @@ -16,6 +16,7 @@ #include "pySurface.hh" #include +#include #include "sdf/Surface.hh" @@ -37,6 +38,11 @@ void defineContact(pybind11::object module) "Get the collide bitmask parameter.") .def("set_collide_bitmask", &sdf::Contact::SetCollideBitmask, "Set the collide bitmask parameter.") + .def("category_bitmask", &sdf::Contact::CategoryBitmask, + "Get the category bitmask parameter.") + .def("set_category_bitmask", &sdf::Contact::SetCategoryBitmask, + "Set the category bitmask parameter.") + .def("__copy__", [](const sdf::Contact &self) { return sdf::Contact(self); }) diff --git a/python/test/pySurface_TEST.py b/python/test/pySurface_TEST.py index 998244e5f..70d539c34 100644 --- a/python/test/pySurface_TEST.py +++ b/python/test/pySurface_TEST.py @@ -24,6 +24,7 @@ class SurfaceTEST(unittest.TestCase): def test_default_construction(self): surface = Surface() self.assertEqual(surface.contact().collide_bitmask(), 0xFF) + self.assertFalse(surface.contact().category_bitmask()) def test_assigment_construction(self): @@ -54,11 +55,13 @@ def test_assigment_construction(self): friction.set_bullet_friction(bullet) friction.set_torsional(torsional) contact.set_collide_bitmask(0x12) + contact.set_category_bitmask(0x22) surface1.set_contact(contact) surface1.set_friction(friction) surface2 = surface1 self.assertEqual(surface2.contact().collide_bitmask(), 0x12) + self.assertEqual(surface2.contact().category_bitmask(), 0x22) self.assertEqual(surface2.friction().ode().mu(), 0.1) self.assertEqual(surface2.friction().ode().mu2(), 0.2) self.assertEqual(surface2.friction().ode().slip1(), 3) @@ -78,9 +81,12 @@ def test_assigment_construction(self): self.assertEqual(surface2.friction().torsional().ode_slip(), 0.01) contact.set_collide_bitmask(0x21) + contact.set_category_bitmask(0x33) surface1.set_contact(contact) self.assertEqual(surface1.contact().collide_bitmask(), 0x21) self.assertEqual(surface2.contact().collide_bitmask(), 0x21) + self.assertEqual(surface1.contact().category_bitmask(), 0x33) + self.assertEqual(surface2.contact().category_bitmask(), 0x33) ode.set_mu(1.1) ode.set_mu2(1.2) @@ -165,11 +171,13 @@ def test_copy_construction(self): friction.set_bullet_friction(bullet) friction.set_torsional(torsional) contact.set_collide_bitmask(0x12) + contact.set_category_bitmask(0x22) surface1.set_contact(contact) surface1.set_friction(friction) surface2 = Surface(surface1) self.assertEqual(surface2.contact().collide_bitmask(), 0x12) + self.assertEqual(surface2.contact().category_bitmask(), 0x22) self.assertEqual(surface2.friction().ode().mu(), 0.1) self.assertEqual(surface2.friction().ode().mu2(), 0.2) self.assertEqual(surface2.friction().ode().slip1(), 3) @@ -189,9 +197,12 @@ def test_copy_construction(self): self.assertEqual(surface2.friction().torsional().ode_slip(), 0.01) contact.set_collide_bitmask(0x21) + contact.set_category_bitmask(0x33) surface1.set_contact(contact) self.assertEqual(surface1.contact().collide_bitmask(), 0x21) self.assertEqual(surface2.contact().collide_bitmask(), 0x12) + self.assertEqual(surface1.contact().category_bitmask(), 0x33) + self.assertEqual(surface2.contact().category_bitmask(), 0x22) ode.set_mu(1.1) ode.set_mu2(1.2) @@ -276,11 +287,13 @@ def test_deepcopy(self): friction.set_bullet_friction(bullet) friction.set_torsional(torsional) contact.set_collide_bitmask(0x12) + contact.set_category_bitmask(0x22) surface1.set_contact(contact) surface1.set_friction(friction) surface2 = copy.deepcopy(surface1) self.assertEqual(surface2.contact().collide_bitmask(), 0x12) + self.assertEqual(surface2.contact().category_bitmask(), 0x22) self.assertEqual(surface2.friction().ode().mu(), 0.1) self.assertEqual(surface2.friction().ode().mu2(), 0.2) self.assertEqual(surface2.friction().ode().slip1(), 3) @@ -300,9 +313,12 @@ def test_deepcopy(self): self.assertEqual(surface2.friction().torsional().ode_slip(), 0.01) contact.set_collide_bitmask(0x21) + contact.set_category_bitmask(0x33) surface1.set_contact(contact) self.assertEqual(surface1.contact().collide_bitmask(), 0x21) self.assertEqual(surface2.contact().collide_bitmask(), 0x12) + self.assertEqual(surface1.contact().category_bitmask(), 0x33) + self.assertEqual(surface2.contact().category_bitmask(), 0x22) ode.set_mu(1.1) ode.set_mu2(1.2) @@ -363,14 +379,17 @@ def test_deepcopy(self): def test_default_contact_construction(self): contact = Contact() self.assertEqual(contact.collide_bitmask(), 0xFF) + self.assertFalse(contact.category_bitmask()) def test_copy_contact_construction(self): contact1 = Contact() contact1.set_collide_bitmask(0x12) + contact1.set_category_bitmask(0x21) contact2 = Contact(contact1) self.assertEqual(contact2.collide_bitmask(), 0x12) + self.assertEqual(contact2.category_bitmask(), 0x21) def test_default_ode_construction(self): ode = ODE() diff --git a/src/Surface.cc b/src/Surface.cc index a0343ecf5..a13fa1823 100644 --- a/src/Surface.cc +++ b/src/Surface.cc @@ -30,9 +30,12 @@ using namespace sdf; class sdf::Contact::Implementation { - // \brief The bitmask used to filter collisions. + // \brief The collide bitmask used to filter collisions. public: uint16_t collideBitmask = 0xff; + // \brief The category bitmask used to filter collisions. + public: std::optional categoryBitmask; + /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf{nullptr}; }; @@ -605,6 +608,13 @@ Errors Contact::Load(ElementPtr _sdf) errors, "collide_bitmask")); } + if (_sdf->HasElement("category_bitmask")) + { + this->dataPtr->categoryBitmask = + static_cast(_sdf->Get( + errors, "category_bitmask")); + } + // \todo(nkoenig) Parse the remaining collide properties. return errors; } @@ -627,6 +637,18 @@ void Contact::SetCollideBitmask(const uint16_t _bitmask) this->dataPtr->collideBitmask = _bitmask; } +///////////////////////////////////////////////// +std::optional Contact::CategoryBitmask() const +{ + return this->dataPtr->categoryBitmask; +} + +///////////////////////////////////////////////// +void Contact::SetCategoryBitmask(const uint16_t _bitmask) +{ + this->dataPtr->categoryBitmask = _bitmask; +} + ///////////////////////////////////////////////// Surface::Surface() : dataPtr(gz::utils::MakeImpl()) @@ -724,6 +746,11 @@ sdf::ElementPtr Surface::ToElement(sdf::Errors &_errors) const sdf::ElementPtr contactElem = elem->GetElement("contact", _errors); contactElem->GetElement("collide_bitmask", _errors)->Set( _errors, this->dataPtr->contact.CollideBitmask()); + if (this->dataPtr->contact.CategoryBitmask().has_value()) + { + contactElem->GetElement("category_bitmask", _errors)->Set( + _errors, this->dataPtr->contact.CategoryBitmask().value()); + } sdf::ElementPtr frictionElem = elem->GetElement("friction", _errors); diff --git a/src/Surface_TEST.cc b/src/Surface_TEST.cc index a7b2341d6..3a9e7ac5d 100644 --- a/src/Surface_TEST.cc +++ b/src/Surface_TEST.cc @@ -26,6 +26,7 @@ TEST(DOMsurface, DefaultConstruction) sdf::Surface surface; EXPECT_EQ(nullptr, surface.Element()); EXPECT_EQ(surface.Contact()->CollideBitmask(), 0xFF); + EXPECT_FALSE(surface.Contact()->CategoryBitmask().has_value()); EXPECT_EQ(surface.Contact()->Element(), nullptr); EXPECT_EQ(surface.Friction()->Element(), nullptr); } @@ -44,11 +45,13 @@ TEST(DOMsurface, CopyOperator) sdf::Friction friction; friction.SetODE(ode); contact.SetCollideBitmask(0x12); + contact.SetCategoryBitmask(0x21); surface1.SetContact(contact); surface1.SetFriction(friction); sdf::Surface surface2(surface1); EXPECT_EQ(surface2.Contact()->CollideBitmask(), 0x12); + EXPECT_EQ(surface2.Contact()->CategoryBitmask(), 0x21); EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Mu(), 0.1); EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Mu2(), 0.2); EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Slip1(), 3); @@ -71,11 +74,13 @@ TEST(DOMsurface, CopyAssignmentOperator) sdf::Friction friction; friction.SetODE(ode); contact.SetCollideBitmask(0x12); + contact.SetCategoryBitmask(0x21); surface1.SetContact(contact); surface1.SetFriction(friction); sdf::Surface surface2 = surface1; EXPECT_EQ(surface2.Contact()->CollideBitmask(), 0x12); + EXPECT_EQ(surface2.Contact()->CategoryBitmask(), 0x21); EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Mu(), 0.1); EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Mu2(), 0.2); EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Slip1(), 3); @@ -92,9 +97,11 @@ TEST(DOMsurface, CopyAssignmentAfterMove) sdf::Contact contact1; contact1.SetCollideBitmask(0x12); + contact1.SetCategoryBitmask(0x21); surface1.SetContact(contact1); sdf::Contact contact2; contact2.SetCollideBitmask(0x34); + contact2.SetCategoryBitmask(0x43); surface2.SetContact(contact2); sdf::ODE ode1; @@ -124,6 +131,8 @@ TEST(DOMsurface, CopyAssignmentAfterMove) EXPECT_EQ(surface2.Contact()->CollideBitmask(), 0x12); EXPECT_EQ(surface1.Contact()->CollideBitmask(), 0x34); + EXPECT_EQ(surface2.Contact()->CategoryBitmask(), 0x21); + EXPECT_EQ(surface1.Contact()->CategoryBitmask(), 0x43); EXPECT_DOUBLE_EQ(surface1.Friction()->ODE()->Mu(), 0.2); EXPECT_DOUBLE_EQ(surface1.Friction()->ODE()->Mu2(), 0.1); EXPECT_DOUBLE_EQ(surface1.Friction()->ODE()->Slip1(), 7); @@ -165,6 +174,7 @@ TEST(DOMsurface, ToElement) torsional.SetODESlip(0.2); friction.SetTorsional(torsional); contact.SetCollideBitmask(0x12); + contact.SetCategoryBitmask(0x21); surface1.SetContact(contact); surface1.SetFriction(friction); @@ -175,6 +185,7 @@ TEST(DOMsurface, ToElement) surface2.Load(elem); EXPECT_EQ(surface2.Contact()->CollideBitmask(), 0x12); + EXPECT_EQ(surface2.Contact()->CategoryBitmask(), 0x21); EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Mu(), 0.1); EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Mu2(), 0.2); EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Slip1(), 3); @@ -224,6 +235,7 @@ TEST(DOMsurface, ToElementErrorOutput) ode.SetFdir1(gz::math::Vector3d(1, 2, 3)); friction.SetODE(ode); contact.SetCollideBitmask(0x12); + contact.SetCategoryBitmask(0x21); surface1.SetContact(contact); surface1.SetFriction(friction); @@ -236,6 +248,7 @@ TEST(DOMsurface, ToElementErrorOutput) EXPECT_TRUE(errors.empty()); EXPECT_EQ(surface2.Contact()->CollideBitmask(), 0x12); + EXPECT_EQ(surface2.Contact()->CategoryBitmask(), 0x21); EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Mu(), 0.1); EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Mu2(), 0.2); EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Slip1(), 3); @@ -254,6 +267,7 @@ TEST(DOMcontact, DefaultConstruction) sdf::Contact contact; EXPECT_EQ(nullptr, contact.Element()); EXPECT_EQ(contact.CollideBitmask(), 0xFF); + EXPECT_FALSE(contact.CategoryBitmask().has_value()); EXPECT_EQ(contact.Element(), nullptr); } @@ -262,9 +276,11 @@ TEST(DOMcontact, CopyOperator) { sdf::Contact contact1; contact1.SetCollideBitmask(0x12); + contact1.SetCategoryBitmask(0x21); sdf::Contact contact2(contact1); EXPECT_EQ(contact2.CollideBitmask(), 0x12); + EXPECT_EQ(contact2.CategoryBitmask(), 0x21); } ///////////////////////////////////////////////// @@ -272,9 +288,11 @@ TEST(DOMcontact, CopyAssignmentOperator) { sdf::Contact contact1; contact1.SetCollideBitmask(0x12); + contact1.SetCategoryBitmask(0x21); sdf::Contact contact2 = contact1; EXPECT_EQ(contact2.CollideBitmask(), 0x12); + EXPECT_EQ(contact2.CategoryBitmask(), 0x21); } ///////////////////////////////////////////////// @@ -285,6 +303,8 @@ TEST(DOMcontact, CopyAssignmentAfterMove) contact1.SetCollideBitmask(0x12); contact2.SetCollideBitmask(0x34); + contact1.SetCategoryBitmask(0x21); + contact2.SetCategoryBitmask(0x43); sdf::Contact tmp = std::move(contact1); contact1 = contact2; @@ -292,6 +312,8 @@ TEST(DOMcontact, CopyAssignmentAfterMove) EXPECT_EQ(contact2.CollideBitmask(), 0x12); EXPECT_EQ(contact1.CollideBitmask(), 0x34); + EXPECT_EQ(contact2.CategoryBitmask(), 0x21); + EXPECT_EQ(contact1.CategoryBitmask(), 0x43); } ///////////////////////////////////////////////// @@ -299,7 +321,9 @@ TEST(DOMcontact, CollideBitmask) { sdf::Contact contact; contact.SetCollideBitmask(0x67); + contact.SetCategoryBitmask(0x76); EXPECT_EQ(contact.CollideBitmask(), 0x67); + EXPECT_EQ(contact.CategoryBitmask(), 0x76); } ///////////////////////////////////////////////// diff --git a/test/integration/surface_dom.cc b/test/integration/surface_dom.cc index f1f7faf94..d20425600 100644 --- a/test/integration/surface_dom.cc +++ b/test/integration/surface_dom.cc @@ -50,6 +50,7 @@ TEST(DOMSurface, Shapes) ASSERT_NE(nullptr, boxCol->Surface()); ASSERT_NE(nullptr, boxCol->Surface()->Contact()); EXPECT_EQ(boxCol->Surface()->Contact()->CollideBitmask(), 0xAB); + EXPECT_EQ(boxCol->Surface()->Contact()->CategoryBitmask(), 0xBA); EXPECT_DOUBLE_EQ(boxCol->Surface()->Friction()->ODE()->Mu(), 0.6); EXPECT_DOUBLE_EQ(boxCol->Surface()->Friction()->ODE()->Mu2(), 0.7); EXPECT_DOUBLE_EQ(boxCol->Surface()->Friction()->ODE()->Slip1(), 4); diff --git a/test/sdf/shapes.sdf b/test/sdf/shapes.sdf index 5a2e72d4f..e208e8a97 100644 --- a/test/sdf/shapes.sdf +++ b/test/sdf/shapes.sdf @@ -11,6 +11,7 @@ 0xAB + 0xBA