Refactor and fix weird bug.

personalrobotics · Jul 17, 2018 · 84a0041 · 84a0041
1 parent 3f36bf1
commit 84a0041
Showing 1 changed file with 11 additions and 15 deletions.
diff --git a/src/distance/ConfigurationRanker.cpp b/src/distance/ConfigurationRanker.cpp
@@ -29,22 +29,18 @@ ConfigurationRanker::ConfigurationRanker(
 void ConfigurationRanker::rankConfigurations(
     std::vector<MetaSkeletonStateSpace::ScopedState>& configurations)
 {
-  std::vector<std::pair<double, MetaSkeletonStateSpace::ScopedState>>
-      evaluatedConfigurations;
+  std::unordered_map<const MetaSkeletonStateSpace::State*, double> costs;
+  costs.reserve(configurations.size());
   for (std::size_t i = 0; i < configurations.size(); ++i)
-  {
-    evaluatedConfigurations.emplace_back(
-        std::make_pair(
-            evaluateConfiguration(configurations[i]),
-            std::move(configurations[i])));
-  }
-  std::sort(evaluatedConfigurations.begin(), evaluatedConfigurations.end());
-
-  configurations.clear();
-  for (std::size_t i = 0; i < evaluatedConfigurations.size(); ++i)
-  {
-    configurations.emplace_back(std::move(evaluatedConfigurations[i].second));
-  }
+    costs[configurations[i]] = evaluateConfiguration(configurations[i]);
+
+  std::sort(
+      configurations.begin(),
+      configurations.end(),
+      [&](const MetaSkeletonStateSpace::ScopedState& left,
+          const MetaSkeletonStateSpace::ScopedState& right) {
+        return costs[left] < costs[right];
+      });
 }
 
 } // namespace distance