diff --git a/CHANGELOG.md b/CHANGELOG.md index 2fdaa16260696..5fd9ee8eaf1b3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,9 +6,13 @@ * Fix compilation warnings for newer versions of compilers: [#1177](https://github.com/dartsim/dart/pull/1177) +* Collision Detection + + * CollisionGroups will automatically update their objects when any changes occur to Skeletons or BodyNodes that they are subscribed to: [#1112](https://github.com/dartsim/dart/pull/1112) + * Math - * Consolidated random functions into Random class : [#1109](https://github.com/dartsim/dart/pull/1109) + * Consolidated random functions into Random class: [#1109](https://github.com/dartsim/dart/pull/1109) * Dynamics @@ -234,7 +238,7 @@ * Misc - * Updated lodepng up to version 20160501 : [#791](https://github.com/dartsim/dart/pull/791) + * Updated lodepng up to version 20160501: [#791](https://github.com/dartsim/dart/pull/791) ### DART 6.1.0 (2016-10-07) diff --git a/dart/collision/CollisionDetector.cpp b/dart/collision/CollisionDetector.cpp index e0a98ef7d551f..31cef09641429 100644 --- a/dart/collision/CollisionDetector.cpp +++ b/dart/collision/CollisionDetector.cpp @@ -159,7 +159,7 @@ CollisionDetector::ManagerForSharableCollisionObjects::claimCollisionObject( { const auto& collObj = search->second; assert(collObj.lock()); - // Ensure all the collision object in the map should be alive pointers. + // Ensure all the collision objects in the map are valid pointers return collObj.lock(); } diff --git a/dart/collision/CollisionDetector.hpp b/dart/collision/CollisionDetector.hpp index 5c40070b1db77..e963c1a29fcae 100644 --- a/dart/collision/CollisionDetector.hpp +++ b/dart/collision/CollisionDetector.hpp @@ -174,6 +174,9 @@ class CollisionDetector : public std::enable_shared_from_this virtual std::unique_ptr createCollisionObject( const dynamics::ShapeFrame* shapeFrame) = 0; + /// Update the collision geometry of a ShapeFrame + virtual void refreshCollisionObject(CollisionObject* object) = 0; + /// Notify that a CollisionObject is destroying. Do nothing by default. virtual void notifyCollisionObjectDestroying(CollisionObject* object); @@ -199,6 +202,9 @@ class CollisionDetector::CollisionObjectManager /// Returns collision detector CollisionDetector* getCollisionDetector(); + /// Virtual destructor + virtual ~CollisionObjectManager() = default; + protected: CollisionDetector* mCollisionDetector; diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index 63dfa9710411b..9ce151312451e 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -44,7 +44,8 @@ namespace collision { //============================================================================== CollisionGroup::CollisionGroup(const CollisionDetectorPtr& collisionDetector) - : mCollisionDetector(collisionDetector) + : mCollisionDetector(collisionDetector), + mUpdateAutomatically(true) { assert(mCollisionDetector); } @@ -64,17 +65,7 @@ ConstCollisionDetectorPtr CollisionGroup::getCollisionDetector() const //============================================================================== void CollisionGroup::addShapeFrame(const dynamics::ShapeFrame* shapeFrame) { - if (!shapeFrame) - return; - - if (hasShapeFrame(shapeFrame)) - return; - - auto collObj = mCollisionDetector->claimCollisionObject(shapeFrame); - - addCollisionObjectToEngine(collObj.get()); - - mShapeFrameMap.push_back(std::make_pair(shapeFrame, collObj)); + addShapeFrameImpl(shapeFrame, nullptr); } //============================================================================== @@ -91,6 +82,12 @@ void CollisionGroup::addShapeFramesOf() // Do nothing } +//============================================================================== +void CollisionGroup::subscribeTo() +{ + // Do nothing +} + //============================================================================== void CollisionGroup::removeShapeFrame(const dynamics::ShapeFrame* shapeFrame) { @@ -98,17 +95,35 @@ void CollisionGroup::removeShapeFrame(const dynamics::ShapeFrame* shapeFrame) return; const auto search - = std::find_if(mShapeFrameMap.begin(), mShapeFrameMap.end(), - [&](const std::pair& pair) - { return pair.first == shapeFrame; }); + = std::find_if(mObjectInfoList.begin(), mObjectInfoList.end(), + [&](const std::unique_ptr& info) + { return info->mFrame == shapeFrame; }); - if (mShapeFrameMap.end() == search) + if (mObjectInfoList.end() == search) return; - removeCollisionObjectFromEngine(search->second.get()); + removeCollisionObjectFromEngine((*search)->mObject.get()); - mShapeFrameMap.erase(search); + // Since the user is explicitly telling us to remove this ShapeFrame, we can + // no longer remain subscribed to any sources that were providing this + // ShapeFrame. Otherwise, this ShapeFrame would just reappear instantly the + // next time an update is performed. + for(const void* source : (*search)->mSources) + { + if(nullptr == source) + continue; + + // We don't know which container this source is kept in, so try erasing it + // from each. + if(mSkeletonSources.erase( + static_cast(source)) > 0) + continue; + + mBodyNodeSources.erase(static_cast(source)); + } + + mObjectInfoList.erase(search); + mObserver.removeShapeFrame(shapeFrame); } //============================================================================== @@ -130,33 +145,33 @@ void CollisionGroup::removeAllShapeFrames() { removeAllCollisionObjectsFromEngine(); - mShapeFrameMap.clear(); + mObjectInfoList.clear(); + mObserver.removeAllShapeFrames(); } //============================================================================== bool CollisionGroup::hasShapeFrame( const dynamics::ShapeFrame* shapeFrame) const { - return std::find_if(mShapeFrameMap.begin(), mShapeFrameMap.end(), - [&](const std::pair& pair) - { return pair.first == shapeFrame; }) - != mShapeFrameMap.end(); + return std::find_if(mObjectInfoList.begin(), mObjectInfoList.end(), + [&](const std::unique_ptr& info) + { return info->mFrame == shapeFrame; }) + != mObjectInfoList.end(); } //============================================================================== std::size_t CollisionGroup::getNumShapeFrames() const { - return mShapeFrameMap.size(); + return mObjectInfoList.size(); } //============================================================================== const dynamics::ShapeFrame* CollisionGroup::getShapeFrame( std::size_t index) const { - assert(index < mShapeFrameMap.size()); - if(index < mShapeFrameMap.size()) - return mShapeFrameMap[index].first; + assert(index < mObjectInfoList.size()); + if(index < mObjectInfoList.size()) + return mObjectInfoList[index]->mFrame; return nullptr; } @@ -165,6 +180,9 @@ const dynamics::ShapeFrame* CollisionGroup::getShapeFrame( bool CollisionGroup::collide( const CollisionOption& option, CollisionResult* result) { + if(mUpdateAutomatically) + update(); + return mCollisionDetector->collide(this, option, result); } @@ -174,6 +192,9 @@ bool CollisionGroup::collide( const CollisionOption& option, CollisionResult* result) { + if(mUpdateAutomatically) + update(); + return mCollisionDetector->collide(this, otherGroup, option, result); } @@ -181,6 +202,9 @@ bool CollisionGroup::collide( double CollisionGroup::distance( const DistanceOption& option, DistanceResult* result) { + if(mUpdateAutomatically) + update(); + return mCollisionDetector->distance(this, option, result); } @@ -190,17 +214,401 @@ double CollisionGroup::distance( const DistanceOption& option, DistanceResult* result) { + if(mUpdateAutomatically) + update(); + return mCollisionDetector->distance(this, otherGroup, option, result); } +//============================================================================== +void CollisionGroup::setAutomaticUpdate(const bool automatic) +{ + mUpdateAutomatically = automatic; +} + +//============================================================================== +bool CollisionGroup::getAutomaticUpdate() const +{ + return mUpdateAutomatically; +} + +//============================================================================== +void CollisionGroup::update() +{ + removeDeletedShapeFrames(); + + for(auto& entry : mSkeletonSources) + updateSkeletonSource(entry); + + for(auto& entry : mBodyNodeSources) + updateBodyNodeSource(entry); +} + +//============================================================================== +void CollisionGroup::removeDeletedShapeFrames() +{ + for(auto shapeFrame : mObserver.mDeletedFrames) + { + const auto search = + std::find_if(mObjectInfoList.begin(), mObjectInfoList.end(), + [&](const std::unique_ptr& info) + { return info->mFrame == shapeFrame; }); + + if(mObjectInfoList.end() == search) + continue; + + // Clear out the ShapeFrame from any subscriber that it might have + for(const void* source : (*search)->mSources) + { + if(nullptr == source) + continue; + + auto skelSearch = mSkeletonSources.find( + static_cast(source)); + if(skelSearch != mSkeletonSources.end()) + { + skelSearch->second.mObjects.erase(shapeFrame); + for(auto& child : skelSearch->second.mChildren) + child.second.mFrames.erase(shapeFrame); + } + + auto bodySearch = mBodyNodeSources.find( + static_cast(source)); + if(bodySearch != mBodyNodeSources.end()) + bodySearch->second.mObjects.erase(shapeFrame); + } + + removeCollisionObjectFromEngine((*search)->mObject.get()); + mObjectInfoList.erase(search); + } + + mObserver.mDeletedFrames.clear(); +} + //============================================================================== void CollisionGroup::updateEngineData() { - for (auto& pair : mShapeFrameMap) - pair.second->updateEngineData(); + for (const auto& info : mObjectInfoList) + info->mObject->updateEngineData(); updateCollisionGroupEngineData(); } +//============================================================================== +void CollisionGroup::ShapeFrameObserver::addShapeFrame( + const dynamics::ShapeFrame* shapeFrame) +{ + addSubject(shapeFrame); + mMap.insert(std::make_pair( + static_cast(shapeFrame), shapeFrame)); +} + +//============================================================================== +void CollisionGroup::ShapeFrameObserver::removeShapeFrame( + const dynamics::ShapeFrame* shapeFrame) +{ + removeSubject(shapeFrame); + mMap.erase(shapeFrame); +} + +//============================================================================== +void CollisionGroup::ShapeFrameObserver::removeAllShapeFrames() +{ + removeAllSubjects(); +} + +//============================================================================== +void CollisionGroup::ShapeFrameObserver::handleDestructionNotification( + const common::Subject* subject) +{ + mDeletedFrames.insert(mMap[subject]); + mMap.erase(subject); +} + +//============================================================================== +auto CollisionGroup::addShapeFrameImpl( + const dynamics::ShapeFrame* shapeFrame, + const void* source) -> ObjectInfo* +{ + if (!shapeFrame) + return nullptr; + + auto it = std::find_if(mObjectInfoList.begin(), mObjectInfoList.end(), + [&](const std::unique_ptr& info) + { return info->mFrame == shapeFrame; }); + + if (it == mObjectInfoList.end()) + { + auto collObj = mCollisionDetector->claimCollisionObject(shapeFrame); + + addCollisionObjectToEngine(collObj.get()); + + const dynamics::ConstShapePtr& shape = shapeFrame->getShape(); + + // If shape is a nullptr, use improbable values for ID and version. ID is + // not likely to ever be 0 because Shape IDs start at + // PRIMITIVE_MAGIC_NUMBER (defined as 1000 in Shape.cpp). Version is not + // likely to be 0 because the Shape class starts its version counter at 1, + // and the version count should monotonically increase. + mObjectInfoList.emplace_back( + new ObjectInfo{shapeFrame, collObj, + shape? shape->getID() : 0, + shape? shape->getVersion() : 0, {}}); + mObserver.addShapeFrame(shapeFrame); + + it = --mObjectInfoList.end(); + } + + (*it)->mSources.insert(source); + + return it->get(); +} + +//============================================================================== +void CollisionGroup::removeShapeFrameInternal( + const dynamics::ShapeFrame* shapeFrame, + const void* source) +{ + if(!shapeFrame) + return; + + const auto search = + std::find_if(mObjectInfoList.begin(), mObjectInfoList.end(), + [&](const std::unique_ptr& info) + { return info->mFrame == shapeFrame; }); + + if(mObjectInfoList.end() == search) + return; + + std::unordered_set& objectSources = (*search)->mSources; + objectSources.erase(source); + + if(objectSources.empty()) + { + removeCollisionObjectFromEngine((*search)->mObject.get()); + mObjectInfoList.erase(search); + mObserver.removeShapeFrame(shapeFrame); + } +} + +//============================================================================== +bool CollisionGroup::updateSkeletonSource(SkeletonSources::value_type& entry) +{ + SkeletonSource& source = entry.second; + + const dynamics::ConstMetaSkeletonPtr& meta = source.mSource.lock(); + if(!meta) + { + // This skeleton no longer exists, so we should remove all its contents from + // the CollisionGroup. + for(const auto& object : source.mObjects) + removeShapeFrameInternal(object.second->mFrame, object.first); + + return true; + } + + // Note: Right now we can only subscribe to Skeletons, not MetaSkeletons, so + // we can safely do a static_cast here. Eventually this static_cast should not + // be needed, if we can figure out a versioning system for MetaSkeleton. + const dynamics::Skeleton* skeleton = + dynamic_cast(meta.get()); + + const std::size_t currentSkeletonVersion = skeleton->getVersion(); + // If the version hasn't changed, then there will be nothing to update. + if(currentSkeletonVersion == source.mLastKnownVersion) + return false; + + source.mLastKnownVersion = currentSkeletonVersion; + bool updateNeeded = false; + + auto unusedChildren = source.mChildren; + + // Check each child to see if its version number has changed. If it has, then + // check to see if any of its ShapeFrames need to be updated, added, or + // removed from the CollisionGroup. + for(std::size_t i = 0; i < skeleton->getNumBodyNodes(); ++i) + { + const dynamics::BodyNode* bn = skeleton->getBodyNode(i); + const std::size_t currentVersion = bn->getVersion(); + + unusedChildren.erase(bn); + + // Attempt an insertion, in case this BodyNode was not already accounted for + auto insertion = source.mChildren.insert( + std::make_pair(bn, SkeletonSource::ChildInfo(currentVersion))); + + const auto child = insertion.first; + + if(insertion.second) + { + // This child was newly introduced, so we need to add it to the + // CollisionGroup. + updateNeeded = true; + + const auto& collisionShapeNodes = + bn->getShapeNodesWith(); + + for(const auto& shapeNode : collisionShapeNodes) + { + source.mObjects.insert( + {shapeNode, addShapeFrameImpl(shapeNode, meta.get())}); + child->second.mFrames.insert(shapeNode); + } + + // Skip the rest of the for-loop, because it's only for updating old + // children + continue; + } + + // If the version of this BodyNode has not changed, then move on to the next + // one. + if(child->second.mLastKnownVersion == currentVersion) + continue; + + child->second.mLastKnownVersion = currentVersion; + + std::unordered_set unusedFrames = + child->second.mFrames; + + const std::vector nodes = + child->first->getShapeNodesWith(); + + for(const dynamics::ShapeNode* node : nodes) + { + unusedFrames.erase(node); + + auto frameInsertion = + source.mObjects.insert(std::make_pair(node, nullptr)); + + const auto& it = frameInsertion.first; + if(frameInsertion.second) + { + // If the insertion occurred, then this is a new ShapeFrame, and we + // need to create a collision object for it. + updateNeeded = true; + + it->second = addShapeFrameImpl(node, meta.get()); + child->second.mFrames.insert(node); + } + else + { + // If the insertion did not occur, then this is an old ShapeFrame, and + // we should check if it needs an update. + updateNeeded |= updateShapeFrame(it->second); + } + } + + for(const dynamics::ShapeFrame* unused : unusedFrames) + { + updateNeeded = true; + removeShapeFrameInternal(unused, meta.get()); + child->second.mFrames.erase(unused); + } + } + + // Remove from this group any BodyNodes that no longer belong to the skeleton + for(const auto& unusedChild : unusedChildren) + { + for(const dynamics::ShapeFrame* unusedFrame : unusedChild.second.mFrames) + { + updateNeeded = true; + removeShapeFrameInternal(unusedFrame, meta.get()); + source.mObjects.erase(unusedFrame); + } + + source.mChildren.erase(unusedChild.first); + } + + return updateNeeded; +} + +//============================================================================== +bool CollisionGroup::updateBodyNodeSource(BodyNodeSources::value_type& entry) +{ + BodyNodeSource& source = entry.second; + + const dynamics::ConstBodyNodePtr bn = source.mSource.lock(); + if(!bn) + { + // This BodyNode no longer exists, so we should remove all i ts contents + // from the CollisionGroup. + for(const auto& object : source.mObjects) + removeShapeFrameInternal(object.second->mFrame, object.first); + + return true; + } + + const std::size_t currentBodyNodeVersion = bn->getVersion(); + + // If the version hasn't changed, then tehre will be nothing to update. + if(currentBodyNodeVersion == source.mLastKnownVersion) + return false; + + source.mLastKnownVersion = currentBodyNodeVersion; + bool updateNeeded = false; + + std::unordered_map unusedFrames = + source.mObjects; + + const std::vector nodes = + bn->getShapeNodesWith(); + + for(const dynamics::ShapeNode* node : nodes) + { + unusedFrames.erase(node); + + auto frameInsertion = source.mObjects.insert(std::make_pair(node, nullptr)); + + const auto& it = frameInsertion.first; + if(frameInsertion.second) + { + updateNeeded = true; + // If the insertion occurred, then this is a new ShapeFrame, and we need + // to create a collision object for it. + it->second = addShapeFrameImpl(node, bn.get()); + } + else + { + // If the insertion did not occur, then this is an old ShapeFrame, and + // we should check if it needs an update. + updateNeeded |= updateShapeFrame(it->second); + } + } + + // Remove from this group and ShapeFrames that no longer belong to the + // BodyNode + for(const auto& unusedFrame : unusedFrames) + { + updateNeeded = true; + removeShapeFrameInternal(unusedFrame.first, bn.get()); + source.mObjects.erase(unusedFrame.first); + } + + return updateNeeded; +} + +//============================================================================== +bool CollisionGroup::updateShapeFrame(ObjectInfo* object) +{ + const dynamics::ConstShapePtr& shape = object->mFrame->getShape(); + const std::size_t currentID = shape? shape->getID() : 0; + const std::size_t currentVersion = shape? shape->getVersion() : 0; + + if(currentID != object->mLastKnownShapeID + || currentVersion != object->mLastKnownVersion) + { + removeCollisionObjectFromEngine(object->mObject.get()); + mCollisionDetector->refreshCollisionObject(object->mObject.get()); + addCollisionObjectToEngine(object->mObject.get()); + + object->mLastKnownShapeID = currentID; + object->mLastKnownVersion = currentVersion; + + return true; + } + + return false; +} + } // namespace collision } // namespace dart diff --git a/dart/collision/CollisionGroup.hpp b/dart/collision/CollisionGroup.hpp index 40e71c6f705b3..4e286efdf90a9 100644 --- a/dart/collision/CollisionGroup.hpp +++ b/dart/collision/CollisionGroup.hpp @@ -33,13 +33,15 @@ #ifndef DART_COLLISION_COLLISIONGROUP_HPP_ #define DART_COLLISION_COLLISIONGROUP_HPP_ -#include +#include +#include #include #include "dart/collision/SmartPointer.hpp" #include "dart/collision/CollisionOption.hpp" #include "dart/collision/CollisionResult.hpp" #include "dart/collision/DistanceOption.hpp" #include "dart/collision/DistanceResult.hpp" +#include "dart/common/Observer.hpp" #include "dart/dynamics/SmartPointer.hpp" namespace dart { @@ -110,7 +112,38 @@ class CollisionGroup /// template function template addShapeFramesOf(). void addShapeFramesOf(); - /// Remove a ShapeFrame from this CollisionGroup + /// Add ShapeFrames of bodyNode, and also subscribe to the BodyNode so that + /// the results from this CollisionGroup automatically reflect any changes + /// that are made to bodyNode. + /// + /// This does likewise for the objects in ...others. + template + void subscribeTo( + const dynamics::ConstBodyNodePtr& bodyNode, + const Others&... others); + + /// Add ShapeFrames of skeleton, and also subscribe to the Skeleton so that + /// the results from this CollisionGroup automatically reflect any changes + /// that are made to skeleton. + /// + /// This does likewise for the objects in ...others. + // + // TODO(MXG): Figure out how to determine a version number for MetaSkeletons + // so that this function can accept a ConstMetaSkeletonPtr instead of only a + // ConstSkeletonPtr. + template + void subscribeTo( + const dynamics::ConstSkeletonPtr& skeleton, + const Others&... others); + + /// Do nothing. This function is for terminating the recursive variadic + /// template. + void subscribeTo(); + + /// Remove a ShapeFrame from this CollisionGroup. If this ShapeFrame was being + /// provided by any subscriptions, then calling this function will unsubscribe + /// from those subscriptions, because otherwise this ShapeFrame would simply + /// be put back into the CollisionGroup the next time the group gets updated. void removeShapeFrame(const dynamics::ShapeFrame* shapeFrame); /// Remove ShapeFrames from this CollisionGroup @@ -137,18 +170,27 @@ class CollisionGroup /// Remove ShapeFrames of other CollisionGroup, and also remove another /// ShapeFrames of other various objects. + /// + /// If this CollisionGroup is subscribed to otherGroup, then this function + /// will also unsubscribe from it. template void removeShapeFramesOf(const CollisionGroup* otherGroup, const Others*... others); /// Remove ShapeFrames of BodyNode, and also remove another ShapeFrames of /// other various objects. + /// + /// If this CollisionGroup is subscribed to bodyNode, then this function will + /// also unsubscribe from it. template void removeShapeFramesOf(const dynamics::BodyNode* bodyNode, const Others*... others); /// Remove ShapeFrames of MetaSkeleton, and also remove another ShapeFrames of /// other various objects. + /// + /// If this CollisionGroup is subscribed to skeleton, then this function will + /// also unsubscribe from it. template void removeShapeFramesOf(const dynamics::MetaSkeleton* skeleton, const Others*... others); @@ -160,6 +202,20 @@ class CollisionGroup /// Remove all the ShapeFrames in this CollisionGroup void removeAllShapeFrames(); + /// Unsubscribe from bodyNode. The ShapeFrames of the BodyNode will also be + /// removed if no other source is requesting them for this group. + template + void unsubscribeFrom( + const dynamics::BodyNode* bodyNode, + const Others*... others); + + /// Unsubscribe from skeleton. The ShapeFrames of the skeleton will also be + /// removed if no other source is requesting them for this group. + template + void unsubscribeFrom( + const dynamics::Skeleton* skeleton, + const Others*... others); + /// Return true if this CollisionGroup contains shapeFrame bool hasShapeFrame(const dynamics::ShapeFrame* shapeFrame) const; @@ -212,6 +268,30 @@ class CollisionGroup const DistanceOption& option = DistanceOption(false, 0.0, nullptr), DistanceResult* result = nullptr); + /// Set whether this CollisionGroup will automatically check for updates. + void setAutomaticUpdate(bool automatic = true); + + /// Get whether this CollisionGroup is set to automatically check for updates. + bool getAutomaticUpdate() const; + + /// Check whether this CollisionGroup's subscriptions or any of its objects + /// need an update, and then update them if they do. + /// + /// If automatic updating is turned on, then this will be called each time + /// collide() is. + /// + /// This can be called manually in order to control when collision objects get + /// created or destroyed. Those procedures can be time-consuming, so it may be + /// useful to control when it occurs. + void update(); + + /// Remove any ShapeFrames that have been deleted. This will be done + /// automatically for ShapeFrames that belong to subscriptions. + /// + /// Returns true if one or more ShapeFrames were removed; otherwise returns + /// false. + void removeDeletedShapeFrames(); + protected: /// Update engine data. This function should be called before the collision @@ -249,9 +329,39 @@ class CollisionGroup // CollisionDetector doesn't get destroyed as long as at least one // CollisionGroup is alive. - /// ShapeFrames and CollisionOjbects added to this CollisionGroup - std::vector> mShapeFrameMap; + /// Information on a collision object belonging to this group. This info + /// enables us to keep track of when a ShapeFrame has changed and therefore + /// when the object needs to be updated. It also remembers what sources + /// instructed this object to belong to this group. + struct ObjectInfo final + { + /// The ShapeFrame for this object + const dynamics::ShapeFrame* mFrame; + + /// The CollisionObject that was generated by the CollisionDetector + CollisionObjectPtr mObject; + + /// The ID of that last known shape that was held by the shape frame + std::size_t mLastKnownShapeID; + + /// The last known version of the last known shape that was held by the + /// shape frame + std::size_t mLastKnownVersion; + + /// The set of all sources that indicate that this object should be in this + /// group. In the current implementation, this may consist of: + /// user (nullptr), Skeleton subscription, and/or BodyNode subscription. + /// + /// When all sources are cleared out (via unsubscribing), this object will + /// be removed from this group. + std::unordered_set mSources; + }; + + using ObjectInfoList = std::vector>; + + /// Information about ShapeFrames and CollisionObjects that have been added to + /// this CollisionGroup. + ObjectInfoList mObjectInfoList; // CollisionGroup also shares the ownership of CollisionObjects across other // CollisionGroups for the same reason with above. // @@ -265,6 +375,148 @@ class CollisionGroup // original and copy are not guranteed to be the same as we copy std::map // (e.g., by world cloning). + +private: + + /// This class watches when ShapeFrames get deleted so that they can be safely + /// removes from the CollisionGroup. We cannot have a weak_ptr to a ShapeFrame + /// because some are managed by std::shared_ptr while others are managed by + /// NodePtr. + /// + /// If we don't carefully track when each ShapeFrame gets destructed, then we + /// can land in a situation where a ShapeFrame gets removed from a BodyNode, + /// deallocated, and then a new ShapeFrame is allocated in the memory address + /// of the old one. This can lead to invalid memory accesses if we neglect to + /// correctly clean up our references to deleted ShapeFrames. + class ShapeFrameObserver final : public common::Observer + { + public: + + /// Add a shape frame to this observer + void addShapeFrame(const dynamics::ShapeFrame* shapeFrame); + + /// Remove a shape frame from this observer + void removeShapeFrame(const dynamics::ShapeFrame* shapeFrame); + + /// Remove all shape frames from this observer + void removeAllShapeFrames(); + + /// Whenever an observed shape frame gets deleted, its pointer will be added + /// to this set. The next time the collision group updates, it will check this + /// set and wipe away any references to the pointers in this set. + std::unordered_set mDeletedFrames; + + protected: + + /// This will be called each time an observed shape frame is deleted. + void handleDestructionNotification(const common::Subject* subject) override; + + private: + + /// A map from a subject pointer to its corresponding ShapeFrame pointer. + /// This needs to be stored because by the time a Subject is being + /// destructed, it can no longer be cast back to its ShapeFrame. + std::unordered_map mMap; + + }; + + /// Implementation of addShapeFrame. The source argument tells us whether this + /// ShapeFrame is being requested explicitly by the user or implicitly through + /// a BodyNode, Skeleton, or other CollisionGroup. + ObjectInfo* addShapeFrameImpl( + const dynamics::ShapeFrame* shapeFrame, + const void* source); + + /// Internal version of removeShapeFrame. This will only remove the ShapeFrame + /// if it is unsubscribed from all sources. + void removeShapeFrameInternal( + const dynamics::ShapeFrame* shapeFrame, + const void* source); + + /// Set this to true to have this CollisionGroup check for updates + /// automatically. Default is true. + bool mUpdateAutomatically; + + /// \private This struct is used to store sources of ShapeFrames that the + /// CollisionGroup is subscribed to, alongside the last version number of that + /// source, as known by this CollisionGroup. + template + struct CollisionSource + { + /// The source of ShapeFrames + Source mSource; + + /// The last known version of that source + std::size_t mLastKnownVersion; + + /// The set of objects that pertain to this source + std::unordered_map mObjects; + + /// This is information pertaining to a child of the source. In the current + /// implementation, this only gets used by SkeletonSources. + struct ChildInfo + { + /// Last known version of this child + std::size_t mLastKnownVersion; + + /// Last known set of frames attached to this child + std::unordered_set mFrames; + + /// A constructor that simply accepts a last known version number. Shape + /// frames can be added later. + explicit ChildInfo(const std::size_t version) + : mLastKnownVersion(version) + { + // Do nothing + } + }; + + /// The last known versions of the children related to this source. This is + /// used by SkeletonSources to keep track of their child BodyNode versions. + std::unordered_map mChildren; + + /// Constructor + CollisionSource(const Source& source, std::size_t lastKnownVersion) + : mSource(source), + mLastKnownVersion(lastKnownVersion) + { + // Do nothing + } + }; + + // Convenient typedefs for Skeleton sources + using SkeletonSource = + CollisionSource; + using SkeletonSources = std::unordered_map< + const dynamics::MetaSkeleton*, SkeletonSource>; + + // Convenient typedefs for BodyNode sources + using BodyNodeSource = CollisionSource; + using BodyNodeSources = std::unordered_map< + const dynamics::BodyNode*, BodyNodeSource>; + + /// Internal function called to update a Skeleton source + /// \returns true if an update was performed + bool updateSkeletonSource(SkeletonSources::value_type& entry); + + /// Internal function called to update a BodyNode source + /// \returns true if an update was performed + bool updateBodyNodeSource(BodyNodeSources::value_type& entry); + + /// Internal function called to update a ShapeFrame + /// \returns true if an update was performed + bool updateShapeFrame(ObjectInfo* object); + + /// Skeleton sources that this group is subscribed to + SkeletonSources mSkeletonSources; + + /// BodyNode sources that this group is susbscribed to + BodyNodeSources mBodyNodeSources; + + /// The object that observes the Shape Frames that this group cares about + ShapeFrameObserver mObserver; + }; } // namespace collision diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index 41147de226cae..d75bc55d13a47 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -339,6 +339,16 @@ std::unique_ptr BulletCollisionDetector::createCollisionObject( new BulletCollisionObject(this, shapeFrame, bulletCollShape)); } +//============================================================================== +void BulletCollisionDetector::refreshCollisionObject(CollisionObject* object) +{ + BulletCollisionObject* bullet = static_cast(object); + + bullet->mBulletCollisionShape = claimBulletCollisionShape(bullet->getShape()); + bullet->mBulletCollisionObject->setCollisionShape( + bullet->mBulletCollisionShape->mCollisionShape.get()); +} + //============================================================================== void BulletCollisionDetector::notifyCollisionObjectDestroying( CollisionObject* object) @@ -347,54 +357,44 @@ void BulletCollisionDetector::notifyCollisionObjectDestroying( } //============================================================================== -BulletCollisionShape* BulletCollisionDetector::claimBulletCollisionShape( +std::shared_ptr +BulletCollisionDetector::claimBulletCollisionShape( const dynamics::ConstShapePtr& shape) { - const auto search = mShapeMap.find(shape); - - if (mShapeMap.end() != search) - { - auto& bulletCollShapeAndCount = search->second; + const std::size_t currentVersion = shape->getVersion(); - auto& bulletCollShape = bulletCollShapeAndCount.first; - auto& count = bulletCollShapeAndCount.second; - assert(0u != count); + const auto search = mShapeMap.insert(std::make_pair(shape, ShapeInfo())); + const bool inserted = search.second; + ShapeInfo& info = search.first->second; - count++; + if (!inserted && currentVersion == info.mLastKnownVersion) + { + const auto& bulletCollShape = info.mShape.lock(); + assert(bulletCollShape); - return bulletCollShape.get(); + return bulletCollShape; } - auto newBulletCollisionShape = createBulletCollisionShape(shape); - auto rawCollisionShape = newBulletCollisionShape.get(); - mShapeMap[shape] = std::make_pair(std::move(newBulletCollisionShape), 1u); + auto newBulletCollisionShape = std::shared_ptr( + createBulletCollisionShape(shape).release(), + BulletCollisionShapeDeleter(this, shape)); + info.mShape = newBulletCollisionShape; + info.mLastKnownVersion = currentVersion; - return rawCollisionShape; + return newBulletCollisionShape; } //============================================================================== void BulletCollisionDetector::reclaimBulletCollisionShape( const dynamics::ConstShapePtr& shape) { - auto search = mShapeMap.find(shape); - - assert(mShapeMap.end() != search); - - auto& bulletCollShapeAndCount = search->second; - - auto& bulletCollShape = bulletCollShapeAndCount.first; - auto& count = bulletCollShapeAndCount.second; - - count--; - - if (0u == count) - { - auto userPointer = bulletCollShape->mCollisionShape->getUserPointer(); - if (userPointer) - delete static_cast(userPointer); + const auto& search = mShapeMap.find(shape); + if(search == mShapeMap.end()) + return; + const auto& bulletShape = search->second.mShape.lock(); + if(!bulletShape || bulletShape.use_count() <= 2) mShapeMap.erase(search); - } } //============================================================================== @@ -601,7 +601,25 @@ BulletCollisionDetector::createBulletCollisionShape( } } +//============================================================================== +BulletCollisionDetector::BulletCollisionShapeDeleter +::BulletCollisionShapeDeleter( + BulletCollisionDetector* cd, + const dynamics::ConstShapePtr& shape) + : mBulletCollisionDetector(cd), + mShape(shape) +{ + // Do nothing +} + +//============================================================================== +void BulletCollisionDetector::BulletCollisionShapeDeleter +::operator()(BulletCollisionShape* shape) const +{ + mBulletCollisionDetector->reclaimBulletCollisionShape(mShape); + delete shape; +} namespace { diff --git a/dart/collision/bullet/BulletCollisionDetector.hpp b/dart/collision/bullet/BulletCollisionDetector.hpp index 5aec2f9f8b93b..174cdf4dc4f7a 100644 --- a/dart/collision/bullet/BulletCollisionDetector.hpp +++ b/dart/collision/bullet/BulletCollisionDetector.hpp @@ -107,12 +107,15 @@ class BulletCollisionDetector : public CollisionDetector std::unique_ptr createCollisionObject( const dynamics::ShapeFrame* shapeFrame) override; + // Documentation inherited + void refreshCollisionObject(CollisionObject* object) override; + // Documentation inherited void notifyCollisionObjectDestroying(CollisionObject* object) override; private: - BulletCollisionShape* claimBulletCollisionShape( + std::shared_ptr claimBulletCollisionShape( const dynamics::ConstShapePtr& shape); void reclaimBulletCollisionShape( @@ -121,10 +124,40 @@ class BulletCollisionDetector : public CollisionDetector std::unique_ptr createBulletCollisionShape( const dynamics::ConstShapePtr& shape); + /// This deleter is responsible for deleting BulletCollsionShape objects and + /// removing them from mShapeMap when they are not shared by any + /// CollisionObjects. + class BulletCollisionShapeDeleter final + { + public: + + BulletCollisionShapeDeleter( + BulletCollisionDetector* cd, + const dynamics::ConstShapePtr& shape); + + void operator()(BulletCollisionShape* shape) const; + + private: + + BulletCollisionDetector* mBulletCollisionDetector; + + dynamics::ConstShapePtr mShape; + + }; + + /// Information for a shape that was generated by this collision detector + struct ShapeInfo final + { + /// A weak reference to the shape + std::weak_ptr mShape; + + /// The last version of the shape, as known by this collision detector + std::size_t mLastKnownVersion; + }; + private: - std::map, std::size_t>> mShapeMap; + std::map mShapeMap; std::unique_ptr mGroupForFiltering; diff --git a/dart/collision/bullet/BulletCollisionGroup.cpp b/dart/collision/bullet/BulletCollisionGroup.cpp index b8d800c84ead3..6543e259ccf96 100644 --- a/dart/collision/bullet/BulletCollisionGroup.cpp +++ b/dart/collision/bullet/BulletCollisionGroup.cpp @@ -101,8 +101,8 @@ void BulletCollisionGroup::removeCollisionObjectFromEngine( //============================================================================== void BulletCollisionGroup::removeAllCollisionObjectsFromEngine() { - for (const auto& pair : mShapeFrameMap) - removeCollisionObjectFromEngine(pair.second.get()); + for (const auto& info : mObjectInfoList) + removeCollisionObjectFromEngine(info->mObject.get()); initializeEngineData(); } diff --git a/dart/collision/bullet/BulletCollisionObject.cpp b/dart/collision/bullet/BulletCollisionObject.cpp index 7afd47bdd17ff..3aaa6d04e9dd8 100644 --- a/dart/collision/bullet/BulletCollisionObject.cpp +++ b/dart/collision/bullet/BulletCollisionObject.cpp @@ -51,16 +51,19 @@ const btCollisionObject* BulletCollisionObject::getBulletCollisionObject() const } //============================================================================== -BulletCollisionObject::BulletCollisionObject(CollisionDetector* collisionDetector, +BulletCollisionObject::BulletCollisionObject( + CollisionDetector* collisionDetector, const dynamics::ShapeFrame* shapeFrame, - BulletCollisionShape* bulletCollisionShape) + const std::shared_ptr& bulletCollisionShape) : CollisionObject(collisionDetector, shapeFrame), - mBulletCollisionObject(new btCollisionObject()), - mBulletCollisionShape(bulletCollisionShape) + mBulletCollisionShape(bulletCollisionShape), + mBulletCollisionObject(new btCollisionObject()) { assert(bulletCollisionShape); - mBulletCollisionObject->setCollisionShape(mBulletCollisionShape->mCollisionShape.get()); + mBulletCollisionObject->setCollisionShape( + mBulletCollisionShape->mCollisionShape.get()); + mBulletCollisionObject->setUserPointer(this); } diff --git a/dart/collision/bullet/BulletCollisionObject.hpp b/dart/collision/bullet/BulletCollisionObject.hpp index 00fb018509b84..c0b7f234439b9 100644 --- a/dart/collision/bullet/BulletCollisionObject.hpp +++ b/dart/collision/bullet/BulletCollisionObject.hpp @@ -59,18 +59,18 @@ class BulletCollisionObject : public CollisionObject protected: /// Constructor - BulletCollisionObject(CollisionDetector* collisionDetector, - const dynamics::ShapeFrame* shapeFrame, - BulletCollisionShape* bulletCollisionShape); + BulletCollisionObject( + CollisionDetector* collisionDetector, + const dynamics::ShapeFrame* shapeFrame, + const std::shared_ptr& bulletCollisionShape); // Documentation inherited void updateEngineData() override; protected: /// Bullet collision object + std::shared_ptr mBulletCollisionShape; std::unique_ptr mBulletCollisionObject; - - BulletCollisionShape* mBulletCollisionShape; }; } // namespace collision diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index df503a37fb0b4..54ec401cc99f1 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -305,6 +305,11 @@ std::unique_ptr DARTCollisionDetector::createCollisionObject( new DARTCollisionObject(this, shapeFrame)); } +//============================================================================== +void DARTCollisionDetector::refreshCollisionObject(CollisionObject* /*object*/) +{ + // Do nothing +} diff --git a/dart/collision/dart/DARTCollisionDetector.hpp b/dart/collision/dart/DARTCollisionDetector.hpp index 2fd7691854444..709d4725d3801 100644 --- a/dart/collision/dart/DARTCollisionDetector.hpp +++ b/dart/collision/dart/DARTCollisionDetector.hpp @@ -95,6 +95,9 @@ class DARTCollisionDetector : public CollisionDetector std::unique_ptr createCollisionObject( const dynamics::ShapeFrame* shapeFrame) override; + // Documentation inherited + void refreshCollisionObject(CollisionObject* object) override; + private: static Registrar mRegistrar; }; diff --git a/dart/collision/detail/CollisionGroup.hpp b/dart/collision/detail/CollisionGroup.hpp index 765e6dcd93207..b3a2b6fc96c3e 100644 --- a/dart/collision/detail/CollisionGroup.hpp +++ b/dart/collision/detail/CollisionGroup.hpp @@ -71,8 +71,8 @@ void CollisionGroup::addShapeFramesOf( if (otherGroup && this != otherGroup) { - for (const auto& pair : otherGroup->mShapeFrameMap) - addShapeFrame(pair.first); + for (const auto& info : otherGroup->mObjectInfoList) + addShapeFrame(info->mFrame); } addShapeFramesOf(others...); @@ -108,6 +108,75 @@ void CollisionGroup::addShapeFramesOf( addShapeFramesOf(others...); } +//============================================================================== +template +void CollisionGroup::subscribeTo( + const dynamics::ConstBodyNodePtr& bodyNode, + const Others&... others) +{ + const auto inserted = mBodyNodeSources.insert( + BodyNodeSources::value_type( + bodyNode.get(), + BodyNodeSource(bodyNode.get(), bodyNode->getVersion())) + ); + + if(inserted.second) + { + const BodyNodeSources::iterator& entry = inserted.first; + + const auto collisionShapeNodes = + bodyNode->getShapeNodesWith(); + + for (const auto& shapeNode : collisionShapeNodes) + { + entry->second.mObjects.insert( + {shapeNode, addShapeFrameImpl(shapeNode, bodyNode.get())}); + } + } + + subscribeTo(others...); +} + +//============================================================================== +template +void CollisionGroup::subscribeTo( + const dynamics::ConstSkeletonPtr& skeleton, + const Others&... others) +{ + const auto inserted = mSkeletonSources.insert( + SkeletonSources::value_type( + skeleton.get(), + SkeletonSource(skeleton, skeleton->getVersion())) + ); + + if(inserted.second) + { + SkeletonSource& entry = inserted.first->second; + + const std::size_t numBodies = skeleton->getNumBodyNodes(); + for (std::size_t i = 0u ; i < numBodies; ++i) + { + const dynamics::BodyNode* bn = skeleton->getBodyNode(i); + + const auto& collisionShapeNodes = + bn->getShapeNodesWith(); + + auto& childInfo = entry.mChildren.insert( + std::make_pair(bn, SkeletonSource::ChildInfo(bn->getVersion()))) + .first->second; + + for (const auto& shapeNode : collisionShapeNodes) + { + entry.mObjects.insert( + {shapeNode, addShapeFrameImpl(shapeNode, skeleton.get())}); + childInfo.mFrames.insert(shapeNode); + } + } + } + + subscribeTo(others...); +} + //============================================================================== template void CollisionGroup::removeShapeFramesOf( @@ -144,8 +213,8 @@ void CollisionGroup::removeShapeFramesOf( return; } - for (const auto& pair : otherGroup->mShapeFrameMap) - removeShapeFrame(pair.first); + for (const auto& info : otherGroup->mObjectInfoList) + removeShapeFrame(info->mFrame); } removeShapeFramesOf(others...); @@ -181,6 +250,45 @@ void CollisionGroup::removeShapeFramesOf( removeShapeFramesOf(others...); } +//============================================================================== +template +void CollisionGroup::unsubscribeFrom( + const dynamics::BodyNode* bodyNode, + const Others*... others) +{ + auto it = mBodyNodeSources.find(bodyNode); + if(it != mBodyNodeSources.end()) + { + for(const auto& entry : it->second.mObjects) + removeShapeFrameInternal(entry.first, bodyNode); + + mBodyNodeSources.erase(it); + } + + unsubscribeFrom(others...); +} + +//============================================================================== +template +void CollisionGroup::unsubscribeFrom( + const dynamics::Skeleton* skeleton, + const Others*... others) +{ + auto it = mSkeletonSources.find(skeleton); + if(it != mSkeletonSources.end()) + { + for(const auto& entry : it->second.mObjects) + { + removeShapeFrameInternal( + entry.first, static_cast(skeleton)); + } + + mSkeletonSources.erase(it); + } + + unsubscribeFrom(others...); +} + } // namespace collision } // namespace dart diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index c634e1026f10b..fe4bf320e1de7 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -891,16 +891,30 @@ std::unique_ptr FCLCollisionDetector::createCollisionObject( new FCLCollisionObject(this, shapeFrame, fclCollGeom)); } +//============================================================================== +void FCLCollisionDetector::refreshCollisionObject(CollisionObject* const object) +{ + FCLCollisionObject* fcl = static_cast(object); + + fcl->mFCLCollisionObject = std::unique_ptr( + new fcl::CollisionObject( + claimFCLCollisionGeometry(object->getShape()))); +} + //============================================================================== fcl_shared_ptr FCLCollisionDetector::claimFCLCollisionGeometry( const dynamics::ConstShapePtr& shape) { - const auto search = mShapeMap.find(shape); + const std::size_t currentVersion = shape->getVersion(); + + const auto search = mShapeMap.insert(std::make_pair(shape, ShapeInfo())); + const bool inserted = search.second; + ShapeInfo& info = search.first->second; - if (mShapeMap.end() != search) + if (!inserted && currentVersion == info.mLastKnownVersion) { - const auto& fclCollGeom = search->second; + const auto& fclCollGeom = info.mShape; assert(fclCollGeom.lock()); // Ensure all the collision geometry in the map should be alive pointers. @@ -909,7 +923,8 @@ FCLCollisionDetector::claimFCLCollisionGeometry( auto newfclCollGeom = createFCLCollisionGeometry( shape, mPrimitiveShapeType, FCLCollisionGeometryDeleter(this, shape)); - mShapeMap[shape] = newfclCollGeom; + info.mShape = newfclCollGeom; + info.mLastKnownVersion = currentVersion; return newfclCollGeom; } diff --git a/dart/collision/fcl/FCLCollisionDetector.hpp b/dart/collision/fcl/FCLCollisionDetector.hpp index af8968f439ff6..563b263976e0a 100644 --- a/dart/collision/fcl/FCLCollisionDetector.hpp +++ b/dart/collision/fcl/FCLCollisionDetector.hpp @@ -142,6 +142,8 @@ class FCLCollisionDetector : public CollisionDetector std::unique_ptr createCollisionObject( const dynamics::ShapeFrame* shapeFrame) override; + void refreshCollisionObject(CollisionObject* object) override; + /// Return fcl::CollisionGeometry associated with give Shape. New /// fcl::CollisionGeome will be created if it hasn't created yet. fcl_shared_ptr claimFCLCollisionGeometry( @@ -174,6 +176,16 @@ class FCLCollisionDetector : public CollisionDetector }; + /// Information for a shape that was generated by this collision detector + struct ShapeInfo final + { + /// A weak reference to the shape + fcl_weak_ptr mShape; + + /// The last version of the shape, as known by this collision detector + std::size_t mLastKnownVersion; + }; + /// Create fcl::CollisionGeometry with the custom deleter /// FCLCollisionGeometryDeleter fcl_shared_ptr createFCLCollisionGeometry( @@ -183,8 +195,7 @@ class FCLCollisionDetector : public CollisionDetector private: - using ShapeMap = std::map>; + using ShapeMap = std::map; ShapeMap mShapeMap; diff --git a/dart/collision/ode/OdeCollisionDetector.cpp b/dart/collision/ode/OdeCollisionDetector.cpp index 9dcd3408705c9..65b43b20041fb 100644 --- a/dart/collision/ode/OdeCollisionDetector.cpp +++ b/dart/collision/ode/OdeCollisionDetector.cpp @@ -237,6 +237,16 @@ std::unique_ptr OdeCollisionDetector::createCollisionObject( new OdeCollisionObject(this, shapeFrame)); } +//============================================================================== +void OdeCollisionDetector::refreshCollisionObject(CollisionObject* object) +{ + OdeCollisionObject temp(this, object->getShapeFrame()); + + static_cast(*object) = +// OdeCollisionObject(this, object->getShapeFrame()); + std::move(temp); +} + //============================================================================== dWorldID OdeCollisionDetector::getOdeWorldId() const { diff --git a/dart/collision/ode/OdeCollisionDetector.hpp b/dart/collision/ode/OdeCollisionDetector.hpp index 863b5748a480b..5e00b48471527 100644 --- a/dart/collision/ode/OdeCollisionDetector.hpp +++ b/dart/collision/ode/OdeCollisionDetector.hpp @@ -107,6 +107,9 @@ class OdeCollisionDetector : public CollisionDetector std::unique_ptr createCollisionObject( const dynamics::ShapeFrame* shapeFrame) override; + // Documentation inherited + void refreshCollisionObject(CollisionObject* object) override; + dWorldID getOdeWorldId() const; protected: diff --git a/dart/collision/ode/OdeCollisionObject.cpp b/dart/collision/ode/OdeCollisionObject.cpp index b17595e62cda3..b1d7abe91a2d7 100644 --- a/dart/collision/ode/OdeCollisionObject.cpp +++ b/dart/collision/ode/OdeCollisionObject.cpp @@ -107,6 +107,28 @@ OdeCollisionObject::OdeCollisionObject( } } +//============================================================================== +OdeCollisionObject& OdeCollisionObject::operator=( + OdeCollisionObject&& other) +{ + // This should only be used for refreshing the collision objects, so the + // detector and the shape frame should never need to change + assert(mCollisionDetector == other.mCollisionDetector); + assert(mShapeFrame == other.mShapeFrame); + + // We should never be assigning a collision object to itself + assert(this != &other); + + // There should never be duplicate body IDs or geometries + assert(!mBodyId || mBodyId != other.mBodyId); + assert(mOdeGeom.get() != other.mOdeGeom.get()); + + mOdeGeom = std::move(other.mOdeGeom); + std::swap(mBodyId, other.mBodyId); + + return *this; +} + //============================================================================== void OdeCollisionObject::updateEngineData() { diff --git a/dart/collision/ode/OdeCollisionObject.hpp b/dart/collision/ode/OdeCollisionObject.hpp index 869ad96c8033f..dc219e8f91dc1 100644 --- a/dart/collision/ode/OdeCollisionObject.hpp +++ b/dart/collision/ode/OdeCollisionObject.hpp @@ -60,6 +60,10 @@ class OdeCollisionObject : public CollisionObject OdeCollisionDetector* collisionDetector, const dynamics::ShapeFrame* shapeFrame); + /// Move assignment operator. This is used to refresh OdeCollisionObjects when + /// their underlying shape information needs to be updated. + OdeCollisionObject& operator=(OdeCollisionObject&& other); + // Documentation inherited void updateEngineData() override; diff --git a/dart/common/Factory.hpp b/dart/common/Factory.hpp index 1a0f5e4583551..dd383bb5f3979 100644 --- a/dart/common/Factory.hpp +++ b/dart/common/Factory.hpp @@ -36,6 +36,7 @@ #include #include #include +#include #include "dart/common/StlHelpers.hpp" #include "dart/common/Singleton.hpp" @@ -99,6 +100,9 @@ class Factory // TODO(JS): Add create() for creating smart_pointers // (see: https://github.com/dartsim/dart/pull/845) + /// Get a set of the keys that are available for this Creator + std::unordered_set getKeys() const; + private: template static HeldT defaultCreator(Args&&... args); diff --git a/dart/common/detail/Factory-impl.hpp b/dart/common/detail/Factory-impl.hpp index 2f923c3e95570..b88e72b37144d 100644 --- a/dart/common/detail/Factory-impl.hpp +++ b/dart/common/detail/Factory-impl.hpp @@ -176,6 +176,20 @@ HeldT Factory::create( return it->second(std::forward(args)...); } +//============================================================================== +template +std::unordered_set Factory::getKeys() const +{ + std::unordered_set keys; + for(const auto& entry : mCreatorMap) + keys.insert(entry.first); + + return keys; +} + //============================================================================== template addShapeFramesOf(skeleton.get()); + mCollisionGroup->subscribeTo(skeleton); mSkeletons.push_back(skeleton); mConstrainedGroups.reserve(mSkeletons.size()); } @@ -105,7 +105,7 @@ void ConstraintSolver::addSkeleton(const SkeletonPtr& skeleton) //============================================================================== void ConstraintSolver::addSkeletons(const std::vector& skeletons) { - for (const auto skeleton : skeletons) + for (const auto& skeleton : skeletons) addSkeleton(skeleton); } diff --git a/dart/dynamics/ArrowShape.cpp b/dart/dynamics/ArrowShape.cpp index 97d014065122e..ee8963e24b2f5 100644 --- a/dart/dynamics/ArrowShape.cpp +++ b/dart/dynamics/ArrowShape.cpp @@ -235,6 +235,8 @@ void ArrowShape::configureArrow(const Eigen::Vector3d& _tail, mIsBoundingBoxDirty = true; mIsVolumeDirty = true; + + incrementVersion(); } //============================================================================== diff --git a/dart/dynamics/BoxShape.cpp b/dart/dynamics/BoxShape.cpp index 4787f1810ed3b..6583377d70042 100644 --- a/dart/dynamics/BoxShape.cpp +++ b/dart/dynamics/BoxShape.cpp @@ -93,6 +93,8 @@ void BoxShape::setSize(const Eigen::Vector3d& _size) mSize = _size; mIsBoundingBoxDirty = true; mIsVolumeDirty = true; + + incrementVersion(); } //============================================================================== diff --git a/dart/dynamics/CapsuleShape.cpp b/dart/dynamics/CapsuleShape.cpp index 0b269beb0466f..da1d4f1cb7bc9 100644 --- a/dart/dynamics/CapsuleShape.cpp +++ b/dart/dynamics/CapsuleShape.cpp @@ -76,6 +76,8 @@ void CapsuleShape::setRadius(double radius) mRadius = radius; mIsBoundingBoxDirty = true; mIsVolumeDirty = true; + + incrementVersion(); } //============================================================================== @@ -91,6 +93,8 @@ void CapsuleShape::setHeight(double height) mHeight = height; mIsBoundingBoxDirty = true; mIsVolumeDirty = true; + + incrementVersion(); } //============================================================================== diff --git a/dart/dynamics/ConeShape.cpp b/dart/dynamics/ConeShape.cpp index 42145d5adb765..716b8a1ced343 100644 --- a/dart/dynamics/ConeShape.cpp +++ b/dart/dynamics/ConeShape.cpp @@ -76,6 +76,8 @@ void ConeShape::setRadius(double radius) mRadius = radius; mIsBoundingBoxDirty = true; mIsVolumeDirty = true; + + incrementVersion(); } //============================================================================== @@ -91,6 +93,8 @@ void ConeShape::setHeight(double height) mHeight = height; mIsBoundingBoxDirty = true; mIsVolumeDirty = true; + + incrementVersion(); } //============================================================================== diff --git a/dart/dynamics/CylinderShape.cpp b/dart/dynamics/CylinderShape.cpp index 50a8a8bc79774..d9427154a07ba 100644 --- a/dart/dynamics/CylinderShape.cpp +++ b/dart/dynamics/CylinderShape.cpp @@ -74,6 +74,8 @@ void CylinderShape::setRadius(double _radius) mRadius = _radius; mIsBoundingBoxDirty = true; mIsVolumeDirty = true; + + incrementVersion(); } //============================================================================== @@ -89,6 +91,8 @@ void CylinderShape::setHeight(double _height) mHeight = _height; mIsBoundingBoxDirty = true; mIsVolumeDirty = true; + + incrementVersion(); } //============================================================================== diff --git a/dart/dynamics/EllipsoidShape.cpp b/dart/dynamics/EllipsoidShape.cpp index 019ffc0f0eb78..a79a941326f37 100644 --- a/dart/dynamics/EllipsoidShape.cpp +++ b/dart/dynamics/EllipsoidShape.cpp @@ -86,6 +86,8 @@ void EllipsoidShape::setDiameters(const Eigen::Vector3d& diameters) mIsBoundingBoxDirty = true; mIsVolumeDirty = true; + + incrementVersion(); } //============================================================================== @@ -98,6 +100,11 @@ const Eigen::Vector3d& EllipsoidShape::getDiameters() const void EllipsoidShape::setRadii(const Eigen::Vector3d& radii) { mDiameters = radii * 2.0; + + mIsBoundingBoxDirty = true; + mIsVolumeDirty = true; + + incrementVersion(); } //============================================================================== diff --git a/dart/dynamics/MeshShape.cpp b/dart/dynamics/MeshShape.cpp index 2b04b1e6d704a..89c8a12ed0e33 100644 --- a/dart/dynamics/MeshShape.cpp +++ b/dart/dynamics/MeshShape.cpp @@ -240,6 +240,8 @@ void MeshShape::setMesh( mMeshPath.clear(); mResourceRetriever = std::move(resourceRetriever); + + incrementVersion(); } //============================================================================== @@ -250,6 +252,8 @@ void MeshShape::setScale(const Eigen::Vector3d& scale) mScale = scale; mIsBoundingBoxDirty = true; mIsVolumeDirty = true; + + incrementVersion(); } //============================================================================== diff --git a/dart/dynamics/MultiSphereConvexHullShape.cpp b/dart/dynamics/MultiSphereConvexHullShape.cpp index 8b5ff2560abd5..5449f91fa3da0 100644 --- a/dart/dynamics/MultiSphereConvexHullShape.cpp +++ b/dart/dynamics/MultiSphereConvexHullShape.cpp @@ -72,6 +72,8 @@ void MultiSphereConvexHullShape::addSpheres(const MultiSphereConvexHullShape::Sp mIsBoundingBoxDirty = true; mIsVolumeDirty = true; + + incrementVersion(); } //============================================================================== @@ -81,6 +83,8 @@ void MultiSphereConvexHullShape::addSphere(const MultiSphereConvexHullShape::Sph mIsBoundingBoxDirty = true; mIsVolumeDirty = true; + + incrementVersion(); } //============================================================================== @@ -96,6 +100,8 @@ void MultiSphereConvexHullShape::removeAllSpheres() mIsBoundingBoxDirty = true; mIsVolumeDirty = true; + + incrementVersion(); } //============================================================================== diff --git a/dart/dynamics/Node.cpp b/dart/dynamics/Node.cpp index 94053d3559745..10660fb4b6c8b 100644 --- a/dart/dynamics/Node.cpp +++ b/dart/dynamics/Node.cpp @@ -249,6 +249,7 @@ void Node::stageForRemoval() return; #endif + mBodyNode->incrementVersion(); BodyNode::NodeMap::iterator it = mBodyNode->mNodeMap.find(typeid(*this)); NodeDestructorPtr destructor = getOrCreateDestructor(); diff --git a/dart/dynamics/PlaneShape.cpp b/dart/dynamics/PlaneShape.cpp index 06db09884d608..2ef70ed8a53b2 100644 --- a/dart/dynamics/PlaneShape.cpp +++ b/dart/dynamics/PlaneShape.cpp @@ -75,6 +75,8 @@ Eigen::Matrix3d PlaneShape::computeInertia(double /*mass*/) const void PlaneShape::setNormal(const Eigen::Vector3d& _normal) { mNormal = _normal.normalized(); + + incrementVersion(); } //============================================================================== @@ -87,6 +89,8 @@ const Eigen::Vector3d& PlaneShape::getNormal() const void PlaneShape::setOffset(double _offset) { mOffset = _offset; + + incrementVersion(); } //============================================================================== diff --git a/dart/dynamics/Shape.cpp b/dart/dynamics/Shape.cpp index c86727b9dec5e..3d3d2554c4be7 100644 --- a/dart/dynamics/Shape.cpp +++ b/dart/dynamics/Shape.cpp @@ -47,9 +47,10 @@ Shape::Shape(ShapeType type) mIsVolumeDirty(true), mID(mCounter++), mVariance(STATIC), - mType(type) + mType(type), + onVersionChanged(mVersionChangedSignal) { - // Do nothing + mVersion = 1; } //============================================================================== @@ -58,9 +59,10 @@ Shape::Shape() mVolume(0.0), mID(mCounter++), mVariance(STATIC), - mType(UNSUPPORTED) + mType(UNSUPPORTED), + onVersionChanged(mVersionChangedSignal) { - // Do nothing + mVersion = 1; } //============================================================================== @@ -100,7 +102,7 @@ double Shape::getVolume() const } //============================================================================== -int Shape::getID() const +std::size_t Shape::getID() const { return mID; } @@ -175,7 +177,16 @@ void Shape::notifyColorUpdated(const Eigen::Vector4d& /*color*/) } //============================================================================== -int Shape::mCounter = PRIMITIVE_MAGIC_NUMBER; +std::size_t Shape::incrementVersion() +{ + ++mVersion; + mVersionChangedSignal.raise(this, mVersion); + + return mVersion; +} + +//============================================================================== +std::atomic_int Shape::mCounter{PRIMITIVE_MAGIC_NUMBER}; } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/Shape.hpp b/dart/dynamics/Shape.hpp index 6d698dfda6ad1..b26d0ffee03de 100644 --- a/dart/dynamics/Shape.hpp +++ b/dart/dynamics/Shape.hpp @@ -38,17 +38,24 @@ #include #include "dart/common/Deprecated.hpp" +#include "dart/common/Signal.hpp" #include "dart/common/Subject.hpp" +#include "dart/common/VersionCounter.hpp" #include "dart/math/Geometry.hpp" #include "dart/dynamics/SmartPointer.hpp" namespace dart { namespace dynamics { -class Shape : public virtual common::Subject +class Shape + : public virtual common::Subject, + public virtual common::VersionCounter { public: + using VersionChangedSignal + = common::Signal; + /// \deprecated Deprecated in 6.1. Please use getType() instead. enum ShapeType { SPHERE, @@ -126,7 +133,7 @@ class Shape : public virtual common::Subject double getVolume() const; /// \brief - int getID() const; + std::size_t getID() const; /// \deprecated Deprecated in 6.1. Please use getType() instead. DART_DEPRECATED(6.1) @@ -168,6 +175,9 @@ class Shape : public virtual common::Subject /// Notify that the color (rgba) of this shape has updated virtual void notifyColorUpdated(const Eigen::Vector4d& color); + /// Increment the version of this Shape and notify its subscribers + std::size_t incrementVersion() override final; + protected: /// Updates volume virtual void updateVolume() const = 0; @@ -188,17 +198,26 @@ class Shape : public virtual common::Subject mutable bool mIsVolumeDirty; /// \brief Unique id. - int mID; + const std::size_t mID; /// The DataVariance of this Shape unsigned int mVariance; /// \brief - static int mCounter; + static std::atomic_int mCounter; /// \deprecated Deprecated in 6.1. Please use getType() instead. /// Type of primitive shpae. ShapeType mType; + +private: + /// Triggered by incrementVersion() + VersionChangedSignal mVersionChangedSignal; + +public: + /// Use this to subscribe to version change signals + common::SlotRegister onVersionChanged; + }; } // namespace dynamics diff --git a/dart/dynamics/ShapeFrame.cpp b/dart/dynamics/ShapeFrame.cpp index b3a94df6b60b9..85bb9b1b3ceee 100644 --- a/dart/dynamics/ShapeFrame.cpp +++ b/dart/dynamics/ShapeFrame.cpp @@ -182,6 +182,15 @@ DynamicsAspect::DynamicsAspect( // Do nothing } +//============================================================================== +ShapeFrame::~ShapeFrame() +{ + // TODO(MXG): Why doesn't ScopedConnection seem to work as a member variable? + // If we could use a ScopedConnection for mConnectionForShapeVersionChange + // instead, then we wouldn't need to explicitly disconnect in this destructor. + mConnectionForShapeVersionChange.disconnect(); +} + //============================================================================== void ShapeFrame::setProperties(const ShapeFrame::UniqueProperties& properties) { @@ -209,6 +218,20 @@ void ShapeFrame::setShape(const ShapePtr& shape) ShapePtr oldShape = ShapeFrame::mAspectProperties.mShape; ShapeFrame::mAspectProperties.mShape = shape; + incrementVersion(); + + mConnectionForShapeVersionChange.disconnect(); + + if(shape) + { + mConnectionForShapeVersionChange = shape->onVersionChanged.connect( + [this](Shape* shape, std::size_t) + { + assert(shape == this->ShapeFrame::mAspectProperties.mShape.get()); + DART_UNUSED(shape); + this->incrementVersion(); + }); + } mShapeUpdatedSignal.raise(this, oldShape, ShapeFrame::mAspectProperties.mShape); } @@ -272,8 +295,7 @@ ShapeFrame::ShapeFrame(Frame* parent, const Properties& properties) } //============================================================================== -ShapeFrame::ShapeFrame(Frame* parent, - const ShapePtr& shape) +ShapeFrame::ShapeFrame(Frame* parent, const ShapePtr& shape) : common::Composite(), Entity(ConstructFrame), Frame(parent), diff --git a/dart/dynamics/ShapeFrame.hpp b/dart/dynamics/ShapeFrame.hpp index 9d57d099d3764..42f96fbebb783 100644 --- a/dart/dynamics/ShapeFrame.hpp +++ b/dart/dynamics/ShapeFrame.hpp @@ -180,7 +180,7 @@ class ShapeFrame : using Properties = UniqueProperties; /// Destructor - virtual ~ShapeFrame() = default; + virtual ~ShapeFrame() override; /// Set the UniqueProperties of this ShapeFrame void setProperties(const UniqueProperties& properties); @@ -245,6 +245,9 @@ class ShapeFrame : /// Relative transformation updated signal RelativeTransformUpdatedSignal mRelativeTransformUpdatedSignal; + /// Connect to changes in the Shape version + common::Connection mConnectionForShapeVersionChange; + public: /// Slot register for shape updated signal diff --git a/dart/dynamics/SphereShape.cpp b/dart/dynamics/SphereShape.cpp index 70f4a04616c9a..66789ebcd6acc 100644 --- a/dart/dynamics/SphereShape.cpp +++ b/dart/dynamics/SphereShape.cpp @@ -72,6 +72,8 @@ void SphereShape::setRadius(double radius) mIsBoundingBoxDirty = true; mIsVolumeDirty = true; + + incrementVersion(); } //============================================================================== diff --git a/dart/dynamics/VoxelGridShape.cpp b/dart/dynamics/VoxelGridShape.cpp index 9ad75a5461ad8..1efe78f9cfd9f 100644 --- a/dart/dynamics/VoxelGridShape.cpp +++ b/dart/dynamics/VoxelGridShape.cpp @@ -115,6 +115,8 @@ void VoxelGridShape::setOctree(fcl_shared_ptr octree) mIsBoundingBoxDirty = true; mIsVolumeDirty = true; + + incrementVersion(); } //============================================================================== @@ -134,6 +136,8 @@ void VoxelGridShape::updateOccupancy( const Eigen::Vector3d& point, bool occupied) { mOctree->updateNode(toPoint3d(point), occupied); + + incrementVersion(); } //============================================================================== @@ -141,6 +145,8 @@ void VoxelGridShape::updateOccupancy( const Eigen::Vector3d& from, const Eigen::Vector3d& to) { mOctree->insertRay(toPoint3d(from), toPoint3d(to)); + + incrementVersion(); } //============================================================================== @@ -152,6 +158,7 @@ void VoxelGridShape::updateOccupancy( if (relativeTo == Frame::World()) { mOctree->insertPointCloud(pointCloud, toPoint3d(sensorOrigin)); + incrementVersion(); } else { @@ -167,6 +174,8 @@ void VoxelGridShape::updateOccupancy( { mOctree->insertPointCloud( pointCloud, toPoint3d(sensorOrigin), toPose6d(relativeTo)); + + incrementVersion(); } //============================================================================== diff --git a/dart/dynamics/detail/HeightmapShape-impl.hpp b/dart/dynamics/detail/HeightmapShape-impl.hpp index 55579fcb28a19..853cb695cdb79 100644 --- a/dart/dynamics/detail/HeightmapShape-impl.hpp +++ b/dart/dynamics/detail/HeightmapShape-impl.hpp @@ -76,6 +76,8 @@ void HeightmapShape::setScale(const Eigen::Vector3d& scale) mScale = scale; mIsBoundingBoxDirty = true; mIsVolumeDirty = true; + + incrementVersion(); } //============================================================================== @@ -125,6 +127,8 @@ void HeightmapShape::setHeightField( } mIsBoundingBoxDirty = true; mIsVolumeDirty = true; + + incrementVersion(); } //============================================================================== diff --git a/unittests/unit/CMakeLists.txt b/unittests/unit/CMakeLists.txt index a5dc7ce96da20..f28f71266c383 100644 --- a/unittests/unit/CMakeLists.txt +++ b/unittests/unit/CMakeLists.txt @@ -1,4 +1,5 @@ dart_add_test("unit" test_Aspect) +dart_add_test("unit" test_CollisionGroups) dart_add_test("unit" test_ContactConstraint) dart_add_test("unit" test_Factory) dart_add_test("unit" test_GenericJoints) @@ -69,3 +70,13 @@ if(TARGET dart-planning) dart_add_test("unit" test_NearestNeighbor) target_link_libraries(test_NearestNeighbor dart-planning) endif() + +foreach(collision_engine + dart-collision-bullet + dart-collision-ode) + + if(TARGET ${collision_engine}) + target_link_libraries(test_CollisionGroups ${collision_engine}) + endif() + +endforeach() diff --git a/unittests/unit/test_CollisionGroups.cpp b/unittests/unit/test_CollisionGroups.cpp new file mode 100644 index 0000000000000..c7eb2116cda75 --- /dev/null +++ b/unittests/unit/test_CollisionGroups.cpp @@ -0,0 +1,269 @@ +/* + * Copyright (c) 2011-2018, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +#include + +#include "dart/dynamics/FreeJoint.hpp" +#include "dart/dynamics/BoxShape.hpp" +#include "dart/dynamics/Skeleton.hpp" +#include + +#include "dart/simulation/World.hpp" +#include "dart/constraint/ConstraintSolver.hpp" + + +class CollisionGroupsTest + : public testing::Test, + public testing::WithParamInterface +{ + +}; + +TEST_P(CollisionGroupsTest, SkeletonSubscription) +{ + if(!dart::collision::CollisionDetector::getFactory()->canCreate(GetParam())) + { + std::cout << "Skipping test for [" << GetParam() << "], because it is not " + << "available" << std::endl; + return; + } + else + { + std::cout << "Running CollisionGroups test for [" << GetParam() << "]" + << std::endl; + } + + // Note: When skeletons are added to a world, the constraint solver will + // subscribe to them. + dart::simulation::WorldPtr world = dart::simulation::World::create(); + world->getConstraintSolver()->setCollisionDetector( + dart::collision::CollisionDetector::getFactory()->create(GetParam())); + + dart::dynamics::SkeletonPtr skel_A = dart::dynamics::Skeleton::create("A"); + dart::dynamics::SkeletonPtr skel_B = dart::dynamics::Skeleton::create("B"); + + world->addSkeleton(skel_A); + world->addSkeleton(skel_B); + + // There should be no collisions because there are no shapes in the world. + EXPECT_FALSE(world->checkCollision()); + + // We will now add some BodyNodes and collision shapes *after* the Skeletons + // have been added to the world, to see that the collision geometries get + // updated automatically. + Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); + tf.translation() = (1.0 + 0.25)*Eigen::Vector3d::UnitX(); + + auto boxShape = std::make_shared( + Eigen::Vector3d::Constant(1.0)); + auto pair = skel_A->createJointAndBodyNodePair(); + pair.first->setTransform(tf); + auto sn1 = pair.second->createShapeNodeWith< + dart::dynamics::CollisionAspect>(boxShape); + + tf.translation() = (1.0 -0.25)*Eigen::Vector3d::UnitX(); + + pair = skel_B->createJointAndBodyNodePair(); + pair.first->setTransform(tf); + auto sn2 = pair.second->createShapeNodeWith< + dart::dynamics::CollisionAspect>(boxShape); + + EXPECT_TRUE(world->checkCollision()); + + // Now we'll change the properties of one the box shape so that there should + // no longer be any collisions. + boxShape->setSize(Eigen::Vector3d::Constant(0.2)); + dart::collision::CollisionResult result2; + EXPECT_FALSE(world->checkCollision()); + + // Now we'll replace one of the boxes with a large one, so that a collision + // will occur again. + auto largeBox = std::make_shared( + Eigen::Vector3d::Constant(0.95)); + sn1->setShape(largeBox); + EXPECT_TRUE(world->checkCollision()); + + // After this, both shapes will have been replaced with new shape instances. + // If the shape map of the collision detector is empty upon destruction, then + // the internal collision shapes are being managed correctly. Otherwise, if + // they are not being managed correctly, then an assertion will fail when + // testing in debug mode. + auto sphereShape = std::make_shared(0.01); + sn2->setShape(sphereShape); + EXPECT_FALSE(world->checkCollision()); + + // Resize the sphere so that there is a collision again. + sphereShape->setRadius(0.5); + EXPECT_TRUE(world->checkCollision()); + + // Remove the shape node so that there should no longer be a collision + sn2->remove(); + EXPECT_FALSE(world->checkCollision()); + + // Create a new shape node so that there should be a collision again + pair.second->createShapeNodeWith< + dart::dynamics::CollisionAspect>(sphereShape); + EXPECT_TRUE(world->checkCollision()); + + // Remove the BodyNode so that there should no longer be a collision + pair.second->remove(); + EXPECT_FALSE(world->checkCollision()); + + // Add a new BodyNode and Shape Node so that there is a collision again + pair = skel_B->createJointAndBodyNodePair(); + pair.first->setTransform(tf); + pair.second->createShapeNodeWith< + dart::dynamics::CollisionAspect>(sphereShape); + EXPECT_TRUE(world->checkCollision()); + + // Remove a skeleton so that there are no longer collisions + world->removeSkeleton(skel_B); + EXPECT_FALSE(world->checkCollision()); +} + +TEST_P(CollisionGroupsTest, BodyNodeSubscription) +{ + if(!dart::collision::CollisionDetector::getFactory()->canCreate(GetParam())) + { + std::cout << "Skipping test for [" << GetParam() << "], because it is not " + << "available" << std::endl; + return; + } + else + { + std::cout << "Running CollisionGroups test for [" << GetParam() << "]" + << std::endl; + } + + auto cd = dart::collision::CollisionDetector::getFactory() + ->create(GetParam()); + + auto group = cd->createCollisionGroup(); + + auto skel1 = dart::dynamics::Skeleton::create("skel1"); + auto skel2 = dart::dynamics::Skeleton::create("skel2"); + + auto pair_1a = skel1->createJointAndBodyNodePair(); + auto pair_2a = skel2->createJointAndBodyNodePair(); + + group->subscribeTo(pair_1a.second, pair_2a.second); + // The BodyNodes currently have no collision geometries, so we expect there to + // be no collisions. + EXPECT_FALSE(group->collide()); + + Eigen::Isometry3d tf{Eigen::Translation3d(0.5, 0.0, 0.0)}; + pair_1a.first->setTransform(tf); + pair_1a.first->setName("1a"); + pair_1a.second->setName("1a"); + + tf.translation()[0] = -0.5; + pair_2a.first->setTransform(tf); + pair_2a.first->setName("2a"); + pair_2a.second->setName("2a"); + + auto sphere = std::make_shared(0.75); + + auto sn_1a = pair_1a.second->createShapeNodeWith< + dart::dynamics::CollisionAspect>(sphere); + + auto sn_2a = pair_2a.second->createShapeNodeWith< + dart::dynamics::CollisionAspect>(sphere); + + // The BodyNodes have been given overlapping shapes, so now we expect the + // collision information to automatically update and identify a collision. + EXPECT_TRUE(group->collide()); + + tf.translation()[0] = 1.0; + pair_1a.first->setTransform(tf); + tf.translation()[0] = -1.0; + pair_2a.first->setTransform(tf); + + // The collision geometries have been moved far enough from each other that + // there should no longer be any collisions + EXPECT_FALSE(group->collide()); + + sphere->setRadius(1.1); + // The shapes of the collision geometries have been expanded enough that they + // should collide again. + EXPECT_TRUE(group->collide()); + + auto box = std::make_shared( + Eigen::Vector3d::Constant(0.5)); + sn_2a->setShape(box); + // Now a shape has been replaced with a smaller one, so there should no longer + // be a collision. + EXPECT_FALSE(group->collide()); + + auto pair_1b = skel1->createJointAndBodyNodePair(); + auto pair_2b = skel2->createJointAndBodyNodePair(); + + tf.translation() = 0.5*Eigen::Vector3d::UnitX(); + pair_1b.first->setTransform(tf); + pair_1b.first->setName("1b"); + pair_1b.second->setName("1b"); + + tf.translation()[0] = -0.5; + pair_2b.first->setTransform(tf); + pair_2b.first->setName("2b"); + pair_2b.second->setName("2b"); + + pair_1b.second->createShapeNodeWith(sphere); + pair_2b.second->createShapeNodeWith(sphere); + + // The collision group should not be tracking the new BodyNodes, so there + // should still be no collisions + EXPECT_FALSE(group->collide()); + + // Change the size of the box so that there are overlapping collision + // geometries. The collision information should update automatically, and a + // collision should be detected. + box->setSize(Eigen::Vector3d::Constant(2.0)); + EXPECT_TRUE(group->collide()); + + sn_1a->remove(); + // A shape node has been removed, so there should no longer be a collision + EXPECT_FALSE(group->collide()); + + pair_1a.second->createShapeNodeWith(sphere); + // A new shape node has been added with the same geometry as the one that was + // removed, so there should be a collision again + EXPECT_TRUE(group->collide()); + + // Remove one of the BodyNodes so that there should no longer be a collision. + group->removeShapeFramesOf(pair_1a.second); + EXPECT_FALSE(group->collide()); +} + +INSTANTIATE_TEST_CASE_P(CollisionEngine, CollisionGroupsTest, + testing::Values("dart", "fcl", "bullet", "ode"));