From de23435221d6be0f089d549f217ff66a899d7ef9 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 23 Jun 2020 15:33:22 -0700 Subject: [PATCH 1/8] Add collide bitmask and CollisionFilterMaskFeature Signed-off-by: Ian Chen --- tpe/lib/src/Collision.cc | 15 ++ tpe/lib/src/Collision.hh | 7 + tpe/lib/src/CollisionDetector.cc | 29 +++- tpe/lib/src/Collision_TEST.cc | 4 + tpe/lib/src/Entity.cc | 11 ++ tpe/lib/src/Entity.hh | 4 + tpe/lib/src/Model_TEST.cc | 39 ++++++ tpe/plugin/src/EntityManagementFeatures.cc | 46 ++++++ tpe/plugin/src/EntityManagementFeatures.hh | 13 +- tpe/plugin/src/SDFFeatures.cc | 22 +++ tpe/plugin/src/SimulationFeatures_TEST.cc | 41 ++++++ tpe/plugin/worlds/shapes_bitmask.sdf | 154 +++++++++++++++++++++ 12 files changed, 382 insertions(+), 3 deletions(-) create mode 100644 tpe/plugin/worlds/shapes_bitmask.sdf diff --git a/tpe/lib/src/Collision.cc b/tpe/lib/src/Collision.cc index 6993474c3..fcb59c494 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,15 @@ 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; +} + +////////////////////////////////////////////////// +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..9cf8ebdc8 100644 --- a/tpe/lib/src/CollisionDetector.cc +++ b/tpe/lib/src/CollisionDetector.cc @@ -31,14 +31,26 @@ std::vector CollisionDetector::CheckCollisions( // contacts to be filled and returned std::vector contacts; + // cache of collide bitmasks + std::map collideBitmasks; + // cache of axis aligned box in world frame std::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 = 0xFF; + auto cb1It = collideBitmasks.find(e1->GetId()); + if (cb1It == collideBitmasks.end()) + collideBitmasks[e1->GetId()] = e1->GetCollideBitmask(); + else + cb1 = cb1It->second; + // 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 +68,22 @@ std::vector CollisionDetector::CheckCollisions( for (auto it2 = std::next(it, 1); it2 != _entities.end(); ++it2) { + std::shared_ptr e2 = it2->second; + + // Get collid bitmask for entity 2 + uint16_t cb2 = 0xFF; + auto cb2It = collideBitmasks.find(e2->GetId()); + if (cb2It == collideBitmasks.end()) + collideBitmasks[e2->GetId()] = e2->GetCollideBitmask(); + else + cb2 = cb2It->second; + + // 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..00bfa92ee 100644 --- a/tpe/lib/src/Entity.cc +++ b/tpe/lib/src/Entity.cc @@ -240,6 +240,17 @@ void Entity::UpdateBoundingBox(bool _force) this->dataPtr->bbox = box; } +////////////////////////////////////////////////// +uint16_t Entity::GetCollideBitmask() const +{ + uint16_t mask = 0u; + for (auto &it : this->dataPtr->children) + { + mask |= it.second->GetCollideBitmask(); + } + return mask; +} + ////////////////////////////////////////////////// std::map> &Entity::GetChildren() const { diff --git a/tpe/lib/src/Entity.hh b/tpe/lib/src/Entity.hh index b9cd1cd5e..eddcd8a6e 100644 --- a/tpe/lib/src/Entity.hh +++ b/tpe/lib/src/Entity.hh @@ -131,6 +131,10 @@ 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; + /// \brief Get number of children /// \return Map of child id's to child entities protected: std::map> &GetChildren() diff --git a/tpe/lib/src/Model_TEST.cc b/tpe/lib/src/Model_TEST.cc index fe07818c1..a9bb22ced 100644 --- a/tpe/lib/src/Model_TEST.cc +++ b/tpe/lib/src/Model_TEST.cc @@ -160,3 +160,42 @@ TEST(Model, BoundingBox) math::Vector3d(-2, -2, -2.5), math::Vector3d(2, 3, 3)); EXPECT_EQ(expectedBoxModelFrame, model.GetBoundingBox(true)); } + +///////////////////////////////////////////////// +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..ffd296383 100644 --- a/tpe/plugin/src/EntityManagementFeatures.cc +++ b/tpe/plugin/src/EntityManagementFeatures.cc @@ -420,3 +420,49 @@ Identity EntityManagementFeatures::ConstructEmptyLink( } return this->GenerateInvalidId(); } + +///////////////////////////////////////////////// +void EntityManagementFeatures::SetCollisionFilterMask( + const Identity &_shapeID, const uint16_t _mask) +{ +/* const auto shapeNode = this->ReferenceInterface(_shapeID)->node; + const std::size_t worldID = GetWorldOfShapeNode(this, shapeNode); + const auto filterPtr = GetFilterPtr(this, worldID); + filterPtr->SetIgnoredCollision(shapeNode, _mask); +*/ + auto collision = + this->ReferenceInterface(_shapeID)->collision; + collision->SetCollideBitmask(_mask); +} + +///////////////////////////////////////////////// +uint16_t EntityManagementFeatures::GetCollisionFilterMask( + const Identity &_shapeID) const +{ +/* + const auto shapeNode = this->ReferenceInterface(_shapeID)->node; + const std::size_t worldID = GetWorldOfShapeNode(this, shapeNode); + const auto filterPtr = GetFilterPtr(this, worldID); + return filterPtr->GetIgnoredCollision(shapeNode); +*/ + const auto collision = + this->ReferenceInterface(_shapeID)->collision; + return collision->GetCollideBitmask(); +} + +///////////////////////////////////////////////// +void EntityManagementFeatures::RemoveCollisionFilterMask( + const Identity &_shapeID) +{ +/* + const auto shapeNode = this->ReferenceInterface(_shapeID)->node; + const std::size_t worldID = GetWorldOfShapeNode(this, shapeNode); + const auto filterPtr = GetFilterPtr(this, worldID); + filterPtr->RemoveIgnoredCollision(shapeNode); +*/ + 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..cfd605728 100644 --- a/tpe/plugin/src/SDFFeatures.cc +++ b/tpe/plugin/src/SDFFeatures.cc @@ -168,5 +168,27 @@ 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/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 + + + + + + + From e89fa8e17db600ec7d03fa71f7592586185c1f14 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 23 Jun 2020 15:36:31 -0700 Subject: [PATCH 2/8] cleanup Signed-off-by: Ian Chen --- tpe/lib/src/CollisionDetector.cc | 2 +- tpe/plugin/src/EntityManagementFeatures.cc | 18 ------------------ 2 files changed, 1 insertion(+), 19 deletions(-) diff --git a/tpe/lib/src/CollisionDetector.cc b/tpe/lib/src/CollisionDetector.cc index 9cf8ebdc8..b28463e96 100644 --- a/tpe/lib/src/CollisionDetector.cc +++ b/tpe/lib/src/CollisionDetector.cc @@ -70,7 +70,7 @@ std::vector CollisionDetector::CheckCollisions( { std::shared_ptr e2 = it2->second; - // Get collid bitmask for entity 2 + // Get collide bitmask for entity 2 uint16_t cb2 = 0xFF; auto cb2It = collideBitmasks.find(e2->GetId()); if (cb2It == collideBitmasks.end()) diff --git a/tpe/plugin/src/EntityManagementFeatures.cc b/tpe/plugin/src/EntityManagementFeatures.cc index ffd296383..60ac6e77c 100644 --- a/tpe/plugin/src/EntityManagementFeatures.cc +++ b/tpe/plugin/src/EntityManagementFeatures.cc @@ -425,11 +425,6 @@ Identity EntityManagementFeatures::ConstructEmptyLink( void EntityManagementFeatures::SetCollisionFilterMask( const Identity &_shapeID, const uint16_t _mask) { -/* const auto shapeNode = this->ReferenceInterface(_shapeID)->node; - const std::size_t worldID = GetWorldOfShapeNode(this, shapeNode); - const auto filterPtr = GetFilterPtr(this, worldID); - filterPtr->SetIgnoredCollision(shapeNode, _mask); -*/ auto collision = this->ReferenceInterface(_shapeID)->collision; collision->SetCollideBitmask(_mask); @@ -439,12 +434,6 @@ void EntityManagementFeatures::SetCollisionFilterMask( uint16_t EntityManagementFeatures::GetCollisionFilterMask( const Identity &_shapeID) const { -/* - const auto shapeNode = this->ReferenceInterface(_shapeID)->node; - const std::size_t worldID = GetWorldOfShapeNode(this, shapeNode); - const auto filterPtr = GetFilterPtr(this, worldID); - return filterPtr->GetIgnoredCollision(shapeNode); -*/ const auto collision = this->ReferenceInterface(_shapeID)->collision; return collision->GetCollideBitmask(); @@ -454,15 +443,8 @@ uint16_t EntityManagementFeatures::GetCollisionFilterMask( void EntityManagementFeatures::RemoveCollisionFilterMask( const Identity &_shapeID) { -/* - const auto shapeNode = this->ReferenceInterface(_shapeID)->node; - const std::size_t worldID = GetWorldOfShapeNode(this, shapeNode); - const auto filterPtr = GetFilterPtr(this, worldID); - filterPtr->RemoveIgnoredCollision(shapeNode); -*/ auto collision = this->ReferenceInterface(_shapeID)->collision; // remove = reset to default bitmask collision->SetCollideBitmask(0xFF); } - From 1555caa64c5d4c7a7462e9e424833b0c3c565420 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 24 Jun 2020 11:42:00 -0700 Subject: [PATCH 3/8] use unordered map in tpe collision checking, remove bitmask cache Signed-off-by: Ian Chen --- tpe/lib/src/CollisionDetector.cc | 21 +++++---------------- 1 file changed, 5 insertions(+), 16 deletions(-) diff --git a/tpe/lib/src/CollisionDetector.cc b/tpe/lib/src/CollisionDetector.cc index b28463e96..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" @@ -31,23 +33,15 @@ std::vector CollisionDetector::CheckCollisions( // contacts to be filled and returned std::vector contacts; - // cache of collide bitmasks - std::map collideBitmasks; - // 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 = 0xFF; - auto cb1It = collideBitmasks.find(e1->GetId()); - if (cb1It == collideBitmasks.end()) - collideBitmasks[e1->GetId()] = e1->GetCollideBitmask(); - else - cb1 = cb1It->second; + uint16_t cb1 = e1->GetCollideBitmask(); // Get world axis aligned box for entity 1 math::AxisAlignedBox wb1; @@ -71,12 +65,7 @@ std::vector CollisionDetector::CheckCollisions( std::shared_ptr e2 = it2->second; // Get collide bitmask for entity 2 - uint16_t cb2 = 0xFF; - auto cb2It = collideBitmasks.find(e2->GetId()); - if (cb2It == collideBitmasks.end()) - collideBitmasks[e2->GetId()] = e2->GetCollideBitmask(); - else - cb2 = cb2It->second; + uint16_t cb2 = e2->GetCollideBitmask(); // collision filtering using collide bitmask if ((cb1 & cb2) == 0) From 849bb90a6501367ab95abee5b4c4261ef48484c7 Mon Sep 17 00:00:00 2001 From: iche033 Date: Mon, 29 Jun 2020 11:36:45 -0700 Subject: [PATCH 4/8] tpe cache collide bitmask and bbox (#73) Signed-off-by: Ian Chen --- tpe/lib/src/Collision.cc | 2 ++ tpe/lib/src/Entity.cc | 48 +++++++++++++++++++++++++++++++++++---- tpe/lib/src/Entity.hh | 15 ++++++++++++ tpe/lib/src/Link.cc | 2 ++ tpe/lib/src/Model.cc | 3 +++ tpe/lib/src/Model_TEST.cc | 8 +++---- 6 files changed, 70 insertions(+), 8 deletions(-) diff --git a/tpe/lib/src/Collision.cc b/tpe/lib/src/Collision.cc index fcb59c494..cd1a6dadc 100644 --- a/tpe/lib/src/Collision.cc +++ b/tpe/lib/src/Collision.cc @@ -115,6 +115,8 @@ math::AxisAlignedBox Collision::GetBoundingBox(bool /*_force*/) // NOLINT void Collision::SetCollideBitmask(uint16_t _mask) { this->dataPtr->collideBitmask = _mask; + if (this->GetParent()) + this->GetParent()->ChildrenChanged(); } ////////////////////////////////////////////////// diff --git a/tpe/lib/src/Entity.cc b/tpe/lib/src/Entity.cc index 00bfa92ee..b9a1b0ecd 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; } } @@ -243,12 +255,18 @@ void Entity::UpdateBoundingBox(bool _force) ////////////////////////////////////////////////// uint16_t Entity::GetCollideBitmask() const { - uint16_t mask = 0u; - for (auto &it : this->dataPtr->children) + if (this->dataPtr->collideBitmaskDirty) { - mask |= it.second->GetCollideBitmask(); + uint16_t mask = 0u; + for (auto &it : this->dataPtr->children) + { + mask |= it.second->GetCollideBitmask(); + } + this->dataPtr->collideBitmask = mask; + this->dataPtr->collideBitmaskDirty = false; } - return mask; + + return this->dataPtr->collideBitmask; } ////////////////////////////////////////////////// @@ -262,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 eddcd8a6e..8bdcfbc78 100644 --- a/tpe/lib/src/Entity.hh +++ b/tpe/lib/src/Entity.hh @@ -135,6 +135,21 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Entity /// \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 a9bb22ced..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,11 +154,11 @@ 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()); } ///////////////////////////////////////////////// From 6c724b59c10e0a7a0ce2e6d2ca9ce42039f7b9b4 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 6 Jul 2020 12:59:08 -0700 Subject: [PATCH 5/8] add sdf include Signed-off-by: Ian Chen --- tpe/plugin/src/SDFFeatures.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/tpe/plugin/src/SDFFeatures.cc b/tpe/plugin/src/SDFFeatures.cc index cfd605728..fb4d31bd1 100644 --- a/tpe/plugin/src/SDFFeatures.cc +++ b/tpe/plugin/src/SDFFeatures.cc @@ -17,6 +17,7 @@ #include #include +#include #include #include #include From 9e1dbbe92f990ba34fcb2ef3b0c88aff6e986c90 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 6 Jul 2020 13:55:19 -0700 Subject: [PATCH 6/8] fixing windows build Signed-off-by: Ian Chen --- tpe/plugin/src/SDFFeatures.cc | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/tpe/plugin/src/SDFFeatures.cc b/tpe/plugin/src/SDFFeatures.cc index fb4d31bd1..fcce9bf0e 100644 --- a/tpe/plugin/src/SDFFeatures.cc +++ b/tpe/plugin/src/SDFFeatures.cc @@ -17,7 +17,6 @@ #include #include -#include #include #include #include @@ -184,7 +183,7 @@ Identity SDFFeatures::ConstructSdfCollision( elem = elem->GetElement("contact"); if (elem->HasElement("collide_bitmask")) { - collideBitmask = elem->Get("collide_bitmask"); + collideBitmask = elem->Get("collide_bitmask"); this->SetCollisionFilterMask(collisionIdentity, collideBitmask); } } From bcda4f7a95d7f005da7de0edc3b5acd43665c676 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 6 Jul 2020 14:15:23 -0700 Subject: [PATCH 7/8] fixing windows Signed-off-by: Ian Chen --- tpe/lib/src/Entity.cc | 2 +- tpe/plugin/src/SDFFeatures.cc | 7 ++++--- tpe/plugin/src/SDFFeatures.hh | 4 ---- 3 files changed, 5 insertions(+), 8 deletions(-) diff --git a/tpe/lib/src/Entity.cc b/tpe/lib/src/Entity.cc index b9a1b0ecd..f2610f06a 100644 --- a/tpe/lib/src/Entity.cc +++ b/tpe/lib/src/Entity.cc @@ -285,7 +285,7 @@ std::size_t Entity::GetNextId() void Entity::ChildrenChanged() { this->dataPtr->bboxDirty = true; - this->dataPtr->collideBitmaskDirty= true; + this->dataPtr->collideBitmaskDirty = true; if (this->dataPtr->parent) this->dataPtr->parent->ChildrenChanged(); diff --git a/tpe/plugin/src/SDFFeatures.cc b/tpe/plugin/src/SDFFeatures.cc index fcce9bf0e..bd902f574 100644 --- a/tpe/plugin/src/SDFFeatures.cc +++ b/tpe/plugin/src/SDFFeatures.cc @@ -15,14 +15,15 @@ * */ +#include "SDFFeatures.hh" + #include #include #include #include +#include #include -#include "SDFFeatures.hh" - using namespace ignition; using namespace physics; using namespace tpeplugin; @@ -183,7 +184,7 @@ Identity SDFFeatures::ConstructSdfCollision( elem = elem->GetElement("contact"); if (elem->HasElement("collide_bitmask")) { - collideBitmask = elem->Get("collide_bitmask"); + collideBitmask = elem->Get("collide_bitmask"); this->SetCollisionFilterMask(collisionIdentity, collideBitmask); } } 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 From 82578e9baf994c3cbeab5468ac3923b99c8aed32 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 6 Jul 2020 14:26:34 -0700 Subject: [PATCH 8/8] fixing windows Signed-off-by: Ian Chen --- tpe/plugin/src/SDFFeatures.cc | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/tpe/plugin/src/SDFFeatures.cc b/tpe/plugin/src/SDFFeatures.cc index bd902f574..d6693d9d1 100644 --- a/tpe/plugin/src/SDFFeatures.cc +++ b/tpe/plugin/src/SDFFeatures.cc @@ -24,9 +24,9 @@ #include #include -using namespace ignition; -using namespace physics; -using namespace tpeplugin; +namespace ignition { +namespace physics { +namespace tpeplugin { ///////////////////////////////////////////////// Identity SDFFeatures::ConstructSdfWorld( @@ -193,3 +193,7 @@ Identity SDFFeatures::ConstructSdfCollision( return collisionIdentity; } + +} +} +}