Skip to content

Commit

Permalink
MaxContacts -> CollisionPairMaxTotalContacts
Browse files Browse the repository at this point in the history
Signed-off-by: Ian Chen <[email protected]>
  • Loading branch information
iche033 committed Jan 8, 2024
1 parent c428a3c commit 98b5e61
Show file tree
Hide file tree
Showing 7 changed files with 51 additions and 45 deletions.
10 changes: 5 additions & 5 deletions dartsim/src/GzOdeCollisionDetector.cc
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@ bool GzOdeCollisionDetector::collide(
CollisionResult *_result)
{
bool ret = OdeCollisionDetector::collide(_group, _option, _result);
this->LimitMaxContacts(_result);
this->LimitCollisionPairMaxTotalContacts(_result);
return ret;
}

Expand All @@ -64,24 +64,24 @@ bool GzOdeCollisionDetector::collide(
CollisionResult *_result)
{
bool ret = OdeCollisionDetector::collide(_group1, _group2, _option, _result);
this->LimitMaxContacts(_result);
this->LimitCollisionPairMaxTotalContacts(_result);
return ret;
}

/////////////////////////////////////////////////
void GzOdeCollisionDetector::SetMaxContacts(std::size_t _maxContacts)
void GzOdeCollisionDetector::SetCollisionPairMaxTotalContacts(std::size_t _maxContacts)
{
this->maxCollisionPairContacts = _maxContacts;
}

/////////////////////////////////////////////////
std::size_t GzOdeCollisionDetector::GetMaxContacts() const
std::size_t GzOdeCollisionDetector::GetCollisionPairMaxTotalContacts() const
{
return this->maxCollisionPairContacts;
}

/////////////////////////////////////////////////
void GzOdeCollisionDetector::LimitMaxContacts(
void GzOdeCollisionDetector::LimitCollisionPairMaxTotalContacts(
CollisionResult *_result)
{
if (this->maxCollisionPairContacts ==
Expand Down
6 changes: 3 additions & 3 deletions dartsim/src/GzOdeCollisionDetector.hh
Original file line number Diff line number Diff line change
Expand Up @@ -42,12 +42,12 @@ class GzOdeCollisionDetector : public dart::collision::OdeCollisionDetector
/// objects
/// \param[in] _maxContacts Maximum number of contacts between a pair of
/// collision objects.
public: void SetMaxContacts(std::size_t _maxContacts);
public: void SetCollisionPairMaxTotalContacts(std::size_t _maxContacts);

/// \brief Get the maximum number of contacts between a pair of collision
/// objects
/// \return Maximum number of contacts between a pair of collision objects.
public: std::size_t GetMaxContacts() const;
public: std::size_t GetCollisionPairMaxTotalContacts() const;


/// \brief Create the GzOdeCollisionDetector
Expand All @@ -60,7 +60,7 @@ class GzOdeCollisionDetector : public dart::collision::OdeCollisionDetector
/// The function modifies the contacts vector inside the CollisionResult
/// object to cap the number of contacts for each collision pair based on the
/// maxCollisionPairContacts value
private: void LimitMaxContacts(CollisionResult *_result);
private: void LimitCollisionPairMaxTotalContacts(CollisionResult *_result);

/// \brief Maximum number of contacts between a pair of collision objects.
private: std::size_t maxCollisionPairContacts =
Expand Down
8 changes: 4 additions & 4 deletions dartsim/src/WorldFeatures.cc
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,7 @@ WorldFeatures::LinearVectorType WorldFeatures::GetWorldGravity(
}

/////////////////////////////////////////////////
void WorldFeatures::SetWorldMaxContacts(
void WorldFeatures::SetWorldCollisionPairMaxTotalContacts(
const Identity &_id, std::size_t _maxContacts)
{
auto world = this->ReferenceInterface<dart::simulation::World>(_id);
Expand All @@ -110,7 +110,7 @@ void WorldFeatures::SetWorldMaxContacts(
collisionDetector);
if (odeCollisionDetector)
{
odeCollisionDetector->SetMaxContacts(_maxContacts);
odeCollisionDetector->SetCollisionPairMaxTotalContacts(_maxContacts);
}
else
{
Expand All @@ -120,7 +120,7 @@ void WorldFeatures::SetWorldMaxContacts(
}

/////////////////////////////////////////////////
std::size_t WorldFeatures::GetWorldMaxContacts(const Identity &_id)
std::size_t WorldFeatures::GetWorldCollisionPairMaxTotalContacts(const Identity &_id)
const
{
auto world = this->ReferenceInterface<dart::simulation::World>(_id);
Expand All @@ -131,7 +131,7 @@ std::size_t WorldFeatures::GetWorldMaxContacts(const Identity &_id)
collisionDetector);
if (odeCollisionDetector)
{
return odeCollisionDetector->GetMaxContacts();
return odeCollisionDetector->GetCollisionPairMaxTotalContacts();
}

return 0u;
Expand Down
6 changes: 3 additions & 3 deletions dartsim/src/WorldFeatures.hh
Original file line number Diff line number Diff line change
Expand Up @@ -30,8 +30,8 @@ namespace dartsim {

struct WorldFeatureList : FeatureList<
CollisionDetector,
CollisionPairMaxTotalContacts,
Gravity,
MaxContacts,
Solver
> { };

Expand All @@ -55,11 +55,11 @@ class WorldFeatures :
public: LinearVectorType GetWorldGravity(const Identity &_id) const override;

// Documentation inherited
public: void SetWorldMaxContacts(
public: void SetWorldCollisionPairMaxTotalContacts(
const Identity &_id, std::size_t _maxContacts) override;

// Documentation inherited
public: std::size_t GetWorldMaxContacts(const Identity &_id)
public: std::size_t GetWorldCollisionPairMaxTotalContacts(const Identity &_id)
const override;

// Documentation inherited
Expand Down
11 changes: 6 additions & 5 deletions include/gz/physics/World.hh
Original file line number Diff line number Diff line change
Expand Up @@ -131,7 +131,8 @@ namespace gz
};

/////////////////////////////////////////////////
class GZ_PHYSICS_VISIBLE MaxContacts: public virtual Feature
class GZ_PHYSICS_VISIBLE CollisionPairMaxTotalContacts:
public virtual Feature
{
/// \brief The World API for getting and setting max contacts.
public: template <typename PolicyT, typename FeaturesT>
Expand All @@ -140,12 +141,12 @@ namespace gz
/// \brief Set the maximum number of contacts allowed between two
/// entities.
/// \param[in] _maxContacts Maximum number of contacts.
public: void SetMaxContacts(std::size_t _maxContacts);
public: void SetCollisionPairMaxTotalContacts(std::size_t _maxContacts);

/// \brief Set the maximum number of contacts allowed between two
/// entities.
/// \return Maximum number of contacts.
public: std::size_t GetMaxContacts() const;
public: std::size_t GetCollisionPairMaxTotalContacts() const;
};

/// \private The implementation API for getting and setting max contacts.
Expand All @@ -156,14 +157,14 @@ namespace gz
/// contacts between two entities.
/// \param[in] _id Identity of the world.
/// \param[in] _maxContacts Maximum number of contacts.
public: virtual void SetWorldMaxContacts(
public: virtual void SetWorldCollisionPairMaxTotalContacts(
const Identity &_id, std::size_t _maxContacts) = 0;

/// \brief Implementation API for getting the maximum number of
/// contacts between two entities.
/// \param[in] _id Identity of the world.
/// \param[in] _maxContacts Maximum number of contacts.
public: virtual std::size_t GetWorldMaxContacts(
public: virtual std::size_t GetWorldCollisionPairMaxTotalContacts(
const Identity &_id) const = 0;
};
};
Expand Down
16 changes: 8 additions & 8 deletions include/gz/physics/detail/World.hh
Original file line number Diff line number Diff line change
Expand Up @@ -101,20 +101,20 @@ auto Gravity::World<PolicyT, FeaturesT>::GetGravity(

/////////////////////////////////////////////////
template <typename PolicyT, typename FeaturesT>
void MaxContacts::World<PolicyT, FeaturesT>::SetMaxContacts(
std::size_t _maxContacts)
void CollisionPairMaxTotalContacts::World<PolicyT, FeaturesT
>::SetCollisionPairMaxTotalContacts(std::size_t _maxContacts)
{
this->template Interface<MaxContacts>()
->SetWorldMaxContacts(this->identity, _maxContacts);
this->template Interface<CollisionPairMaxTotalContacts>()
->SetWorldCollisionPairMaxTotalContacts(this->identity, _maxContacts);
}

/////////////////////////////////////////////////
template <typename PolicyT, typename FeaturesT>
std::size_t MaxContacts::World<PolicyT, FeaturesT>::
GetMaxContacts() const
std::size_t CollisionPairMaxTotalContacts::World<PolicyT, FeaturesT>::
GetCollisionPairMaxTotalContacts() const
{
return this->template Interface<MaxContacts>()
->GetWorldMaxContacts(this->identity);
return this->template Interface<CollisionPairMaxTotalContacts>()
->GetWorldCollisionPairMaxTotalContacts(this->identity);
}

/////////////////////////////////////////////////
Expand Down
39 changes: 22 additions & 17 deletions test/common_test/simulation_features.cc
Original file line number Diff line number Diff line change
Expand Up @@ -220,49 +220,54 @@ TYPED_TEST(SimulationFeaturesContactsTest, Contacts)
}

// The features that an engine must have to be loaded by this loader.
struct FeaturesMaxContacts : gz::physics::FeatureList<
struct FeaturesCollisionPairMaxTotalContacts : gz::physics::FeatureList<
gz::physics::sdf::ConstructSdfWorld,
gz::physics::GetContactsFromLastStepFeature,
gz::physics::ForwardStep,
gz::physics::MaxContacts
gz::physics::CollisionPairMaxTotalContacts
> {};

template <class T>
class SimulationFeaturesMaxContactsTest :
class SimulationFeaturesCollisionPairMaxTotalContactsTest :
public SimulationFeaturesTest<T>{};
using SimulationFeaturesMaxContactsTestTypes =
::testing::Types<FeaturesMaxContacts>;
TYPED_TEST_SUITE(SimulationFeaturesMaxContactsTest,
SimulationFeaturesMaxContactsTestTypes);
using SimulationFeaturesCollisionPairMaxTotalContactsTestTypes =
::testing::Types<FeaturesCollisionPairMaxTotalContacts>;
TYPED_TEST_SUITE(SimulationFeaturesCollisionPairMaxTotalContactsTest,
SimulationFeaturesCollisionPairMaxTotalContactsTestTypes);

/////////////////////////////////////////////////
TYPED_TEST(SimulationFeaturesMaxContactsTest, MaxContacts)
TYPED_TEST(SimulationFeaturesCollisionPairMaxTotalContactsTest,
CollisionPairMaxTotalContacts)
{
for (const std::string &name : this->pluginNames)
{
auto world = LoadPluginAndWorld<FeaturesMaxContacts>(
auto world = LoadPluginAndWorld<FeaturesCollisionPairMaxTotalContacts>(
this->loader,
name,
gz::common::joinPaths(TEST_WORLD_DIR, "shapes.world"));
auto checkedOutput = StepWorld<FeaturesMaxContacts>(world, true, 1).first;
auto checkedOutput = StepWorld<FeaturesCollisionPairMaxTotalContacts>(
world, true, 1).first;
EXPECT_TRUE(checkedOutput);

auto contacts = world->GetContactsFromLastStep();
EXPECT_EQ(std::numeric_limits<std::size_t>::max(), world->GetMaxContacts());
EXPECT_EQ(std::numeric_limits<std::size_t>::max(),
world->GetCollisionPairMaxTotalContacts());
// Large box collides with other shapes
EXPECT_GT(contacts.size(), 30u);

world->SetMaxContacts(1u);
EXPECT_EQ(1u, world->GetMaxContacts());
checkedOutput = StepWorld<FeaturesMaxContacts>(world, true, 1).first;
world->SetCollisionPairMaxTotalContacts(1u);
EXPECT_EQ(1u, world->GetCollisionPairMaxTotalContacts());
checkedOutput = StepWorld<FeaturesCollisionPairMaxTotalContacts>(
world, true, 1).first;
EXPECT_TRUE(checkedOutput);

contacts = world->GetContactsFromLastStep();
EXPECT_EQ(4u, contacts.size());

world->SetMaxContacts(0u);
EXPECT_EQ(0u, world->GetMaxContacts());
checkedOutput = StepWorld<FeaturesMaxContacts>(world, true, 1).first;
world->SetCollisionPairMaxTotalContacts(0u);
EXPECT_EQ(0u, world->GetCollisionPairMaxTotalContacts());
checkedOutput = StepWorld<FeaturesCollisionPairMaxTotalContacts>(
world, true, 1).first;
EXPECT_TRUE(checkedOutput);

contacts = world->GetContactsFromLastStep();
Expand Down

0 comments on commit 98b5e61

Please sign in to comment.