diff --git a/CHANGELOG.md b/CHANGELOG.md index 53773ee61f86b..0938061aa2c1f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,10 @@ * Added FOV API to OSG viewer: [#1048](https://github.com/dartsim/dart/pull/1048) +* Simulation + + * Added World::hasSkeleton(): [#1050](https://github.com/dartsim/dart/pull/1050) + ### [DART 6.4.0 (2018-03-26)](https://github.com/dartsim/dart/milestone/39?closed=1) * Common diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index db31e47066f30..51ff870a224ce 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -391,6 +391,13 @@ std::set World::removeAllSkeletons() return ptrs; } +//============================================================================== +bool World::hasSkeleton(const dynamics::ConstSkeletonPtr& skeleton) const +{ + return std::find(mSkeletons.begin(), mSkeletons.end(), skeleton) + != mSkeletons.end(); +} + //============================================================================== int World::getIndex(int _index) const { diff --git a/dart/simulation/World.hpp b/dart/simulation/World.hpp index 90e840bede515..026743f3949fe 100644 --- a/dart/simulation/World.hpp +++ b/dart/simulation/World.hpp @@ -149,6 +149,9 @@ class World : public virtual common::Subject /// pointers to them, in case you want to recycle them std::set removeAllSkeletons(); + /// Returns wether this World contains a Skeleton. + bool hasSkeleton(const dynamics::ConstSkeletonPtr& skeleton) const; + /// Get the dof index for the indexed skeleton int getIndex(int _index) const; diff --git a/unittests/comprehensive/test_World.cpp b/unittests/comprehensive/test_World.cpp index 63e4c37357380..c45855de92310 100644 --- a/unittests/comprehensive/test_World.cpp +++ b/unittests/comprehensive/test_World.cpp @@ -95,9 +95,16 @@ TEST(World, AddingAndRemovingSkeletons) for (int i = 0; i < nSteps; ++i) world->step(); + EXPECT_FALSE(world->hasSkeleton(skeleton1)); + EXPECT_FALSE(world->hasSkeleton(skeleton2)); + EXPECT_FALSE(world->hasSkeleton(skeleton3)); + EXPECT_FALSE(world->hasSkeleton(skeleton4)); + // Add skeleton1, skeleton2 world->addSkeleton(skeleton1); + EXPECT_TRUE(world->hasSkeleton(skeleton1)); world->addSkeleton(skeleton2); + EXPECT_TRUE(world->hasSkeleton(skeleton2)); EXPECT_TRUE(world->getNumSkeletons() == 2); for (int i = 0; i < nSteps; ++i) world->step(); @@ -119,7 +126,9 @@ TEST(World, AddingAndRemovingSkeletons) // Add skeleton3, skeleton4 world->addSkeleton(skeleton3); + EXPECT_TRUE(world->hasSkeleton(skeleton3)); world->addSkeleton(skeleton4); + EXPECT_TRUE(world->hasSkeleton(skeleton4)); EXPECT_TRUE(world->getNumSkeletons() == 3); for (int i = 0; i < nSteps; ++i) world->step();