diff --git a/descartes_light/solvers/include/descartes_light/solvers/bgl/bgl_dijkstra_solver.h b/descartes_light/solvers/include/descartes_light/solvers/bgl/bgl_dijkstra_solver.h
new file mode 100644
index 00000000..5bcfbccf
--- /dev/null
+++ b/descartes_light/solvers/include/descartes_light/solvers/bgl/bgl_dijkstra_solver.h
@@ -0,0 +1,43 @@
+#ifndef DESCARTES_LIGHT_SOLVERS_BGL_BGL_DIJKSTRA_SOLVER_H
+#define DESCARTES_LIGHT_SOLVERS_BGL_BGL_DIJKSTRA_SOLVER_H
+
+#include <descartes_light/solvers/bgl/bgl_solver.h>
+
+namespace descartes_light
+{
+/**
+ * @brief BGL solver implementation that constructs vertices and edges in the build function and uses Dijkstra's
+ * algorithm with a default visitor to search the graph
+ */
+template <typename FloatType>
+class BGLDijkstraSVSESolver : public BGLSolverBaseSVSE<FloatType>
+{
+public:
+  using BGLSolverBaseSVSE<FloatType>::BGLSolverBaseSVSE;
+
+  SearchResult<FloatType> search() override;
+};
+
+using BGLDijkstraSVSESolverF = BGLDijkstraSVSESolver<float>;
+using BGLDijkstraSVSESolverD = BGLDijkstraSVSESolver<double>;
+
+/**
+ * @brief BGL solver implementation that constructs vertices and edges in the build function and uses Dijkstra's
+ * algorithm with a visitor that terminates the search once a vertex in the last rung of the graph is encountered rather
+ * than allowing it to continue until the distance to all nodes in the graph has been calculated
+ */
+template <typename FloatType>
+class BGLEfficientDijkstraSVSESolver : public BGLSolverBaseSVSE<FloatType>
+{
+public:
+  using BGLSolverBaseSVSE<FloatType>::BGLSolverBaseSVSE;
+
+  SearchResult<FloatType> search() override;
+};
+
+using BGLEfficientDijkstraSVSESolverF = BGLEfficientDijkstraSVSESolver<float>;
+using BGLEfficientDijkstraSVSESolverD = BGLEfficientDijkstraSVSESolver<double>;
+
+}  // namespace descartes_light
+
+#endif  // DESCARTES_LIGHT_SOLVERS_BGL_BGL_DIJKSTRA_SOLVER_H
diff --git a/descartes_light/solvers/include/descartes_light/solvers/bgl/bgl_solver.h b/descartes_light/solvers/include/descartes_light/solvers/bgl/bgl_solver.h
index 014d90b1..72bfc109 100644
--- a/descartes_light/solvers/include/descartes_light/solvers/bgl/bgl_solver.h
+++ b/descartes_light/solvers/include/descartes_light/solvers/bgl/bgl_solver.h
@@ -69,11 +69,12 @@ class BGLSolverBase : public Solver<FloatType>
 };
 
 /**
- * @brief BGL solver partial implementation that constructs only vertices in the build function with the assumption that
- * edges will be added by discovery during the search
+ * @brief BGL Solver Static Vertex Dynamic Edge (SVDE) partial implementation
+ * @details Constructs only vertices in the build function (i.e. statically) with the assumption that edges will be
+ * added during the search (i.e. dynamically)
  */
 template <typename FloatType>
-class BGLSolverBaseV : public BGLSolverBase<FloatType>
+class BGLSolverBaseSVDE : public BGLSolverBase<FloatType>
 {
 public:
   using BGLSolverBase<FloatType>::BGLSolverBase;
@@ -84,33 +85,18 @@ class BGLSolverBaseV : public BGLSolverBase<FloatType>
 };
 
 /**
- * @brief BGL solver partial implementation that constructs both vertices and edges in the build function
+ * @brief BGL solver Static Vertex Static Edge (SVSE) partial implementation
+ * @details Constructs both vertices and edges in the build function (i.e. statically)
  */
 template <typename FloatType>
-class BGLSolverBaseVE : public BGLSolverBaseV<FloatType>
+class BGLSolverBaseSVSE : public BGLSolverBaseSVDE<FloatType>
 {
 public:
-  using BGLSolverBaseV<FloatType>::BGLSolverBaseV;
+  using BGLSolverBaseSVDE<FloatType>::BGLSolverBaseSVDE;
 
   BuildStatus buildImpl(const std::vector<typename WaypointSampler<FloatType>::ConstPtr>& trajectory,
                         const std::vector<typename EdgeEvaluator<FloatType>::ConstPtr>& edge_eval,
                         const std::vector<typename StateEvaluator<FloatType>::ConstPtr>& state_eval) override;
 };
 
-/**
- * @brief BGL solver implementation that constructs vertices and edges in the build function and uses Dijkstra's
- * algorithm with a default visitor to search the graph
- */
-template <typename FloatType>
-class BGLDijkstraSolverVE : public BGLSolverBaseVE<FloatType>
-{
-public:
-  using BGLSolverBaseVE<FloatType>::BGLSolverBaseVE;
-
-  SearchResult<FloatType> search() override;
-};
-
-using BGLDijkstraSolverVEF = BGLDijkstraSolverVE<float>;
-using BGLDijkstraSolverVED = BGLDijkstraSolverVE<double>;
-
 }  // namespace descartes_light
diff --git a/descartes_light/solvers/include/descartes_light/solvers/bgl/impl/bgl_dijkstra_solver.hpp b/descartes_light/solvers/include/descartes_light/solvers/bgl/impl/bgl_dijkstra_solver.hpp
new file mode 100644
index 00000000..a2e5920a
--- /dev/null
+++ b/descartes_light/solvers/include/descartes_light/solvers/bgl/impl/bgl_dijkstra_solver.hpp
@@ -0,0 +1,146 @@
+#ifndef DESCARTES_LIGHT_SOLVERS_BGL_IMPL_BGL_DIJKSTRA_SOLVER_HPP
+#define DESCARTES_LIGHT_SOLVERS_BGL_IMPL_BGL_DIJKSTRA_SOLVER_HPP
+
+#include <descartes_light/solvers/bgl/bgl_dijkstra_solver.h>
+
+#include <descartes_light/descartes_macros.h>
+DESCARTES_IGNORE_WARNINGS_PUSH
+#include <boost/graph/dijkstra_shortest_paths.hpp>
+DESCARTES_IGNORE_WARNINGS_POP
+
+namespace descartes_light
+{
+template <typename FloatType>
+SearchResult<FloatType> BGLDijkstraSVSESolver<FloatType>::search()
+{
+  // Convenience aliases
+  auto& graph_ = BGLSolverBase<FloatType>::graph_;
+  const auto& source_ = BGLSolverBase<FloatType>::source_;
+  auto& predecessor_map_ = BGLSolverBase<FloatType>::predecessor_map_;
+  const auto& ladder_rungs_ = BGLSolverBase<FloatType>::ladder_rungs_;
+
+  // Internal properties
+  auto index_prop_map = boost::get(boost::vertex_index, graph_);
+  auto weight_prop_map = boost::get(boost::edge_weight, graph_);
+  auto color_prop_map = boost::get(&Vertex<FloatType>::color, graph_);
+  auto distance_prop_map = boost::get(&Vertex<FloatType>::distance, graph_);
+
+  predecessor_map_.clear();
+  boost::associative_property_map<std::map<VertexDesc<FloatType>, VertexDesc<FloatType>>> predecessor_prop_map(
+      predecessor_map_);
+
+  // Perform the search
+  boost::dijkstra_shortest_paths(graph_,
+                                 source_,
+                                 predecessor_prop_map,
+                                 distance_prop_map,
+                                 weight_prop_map,
+                                 index_prop_map,
+                                 std::less<>(),
+                                 std::plus<>(),
+                                 std::numeric_limits<FloatType>::max(),
+                                 static_cast<FloatType>(0.0),
+                                 boost::default_dijkstra_visitor(),
+                                 color_prop_map);
+
+  // Find lowest cost node in last rung
+  auto target = std::min_element(ladder_rungs_.back().begin(),
+                                 ladder_rungs_.back().end(),
+                                 [&](const VertexDesc<FloatType>& a, const VertexDesc<FloatType>& b) {
+                                   return graph_[a].distance < graph_[b].distance;
+                                 });
+
+  SearchResult<FloatType> result;
+
+  // Reconstruct the path from the predecesor map; remove the artificial start state
+  const auto vd_path = BGLSolverBase<FloatType>::reconstructPath(source_, *target);
+  result.trajectory = BGLSolverBase<FloatType>::toStates(vd_path);
+  result.trajectory.erase(result.trajectory.begin());
+
+  result.cost = graph_[*target].distance;
+
+  return result;
+}
+
+/**
+ * @brief Visitor for Dijkstra search that terminates the search once a vertex in the last rung has been encountered
+ */
+template <typename FloatType>
+class DijkstraTerminateEarlyVisitor : public boost::default_dijkstra_visitor
+{
+public:
+  DijkstraTerminateEarlyVisitor(long last_rung_idx) : last_rung_idx_(last_rung_idx) {}
+
+  /**
+   * @brief Hook for introspecting the search when a new vertex is opened by the search.
+   * @details Throws the vertex descriptor that is the termination of the path once the first vertex in the last rung of
+   * the graph is encountered
+   */
+  void examine_vertex(VertexDesc<FloatType> u, const BGLGraph<FloatType>& g)
+  {
+    if (g[u].rung_idx == last_rung_idx_)
+      throw u;
+  }
+
+private:
+  const long last_rung_idx_;
+};
+
+template <typename FloatType>
+SearchResult<FloatType> BGLEfficientDijkstraSVSESolver<FloatType>::search()
+{
+  // Convenience aliases
+  auto& graph_ = BGLSolverBase<FloatType>::graph_;
+  const auto& source_ = BGLSolverBase<FloatType>::source_;
+  auto& predecessor_map_ = BGLSolverBase<FloatType>::predecessor_map_;
+  const auto& ladder_rungs_ = BGLSolverBase<FloatType>::ladder_rungs_;
+
+  // Internal properties
+  auto index_prop_map = boost::get(boost::vertex_index, graph_);
+  auto weight_prop_map = boost::get(boost::edge_weight, graph_);
+  auto color_prop_map = boost::get(&Vertex<FloatType>::color, graph_);
+  auto distance_prop_map = boost::get(&Vertex<FloatType>::distance, graph_);
+
+  predecessor_map_.clear();
+  boost::associative_property_map<std::map<VertexDesc<FloatType>, VertexDesc<FloatType>>> predecessor_prop_map(
+      predecessor_map_);
+
+  DijkstraTerminateEarlyVisitor<FloatType> visitor(static_cast<long>(ladder_rungs_.size() - 1));
+
+  // Perform the search
+  try
+  {
+    boost::dijkstra_shortest_paths(graph_,
+                                   source_,
+                                   predecessor_prop_map,
+                                   distance_prop_map,
+                                   weight_prop_map,
+                                   index_prop_map,
+                                   std::less<>(),
+                                   std::plus<>(),
+                                   std::numeric_limits<FloatType>::max(),
+                                   static_cast<FloatType>(0.0),
+                                   visitor,
+                                   color_prop_map);
+  }
+  catch (const VertexDesc<FloatType>& target)
+  {
+    SearchResult<FloatType> result;
+
+    // Reconstruct the path from the predecesor map; remove the artificial start state
+    const auto vd_path = BGLSolverBase<FloatType>::reconstructPath(source_, target);
+    result.trajectory = BGLSolverBase<FloatType>::toStates(vd_path);
+    result.trajectory.erase(result.trajectory.begin());
+
+    result.cost = graph_[target].distance;
+
+    return result;
+  }
+
+  // If the visitor never threw the vertex descriptor, there was an issue with the search
+  throw std::runtime_error("Search failed to encounter vertex associated with the last waypoint in the trajectory");
+}
+
+}  // namespace descartes_light
+
+#endif  // DESCARTES_LIGHT_SOLVERS_BGL_IMPL_BGL_DIJKSTRA_SOLVER_HPP
diff --git a/descartes_light/solvers/include/descartes_light/solvers/bgl/impl/bgl_solver.hpp b/descartes_light/solvers/include/descartes_light/solvers/bgl/impl/bgl_solver.hpp
index 098520c6..11a442c2 100644
--- a/descartes_light/solvers/include/descartes_light/solvers/bgl/impl/bgl_solver.hpp
+++ b/descartes_light/solvers/include/descartes_light/solvers/bgl/impl/bgl_solver.hpp
@@ -19,7 +19,6 @@
 
 #include <descartes_light/descartes_macros.h>
 DESCARTES_IGNORE_WARNINGS_PUSH
-#include <boost/graph/dijkstra_shortest_paths.hpp>
 #include <console_bridge/console.h>
 #include <fstream>
 #include <omp.h>
@@ -132,10 +131,10 @@ void BGLSolverBase<FloatType>::writeGraphWithPath(const std::string& filename) c
 }
 
 template <typename FloatType>
-BuildStatus
-BGLSolverBaseV<FloatType>::buildImpl(const std::vector<typename WaypointSampler<FloatType>::ConstPtr>& trajectory,
-                                     const std::vector<typename EdgeEvaluator<FloatType>::ConstPtr>&,
-                                     const std::vector<typename StateEvaluator<FloatType>::ConstPtr>& state_evaluators)
+BuildStatus BGLSolverBaseSVDE<FloatType>::buildImpl(
+    const std::vector<typename WaypointSampler<FloatType>::ConstPtr>& trajectory,
+    const std::vector<typename EdgeEvaluator<FloatType>::ConstPtr>&,
+    const std::vector<typename StateEvaluator<FloatType>::ConstPtr>& state_evaluators)
 {
   // Convenience aliases
   auto& ladder_rungs_ = BGLSolverBase<FloatType>::ladder_rungs_;
@@ -220,17 +219,17 @@ BGLSolverBaseV<FloatType>::buildImpl(const std::vector<typename WaypointSampler<
 }
 
 template <typename FloatType>
-BuildStatus
-BGLSolverBaseVE<FloatType>::buildImpl(const std::vector<typename WaypointSampler<FloatType>::ConstPtr>& trajectory,
-                                      const std::vector<typename EdgeEvaluator<FloatType>::ConstPtr>& edge_evaluators,
-                                      const std::vector<typename StateEvaluator<FloatType>::ConstPtr>& state_evaluators)
+BuildStatus BGLSolverBaseSVSE<FloatType>::buildImpl(
+    const std::vector<typename WaypointSampler<FloatType>::ConstPtr>& trajectory,
+    const std::vector<typename EdgeEvaluator<FloatType>::ConstPtr>& edge_evaluators,
+    const std::vector<typename StateEvaluator<FloatType>::ConstPtr>& state_evaluators)
 {
   // Convenience aliases
   auto& ladder_rungs_ = BGLSolverBase<FloatType>::ladder_rungs_;
   auto& graph_ = BGLSolverBase<FloatType>::graph_;
 
   // Use the base class to build the vertices
-  BuildStatus status = BGLSolverBaseV<FloatType>::buildImpl(trajectory, edge_evaluators, state_evaluators);
+  BuildStatus status = BGLSolverBaseSVDE<FloatType>::buildImpl(trajectory, edge_evaluators, state_evaluators);
 
   // Build Edges
   long cnt = 0;
@@ -299,56 +298,4 @@ BGLSolverBaseVE<FloatType>::buildImpl(const std::vector<typename WaypointSampler
   return status;
 }
 
-template <typename FloatType>
-SearchResult<FloatType> BGLDijkstraSolverVE<FloatType>::search()
-{
-  // Convenience aliases
-  auto& graph_ = BGLSolverBase<FloatType>::graph_;
-  const auto& source_ = BGLSolverBase<FloatType>::source_;
-  auto& predecessor_map_ = BGLSolverBase<FloatType>::predecessor_map_;
-  const auto& ladder_rungs_ = BGLSolverBase<FloatType>::ladder_rungs_;
-
-  // Internal properties
-  auto index_prop_map = boost::get(boost::vertex_index, graph_);
-  auto weight_prop_map = boost::get(boost::edge_weight, graph_);
-  auto color_prop_map = boost::get(&Vertex<FloatType>::color, graph_);
-  auto distance_prop_map = boost::get(&Vertex<FloatType>::distance, graph_);
-
-  predecessor_map_.clear();
-  boost::associative_property_map<std::map<VertexDesc<FloatType>, VertexDesc<FloatType>>> predecessor_prop_map(
-      predecessor_map_);
-
-  // Perform the search
-  boost::dijkstra_shortest_paths(graph_,
-                                 source_,
-                                 predecessor_prop_map,
-                                 distance_prop_map,
-                                 weight_prop_map,
-                                 index_prop_map,
-                                 std::less<>(),
-                                 std::plus<>(),
-                                 std::numeric_limits<FloatType>::max(),
-                                 static_cast<FloatType>(0.0),
-                                 boost::default_dijkstra_visitor(),
-                                 color_prop_map);
-
-  // Find lowest cost node in last rung
-  auto target = std::min_element(ladder_rungs_.back().begin(),
-                                 ladder_rungs_.back().end(),
-                                 [&](const VertexDesc<FloatType>& a, const VertexDesc<FloatType>& b) {
-                                   return graph_[a].distance < graph_[b].distance;
-                                 });
-
-  SearchResult<FloatType> result;
-
-  // Reconstruct the path from the predecesor map; remove the artificial start state
-  const auto vd_path = BGLSolverBase<FloatType>::reconstructPath(source_, *target);
-  result.trajectory = BGLSolverBase<FloatType>::toStates(vd_path);
-  result.trajectory.erase(result.trajectory.begin());
-
-  result.cost = graph_[*target].distance;
-
-  return result;
-}
-
 }  // namespace descartes_light
diff --git a/descartes_light/solvers/src/bgl/bgl_solver.cpp b/descartes_light/solvers/src/bgl/bgl_solver.cpp
index bdee26b4..4aee5211 100644
--- a/descartes_light/solvers/src/bgl/bgl_solver.cpp
+++ b/descartes_light/solvers/src/bgl/bgl_solver.cpp
@@ -16,6 +16,7 @@
  * limitations under the License.
  */
 #include <descartes_light/solvers/bgl/impl/bgl_solver.hpp>
+#include <descartes_light/solvers/bgl/impl/bgl_dijkstra_solver.hpp>
 #include <descartes_light/solvers/bgl/impl/utils.hpp>
 
 namespace descartes_light
@@ -25,15 +26,18 @@ namespace descartes_light
 template class BGLSolverBase<double>;
 template class BGLSolverBase<float>;
 
-template class BGLSolverBaseV<double>;
-template class BGLSolverBaseV<float>;
+template class BGLSolverBaseSVDE<double>;
+template class BGLSolverBaseSVDE<float>;
 
-template class BGLSolverBaseVE<double>;
-template class BGLSolverBaseVE<float>;
+template class BGLSolverBaseSVSE<double>;
+template class BGLSolverBaseSVSE<float>;
 
 // Full Implementations
-template class BGLDijkstraSolverVE<double>;
-template class BGLDijkstraSolverVE<float>;
+template class BGLDijkstraSVSESolver<double>;
+template class BGLDijkstraSVSESolver<float>;
+
+template class BGLEfficientDijkstraSVSESolver<double>;
+template class BGLEfficientDijkstraSVSESolver<float>;
 
 // Free functions
 template SubGraph<double> createDecoratedSubGraph(const BGLGraph<double>& g);
diff --git a/descartes_light/test/benchmarks/benchmarks.cpp b/descartes_light/test/benchmarks/benchmarks.cpp
index 3dd780f0..ae7241a0 100644
--- a/descartes_light/test/benchmarks/benchmarks.cpp
+++ b/descartes_light/test/benchmarks/benchmarks.cpp
@@ -3,7 +3,7 @@
 #include <descartes_light/test/solver_factory.h>
 // Solvers
 #include <descartes_light/solvers/ladder_graph/ladder_graph_solver.h>
-#include <descartes_light/solvers/bgl/bgl_solver.h>
+#include <descartes_light/solvers/bgl/bgl_dijkstra_solver.h>
 
 DESCARTES_IGNORE_WARNINGS_PUSH
 #include <boost/core/demangle.hpp>
@@ -80,9 +80,13 @@ int main(int, char**)
   benchmark(SolverFactory<LadderGraphSolverD>());
   benchmark(SolverFactory<LadderGraphSolverF>());
 
-  // BGL ladder graph
-  benchmark(SolverFactory<BGLDijkstraSolverVED>());
-  benchmark(SolverFactory<BGLDijkstraSolverVEF>());
+  // BGL full Dijkstra solver
+  benchmark(SolverFactory<BGLDijkstraSVSESolverD>());
+  benchmark(SolverFactory<BGLDijkstraSVSESolverF>());
+
+  // BGL efficient Dijkstra solver
+  benchmark(SolverFactory<BGLEfficientDijkstraSVSESolverD>());
+  benchmark(SolverFactory<BGLEfficientDijkstraSVSESolverF>());
 
   return 0;
 }
diff --git a/descartes_light/test/include/descartes_light/test/impl/solver_factory.hpp b/descartes_light/test/include/descartes_light/test/impl/solver_factory.hpp
index e807588c..61e6c5f1 100644
--- a/descartes_light/test/include/descartes_light/test/impl/solver_factory.hpp
+++ b/descartes_light/test/include/descartes_light/test/impl/solver_factory.hpp
@@ -2,24 +2,32 @@
 
 #include <descartes_light/test/solver_factory.h>
 #include <descartes_light/solvers/ladder_graph/ladder_graph_solver.h>
-#include <descartes_light/solvers/bgl/bgl_solver.h>
+#include <descartes_light/solvers/bgl/bgl_dijkstra_solver.h>
 
 namespace descartes_light
 {
-// Ladder graph solver Factory
-template <typename FloatT>
-struct SolverFactory<LadderGraphSolver<FloatT>>
+// Ladder graph solver factory
+template <typename FloatType>
+struct SolverFactory<LadderGraphSolver<FloatType>>
 {
-  using FloatType = FloatT;
-  typename Solver<FloatT>::Ptr create() const { return std::make_unique<LadderGraphSolver<FloatT>>(1); }
+  typename Solver<FloatType>::Ptr create() const { return std::make_shared<LadderGraphSolver<FloatType>>(1); }
 };
 
-// Boost Ladder graph solver Factory
-template <typename FloatT>
-struct SolverFactory<BGLDijkstraSolverVE<FloatT>>
+// Boost full Dijkstra graph solver factory
+template <typename FloatType>
+struct SolverFactory<BGLDijkstraSVSESolver<FloatType>>
 {
-  using FloatType = FloatT;
-  typename Solver<FloatT>::Ptr create() const { return std::make_unique<BGLDijkstraSolverVE<FloatT>>(1); }
+  typename Solver<FloatType>::Ptr create() const { return std::make_shared<BGLDijkstraSVSESolver<FloatType>>(1); }
+};
+
+// Boost efficient Dijkstra graph solver factory
+template <typename FloatType>
+struct SolverFactory<BGLEfficientDijkstraSVSESolver<FloatType>>
+{
+  typename Solver<FloatType>::Ptr create() const
+  {
+    return std::make_shared<BGLEfficientDijkstraSVSESolver<FloatType>>(1);
+  }
 };
 
 }  // namespace descartes_light
diff --git a/descartes_light/test/include/descartes_light/test/impl/utils.hpp b/descartes_light/test/include/descartes_light/test/impl/utils.hpp
index a469e81d..e8eb8e19 100644
--- a/descartes_light/test/include/descartes_light/test/impl/utils.hpp
+++ b/descartes_light/test/include/descartes_light/test/impl/utils.hpp
@@ -13,15 +13,13 @@ DESCARTES_IGNORE_WARNINGS_POP
 
 namespace descartes_light
 {
-static std::mt19937 RAND_GEN(0);
-
 template <typename FloatType>
-typename State<FloatType>::Ptr generateRandomState(Eigen::Index dof)
+typename State<FloatType>::Ptr generateRandomState(Eigen::Index dof, std::mt19937& rand_gen)
 {
   std::normal_distribution<FloatType> dist;
   Eigen::Matrix<FloatType, Eigen::Dynamic, 1> sol(dof);
   for (Eigen::Index i = 0; i < dof; ++i)
-    sol(i) = dist(RAND_GEN);
+    sol(i) = dist(rand_gen);
 
   return std::make_shared<State<FloatType>>(sol);
 }
@@ -29,9 +27,14 @@ typename State<FloatType>::Ptr generateRandomState(Eigen::Index dof)
 template <typename FloatType>
 RandomStateSampler<FloatType>::RandomStateSampler(const Eigen::Index dof,
                                                   const std::size_t n_samples,
+                                                  const FloatType state_cost,
                                                   const std::size_t zero_state_idx,
-                                                  const FloatType state_cost)
-  : dof_(dof), n_samples_(n_samples), zero_state_idx_(zero_state_idx), state_cost_(state_cost)
+                                                  std::shared_ptr<std::mt19937> rand_gen)
+  : dof_(dof)
+  , n_samples_(n_samples)
+  , state_cost_(state_cost)
+  , zero_state_idx_(zero_state_idx)
+  , rand_gen_(std::move(rand_gen))
 {
 }
 
@@ -42,7 +45,7 @@ typename std::vector<StateSample<FloatType>> RandomStateSampler<FloatType>::samp
   typename std::vector<StateSample<FloatType>> waypoints;
   waypoints.reserve(n_samples_);
   std::generate_n(std::back_inserter(waypoints), n_samples_, [this]() {
-    return StateSample<FloatType>{ generateRandomState<FloatType>(dof_), state_cost_ };
+    return StateSample<FloatType>{ generateRandomState<FloatType>(dof_, *rand_gen_), state_cost_ };
   });
 
   // Set one of the joint states to all zeros
@@ -81,20 +84,19 @@ std::vector<typename WaypointSampler<FloatType>::ConstPtr>
 createSamplers(std::size_t dof, std::size_t n_waypoints, std::size_t samples_per_waypoint, FloatType state_cost)
 {
   std::vector<typename WaypointSampler<FloatType>::ConstPtr> samplers;
-  std::vector<std::size_t> zero_state_indices;
   samplers.reserve(n_waypoints);
-  zero_state_indices.reserve(n_waypoints);
 
+  auto rand_gen = std::make_shared<std::mt19937>(0);
   std::uniform_int_distribution<std::size_t> dist(0, samples_per_waypoint - 1);
 
   // Create waypoint samplers
   for (std::size_t i = 0; i < n_waypoints; ++i)
   {
-    auto zero_state_idx = dist(RAND_GEN);
-    zero_state_indices.push_back(zero_state_idx);
-    samplers.push_back(
-        std::make_shared<RandomStateSampler<FloatType>>(dof, samples_per_waypoint, zero_state_idx, state_cost));
+    const std::size_t zero_state_idx = dist(*rand_gen);
+    samplers.push_back(std::make_shared<RandomStateSampler<FloatType>>(
+        dof, samples_per_waypoint, state_cost, zero_state_idx, rand_gen));
   }
+
   return samplers;
 }
 
diff --git a/descartes_light/test/include/descartes_light/test/utils.h b/descartes_light/test/include/descartes_light/test/utils.h
index 6c7aad6a..4d8efcda 100644
--- a/descartes_light/test/include/descartes_light/test/utils.h
+++ b/descartes_light/test/include/descartes_light/test/utils.h
@@ -4,6 +4,11 @@
 #include <descartes_light/core/state_evaluator.h>
 #include <descartes_light/core/waypoint_sampler.h>
 
+#include <descartes_light/descartes_macros.h>
+DESCARTES_IGNORE_WARNINGS_PUSH
+#include <random>
+DESCARTES_IGNORE_WARNINGS_POP
+
 namespace descartes_light
 {
 /**
@@ -15,16 +20,18 @@ class RandomStateSampler : public WaypointSampler<FloatType>
 public:
   RandomStateSampler(const Eigen::Index dof,
                      const std::size_t n_samples,
+                     const FloatType state_cost,
                      const std::size_t zero_state_idx,
-                     const FloatType state_cost);
+                     std::shared_ptr<std::mt19937> rand_gen);
 
   typename std::vector<StateSample<FloatType>> sample() const override;
 
 private:
   const Eigen::Index dof_;
   const std::size_t n_samples_;
-  const std::size_t zero_state_idx_;
   const FloatType state_cost_;
+  const std::size_t zero_state_idx_;
+  std::shared_ptr<std::mt19937> rand_gen_;
 };
 
 /**
diff --git a/descartes_light/test/src/solver_factory.cpp b/descartes_light/test/src/solver_factory.cpp
index 82b5e1a4..661bfea7 100644
--- a/descartes_light/test/src/solver_factory.cpp
+++ b/descartes_light/test/src/solver_factory.cpp
@@ -4,6 +4,8 @@ namespace descartes_light
 {
 template struct SolverFactory<LadderGraphSolverF>;
 template struct SolverFactory<LadderGraphSolverD>;
-template struct SolverFactory<BGLDijkstraSolverVEF>;
-template struct SolverFactory<BGLDijkstraSolverVED>;
+template struct SolverFactory<BGLDijkstraSVSESolverF>;
+template struct SolverFactory<BGLDijkstraSVSESolverD>;
+template struct SolverFactory<BGLEfficientDijkstraSVSESolverF>;
+template struct SolverFactory<BGLEfficientDijkstraSVSESolverD>;
 }  // namespace descartes_light
diff --git a/descartes_light/test/utest.cpp b/descartes_light/test/utest.cpp
index 5a27e84c..6d2f723d 100644
--- a/descartes_light/test/utest.cpp
+++ b/descartes_light/test/utest.cpp
@@ -1,7 +1,7 @@
 #include <descartes_light/core/solver.h>
 #include <descartes_light/edge_evaluators/euclidean_distance_edge_evaluator.h>
 #include <descartes_light/solvers/ladder_graph/ladder_graph_solver.h>
-#include <descartes_light/solvers/bgl/bgl_solver.h>
+#include <descartes_light/solvers/bgl/bgl_dijkstra_solver.h>
 #include <descartes_light/test/utils.h>
 #include <descartes_light/test/solver_factory.h>
 
@@ -41,8 +41,10 @@ class SolverFixture : public ::testing::Test
 
 using Implementations = ::testing::Types<SolverFactory<LadderGraphSolverF>,
                                          SolverFactory<LadderGraphSolverD>,
-                                         SolverFactory<BGLDijkstraSolverVEF>,
-                                         SolverFactory<BGLDijkstraSolverVED>>;
+                                         SolverFactory<BGLDijkstraSVSESolverF>,
+                                         SolverFactory<BGLDijkstraSVSESolverD>,
+                                         SolverFactory<BGLEfficientDijkstraSVSESolverF>,
+                                         SolverFactory<BGLEfficientDijkstraSVSESolverD>>;
 
 TYPED_TEST_CASE(SolverFixture, Implementations);
 
@@ -87,7 +89,7 @@ TYPED_TEST(SolverFixture, KnownPathTest)
   // Total path cost should be zero for edge costs and 2 * state_cost (one from sampling, one from state evaluation) for
   // state costs
   FloatType total_cost = static_cast<FloatType>(this->n_waypoints) * this->state_cost * 2;
-  ASSERT_TRUE(std::abs(result.cost - total_cost) < std::numeric_limits<FloatType>::epsilon());
+  ASSERT_DOUBLE_EQ(static_cast<double>(result.cost), static_cast<double>(total_cost));
 
   for (const auto& state : result.trajectory)
   {