Rollback the replacing parallel vectors with map in CollisionGroup (s…

…ee the comment for the detail)
dartsim · Apr 6, 2016 · df324db · df324db
1 parent 8927ebb
commit df324db
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 4 deletions.
diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp
@@ -78,7 +78,7 @@ void CollisionGroup::addShapeFrame(const dynamics::ShapeFrame* shapeFrame)
 
   addCollisionObjectToEngine(collObj.get());
 
-  mShapeFrameMap[shapeFrame] = collObj;
+  mShapeFrameMap.push_back(std::make_pair(shapeFrame, collObj));
 }
 
 //==============================================================================
@@ -101,7 +101,11 @@ void CollisionGroup::removeShapeFrame(const dynamics::ShapeFrame* shapeFrame)
   if (!shapeFrame)
     return;
 
-  const auto search = mShapeFrameMap.find(shapeFrame);
+  const auto search
+      = std::find_if(mShapeFrameMap.begin(), mShapeFrameMap.end(),
+                     [&](const std::pair<const dynamics::ShapeFrame*,
+                                         CollisionObjectPtr>& pair)
+                     { return pair.first == shapeFrame; });
 
   if (mShapeFrameMap.end() == search)
     return;
@@ -137,7 +141,11 @@ void CollisionGroup::removeAllShapeFrames()
 bool CollisionGroup::hasShapeFrame(
     const dynamics::ShapeFrame* shapeFrame) const
 {
-  return mShapeFrameMap.find(shapeFrame) != mShapeFrameMap.end();
+  return std::find_if(mShapeFrameMap.begin(), mShapeFrameMap.end(),
+                      [&](const std::pair<const dynamics::ShapeFrame*,
+                                          CollisionObjectPtr>& pair)
+                      { return pair.first == shapeFrame; })
+      != mShapeFrameMap.end();
 }
 
 //==============================================================================

diff --git a/dart/collision/CollisionGroup.h b/dart/collision/CollisionGroup.h
@@ -218,9 +218,20 @@ class CollisionGroup
   // CollisionGroup is alive.
 
   /// ShapeFrames and CollisionOjbects added to this CollisionGroup
-  std::map<const dynamics::ShapeFrame*, CollisionObjectPtr> mShapeFrameMap;
+  std::vector<std::pair<const dynamics::ShapeFrame*,
+                        CollisionObjectPtr>> mShapeFrameMap;
   // CollisionGroup also shares the ownership of CollisionObjects across other
   // CollisionGroups for the same reason with above.
+  //
+  // Dev note: This was supposed to be std::map rather than std::vector for
+  // better search performance. The reason we use std::vector is to get
+  // deterministic contact results regardless of the order of CollisionObjects
+  // in this container for FCLCollisionDetector.
+  //
+  // fcl's collision result is dependent on the order of objects in the broad
+  // phase classes. If we use std::map, the orders of element between the
+  // original and copy are not guranteed to be the same as we copy std::map
+  // (e.g., by world cloning).
 
 };