diff --git a/xs/src/libslic3r/GCode.cpp b/xs/src/libslic3r/GCode.cpp
index 068652bb6..1f3509c8c 100644
--- a/xs/src/libslic3r/GCode.cpp
+++ b/xs/src/libslic3r/GCode.cpp
@@ -406,28 +406,28 @@ bool GCode::do_export(FILE *file, Print &print)
 
     // Get optimal tool ordering to minimize tool switches of a multi-exruder print.
     // For a print by objects, find the 1st printing object.
-    std::vector<ToolOrdering::LayerTools> tool_ordering;
-    unsigned int                          initial_extruder_id = (unsigned int)-1;
-    unsigned int                          final_extruder_id   = (unsigned int)-1;
-    size_t                                initial_print_object_id = 0;
+    ToolOrdering tool_ordering;
+    unsigned int initial_extruder_id = (unsigned int)-1;
+    unsigned int final_extruder_id   = (unsigned int)-1;
+    size_t       initial_print_object_id = 0;
     if (print.config.complete_objects.value) {
 		// Find the 1st printing object, find its tool ordering and the initial extruder ID.
 		for (; initial_print_object_id < print.objects.size(); ++initial_print_object_id) {
-			tool_ordering = ToolOrdering::tool_ordering(*print.objects[initial_print_object_id], initial_extruder_id);
-			if ((initial_extruder_id = ToolOrdering::first_extruder(tool_ordering)) != (unsigned int)-1)
+			tool_ordering = ToolOrdering(*print.objects[initial_print_object_id], initial_extruder_id);
+			if ((initial_extruder_id = tool_ordering.first_extruder()) != (unsigned int)-1)
 				break;
 		}
 	} else {
 		// Find tool ordering for all the objects at once, and the initial extruder ID.
-		tool_ordering = ToolOrdering::tool_ordering(print, initial_extruder_id);
-		initial_extruder_id = ToolOrdering::first_extruder(tool_ordering);
+		tool_ordering = ToolOrdering(print, initial_extruder_id);
+		initial_extruder_id = tool_ordering.first_extruder();
     }
     if (initial_extruder_id == (unsigned int)-1) {
         // Nothing to print!
         initial_extruder_id = 0;
         final_extruder_id   = 0;
     } else {
-        final_extruder_id = ToolOrdering::last_extruder(tool_ordering);
+        final_extruder_id = tool_ordering.last_extruder();
         assert(final_extruder_id != (unsigned int)-1);
     }
 
@@ -510,13 +510,13 @@ bool GCode::do_export(FILE *file, Print &print)
                 // Get optimal tool ordering to minimize tool switches of a multi-exruder print.
                 if (object_id != initial_print_object_id || &copy != object._shifted_copies.data()) {
                     // Don't initialize for the first object and first copy.
-                    tool_ordering = ToolOrdering::tool_ordering(object, final_extruder_id);
-                    unsigned int new_extruder_id = ToolOrdering::first_extruder(tool_ordering);
+                    tool_ordering = ToolOrdering(object, final_extruder_id);
+                    unsigned int new_extruder_id = tool_ordering.first_extruder();
                     if (new_extruder_id == (unsigned int)-1)
                         // Skip this object.
                         continue;
                     initial_extruder_id = new_extruder_id;
-                    final_extruder_id   = ToolOrdering::last_extruder(tool_ordering);
+                    final_extruder_id   = tool_ordering.last_extruder();
                     assert(final_extruder_id != (unsigned int)-1);
                 }
                 this->set_origin(unscale(copy.x), unscale(copy.y));
@@ -555,9 +555,7 @@ bool GCode::do_export(FILE *file, Print &print)
                             -- idx_object_layer;
                         }
                     }
-                    auto it_layer_tools = std::lower_bound(tool_ordering.begin(), tool_ordering.end(), ToolOrdering::LayerTools(layer_to_print.layer()->print_z));
-                    assert(it_layer_tools != tool_ordering.end() && it_layer_tools->print_z == layer_to_print.layer()->print_z);
-                    this->process_layer(file, print, layers_to_print, *it_layer_tools, &copy - object._shifted_copies.data());
+                    this->process_layer(file, print, layers_to_print, tool_ordering.tools_for_layer(layer_to_print.layer()->print_z), &copy - object._shifted_copies.data());
                 }
                 write(file, this->filter(m_cooling_buffer->flush(), true));
                 ++ finished_objects;
@@ -601,22 +599,19 @@ bool GCode::do_export(FILE *file, Print &print)
         for (auto &layer : layers) {
             // layer.second is of type std::vector<LayerToPrint>,
             // wher the objects are sorted by their sorted order given by object_indices.
-            auto it_layer_tools = std::lower_bound(tool_ordering.begin(), tool_ordering.end(), ToolOrdering::LayerTools(layer.first));
-            assert(it_layer_tools != tool_ordering.end() && layer.first);
-            if (it_layer_tools->has_wipe_tower && m_wipe_tower) {
+            const ToolOrdering::LayerTools &layer_tools = tool_ordering.tools_for_layer(layer.first);
+            if (layer_tools.has_wipe_tower && m_wipe_tower) {
                 bool first_layer = layer.first == layers.begin()->first;
-                auto it_layer_tools_next = it_layer_tools;
-                ++ it_layer_tools_next;
                 m_wipe_tower->set_layer(
                     layer.first, 
                     first_layer ? 
                         print.objects.front()->config.first_layer_height.get_abs_value(print.objects.front()->config.layer_height.value) :
                         print.objects.front()->config.layer_height.value,
-                    it_layer_tools->wipe_tower_partitions,
+                    layer_tools.wipe_tower_partitions,
                     first_layer,
-                    it_layer_tools->wipe_tower_partitions == 0 || (it_layer_tools_next == tool_ordering.end() || it_layer_tools_next->wipe_tower_partitions == 0));
+                    layer_tools.wipe_tower_partitions == 0 || (&layer_tools == &tool_ordering.back() || (&layer_tools + 1)->wipe_tower_partitions == 0));
             }
-            this->process_layer(file, print, layer.second, *it_layer_tools, size_t(-1));
+            this->process_layer(file, print, layer.second, layer_tools, size_t(-1));
         }
         write(file, this->filter(m_cooling_buffer->flush(), true));
     }
diff --git a/xs/src/libslic3r/GCode/ToolOrdering.cpp b/xs/src/libslic3r/GCode/ToolOrdering.cpp
index 1eba62963..cfe7b6df0 100644
--- a/xs/src/libslic3r/GCode/ToolOrdering.cpp
+++ b/xs/src/libslic3r/GCode/ToolOrdering.cpp
@@ -1,15 +1,113 @@
 #include "ToolOrdering.hpp"
 
+#include <assert.h>
+
 namespace Slic3r {
-namespace ToolOrdering {
+
+// For the use case when each object is printed separately
+// (print.config.complete_objects is true).
+ToolOrdering::ToolOrdering(const PrintObject &object, unsigned int first_extruder)
+{
+    // Initialize the print layers for just a single object.
+    {
+        std::vector<coordf_t> zs;
+        zs.reserve(zs.size() + object.layers.size() + object.support_layers.size());
+        for (auto layer : object.layers)
+            zs.emplace_back(layer->print_z);
+        for (auto layer : object.support_layers)
+            zs.emplace_back(layer->print_z);
+        this->initialize_layers(zs);
+    }
+
+    // Collect extruders reuqired to print the layers.
+    this->collect_extruders(object);
+
+    // Reorder the extruders to minimize tool switches.
+    this->reorder_extruders(first_extruder);
+
+    this->fill_wipe_tower_partitions();
+}
+
+// For the use case when all objects are printed at once.
+// (print.config.complete_objects is false).
+ToolOrdering::ToolOrdering(const Print &print, unsigned int first_extruder)
+{
+    // Initialize the print layers for all objects and all layers. 
+    {
+        std::vector<coordf_t> zs;
+        for (auto object : print.objects) {
+            zs.reserve(zs.size() + object->layers.size() + object->support_layers.size());
+            for (auto layer : object->layers)
+                zs.emplace_back(layer->print_z);
+            for (auto layer : object->support_layers)
+                zs.emplace_back(layer->print_z);
+        }
+        this->initialize_layers(zs);
+    }
+
+    // Collect extruders reuqired to print the layers.
+    for (auto object : print.objects)
+        this->collect_extruders(*object);
+
+    // Reorder the extruders to minimize tool switches.
+    this->reorder_extruders(first_extruder);
+
+    this->fill_wipe_tower_partitions();
+}
+
+unsigned int ToolOrdering::first_extruder() const
+{
+    for (const auto &lt : m_layer_tools)
+        if (! lt.extruders.empty())
+            return lt.extruders.front();
+    return (unsigned int)-1;
+}
+
+unsigned int ToolOrdering::last_extruder() const
+{
+    for (auto lt_it = m_layer_tools.rbegin(); lt_it != m_layer_tools.rend(); ++ lt_it)
+        if (! lt_it->extruders.empty())
+            return lt_it->extruders.back();
+    return (unsigned int)-1;
+}
+
+ToolOrdering::LayerTools&  ToolOrdering::tools_for_layer(coordf_t print_z)
+{
+    auto it_layer_tools = std::lower_bound(m_layer_tools.begin(), m_layer_tools.end(), ToolOrdering::LayerTools(print_z - EPSILON));
+    assert(it_layer_tools != m_layer_tools.end());
+    coordf_t dist_min = std::abs(it_layer_tools->print_z - print_z);
+	for (++ it_layer_tools; it_layer_tools != m_layer_tools.end(); ++it_layer_tools) {
+        coordf_t d = std::abs(it_layer_tools->print_z - print_z);
+        if (d >= dist_min)
+            break;
+        dist_min = d;
+    }
+    -- it_layer_tools;
+    assert(dist_min < EPSILON);
+    return *it_layer_tools;
+}
+
+void ToolOrdering::initialize_layers(std::vector<coordf_t> &zs)
+{
+    sort_remove_duplicates(zs);
+    // Merge numerically very close Z values.
+    for (size_t i = 0; i < zs.size();) {
+        // Find the last layer with roughly the same print_z.
+        size_t j = i + 1;
+        coordf_t zmax = zs[i] + EPSILON;
+        for (; j < zs.size() && zs[j] <= zmax; ++ j) ;
+        // Assign an average print_z to the set of layers with nearly equal print_z.
+        m_layer_tools.emplace_back(LayerTools(0.5 * (zs[i] + zs[j-1])));
+        i = j;
+    }
+}
 
 // Collect extruders reuqired to print layers.
-static void collect_extruders(const PrintObject &object, std::vector<LayerTools> &layers)
+void ToolOrdering::collect_extruders(const PrintObject &object)
 {
     // Collect the support extruders.
     for (auto support_layer : object.support_layers) {
-        auto it_layer = std::find(layers.begin(), layers.end(), LayerTools(support_layer->print_z));
-        assert(it_layer != layers.end());
+        LayerTools   &layer_tools = this->tools_for_layer(support_layer->print_z);
         ExtrusionRole role = support_layer->support_fills.role();
         bool         has_support        = role == erMixed || role == erSupportMaterial;
         bool         has_interface      = role == erMixed || role == erSupportMaterialInterface;
@@ -24,16 +122,15 @@ static void collect_extruders(const PrintObject &object, std::vector<LayerTools>
                 extruder_interface = extruder_support;
         }
         if (has_support)
-            it_layer->extruders.push_back(extruder_support);
+            layer_tools.extruders.push_back(extruder_support);
         if (has_interface)
-            it_layer->extruders.push_back(extruder_interface);
+            layer_tools.extruders.push_back(extruder_interface);
         if (has_support || has_interface)
-            it_layer->has_support = true;
+            layer_tools.has_support = true;
     }
     // Collect the object extruders.
     for (auto layer : object.layers) {
-        auto it_layer = std::find(layers.begin(), layers.end(), LayerTools(layer->print_z));
-        assert(it_layer != layers.end());
+        LayerTools &layer_tools = this->tools_for_layer(layer->print_z);
         // What extruders are required to print this object layer?
         for (size_t region_id = 0; region_id < object.print()->regions.size(); ++ region_id) {
             const LayerRegion *layerm = layer->regions[region_id];
@@ -41,8 +138,8 @@ static void collect_extruders(const PrintObject &object, std::vector<LayerTools>
                 continue;
             const PrintRegion &region = *object.print()->regions[region_id];
             if (! layerm->perimeters.entities.empty()) {
-                it_layer->extruders.push_back(region.config.perimeter_extruder.value);
-                it_layer->has_object = true;
+                layer_tools.extruders.push_back(region.config.perimeter_extruder.value);
+                layer_tools.has_object = true;
             }
             bool has_infill       = false;
             bool has_solid_infill = false;
@@ -56,31 +153,31 @@ static void collect_extruders(const PrintObject &object, std::vector<LayerTools>
                     has_infill = true;
             }
             if (has_solid_infill)
-                it_layer->extruders.push_back(region.config.solid_infill_extruder);
+                layer_tools.extruders.push_back(region.config.solid_infill_extruder);
             if (has_infill)
-                it_layer->extruders.push_back(region.config.infill_extruder);
+                layer_tools.extruders.push_back(region.config.infill_extruder);
             if (has_solid_infill || has_infill)
-                it_layer->has_object = true;
+                layer_tools.has_object = true;
         }
     }
 
     // Sort and remove duplicates
-    for (LayerTools &lt : layers)
+    for (LayerTools &lt : m_layer_tools)
         sort_remove_duplicates(lt.extruders);
 }
 
 // Reorder extruders to minimize layer changes.
-static void reorder_extruders(std::vector<LayerTools> &layers, unsigned int last_extruder_id)
+void ToolOrdering::reorder_extruders(unsigned int last_extruder_id)
 {
-    if (layers.empty())
+    if (m_layer_tools.empty())
         return;
 
     if (last_extruder_id == (unsigned int)-1) {
         // The initial print extruder has not been decided yet.
         // Initialize the last_extruder_id with the first non-zero extruder id used for the print.
         last_extruder_id = 0;
-        for (size_t i = 0; i < layers.size() && last_extruder_id == 0; ++ i) {
-            const LayerTools &lt = layers[i];
+        for (size_t i = 0; i < m_layer_tools.size() && last_extruder_id == 0; ++ i) {
+            const LayerTools &lt = m_layer_tools[i];
             for (unsigned int extruder_id : lt.extruders)
                 if (extruder_id > 0) {
                     last_extruder_id = extruder_id;
@@ -94,7 +191,7 @@ static void reorder_extruders(std::vector<LayerTools> &layers, unsigned int last
         // 1 based index
         ++ last_extruder_id;
 
-    for (LayerTools &lt : layers) {
+    for (LayerTools &lt : m_layer_tools) {
         if (lt.extruders.empty())
             continue;
         if (lt.extruders.size() == 1 && lt.extruders.front() == 0)
@@ -116,21 +213,21 @@ static void reorder_extruders(std::vector<LayerTools> &layers, unsigned int last
     }
 
     // Reindex the extruders, so they are zero based, not 1 based.
-    for (LayerTools &lt : layers)
+    for (LayerTools &lt : m_layer_tools)
         for (unsigned int &extruder_id : lt.extruders) {
             assert(extruder_id > 0);
             -- extruder_id;
         }
 }
 
-static void fill_wipe_tower_partitions(std::vector<LayerTools> &layers)
+void ToolOrdering::fill_wipe_tower_partitions()
 {
-    if (layers.empty())
+    if (m_layer_tools.empty())
         return;
 
     // Count the minimum number of tool changes per layer.
     size_t last_extruder = size_t(-1);
-    for (LayerTools &lt : layers) {
+    for (LayerTools &lt : m_layer_tools) {
         lt.wipe_tower_partitions = lt.extruders.size();
         if (! lt.extruders.empty()) {
             if (last_extruder == size_t(-1) || last_extruder == lt.extruders.front())
@@ -141,88 +238,12 @@ static void fill_wipe_tower_partitions(std::vector<LayerTools> &layers)
     }
 
     // Propagate the wipe tower partitions down to support the upper partitions by the lower partitions.
-    for (int i = int(layers.size()) - 2; i >= 0; -- i)
-        layers[i].wipe_tower_partitions = std::max(layers[i + 1].wipe_tower_partitions, layers[i].wipe_tower_partitions);
+    for (int i = int(m_layer_tools.size()) - 2; i >= 0; -- i)
+        m_layer_tools[i].wipe_tower_partitions = std::max(m_layer_tools[i + 1].wipe_tower_partitions, m_layer_tools[i].wipe_tower_partitions);
 
     //FIXME this is a hack to get the ball rolling.
-    for (LayerTools &lt : layers)
+    for (LayerTools &lt : m_layer_tools)
         lt.has_wipe_tower = lt.has_object;
 }
 
-// For the use case when each object is printed separately
-// (print.config.complete_objects is true).
-std::vector<LayerTools> tool_ordering(const PrintObject &object, unsigned int first_extruder)
-{
-    // Initialize the print layers for just a single object.
-    std::vector<LayerTools> layers;
-    {
-        std::vector<coordf_t> zs;
-        zs.reserve(zs.size() + object.layers.size() + object.support_layers.size());
-        for (auto layer : object.layers)
-            zs.emplace_back(layer->print_z);
-        for (auto layer : object.support_layers)
-            zs.emplace_back(layer->print_z);
-        sort_remove_duplicates(zs);
-        for (coordf_t z : zs)
-            layers.emplace_back(LayerTools(z));
-    }
-
-    // Collect extruders reuqired to print the layers.
-    collect_extruders(object, layers);
-
-    // Reorder the extruders to minimize tool switches.
-    reorder_extruders(layers, first_extruder);
-
-    fill_wipe_tower_partitions(layers);
-    return layers;
-}
-
-// For the use case when all objects are printed at once.
-// (print.config.complete_objects is false).
-std::vector<LayerTools> tool_ordering(const Print &print, unsigned int first_extruder)
-{
-    // Initialize the print layers for all objects and all layers. 
-    std::vector<LayerTools> layers;
-    {
-        std::vector<coordf_t> zs;
-        for (auto object : print.objects) {
-            zs.reserve(zs.size() + object->layers.size() + object->support_layers.size());
-            for (auto layer : object->layers)
-                zs.emplace_back(layer->print_z);
-            for (auto layer : object->support_layers)
-                zs.emplace_back(layer->print_z);
-        }
-        sort_remove_duplicates(zs);
-        for (coordf_t z : zs)
-            layers.emplace_back(LayerTools(z));
-    }
-
-    // Collect extruders reuqired to print the layers.
-    for (auto object : print.objects)
-        collect_extruders(*object, layers);
-
-    // Reorder the extruders to minimize tool switches.
-    reorder_extruders(layers, first_extruder);
-
-    fill_wipe_tower_partitions(layers);
-    return layers;
-}
-
-unsigned int first_extruder(const std::vector<LayerTools> &layer_tools)
-{
-    for (const auto &lt : layer_tools)
-        if (! lt.extruders.empty())
-            return lt.extruders.front();
-    return (unsigned int)-1;
-}
-
-unsigned int last_extruder(const std::vector<LayerTools> &layer_tools)
-{
-    for (auto lt_it = layer_tools.rbegin(); lt_it != layer_tools.rend(); ++ lt_it)
-        if (! lt_it->extruders.empty())
-            return lt_it->extruders.back();
-    return (unsigned int)-1;
-}
-
-} // namespace ToolOrdering
 } // namespace Slic3r
diff --git a/xs/src/libslic3r/GCode/ToolOrdering.hpp b/xs/src/libslic3r/GCode/ToolOrdering.hpp
index e48a578ad..64e090b30 100644
--- a/xs/src/libslic3r/GCode/ToolOrdering.hpp
+++ b/xs/src/libslic3r/GCode/ToolOrdering.hpp
@@ -7,49 +7,70 @@
 #include "Print.hpp"
 
 namespace Slic3r {
-namespace ToolOrdering {
 
-struct LayerTools
+class ToolOrdering 
 {
-    LayerTools(const coordf_t z) : 
-    	print_z(z), 
-    	has_object(false),
-		has_support(false),
-		has_wipe_tower(false),
-    	wipe_tower_partitions(0) {}
+public:
+	struct LayerTools
+	{
+	    LayerTools(const coordf_t z) :
+	    	print_z(z), 
+	    	has_object(false),
+			has_support(false),
+			has_wipe_tower(false),
+	    	wipe_tower_partitions(0) {}
 
-    bool operator< (const LayerTools &rhs) const { return print_z <  rhs.print_z; }
-    bool operator==(const LayerTools &rhs) const { return print_z == rhs.print_z; }
+	    bool operator< (const LayerTools &rhs) const { return print_z <  rhs.print_z; }
+	    bool operator==(const LayerTools &rhs) const { return print_z == rhs.print_z; }
 
-	coordf_t 					print_z;
-	bool 						has_object;
-	bool						has_support;
-	// Zero based extruder IDs, ordered to minimize tool switches.
-	std::vector<unsigned int> 	extruders;
-	// Will there be anything extruded on this layer for the wipe tower?
-	// Due to the support layers possibly interleaving the object layers,
-	// wipe tower will be disabled for some support only layers.
-	bool 						has_wipe_tower;
-	// Number of wipe tower partitions to support the required number of tool switches
-	// and to support the wipe tower partitions above this one.
-    size_t                      wipe_tower_partitions;
+		coordf_t 					print_z;
+		bool 						has_object;
+		bool						has_support;
+		// Zero based extruder IDs, ordered to minimize tool switches.
+		std::vector<unsigned int> 	extruders;
+		// Will there be anything extruded on this layer for the wipe tower?
+		// Due to the support layers possibly interleaving the object layers,
+		// wipe tower will be disabled for some support only layers.
+		bool 						has_wipe_tower;
+		// Number of wipe tower partitions to support the required number of tool switches
+		// and to support the wipe tower partitions above this one.
+	    size_t                      wipe_tower_partitions;
+	};
+
+	ToolOrdering() {}
+
+	// For the use case when each object is printed separately
+	// (print.config.complete_objects is true).
+	ToolOrdering(const PrintObject &object, unsigned int first_extruder = (unsigned int)-1);
+
+	// For the use case when all objects are printed at once.
+	// (print.config.complete_objects is false).
+	ToolOrdering(const Print &print, unsigned int first_extruder = (unsigned int)-1);
+
+	// Get the first extruder printing the layer_tools, returns -1 if there is no layer printed.
+	unsigned int   		first_extruder() const;
+
+	// Get the first extruder printing the layer_tools, returns -1 if there is no layer printed.
+	unsigned int   		last_extruder() const;
+
+	// Find LayerTools with the closest print_z.
+	LayerTools&			tools_for_layer(coordf_t print_z);
+	const LayerTools&	tools_for_layer(coordf_t print_z) const 
+		{ return *const_cast<const LayerTools*>(&const_cast<const ToolOrdering*>(this)->tools_for_layer(print_z)); }
+
+	const LayerTools&   front()       const { return m_layer_tools.front(); }
+	const LayerTools&   back()        const { return m_layer_tools.back(); }
+	bool 				empty()       const { return m_layer_tools.empty(); }
+
+private:
+	void				initialize_layers(std::vector<coordf_t> &zs);
+	void 				collect_extruders(const PrintObject &object);
+	void				reorder_extruders(unsigned int last_extruder_id);
+	void 				fill_wipe_tower_partitions();
+
+	std::vector<LayerTools> m_layer_tools;
 };
 
-// For the use case when each object is printed separately
-// (print.config.complete_objects is true).
-extern std::vector<LayerTools> tool_ordering(const PrintObject &object, unsigned int first_extruder = (unsigned int)-1);
-
-// For the use case when all objects are printed at once.
-// (print.config.complete_objects is false).
-extern std::vector<LayerTools> tool_ordering(const Print &print, unsigned int first_extruder = (unsigned int)-1);
-
-// Get the first extruder printing the layer_tools, returns -1 if there is no layer printed.
-extern unsigned int			   first_extruder(const std::vector<LayerTools> &layer_tools);
-
-// Get the first extruder printing the layer_tools, returns -1 if there is no layer printed.
-extern unsigned int			   last_extruder(const std::vector<LayerTools> &layer_tools);
-
-} // namespace ToolOrdering
 } // namespace SLic3r
 
 #endif /* slic3r_ToolOrdering_hpp_ */