Skip to content

Commit

Permalink
CollisionPairMaxTotalContacts -> CollisionPairMaxContacts
Browse files Browse the repository at this point in the history
Signed-off-by: Ian Chen <ichen@openrobotics.org>
  • Loading branch information
iche033 committed Jan 13, 2024
1 parent e4e51e1 commit b140329
Show file tree
Hide file tree
Showing 7 changed files with 46 additions and 46 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->LimitCollisionPairMaxTotalContacts(_result);
this->LimitCollisionPairMaxContacts(_result);
return ret;
}

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

Check warning on line 68 in dartsim/src/GzOdeCollisionDetector.cc

View check run for this annotation

Codecov / codecov/patch

dartsim/src/GzOdeCollisionDetector.cc#L66-L68

Added lines #L66 - L68 were not covered by tests
}

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

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

/////////////////////////////////////////////////
void GzOdeCollisionDetector::LimitCollisionPairMaxTotalContacts(
void GzOdeCollisionDetector::LimitCollisionPairMaxContacts(
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 SetCollisionPairMaxTotalContacts(std::size_t _maxContacts);
public: void SetCollisionPairMaxContacts(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 GetCollisionPairMaxTotalContacts() const;
public: std::size_t GetCollisionPairMaxContacts() 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 LimitCollisionPairMaxTotalContacts(CollisionResult *_result);
private: void LimitCollisionPairMaxContacts(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::SetWorldCollisionPairMaxTotalContacts(
void WorldFeatures::SetWorldCollisionPairMaxContacts(
const Identity &_id, std::size_t _maxContacts)
{
auto world = this->ReferenceInterface<dart::simulation::World>(_id);
Expand All @@ -110,7 +110,7 @@ void WorldFeatures::SetWorldCollisionPairMaxTotalContacts(
collisionDetector);
if (odeCollisionDetector)
{
odeCollisionDetector->SetCollisionPairMaxTotalContacts(_maxContacts);
odeCollisionDetector->SetCollisionPairMaxContacts(_maxContacts);
}
else
{
Expand All @@ -120,7 +120,7 @@ void WorldFeatures::SetWorldCollisionPairMaxTotalContacts(
}

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

return 0u;

Check warning on line 137 in dartsim/src/WorldFeatures.cc

View check run for this annotation

Codecov / codecov/patch

dartsim/src/WorldFeatures.cc#L137

Added line #L137 was not covered by tests
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,7 +30,7 @@ namespace dartsim {

struct WorldFeatureList : FeatureList<
CollisionDetector,
CollisionPairMaxTotalContacts,
CollisionPairMaxContacts,
Gravity,
Solver
> { };
Expand All @@ -55,11 +55,11 @@ class WorldFeatures :
public: LinearVectorType GetWorldGravity(const Identity &_id) const override;

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

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

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

/////////////////////////////////////////////////
class GZ_PHYSICS_VISIBLE CollisionPairMaxTotalContacts:
class GZ_PHYSICS_VISIBLE CollisionPairMaxContacts:
public virtual Feature
{
/// \brief The World API for getting and setting max contacts.
Expand All @@ -141,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 SetCollisionPairMaxTotalContacts(std::size_t _maxContacts);
public: void SetCollisionPairMaxContacts(std::size_t _maxContacts);

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

/// \private The implementation API for getting and setting max contacts.
Expand All @@ -157,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 SetWorldCollisionPairMaxTotalContacts(
public: virtual void SetWorldCollisionPairMaxContacts(
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 GetWorldCollisionPairMaxTotalContacts(
public: virtual std::size_t GetWorldCollisionPairMaxContacts(
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 CollisionPairMaxTotalContacts::World<PolicyT, FeaturesT
>::SetCollisionPairMaxTotalContacts(std::size_t _maxContacts)
void CollisionPairMaxContacts::World<PolicyT, FeaturesT
>::SetCollisionPairMaxContacts(std::size_t _maxContacts)
{
this->template Interface<CollisionPairMaxTotalContacts>()
->SetWorldCollisionPairMaxTotalContacts(this->identity, _maxContacts);
this->template Interface<CollisionPairMaxContacts>()
->SetWorldCollisionPairMaxContacts(this->identity, _maxContacts);
}

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

/////////////////////////////////////////////////
Expand Down
36 changes: 18 additions & 18 deletions test/common_test/simulation_features.cc
Original file line number Diff line number Diff line change
Expand Up @@ -220,53 +220,53 @@ TYPED_TEST(SimulationFeaturesContactsTest, Contacts)
}

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

template <class T>
class SimulationFeaturesCollisionPairMaxTotalContactsTest :
class SimulationFeaturesCollisionPairMaxContactsTest :
public SimulationFeaturesTest<T>{};
using SimulationFeaturesCollisionPairMaxTotalContactsTestTypes =
::testing::Types<FeaturesCollisionPairMaxTotalContacts>;
TYPED_TEST_SUITE(SimulationFeaturesCollisionPairMaxTotalContactsTest,
SimulationFeaturesCollisionPairMaxTotalContactsTestTypes);
using SimulationFeaturesCollisionPairMaxContactsTestTypes =
::testing::Types<FeaturesCollisionPairMaxContacts>;
TYPED_TEST_SUITE(SimulationFeaturesCollisionPairMaxContactsTest,
SimulationFeaturesCollisionPairMaxContactsTestTypes);

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

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

world->SetCollisionPairMaxTotalContacts(1u);
EXPECT_EQ(1u, world->GetCollisionPairMaxTotalContacts());
checkedOutput = StepWorld<FeaturesCollisionPairMaxTotalContacts>(
world->SetCollisionPairMaxContacts(1u);
EXPECT_EQ(1u, world->GetCollisionPairMaxContacts());
checkedOutput = StepWorld<FeaturesCollisionPairMaxContacts>(
world, true, 1).first;
EXPECT_TRUE(checkedOutput);

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

world->SetCollisionPairMaxTotalContacts(0u);
EXPECT_EQ(0u, world->GetCollisionPairMaxTotalContacts());
checkedOutput = StepWorld<FeaturesCollisionPairMaxTotalContacts>(
world->SetCollisionPairMaxContacts(0u);
EXPECT_EQ(0u, world->GetCollisionPairMaxContacts());
checkedOutput = StepWorld<FeaturesCollisionPairMaxContacts>(
world, true, 1).first;
EXPECT_TRUE(checkedOutput);

Expand Down

0 comments on commit b140329

Please sign in to comment.