From 5490d9b32269f3851fe9149b68d3e676a1f8628d Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 20 Dec 2023 00:11:10 +0000 Subject: [PATCH 1/3] parse and set max_contacts Signed-off-by: Ian Chen --- src/systems/physics/Physics.cc | 35 +++++++++++++++++++++++++++++++++- 1 file changed, 34 insertions(+), 1 deletion(-) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 33e3db4224..27d33f531f 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -617,6 +617,12 @@ class gz::sim::systems::PhysicsPrivate public: struct SolverFeatureList : gz::physics::FeatureList< gz::physics::Solver>{}; + ////////////////////////////////////////////////// + // MaxContacts + /// \brief Feature list for setting and getting the solver + public: struct MaxContactsFeatureList : gz::physics::FeatureList< + gz::physics::MaxContacts>{}; + ////////////////////////////////////////////////// // Nested Models @@ -642,7 +648,8 @@ class gz::sim::systems::PhysicsPrivate NestedModelFeatureList, CollisionDetectorFeatureList, SolverFeatureList, - WorldModelFeatureList + WorldModelFeatureList, + MaxContactsFeatureList >; /// \brief A map between world entity ids in the ECM to World Entities in @@ -1045,6 +1052,32 @@ void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm, } } + auto physicsComp = + _ecm.Component(_entity); + if (physicsComp) + { + auto maxContactsFeature = + this->entityWorldMap.EntityCast( + _entity); + if (!maxContactsFeature) + { + static bool informed{false}; + if (!informed) + { + gzdbg << "Attempting to set physics options, but the " + << "phyiscs engine doesn't support feature " + << "[MaxContacts]. Options will be ignored." + << std::endl; + informed = true; + } + } + else + { + maxContactsFeature->SetMaxContacts( + physicsComp->Data().MaxContacts()); + } + } + // World Model proxy (used for joints directly under in SDF) auto worldModelFeature = this->entityWorldMap.EntityCast(_entity); From ccc4c15bd451dc573cd52e131988b6e179ad36f0 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 8 Jan 2024 21:32:18 +0000 Subject: [PATCH 2/3] MaxContacts -> CollisionPairMaxTotalContacts Signed-off-by: Ian Chen --- src/systems/physics/Physics.cc | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 27d33f531f..0cc202ed6a 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -618,10 +618,12 @@ class gz::sim::systems::PhysicsPrivate gz::physics::Solver>{}; ////////////////////////////////////////////////// - // MaxContacts - /// \brief Feature list for setting and getting the solver - public: struct MaxContactsFeatureList : gz::physics::FeatureList< - gz::physics::MaxContacts>{}; + // CollisionPairMaxTotalContacts + /// \brief Feature list for setting and getting the max total contacts for + /// collision pairs + public: struct CollisionPairMaxTotalContactsFeatureList : + gz::physics::FeatureList< + gz::physics::CollisionPairMaxTotalContacts>{}; ////////////////////////////////////////////////// // Nested Models @@ -649,7 +651,7 @@ class gz::sim::systems::PhysicsPrivate CollisionDetectorFeatureList, SolverFeatureList, WorldModelFeatureList, - MaxContactsFeatureList + CollisionPairMaxTotalContactsFeatureList >; /// \brief A map between world entity ids in the ECM to World Entities in @@ -1057,8 +1059,8 @@ void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm, if (physicsComp) { auto maxContactsFeature = - this->entityWorldMap.EntityCast( - _entity); + this->entityWorldMap.EntityCast< + CollisionPairMaxTotalContactsFeatureList>(_entity); if (!maxContactsFeature) { static bool informed{false}; @@ -1066,14 +1068,15 @@ void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm, { gzdbg << "Attempting to set physics options, but the " << "phyiscs engine doesn't support feature " - << "[MaxContacts]. Options will be ignored." + << "[CollisionPairMaxTotalContacts]. " + << "Options will be ignored." << std::endl; informed = true; } } else { - maxContactsFeature->SetMaxContacts( + maxContactsFeature->SetCollisionPairMaxTotalContacts( physicsComp->Data().MaxContacts()); } } From f3b06c338cef4b46d57e5ab8453d93094a66a926 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 13 Jan 2024 01:19:19 +0000 Subject: [PATCH 3/3] CollisionPairMaxTotalContacts -> CollisionPairMaxContacts Signed-off-by: Ian Chen --- src/systems/physics/Physics.cc | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 0cc202ed6a..fd4b69cead 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -618,12 +618,12 @@ class gz::sim::systems::PhysicsPrivate gz::physics::Solver>{}; ////////////////////////////////////////////////// - // CollisionPairMaxTotalContacts + // CollisionPairMaxContacts /// \brief Feature list for setting and getting the max total contacts for /// collision pairs - public: struct CollisionPairMaxTotalContactsFeatureList : + public: struct CollisionPairMaxContactsFeatureList : gz::physics::FeatureList< - gz::physics::CollisionPairMaxTotalContacts>{}; + gz::physics::CollisionPairMaxContacts>{}; ////////////////////////////////////////////////// // Nested Models @@ -651,7 +651,7 @@ class gz::sim::systems::PhysicsPrivate CollisionDetectorFeatureList, SolverFeatureList, WorldModelFeatureList, - CollisionPairMaxTotalContactsFeatureList + CollisionPairMaxContactsFeatureList >; /// \brief A map between world entity ids in the ECM to World Entities in @@ -1060,7 +1060,7 @@ void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm, { auto maxContactsFeature = this->entityWorldMap.EntityCast< - CollisionPairMaxTotalContactsFeatureList>(_entity); + CollisionPairMaxContactsFeatureList>(_entity); if (!maxContactsFeature) { static bool informed{false}; @@ -1068,7 +1068,7 @@ void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm, { gzdbg << "Attempting to set physics options, but the " << "phyiscs engine doesn't support feature " - << "[CollisionPairMaxTotalContacts]. " + << "[CollisionPairMaxContacts]. " << "Options will be ignored." << std::endl; informed = true; @@ -1076,7 +1076,7 @@ void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm, } else { - maxContactsFeature->SetCollisionPairMaxTotalContacts( + maxContactsFeature->SetCollisionPairMaxContacts( physicsComp->Data().MaxContacts()); } }