diff --git a/lib/Slic3r/ExtrusionPath/Collection.pm b/lib/Slic3r/ExtrusionPath/Collection.pm
index 3354efaee..86d7be1c8 100644
--- a/lib/Slic3r/ExtrusionPath/Collection.pm
+++ b/lib/Slic3r/ExtrusionPath/Collection.pm
@@ -23,7 +23,7 @@ sub chained_path {
     while (@my_paths) {
         # find nearest point
         my $start_index = defined $start_near
-            ? Slic3r::Geometry::nearest_point_index($start_near, $endpoints)
+            ? $start_near->nearest_point_index($endpoints)
             : 0;
 
         my $path_index = int($start_index/2);
diff --git a/lib/Slic3r/Fill/Concentric.pm b/lib/Slic3r/Fill/Concentric.pm
index 311bfa1f5..d176fb051 100644
--- a/lib/Slic3r/Fill/Concentric.pm
+++ b/lib/Slic3r/Fill/Concentric.pm
@@ -3,7 +3,7 @@ use Moo;
 
 extends 'Slic3r::Fill::Base';
 
-use Slic3r::Geometry qw(scale unscale X nearest_point_index);
+use Slic3r::Geometry qw(scale unscale X);
 use Slic3r::Geometry::Clipper qw(offset offset2 union_pt traverse_pt PFT_EVENODD);
 
 sub fill_surface {
@@ -43,7 +43,7 @@ sub fill_surface {
     my @paths = ();
     my $last_pos = [0,0];
     foreach my $loop (@loops) {
-        push @paths, $loop->split_at_index(nearest_point_index($last_pos, $loop));
+        push @paths, $loop->split_at_index($last_pos->nearest_point_index($loop));
         $last_pos = $paths[-1][-1];
     }
     
diff --git a/lib/Slic3r/GCode.pm b/lib/Slic3r/GCode.pm
index 1f61ed06d..ac93938cb 100644
--- a/lib/Slic3r/GCode.pm
+++ b/lib/Slic3r/GCode.pm
@@ -211,7 +211,7 @@ sub extrude_loop {
     }
     
     # split the loop at the starting point and make a path
-    my $start_at = Slic3r::Geometry::nearest_point($last_pos, \@candidates);
+    my $start_at = $last_pos->nearest_point(\@candidates);
     my $extrusion_path = $loop->split_at($start_at);
     
     # clip the path to avoid the extruder to get exactly on the first point of the loop;
diff --git a/lib/Slic3r/GCode/MotionPlanner.pm b/lib/Slic3r/GCode/MotionPlanner.pm
index ec3a63b1c..dae500ea8 100644
--- a/lib/Slic3r/GCode/MotionPlanner.pm
+++ b/lib/Slic3r/GCode/MotionPlanner.pm
@@ -13,7 +13,7 @@ has '_crossing_edges' => (is => 'rw', default => sub { {} });  # edge_idx => boo
 has '_tolerance'    => (is => 'lazy');
 
 use List::Util qw(first);
-use Slic3r::Geometry qw(A B scale epsilon nearest_point);
+use Slic3r::Geometry qw(A B scale epsilon);
 use Slic3r::Geometry::Clipper qw(diff_ex offset JT_MITER);
 
 # clearance (in mm) from the perimeters
@@ -196,13 +196,13 @@ sub find_node {
     # if we're inside a hole, move to a point on hole;
     {
         my $polygon = first { $_->encloses_point($point) } (map @{$_->holes}, map @$_, @{$self->_inner});
-        return nearest_point($point, $polygon) if $polygon;
+        return $point->nearest_point([ @$polygon ]) if $polygon;
     }
     
     # if we're inside an expolygon move to a point on contour or holes
     {
         my $expolygon = first { $_->encloses_point_quick($point) } (map @$_, @{$self->_inner});
-        return nearest_point($point, [ map @$_, @$expolygon ]) if $expolygon;
+        return $point->nearest_point([ map @$_, @$expolygon ]) if $expolygon;
     }
     
     {
@@ -221,7 +221,7 @@ sub find_node {
             : [ map @$_, map @$_, @{$self->_outer} ];
         $candidates = [ map @$_, @{$self->_outer->[$outer_polygon_idx]} ]
             if @$candidates == 0;
-        return nearest_point($point, $candidates);
+        return $point->nearest_point($candidates);
     }
 }
 
diff --git a/lib/Slic3r/Geometry.pm b/lib/Slic3r/Geometry.pm
index 7f231efc5..f3d7368f2 100644
--- a/lib/Slic3r/Geometry.pm
+++ b/lib/Slic3r/Geometry.pm
@@ -9,10 +9,10 @@ our @EXPORT_OK = qw(
     line_point_belongs_to_segment points_coincide distance_between_points 
     chained_path_items chained_path_points normalize tan move_points_3D
     line_length midpoint point_in_polygon point_in_segment segment_in_segment
-    point_is_on_left_of_segment polyline_lines polygon_lines nearest_point
+    point_is_on_left_of_segment polyline_lines polygon_lines
     point_along_segment polygon_segment_having_point polygon_has_subsegment
     polygon_has_vertex polyline_length can_connect_points deg2rad rad2deg
-    rotate_points move_points clip_segment_polygon nearest_point_index
+    rotate_points move_points clip_segment_polygon
     sum_vectors multiply_vector subtract_vectors dot perp polygon_points_visibility
     line_intersection bounding_box bounding_box_intersect same_point same_line
     longest_segment angle3points three_points_aligned line_direction
@@ -239,41 +239,6 @@ sub polygon_lines {
     return polyline_lines([ @$polygon, $polygon->[0] ]);
 }
 
-sub nearest_point {
-    my ($point, $points) = @_;
-    my $index = nearest_point_index(@_);
-    return undef if !defined $index;
-    return $points->[$index];
-}
-
-sub nearest_point_index {
-    my ($point, $points) = @_;
-    
-    my ($nearest_point_index, $distance) = ();
-    
-    my ($point_x, $point_y) = @$point;
-    my @points_pp = map $_->pp, @$points;
-    
-    for my $i (0..$#$points) {
-        my $d = ($point_x - $points_pp[$i][X])**2;
-        # If the X distance of the candidate is > than the total distance of the
-        # best previous candidate, we know we don't want it
-        next if (defined $distance && $d > $distance);
-   
-        # If the total distance of the candidate is > than the total distance of the
-        # best previous candidate, we know we don't want it
-        $d += ($point_y - $points_pp[$i][Y])**2;
-        next if (defined $distance && $d > $distance);
-
-        $nearest_point_index = $i;
-        $distance = $d;
-        
-        last if $distance < epsilon;
-    }
-
-    return $nearest_point_index;
-}
-
 # given a segment $p1-$p2, get the point at $distance from $p1 along segment
 sub point_along_segment {
     my ($p1, $p2, $distance) = @_;
@@ -833,7 +798,7 @@ sub chained_path {
         push @result, $indices{$start_near} if $start_near;
     }
     while (@points) {
-        $start_near = nearest_point($start_near, [@points]);
+        $start_near = $start_near->nearest_point(\@points);
         @points = grep $_ ne $start_near, @points;
         push @result, $indices{$start_near};
     }
diff --git a/lib/Slic3r/Polyline.pm b/lib/Slic3r/Polyline.pm
index b26761b31..6ed0c2ce1 100644
--- a/lib/Slic3r/Polyline.pm
+++ b/lib/Slic3r/Polyline.pm
@@ -54,20 +54,6 @@ sub grow {
     )};
 }
 
-sub nearest_point_to {
-    my $self = shift;
-    my ($point) = @_;
-    
-    $point = Slic3r::Geometry::nearest_point($point, $self);
-    return Slic3r::Point->new($point);
-}
-
-sub nearest_point_index_to {
-    my $self = shift;
-    my ($point) = @_;
-    return Slic3r::Geometry::nearest_point_index($point, $self);
-}
-
 sub clip_with_polygon {
     my $self = shift;
     my ($polygon) = @_;
@@ -197,7 +183,7 @@ sub chained_path {
     while (@my_paths) {
         # find nearest point
         my $start_index = defined $start_near
-            ? Slic3r::Geometry::nearest_point_index($start_near, $endpoints)
+            ? $start_near->nearest_point_index($endpoints)
             : 0;
 
         my $path_index = int($start_index/2);
diff --git a/lib/Slic3r/Print.pm b/lib/Slic3r/Print.pm
index 455544275..148166138 100644
--- a/lib/Slic3r/Print.pm
+++ b/lib/Slic3r/Print.pm
@@ -6,8 +6,7 @@ use File::Spec;
 use List::Util qw(min max first);
 use Math::ConvexHull::MonotoneChain qw(convex_hull);
 use Slic3r::ExtrusionPath ':roles';
-use Slic3r::Geometry qw(X Y Z X1 Y1 X2 Y2 MIN MAX PI scale unscale move_points
-    nearest_point chained_path);
+use Slic3r::Geometry qw(X Y Z X1 Y1 X2 Y2 MIN MAX PI scale unscale move_points chained_path);
 use Slic3r::Geometry::Clipper qw(diff_ex union_ex union_pt intersection_ex offset
     offset2 traverse_pt JT_ROUND JT_SQUARE PFT_EVENODD);
 use Time::HiRes qw(gettimeofday tv_interval);
@@ -552,9 +551,10 @@ EOF
             foreach my $expolygon (@unsupported_slices) {
                 # look for the nearest point to this island among all
                 # supported points
-                my $support_point = nearest_point($expolygon->contour->[0], \@supported_points)
+                my $contour = $expolygon->contour;
+                my $support_point = $contour->first_point->nearest_point(\@supported_points)
                     or next;
-                my $anchor_point = nearest_point($support_point, $expolygon->contour);
+                my $anchor_point = $support_point->nearest_point([ @$contour ]);
                 printf $fh qq{    <line x1="%s" y1="%s" x2="%s" y2="%s" style="stroke-width: 2; stroke: white" />\n},
                     map @$_, $support_point, $anchor_point;
             }
diff --git a/xs/src/MultiPoint.cpp b/xs/src/MultiPoint.cpp
index fae286994..76cd9454c 100644
--- a/xs/src/MultiPoint.cpp
+++ b/xs/src/MultiPoint.cpp
@@ -32,6 +32,12 @@ MultiPoint::reverse()
     std::reverse(this->points.begin(), this->points.end());
 }
 
+const Point*
+MultiPoint::first_point() const
+{
+    return &(this->points.front());
+}
+
 void
 MultiPoint::from_SV(SV* poly_sv)
 {
diff --git a/xs/src/MultiPoint.hpp b/xs/src/MultiPoint.hpp
index 650733a80..3bbdcc690 100644
--- a/xs/src/MultiPoint.hpp
+++ b/xs/src/MultiPoint.hpp
@@ -19,6 +19,7 @@ class MultiPoint
     void translate(double x, double y);
     void rotate(double angle, Point* center);
     void reverse();
+    const Point* first_point() const;
 };
 
 }
diff --git a/xs/src/Point.cpp b/xs/src/Point.cpp
index 15d37177f..e534df089 100644
--- a/xs/src/Point.cpp
+++ b/xs/src/Point.cpp
@@ -1,4 +1,5 @@
 #include "Point.hpp"
+#include <math.h>
 
 namespace Slic3r {
 
@@ -31,6 +32,38 @@ Point::coincides_with(const Point* point) const
     return this->x == point->x && this->y == point->y;
 }
 
+int
+Point::nearest_point_index(const Points points) const
+{
+    int idx = -1;
+    long distance = -1;
+    
+    for (Points::const_iterator it = points.begin(); it != points.end(); ++it) {
+        /* If the X distance of the candidate is > than the total distance of the
+           best previous candidate, we know we don't want it */
+        long d = pow(this->x - (*it).x, 2);
+        if (distance != -1 && d > distance) continue;
+        
+        /* If the Y distance of the candidate is > than the total distance of the
+           best previous candidate, we know we don't want it */
+        d += pow(this->y - (*it).y, 2);
+        if (distance != -1 && d > distance) continue;
+        
+        idx = it - points.begin();
+        distance = d;
+        
+        if (distance < EPSILON) break;
+    }
+    
+    return idx;
+}
+
+Point*
+Point::nearest_point(Points points) const
+{
+    return &(points.at(this->nearest_point_index(points)));
+}
+
 SV*
 Point::to_SV_pureperl() {
     AV* av = newAV();
diff --git a/xs/src/Point.hpp b/xs/src/Point.hpp
index bb23b2947..fdb405097 100644
--- a/xs/src/Point.hpp
+++ b/xs/src/Point.hpp
@@ -7,6 +7,9 @@
 
 namespace Slic3r {
 
+class Point;
+typedef std::vector<Point> Points;
+
 class Point
 {
     public:
@@ -20,10 +23,10 @@ class Point
     void translate(double x, double y);
     void rotate(double angle, Point* center);
     bool coincides_with(const Point* point) const;
+    int nearest_point_index(const Points points) const;
+    Point* nearest_point(Points points) const;
 };
 
-typedef std::vector<Point> Points;
-
 }
 
 #endif
diff --git a/xs/src/myinit.h b/xs/src/myinit.h
index 369dd0655..e3e143ee8 100644
--- a/xs/src/myinit.h
+++ b/xs/src/myinit.h
@@ -13,6 +13,8 @@ extern "C" {
 #undef do_close
 }
 
+#define EPSILON 1e-4
+
 namespace Slic3r {}
 using namespace Slic3r;
 
diff --git a/xs/t/03_point.t b/xs/t/03_point.t
index 687006b98..d8268602c 100644
--- a/xs/t/03_point.t
+++ b/xs/t/03_point.t
@@ -4,7 +4,7 @@ use strict;
 use warnings;
 
 use Slic3r::XS;
-use Test::More tests => 7;
+use Test::More tests => 8;
 
 my $point = Slic3r::Point->new(10, 15);
 is_deeply [ @$point ], [10, 15], 'point roundtrip';
@@ -22,7 +22,12 @@ ok !$point->coincides_with($point2), 'coincides_with';
 {
     my $point3 = Slic3r::Point->new(4300000, -9880845);
     is $point->[0], $point->x, 'x accessor';
-    is $point->[1], $point->y, 'y accessor';
+    is $point->[1], $point->y, 'y accessor';  #,,
+}
+
+{
+    my $nearest = $point->nearest_point([ $point2, Slic3r::Point->new(100, 200) ]);
+    ok $nearest->coincides_with($point2), 'nearest_point';
 }
 
 __END__
diff --git a/xs/xsp/Point.xsp b/xs/xsp/Point.xsp
index 6d308d62b..3ff35fac3 100644
--- a/xs/xsp/Point.xsp
+++ b/xs/xsp/Point.xsp
@@ -20,6 +20,9 @@
         %code{% RETVAL = THIS->x; %};
     long y()
         %code{% RETVAL = THIS->y; %};
+    int nearest_point_index(Points points);
+    Point* nearest_point(Points points)
+        %code{% const char* CLASS = "Slic3r::Point"; RETVAL = new Point(*(THIS->nearest_point(points))); %};
 
 %{
 
diff --git a/xs/xsp/Polygon.xsp b/xs/xsp/Polygon.xsp
index 78a1e2edc..d2d09d08a 100644
--- a/xs/xsp/Polygon.xsp
+++ b/xs/xsp/Polygon.xsp
@@ -28,6 +28,8 @@
     bool make_counter_clockwise();
     bool make_clockwise();
     bool is_valid();
+    Point* first_point()
+        %code{% const char* CLASS = "Slic3r::Point"; RETVAL = new Point(*(THIS->first_point())); %};
 %{
 
 Polygon*
diff --git a/xs/xsp/Polyline.xsp b/xs/xsp/Polyline.xsp
index c2e0a548b..427c6d0d8 100644
--- a/xs/xsp/Polyline.xsp
+++ b/xs/xsp/Polyline.xsp
@@ -19,6 +19,8 @@
         %code{% THIS->points.pop_back(); %};
     void reverse();
     Lines lines();
+    Point* first_point()
+        %code{% const char* CLASS = "Slic3r::Point"; RETVAL = new Point(*(THIS->first_point())); %};
 %{
 
 Polyline*
diff --git a/xs/xsp/my.map b/xs/xsp/my.map
index 9bfaaeb47..386112ac1 100644
--- a/xs/xsp/my.map
+++ b/xs/xsp/my.map
@@ -17,6 +17,7 @@ SurfaceType     T_UV
 ClipperLib::JoinType		T_UV
 ClipperLib::PolyFillType		T_UV
 
+Points          T_ARRAYREF
 Lines           T_ARRAYREF
 Polygons        T_ARRAYREF
 ExPolygons      T_ARRAYREF
diff --git a/xs/xsp/typemap.xspt b/xs/xsp/typemap.xspt
index a4ac930d7..7e8445b22 100644
--- a/xs/xsp/typemap.xspt
+++ b/xs/xsp/typemap.xspt
@@ -10,6 +10,7 @@
 %typemap{ExtrusionEntityCollection*};
 %typemap{ExtrusionPath*};
 %typemap{ExtrusionLoop*};
+%typemap{Points};
 %typemap{Lines};
 %typemap{Polygons};
 %typemap{ExPolygons};