better performance

haosulab · Mar 28, 2024 · 97570f8 · 97570f8
1 parent 4db4caa
commit 97570f8
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 9 deletions.
diff --git a/include/mplib/planning_world.h b/include/mplib/planning_world.h
@@ -70,8 +70,8 @@ class PlanningWorldTpl {
    *
    * @return: list of non-articulated collision objects
    */
-  std::vector<CollisionObjectPtr> getNormalObjects() const {
-    std::vector<CollisionObjectPtr> ret;
+  std::vector<FCLObject> getNormalObjects() const {
+    std::vector<FCLObject> ret;
     for (const auto &itm : normal_object_map_) ret.push_back(itm.second);
     return ret;
   }
@@ -122,7 +122,10 @@ class PlanningWorldTpl {
    */
   void setNormalObject(const std::string &name,
                        const CollisionObjectPtr &collision_object) {
-    normal_object_map_[name] = collision_object;
+    FCLObject fcl_obj;
+    fcl_obj.collision_objects_.push_back(collision_object);
+    fcl_obj.tfs_.push_back(Isometry3<S>::Identity());
+    normal_object_map_[name] = fcl_obj;
   }
 
   /**
@@ -274,7 +277,7 @@ class PlanningWorldTpl {
   std::vector<ArticulatedModelPtr> articulations_;
   std::vector<std::string> articulation_names_;
 
-  std::unordered_map<std::string, CollisionObjectPtr> normal_object_map_;
+  std::unordered_map<std::string, FCLObject> normal_object_map_;
 
   int move_articulation_id_, attach_link_id_;
   CollisionObjectPtr point_cloud_, attached_tool_;

diff --git a/src/planning_world.cpp b/src/planning_world.cpp
@@ -32,7 +32,7 @@ PlanningWorldTpl<S>::PlanningWorldTpl(
   ASSERT(normal_objects.size() == normal_object_names.size(),
          "normal_objects and normal_object_names should have the same size");
   for (size_t i = 0; i < normal_objects.size(); i++) {
-    normal_object_map_[normal_object_names[i]] = normal_objects[i];
+    setNormalObject(normal_object_names[i], normal_objects[i]);
   }
 }
 
@@ -169,11 +169,8 @@ std::vector<WorldCollisionResultTpl<S>> PlanningWorldTpl<S>::collideWithOthers(
       result.clear();
       const std::string &normal_object_name = itm.first;
       const auto &normal_object = itm.second;
-      FCLObject normal_object_fcl;
-      normal_object_fcl.collision_objects_.push_back(normal_object);
-      normal_object_fcl.tfs_.push_back(Isometry3<S>::Identity());
       collision_detection::fcl::collideFCLObjects(
-          CollisionObjects[i], normal_object_fcl, request, result);
+          CollisionObjects[i], normal_object, request, result);
       if (result.isCollision()) {
         WorldCollisionResult tmp;
         tmp.res = result;