diff --git a/lib/Slic3r/Geometry.pm b/lib/Slic3r/Geometry.pm
index fec771561..1fa63f3b8 100644
--- a/lib/Slic3r/Geometry.pm
+++ b/lib/Slic3r/Geometry.pm
@@ -602,119 +602,4 @@ sub douglas_peucker2 {
     return [ map $points->[$_], sort keys %keep ];
 }
 
-sub arrange {
-    my ($total_parts, $partx, $party, $dist, $bb) = @_;
-    
-    my $linint = sub {
-        my ($value, $oldmin, $oldmax, $newmin, $newmax) = @_;
-        return ($value - $oldmin) * ($newmax - $newmin) / ($oldmax - $oldmin) + $newmin;
-    };
-    
-    # use actual part size (the largest) plus separation distance (half on each side) in spacing algorithm
-    $partx += $dist;
-    $party += $dist;
-    
-    my ($areax, $areay);
-    if (defined $bb) {
-        my $size = $bb->size;
-        ($areax, $areay) = @$size[X,Y];
-    } else {
-        # bogus area size, large enough not to trigger the error below
-        $areax = $partx * $total_parts;
-        $areay = $party * $total_parts;
-    }
-    
-    # this is how many cells we have available into which to put parts
-    my $cellw = int(($areax + $dist) / $partx);
-    my $cellh = int(($areay + $dist) / $party);
-    
-    die "$total_parts parts won't fit in your print area!\n" if $total_parts > ($cellw * $cellh);
-    
-    # width and height of space used by cells
-    my $w = $cellw * $partx;
-    my $h = $cellh * $party;
-    
-    # left and right border positions of space used by cells
-    my $l = ($areax - $w) / 2;
-    my $r = $l + $w;
-    
-    # top and bottom border positions
-    my $t = ($areay - $h) / 2;
-    my $b = $t + $h;
-    
-    # list of cells, sorted by distance from center
-    my @cellsorder;
-    
-    # work out distance for all cells, sort into list
-    for my $i (0..$cellw-1) {
-        for my $j (0..$cellh-1) {
-            my $cx = $linint->($i + 0.5, 0, $cellw, $l, $r);
-            my $cy = $linint->($j + 0.5, 0, $cellh, $t, $b);
-            
-            my $xd = abs(($areax / 2) - $cx);
-            my $yd = abs(($areay / 2) - $cy);
-            
-            my $c = {
-                location => [$cx, $cy],
-                index => [$i, $j],
-                distance => $xd * $xd + $yd * $yd - abs(($cellw / 2) - ($i + 0.5)),
-            };
-            
-            BINARYINSERTIONSORT: {
-                my $index = $c->{distance};
-                my $low = 0;
-                my $high = @cellsorder;
-                while ($low < $high) {
-                    my $mid = ($low + (($high - $low) / 2)) | 0;
-                    my $midval = $cellsorder[$mid]->[0];
-                    
-                    if ($midval < $index) {
-                        $low = $mid + 1;
-                    } elsif ($midval > $index) {
-                        $high = $mid;
-                    } else {
-                        splice @cellsorder, $mid, 0, [$index, $c];
-                        last BINARYINSERTIONSORT;
-                    }
-                }
-                splice @cellsorder, $low, 0, [$index, $c];
-            }
-        }
-    }
-    
-    # the extents of cells actually used by objects
-    my ($lx, $ty, $rx, $by) = (0, 0, 0, 0);
-
-    # now find cells actually used by objects, map out the extents so we can position correctly
-    for my $i (1..$total_parts) {
-        my $c = $cellsorder[$i - 1];
-        my $cx = $c->[1]->{index}->[0];
-        my $cy = $c->[1]->{index}->[1];
-        if ($i == 1) {
-            $lx = $rx = $cx;
-            $ty = $by = $cy;
-        } else {
-            $rx = $cx if $cx > $rx;
-            $lx = $cx if $cx < $lx;
-            $by = $cy if $cy > $by;
-            $ty = $cy if $cy < $ty;
-        }
-    }
-    # now we actually place objects into cells, positioned such that the left and bottom borders are at 0
-    my @positions = ();
-    for (1..$total_parts) {
-        my $c = shift @cellsorder;
-        my $cx = $c->[1]->{index}->[0] - $lx;
-        my $cy = $c->[1]->{index}->[1] - $ty;
-
-        push @positions, [$cx * $partx, $cy * $party];
-    }
-    
-    if (defined $bb) {
-        $_->[X] += $bb->x_min for @positions;
-        $_->[Y] += $bb->y_min for @positions;
-    }
-    return @positions;
-}
-
 1;
diff --git a/lib/Slic3r/Model.pm b/lib/Slic3r/Model.pm
index 099f6461c..63224727e 100644
--- a/lib/Slic3r/Model.pm
+++ b/lib/Slic3r/Model.pm
@@ -152,13 +152,15 @@ sub _arrange {
     my ($self, $sizes, $distance, $bb) = @_;
     
     # we supply unscaled data to arrange()
-    return Slic3r::Geometry::arrange(
+    return @{Slic3r::Geometry::arrange(
         scalar(@$sizes),                # number of parts
-        max(map $_->x, @$sizes),        # cell width
-        max(map $_->y, @$sizes),        # cell height ,
+        Slic3r::Pointf->new(
+            max(map $_->x, @$sizes),        # cell width
+            max(map $_->y, @$sizes),        # cell height  ,
+        ),
         $distance,                      # distance between cells
         $bb,                            # bounding box of the area to fill (can be undef)
-    );
+    )};
 }
 
 sub print_info {
diff --git a/xs/src/libslic3r/Geometry.cpp b/xs/src/libslic3r/Geometry.cpp
index 3dc11c0cb..5ddbb4f21 100644
--- a/xs/src/libslic3r/Geometry.cpp
+++ b/xs/src/libslic3r/Geometry.cpp
@@ -161,6 +161,133 @@ simplify_polygons(const Polygons &polygons, double tolerance, Polygons* retval)
     Slic3r::simplify_polygons(pp, retval);
 }
 
+double
+linint(double value, double oldmin, double oldmax, double newmin, double newmax)
+{
+    return (value - oldmin) * (newmax - newmin) / (oldmax - oldmin) + newmin;
+}
+
+Pointfs
+arrange(size_t total_parts, Pointf part, coordf_t dist, const BoundingBoxf &bb)
+{
+    // use actual part size (the largest) plus separation distance (half on each side) in spacing algorithm
+    part.x += dist;
+    part.y += dist;
+    
+    Pointf area;
+    if (bb.defined) {
+        area = bb.size();
+    } else {
+        // bogus area size, large enough not to trigger the error below
+        area.x = part.x * total_parts;
+        area.y = part.y * total_parts;
+    }
+    
+    // this is how many cells we have available into which to put parts
+    size_t cellw = floor((area.x + dist) / part.x);
+    size_t cellh = floor((area.x + dist) / part.x);
+    
+    if (total_parts > (cellw * cellh))
+        CONFESS("%zu parts won't fit in your print area!\n", total_parts);
+    
+    // total space used by cells
+    Pointf cells(cellw * part.x, cellh * part.y);
+    
+    // bounding box of total space used by cells
+    BoundingBoxf cells_bb;
+    cells_bb.merge(Pointf(0,0)); // min
+    cells_bb.merge(cells);  // max
+    
+    // center bounding box to area
+    cells_bb.translate(
+        -(area.x - cells.x) / 2,
+        -(area.y - cells.y) / 2
+    );
+    
+    // list of cells, sorted by distance from center
+    std::vector<ArrangeItemIndex> cellsorder;
+    
+    // work out distance for all cells, sort into list
+    for (size_t i = 0; i <= cellw-1; ++i) {
+        for (size_t j = 0; j <= cellh-1; ++j) {
+            coordf_t cx = linint(i + 0.5, 0, cellw, cells_bb.min.x, cells_bb.max.x);
+            coordf_t cy = linint(j + 0.5, 0, cellh, cells_bb.max.y, cells_bb.min.y);
+            
+            coordf_t xd = fabs((area.x / 2) - cx);
+            coordf_t yd = fabs((area.y / 2) - cy);
+            
+            ArrangeItem c;
+            c.pos.x = cx;
+            c.pos.y = cy;
+            c.index_x = i;
+            c.index_y = j;
+            c.dist = xd * xd + yd * yd - fabs((cellw / 2) - (i + 0.5));
+            
+            // binary insertion sort
+            {
+                coordf_t index = c.dist;
+                size_t low = 0;
+                size_t high = cellsorder.size();
+                while (low < high) {
+                    size_t mid = (low + ((high - low) / 2)) | 0;
+                    coordf_t midval = cellsorder[mid].index;
+                    
+                    if (midval < index) {
+                        low = mid + 1;
+                    } else if (midval > index) {
+                        high = mid;
+                    } else {
+                        cellsorder.insert(cellsorder.begin() + mid, ArrangeItemIndex(index, c));
+                        goto ENDSORT;
+                    }
+                }
+                cellsorder.insert(cellsorder.begin() + low, ArrangeItemIndex(index, c));
+            }
+            ENDSORT: true;
+        }
+    }
+    
+    // the extents of cells actually used by objects
+    coordf_t lx = 0;
+    coordf_t ty = 0;
+    coordf_t rx = 0;
+    coordf_t by = 0;
+
+    // now find cells actually used by objects, map out the extents so we can position correctly
+    for (size_t i = 1; i <= total_parts; ++i) {
+        ArrangeItemIndex c = cellsorder[i - 1];
+        coordf_t cx = c.item.index_x;
+        coordf_t cy = c.item.index_y;
+        if (i == 1) {
+            lx = rx = cx;
+            ty = by = cy;
+        } else {
+            if (cx > rx) rx = cx;
+            if (cx < lx) lx = cx;
+            if (cy > by) by = cy;
+            if (cy < ty) ty = cy;
+        }
+    }
+    // now we actually place objects into cells, positioned such that the left and bottom borders are at 0
+    Pointfs positions;
+    for (size_t i = 1; i <= total_parts; ++i) {
+        ArrangeItemIndex c = cellsorder.front();
+        cellsorder.erase(cellsorder.begin());
+        coordf_t cx = c.item.index_x - lx;
+        coordf_t cy = c.item.index_y - ty;
+        
+        positions.push_back(Pointf(cx * part.x, cy * part.y));
+    }
+    
+    if (bb.defined) {
+        for (Pointfs::iterator p = positions.begin(); p != positions.end(); ++p) {
+            p->x += bb.min.x;
+            p->y += bb.min.y;
+        }
+    }
+    return positions;
+}
+
 Line
 MedialAxis::edge_to_line(const VD::edge_type &edge) const
 {
diff --git a/xs/src/libslic3r/Geometry.hpp b/xs/src/libslic3r/Geometry.hpp
index c2cacfa33..ef6d70559 100644
--- a/xs/src/libslic3r/Geometry.hpp
+++ b/xs/src/libslic3r/Geometry.hpp
@@ -23,6 +23,21 @@ double rad2deg_dir(double angle);
 double deg2rad(double angle);
 void simplify_polygons(const Polygons &polygons, double tolerance, Polygons* retval);
 
+class ArrangeItem {
+    public:
+    Pointf pos;
+    size_t index_x, index_y;
+    coordf_t dist;
+};
+class ArrangeItemIndex {
+    public:
+    coordf_t index;
+    ArrangeItem item;
+    ArrangeItemIndex(coordf_t _index, ArrangeItem _item) : index(_index), item(_item) {};
+};
+double linint(double value, double oldmin, double oldmax, double newmin, double newmax);
+Pointfs arrange(size_t total_parts, Pointf part, coordf_t dist, const BoundingBoxf &bb = BoundingBoxf());
+
 class MedialAxis {
     public:
     Points points;
diff --git a/xs/t/14_geometry.t b/xs/t/14_geometry.t
index 8dfdecb78..70b623029 100644
--- a/xs/t/14_geometry.t
+++ b/xs/t/14_geometry.t
@@ -4,7 +4,7 @@ use strict;
 use warnings;
 
 use Slic3r::XS;
-use Test::More tests => 8;
+use Test::More tests => 9;
 
 use constant PI => 4 * atan2(1, 1);
 
@@ -31,4 +31,11 @@ use constant PI => 4 * atan2(1, 1);
     ok !Slic3r::Geometry::directions_parallel_within(PI/2, PI, 0), 'directions_parallel_within';
     ok !Slic3r::Geometry::directions_parallel_within(PI/2, PI, PI/180), 'directions_parallel_within';
 }
+
+{
+    my $positions = Slic3r::Geometry::arrange(4, Slic3r::Pointf->new(20, 20),
+        5, Slic3r::Geometry::BoundingBoxf->new);
+    is scalar(@$positions), 4, 'arrange() returns expected number of positions';
+}
+
 __END__
diff --git a/xs/xsp/Geometry.xsp b/xs/xsp/Geometry.xsp
index 98d5bcdd5..c5442a0b6 100644
--- a/xs/xsp/Geometry.xsp
+++ b/xs/xsp/Geometry.xsp
@@ -8,6 +8,9 @@
 
 %package{Slic3r::Geometry};
 
+Pointfs arrange(size_t total_parts, Pointf* part, coordf_t dist, BoundingBoxf* bb)
+    %code{% RETVAL = Slic3r::Geometry::arrange(total_parts, *part, dist, *bb); %};
+
 %{
 
 bool
@@ -87,6 +90,7 @@ simplify_polygons(polygons, tolerance)
     OUTPUT:
         RETVAL
 
+
 IV
 _constant()
   ALIAS: