diff --git a/tpe/lib/src/Collision.cc b/tpe/lib/src/Collision.cc index 6993474c3..cd1a6dadc 100644 --- a/tpe/lib/src/Collision.cc +++ b/tpe/lib/src/Collision.cc @@ -24,6 +24,9 @@ class ignition::physics::tpelib::CollisionPrivate { /// \brief Collision's geometry shape public: std::shared_ptr shape = nullptr; + + /// \brief Collide bitmask + public: uint16_t collideBitmask = 0xFF; }; using namespace ignition; @@ -107,3 +110,17 @@ math::AxisAlignedBox Collision::GetBoundingBox(bool /*_force*/) // NOLINT return this->dataPtr->shape->GetBoundingBox(); return math::AxisAlignedBox(); } + +////////////////////////////////////////////////// +void Collision::SetCollideBitmask(uint16_t _mask) +{ + this->dataPtr->collideBitmask = _mask; + if (this->GetParent()) + this->GetParent()->ChildrenChanged(); +} + +////////////////////////////////////////////////// +uint16_t Collision::GetCollideBitmask() const +{ + return this->dataPtr->collideBitmask; +} diff --git a/tpe/lib/src/Collision.hh b/tpe/lib/src/Collision.hh index 61b0786e0..bf6414720 100644 --- a/tpe/lib/src/Collision.hh +++ b/tpe/lib/src/Collision.hh @@ -62,6 +62,13 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Collision : public Entity /// \return shape of collision public: Shape *GetShape() const; + /// \brief Set collide bitmask + /// \param[in] _mask Bitmask to set + public: void SetCollideBitmask(uint16_t _mask); + + // Documentation Inherited + public: uint16_t GetCollideBitmask() const override; + // Documentation inherited public: math::AxisAlignedBox GetBoundingBox(bool _force) override; diff --git a/tpe/lib/src/CollisionDetector.cc b/tpe/lib/src/CollisionDetector.cc index ef3f1f5ca..a0f877c29 100644 --- a/tpe/lib/src/CollisionDetector.cc +++ b/tpe/lib/src/CollisionDetector.cc @@ -15,6 +15,8 @@ * */ +#include + #include "CollisionDetector.hh" #include "Utils.hh" @@ -32,13 +34,17 @@ std::vector CollisionDetector::CheckCollisions( std::vector contacts; // cache of axis aligned box in world frame - std::map worldAabb; + std::unordered_map worldAabb; for (auto it = _entities.begin(); it != _entities.end(); ++it) { + std::shared_ptr e1 = it->second; + + // Get collide bitmask for enitty 1 + uint16_t cb1 = e1->GetCollideBitmask(); + // Get world axis aligned box for entity 1 math::AxisAlignedBox wb1; - std::shared_ptr e1 = it->second; auto wb1It = worldAabb.find(e1->GetId()); if (wb1It == worldAabb.end()) { @@ -56,9 +62,17 @@ std::vector CollisionDetector::CheckCollisions( for (auto it2 = std::next(it, 1); it2 != _entities.end(); ++it2) { + std::shared_ptr e2 = it2->second; + + // Get collide bitmask for entity 2 + uint16_t cb2 = e2->GetCollideBitmask(); + + // collision filtering using collide bitmask + if ((cb1 & cb2) == 0) + continue; + // Get world axis aligned box for entity 2 math::AxisAlignedBox wb2; - std::shared_ptr e2 = it2->second; auto wb2It = worldAabb.find(e2->GetId()); if (wb2It == worldAabb.end()) { diff --git a/tpe/lib/src/Collision_TEST.cc b/tpe/lib/src/Collision_TEST.cc index 13878dae3..a2a66efb7 100644 --- a/tpe/lib/src/Collision_TEST.cc +++ b/tpe/lib/src/Collision_TEST.cc @@ -37,6 +37,10 @@ TEST(Collision, BasicAPI) collision.SetPose(math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3)); EXPECT_EQ(math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3), collision.GetPose()); + EXPECT_EQ(0xFF, collision.GetCollideBitmask()); + collision.SetCollideBitmask(0x03); + EXPECT_EQ(0x03, collision.GetCollideBitmask()); + Collision collision2; EXPECT_NE(collision.GetId(), collision2.GetId()); } diff --git a/tpe/lib/src/Entity.cc b/tpe/lib/src/Entity.cc index 259578417..f2610f06a 100644 --- a/tpe/lib/src/Entity.cc +++ b/tpe/lib/src/Entity.cc @@ -36,8 +36,17 @@ class ignition::physics::tpelib::EntityPrivate /// \brief Bounding Box public: math::AxisAlignedBox bbox; + /// \brief Collide bitmask + public: uint16_t collideBitmask = 0xFF; + /// \brief Flag to indicate if bounding box changed public: bool bboxDirty = true; + + /// \brief Flag to indicate if collide bitmask changed + public: bool collideBitmaskDirty = true; + + /// \brief Parent of this entity + public: Entity *parent = nullptr; }; using namespace ignition; @@ -63,6 +72,7 @@ Entity::Entity(const Entity &_other) this->dataPtr->pose = _other.dataPtr->pose; this->dataPtr->children = _other.dataPtr->children; this->dataPtr->bbox = _other.dataPtr->bbox; + this->dataPtr->collideBitmask = _other.dataPtr->collideBitmask; } ////////////////////////////////////////////////// @@ -187,6 +197,7 @@ bool Entity::RemoveChildById(std::size_t _id) if (it != this->dataPtr->children.end()) { this->dataPtr->children.erase(it); + this->ChildrenChanged(); return true; } @@ -202,6 +213,7 @@ bool Entity::RemoveChildByName(const std::string &_name) if (it->second->GetName() == _name) { this->dataPtr->children.erase(it); + this->ChildrenChanged(); return true; } } @@ -240,6 +252,23 @@ void Entity::UpdateBoundingBox(bool _force) this->dataPtr->bbox = box; } +////////////////////////////////////////////////// +uint16_t Entity::GetCollideBitmask() const +{ + if (this->dataPtr->collideBitmaskDirty) + { + uint16_t mask = 0u; + for (auto &it : this->dataPtr->children) + { + mask |= it.second->GetCollideBitmask(); + } + this->dataPtr->collideBitmask = mask; + this->dataPtr->collideBitmaskDirty = false; + } + + return this->dataPtr->collideBitmask; +} + ////////////////////////////////////////////////// std::map> &Entity::GetChildren() const { @@ -251,3 +280,25 @@ std::size_t Entity::GetNextId() { return nextId++; } + +////////////////////////////////////////////////// +void Entity::ChildrenChanged() +{ + this->dataPtr->bboxDirty = true; + this->dataPtr->collideBitmaskDirty = true; + + if (this->dataPtr->parent) + this->dataPtr->parent->ChildrenChanged(); +} + +////////////////////////////////////////////////// +void Entity::SetParent(Entity *_parent) +{ + this->dataPtr->parent = _parent; +} + +////////////////////////////////////////////////// +Entity *Entity::GetParent() const +{ + return this->dataPtr->parent; +} diff --git a/tpe/lib/src/Entity.hh b/tpe/lib/src/Entity.hh index b9cd1cd5e..8bdcfbc78 100644 --- a/tpe/lib/src/Entity.hh +++ b/tpe/lib/src/Entity.hh @@ -131,6 +131,25 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Entity /// \return Entity bounding box public: virtual math::AxisAlignedBox GetBoundingBox(bool _force = false); + /// \brief Get collide bitmask + /// \return Collision's collide bitmask + public: virtual uint16_t GetCollideBitmask() const; + + /// \internal + /// \brief Set the parent of this entity. + /// \param[in] _parent Parent to set to + public: void SetParent(Entity *_parent); + + /// \internal + /// \brief Get the parent of this entity. + /// \return Parent of this entity + public: Entity *GetParent() const; + + /// \internal + /// \brief Mark that the children of the entity has changed, e.g. a child + /// entity is added or removed, or child entity properties changed. + public: void ChildrenChanged(); + /// \brief Get number of children /// \return Map of child id's to child entities protected: std::map> &GetChildren() diff --git a/tpe/lib/src/Link.cc b/tpe/lib/src/Link.cc index a8cb933f2..d464141c1 100644 --- a/tpe/lib/src/Link.cc +++ b/tpe/lib/src/Link.cc @@ -38,5 +38,7 @@ Entity &Link::AddCollision() std::size_t collisionId = Entity::GetNextId(); const auto[it, success] = this->GetChildren().insert( {collisionId, std::make_shared(collisionId)}); + it->second->SetParent(this); + this->ChildrenChanged(); return *it->second.get(); } diff --git a/tpe/lib/src/Model.cc b/tpe/lib/src/Model.cc index 37d335904..dc066de96 100644 --- a/tpe/lib/src/Model.cc +++ b/tpe/lib/src/Model.cc @@ -43,6 +43,9 @@ Entity &Model::AddLink() std::size_t linkId = Entity::GetNextId(); const auto[it, success] = this->GetChildren().insert( {linkId, std::make_shared(linkId)}); + + it->second->SetParent(this); + this->ChildrenChanged(); return *it->second.get(); } diff --git a/tpe/lib/src/Model_TEST.cc b/tpe/lib/src/Model_TEST.cc index fe07818c1..cd692e3bb 100644 --- a/tpe/lib/src/Model_TEST.cc +++ b/tpe/lib/src/Model_TEST.cc @@ -133,11 +133,11 @@ TEST(Model, BoundingBox) math::AxisAlignedBox expectedBoxLinkFrame( math::Vector3d(-2, -2, -2), math::Vector3d(2, 2, 2)); - EXPECT_EQ(expectedBoxLinkFrame, linkEnt.GetBoundingBox(true)); + EXPECT_EQ(expectedBoxLinkFrame, linkEnt.GetBoundingBox()); math::AxisAlignedBox expectedBoxModelFrame( math::Vector3d(-2, -2, -1), math::Vector3d(2, 2, 3)); - EXPECT_EQ(expectedBoxModelFrame, model.GetBoundingBox(true)); + EXPECT_EQ(expectedBoxModelFrame, model.GetBoundingBox()); // add another link with box collision shape Entity &linkEnt2 = model.AddLink(); @@ -154,9 +154,48 @@ TEST(Model, BoundingBox) expectedBoxLinkFrame = math::AxisAlignedBox( math::Vector3d(-1.5, -2, -2.5), math::Vector3d(1.5, 2, 2.5)); - EXPECT_EQ(expectedBoxLinkFrame, linkEnt2.GetBoundingBox(true)); + EXPECT_EQ(expectedBoxLinkFrame, linkEnt2.GetBoundingBox()); expectedBoxModelFrame = math::AxisAlignedBox( math::Vector3d(-2, -2, -2.5), math::Vector3d(2, 3, 3)); - EXPECT_EQ(expectedBoxModelFrame, model.GetBoundingBox(true)); + EXPECT_EQ(expectedBoxModelFrame, model.GetBoundingBox()); +} + +///////////////////////////////////////////////// +TEST(Model, CollideBitmask) +{ + Model model; + EXPECT_EQ(0x00, model.GetCollideBitmask()); + + // add link and verify bitmask is still empty + Entity &linkEnt = model.AddLink(); + EXPECT_EQ(0x00, linkEnt.GetCollideBitmask()); + EXPECT_EQ(0x00, model.GetCollideBitmask()); + + // add a collision and verify the model has the same collision bitmask + Link *link = static_cast(&linkEnt); + Entity &collisionEnt = link->AddCollision(); + Collision *collision = static_cast(&collisionEnt); + collision->SetCollideBitmask(0x01); + EXPECT_EQ(0x01, collision->GetCollideBitmask()); + EXPECT_EQ(0x01, linkEnt.GetCollideBitmask()); + EXPECT_EQ(0x01, model.GetCollideBitmask()); + + // add another collision and verify bitmasks are bitwise OR'd. + Entity &collisionEnt2 = link->AddCollision(); + Collision *collision2 = static_cast(&collisionEnt2); + collision2->SetCollideBitmask(0x04); + EXPECT_EQ(0x04, collision2->GetCollideBitmask()); + EXPECT_EQ(0x05, linkEnt.GetCollideBitmask()); + EXPECT_EQ(0x05, model.GetCollideBitmask()); + + // add another link with collision and verify + Entity &linkEnt2 = model.AddLink(); + Link *link2 = static_cast(&linkEnt2); + Entity &collisionEnt3 = link2->AddCollision(); + Collision *collision3 = static_cast(&collisionEnt3); + collision3->SetCollideBitmask(0x09); + EXPECT_EQ(0x09, collision3->GetCollideBitmask()); + EXPECT_EQ(0x09, linkEnt2.GetCollideBitmask()); + EXPECT_EQ(0x0D, model.GetCollideBitmask()); } diff --git a/tpe/plugin/src/EntityManagementFeatures.cc b/tpe/plugin/src/EntityManagementFeatures.cc index de33ccc5e..60ac6e77c 100644 --- a/tpe/plugin/src/EntityManagementFeatures.cc +++ b/tpe/plugin/src/EntityManagementFeatures.cc @@ -420,3 +420,31 @@ Identity EntityManagementFeatures::ConstructEmptyLink( } return this->GenerateInvalidId(); } + +///////////////////////////////////////////////// +void EntityManagementFeatures::SetCollisionFilterMask( + const Identity &_shapeID, const uint16_t _mask) +{ + auto collision = + this->ReferenceInterface(_shapeID)->collision; + collision->SetCollideBitmask(_mask); +} + +///////////////////////////////////////////////// +uint16_t EntityManagementFeatures::GetCollisionFilterMask( + const Identity &_shapeID) const +{ + const auto collision = + this->ReferenceInterface(_shapeID)->collision; + return collision->GetCollideBitmask(); +} + +///////////////////////////////////////////////// +void EntityManagementFeatures::RemoveCollisionFilterMask( + const Identity &_shapeID) +{ + auto collision = + this->ReferenceInterface(_shapeID)->collision; + // remove = reset to default bitmask + collision->SetCollideBitmask(0xFF); +} diff --git a/tpe/plugin/src/EntityManagementFeatures.hh b/tpe/plugin/src/EntityManagementFeatures.hh index 81b8296da..8f28d50a3 100644 --- a/tpe/plugin/src/EntityManagementFeatures.hh +++ b/tpe/plugin/src/EntityManagementFeatures.hh @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -40,7 +41,8 @@ struct EntityManagementFeatureList : FeatureList< RemoveEntities, ConstructEmptyWorldFeature, ConstructEmptyModelFeature, - ConstructEmptyLinkFeature + ConstructEmptyLinkFeature, + CollisionFilterMaskFeature > { }; class EntityManagementFeatures : @@ -135,6 +137,15 @@ class EntityManagementFeatures : public: Identity ConstructEmptyLink( const Identity &_modelID, const std::string &_name) override; + + // ----- Manage collision filter masks ----- + public: void SetCollisionFilterMask( + const Identity &_shapeID, const uint16_t _mask) override; + + public: uint16_t GetCollisionFilterMask( + const Identity &_shapeID) const override; + + public: void RemoveCollisionFilterMask(const Identity &_shapeID) override; }; } diff --git a/tpe/plugin/src/SDFFeatures.cc b/tpe/plugin/src/SDFFeatures.cc index 695b0ad62..d6693d9d1 100644 --- a/tpe/plugin/src/SDFFeatures.cc +++ b/tpe/plugin/src/SDFFeatures.cc @@ -15,17 +15,18 @@ * */ +#include "SDFFeatures.hh" + #include #include #include #include +#include #include -#include "SDFFeatures.hh" - -using namespace ignition; -using namespace physics; -using namespace tpeplugin; +namespace ignition { +namespace physics { +namespace tpeplugin { ///////////////////////////////////////////////// Identity SDFFeatures::ConstructSdfWorld( @@ -168,5 +169,31 @@ Identity SDFFeatures::ConstructSdfCollision( // and passed in as argument as there is no logic for searching resources // in ign-physics const auto collisionIdentity = this->AddCollision(link->GetId(), *collision); + + // set collide bitmask + uint16_t collideBitmask = 0xFF; + if (_sdfCollision.Element()) + { + // TODO(anyone) add category_bitmask as well + auto elem = _sdfCollision.Element(); + if (elem->HasElement("surface")) + { + elem = elem->GetElement("surface"); + if (elem->HasElement("contact")) + { + elem = elem->GetElement("contact"); + if (elem->HasElement("collide_bitmask")) + { + collideBitmask = elem->Get("collide_bitmask"); + this->SetCollisionFilterMask(collisionIdentity, collideBitmask); + } + } + } + } + return collisionIdentity; } + +} +} +} diff --git a/tpe/plugin/src/SDFFeatures.hh b/tpe/plugin/src/SDFFeatures.hh index 11180f609..0c9ca0b7c 100644 --- a/tpe/plugin/src/SDFFeatures.hh +++ b/tpe/plugin/src/SDFFeatures.hh @@ -18,10 +18,6 @@ #ifndef IGNITION_PHYSICS_TPE_PLUGIN_SRC_SDFFEATURES_HH_ #define IGNITION_PHYSICS_TPE_PLUGIN_SRC_SDFFEATURES_HH_ -#include -#include -#include - #include #include #include diff --git a/tpe/plugin/src/SimulationFeatures_TEST.cc b/tpe/plugin/src/SimulationFeatures_TEST.cc index 767de90aa..741698d8f 100644 --- a/tpe/plugin/src/SimulationFeatures_TEST.cc +++ b/tpe/plugin/src/SimulationFeatures_TEST.cc @@ -237,6 +237,47 @@ TEST_P(SimulationFeatures_TEST, FreeGroup) } } +TEST_P(SimulationFeatures_TEST, CollideBitmasks) +{ + const std::string library = GetParam(); + if (library.empty()) + return; + + auto worlds = LoadWorlds(library, TEST_WORLD_DIR "/shapes_bitmask.sdf"); + + for (const auto &world : worlds) + { + auto baseBox = world->GetModel("box_base"); + auto filteredBox = world->GetModel("box_filtered"); + auto collidingBox = world->GetModel("box_colliding"); + + StepWorld(world); + auto contacts = world->GetContactsFromLastStep(); + // Only box_colliding should collide with box_base + EXPECT_EQ(1u, contacts.size()); + + // Now disable collisions for the colliding box as well + auto collidingShape = collidingBox->GetLink(0)->GetShape(0); + auto filteredShape = filteredBox->GetLink(0)->GetShape(0); + collidingShape->SetCollisionFilterMask(0xF0); + // Also test the getter + EXPECT_EQ(0xF0, collidingShape->GetCollisionFilterMask()); + // Step and make sure there are no collisions + StepWorld(world); + contacts = world->GetContactsFromLastStep(); + EXPECT_EQ(0u, contacts.size()); + + // Now remove both filter masks (no collisions will be filtered) + // Equivalent to 0xFF + collidingShape->RemoveCollisionFilterMask(); + filteredShape->RemoveCollisionFilterMask(); + StepWorld(world); + // Expect box_filtered and box_colliding to collide with box_base + contacts = world->GetContactsFromLastStep(); + EXPECT_EQ(2u, contacts.size()); + } +} + TEST_P(SimulationFeatures_TEST, RetrieveContacts) { const std::string library = GetParam(); diff --git a/tpe/plugin/worlds/shapes_bitmask.sdf b/tpe/plugin/worlds/shapes_bitmask.sdf new file mode 100644 index 000000000..8aa289fce --- /dev/null +++ b/tpe/plugin/worlds/shapes_bitmask.sdf @@ -0,0 +1,154 @@ + + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 10.0 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + 0x01 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + 0 0.75 10.0 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + 0x02 + + + + + + + 1 1 1 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + + + + + 0 -0.75 10.0 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + 0x03 + + + + + + + 1 1 1 + + + + 1 1 0 1 + 1 1 0 1 + 1 1 0 1 + + + + + + +