diff --git a/lib/Slic3r/Fill/Rectilinear.pm b/lib/Slic3r/Fill/Rectilinear.pm
index 53ef2b9d9a..81b9cb9a40 100644
--- a/lib/Slic3r/Fill/Rectilinear.pm
+++ b/lib/Slic3r/Fill/Rectilinear.pm
@@ -185,7 +185,7 @@ sub find_connectable_points {
     
     my @connectable_points = ();
     foreach my $p (@$points) {
-        if (!$self->can_connect($polygon, $point, [ $c, $p ])) {
+        if (!Slic3r::Geometry::can_connect_points($point, [ $c, $p ], [ $polygon->get_polygons ])) {
              @connectable_points ? last : next;
         }
         push @connectable_points, $p;
@@ -194,108 +194,6 @@ sub find_connectable_points {
     return @connectable_points;
 }
 
-# this subroutine tries to determine whether two points in a surface
-# are connectable without crossing contour or holes
-sub can_connect {
-    my $self = shift;
-    my ($polygon, $p1, $p2) = @_;
-    #printf "  Checking connectability of point %d\n", $p2->[1];
-    
-    # there's room for optimization here
-    
-    # this is not needed since we assume that $p1 and $p2 belong to $polygon
-    for ($p1, $p2) {
-        #return 0 unless $polygon->isinside($_);
-        
-        # TODO: re-enable this one after testing point_in_polygon() which
-        # doesn't detect well points on the contour of polygon
-        #return 0 unless Slic3r::Geometry::point_in_polygon($_, $polygon->points);
-    }
-    
-    # check whether the $p1-$p2 segment doesn't intersect any segment
-    # of the contour or of holes
-    my ($contour_p, @holes_p) = $polygon->get_polygons;
-    foreach my $points ($contour_p, @holes_p) {
-        foreach my $line ($self->_lines_from_mgp_points($points)) {
-            
-            # theoretically speaking, SegmentIntersection() would be the right tool for the 
-            # job; however floating point math often makes it not return any intersection
-            # point between our hypothetical extrusion segment and any other one, even 
-            # if, of course, the final point of the extrusion segment is taken from
-            # $point and thus it's a point that belongs for sure to a segment.
-            # then, let's calculate intersection considering extrusion segment as a ray
-            # instead of a segment, and then check whether the intersection point 
-            # belongs to the segment
-            my $point = SegmentRayIntersection([@$line, $p1, $p2]);
-            #printf "    intersecting ray %f,%f - %f,%f and segment %f,%f - %f,%f\n",
-            #    @$p1, @$p2, map @$_, @$line;
-            
-            if ($point && Slic3r::Geometry::line_point_belongs_to_segment($point, [$p1, $p2])) {
-                #printf "  ...point intersects!\n";
-                #YYY [ $point, $p1, $p2 ];
-                
-                # our $p1-$p2 line intersects $line
-                
-                # if the intersection point is an intermediate point of $p1-$p2
-                # it means that $p1-$p2 crosses $line, thus we're sure that 
-                # $p1 and $p2 are not connectible (one is inside polygon and one
-                # is outside), unless $p1-$p2 and $line coincide but we've got
-                # an intersection due to floating point math
-                my @points_not_belonging_to_line = grep !Slic3r::Geometry::points_coincide($point, $_), $p1, $p2;
-                if (@points_not_belonging_to_line == 2) {
-                
-                    # make sure $p1-$p2 and $line are two distinct lines; we do this
-                    # by checking their slopes
-                    if (!Slic3r::Geometry::lines_parallel([$p1, $p2], $line)) {
-                        #printf "  ...lines cross!\n";
-                        #Slic3r::SVG::output_lines($main::print, "lines" . $n++ . ".svg", [ @lines, [$p1, $p2] ]);
-                        return 0;
-                    }
-                    
-                }
-                
-                # defensive programming, this shouldn't happen
-                if (@points_not_belonging_to_line == 0) {
-                    die "SegmentIntersection is not expected to return an intersection point "
-                        . "if \$line coincides with \$p1-\$p2";
-                }
-                
-                # if we're here, then either $p1 or $p2 belong to $line
-                # so we have to check whether the other point falls inside
-                # the polygon or not
-                # we rely on Math::Geometry::Planar returning contour points
-                # in counter-clockwise order and hole points in clockwise
-                # order, so that if the point falls on the left of $line
-                # it's inside the polygon and viceversa
-                my $C = $points_not_belonging_to_line[0];
-                my $isInside = (($line->[B][X] - $line->[A][X])*($C->[Y] - $line->[A][Y]) 
-                    - ($line->[B][Y] - $line->[A][Y])*($C->[X] - $line->[A][X])) > 0;
-                
-                #printf "  \$line is inside polygon: %d\n", $isInside;
-                
-                
-                # if the line is outside the polygon then points are not connectable
-                return 0 if !$isInside;
-                #Slic3r::SVG::output_lines($main::print, "lines" . $n++ . ".svg", [ @lines, [$p1, $p2] ])
-                #    if !$isInside;
-            }
-        }
-    }
-
-    # even if no intersection is found, we should check whether both $p1 and $p2 are
-    # inside a hole; this may happen due to floating point path
-    #foreach my $hole_p (map $self->_mgp_from_points_ref($_), @holes_p) {
-    #    if ($hole_p->isinside($p1) || $hole_p->isinside($p2)) {
-    #        return 0;
-    #    }
-    #}
-    
-    #use Slic3r::SVG;
-    #Slic3r::SVG::output_lines($main::print, "lines" . $n++ . ".svg", [ @lines, [$p1, $p2] ]);
-    
-    return 1;
-}
-
 sub _lines_from_mgp_points {
     my $self = shift;
     my ($points) = @_;
diff --git a/lib/Slic3r/Geometry.pm b/lib/Slic3r/Geometry.pm
index 51a1c203de..20c49100b5 100644
--- a/lib/Slic3r/Geometry.pm
+++ b/lib/Slic3r/Geometry.pm
@@ -10,7 +10,7 @@ use constant A => 0;
 use constant B => 1;
 use constant X => 0;
 use constant Y => 1;
-use constant epsilon => 1E-6;
+use constant epsilon => 1E-4;
 our $parallel_degrees_limit = abs(deg2rad(3));
 
 sub slope {
@@ -120,6 +120,13 @@ sub point_in_segment {
     return abs($y3 - $y) < epsilon ? 1 : 0;
 }
 
+sub point_is_on_left_of_segment {
+    my ($point, $line) = @_;
+    
+    return (($line->[B][X] - $line->[A][X])*($point->[Y] - $line->[A][Y]) 
+        - ($line->[B][Y] - $line->[A][Y])*($point->[X] - $line->[A][X])) > 0;
+}
+
 sub polygon_lines {
     my ($polygon) = @_;
     
@@ -148,6 +155,7 @@ sub nearest_point {
     return $nearest_point;
 }
 
+# given a segment $p1-$p2, get the point at $distance from $p1 along segment
 sub point_along_segment {
     my ($p1, $p2, $distance) = @_;
     
@@ -163,6 +171,39 @@ sub point_along_segment {
     return $point;
 }
 
+# given a $polygon, return the (first) segment having $point
+sub polygon_segment_having_point {
+    my ($polygon, $point) = @_;
+    
+    foreach my $line (polygon_lines($polygon)) {
+        return $line if point_in_segment($point, $line);
+    }
+    return undef;
+}
+
+sub can_connect_points {
+    my ($p1, $p2, $polygons) = @_;
+    
+    # check that the two points are visible from each other
+    return 0 if grep !polygon_points_visibility($_, $p1, $p2), @$polygons;
+    
+    # get segment where $p1 lies
+    my $p1_segment;
+    for (@$polygons) {
+        $p1_segment = polygon_segment_having_point($_, $p1);
+        last if $p1_segment;
+    }
+    
+    # defensive programming, this shouldn't happen
+    if (!$p1_segment) {
+        die sprintf "Point %f,%f wasn't found in polygon contour or holes!", @$p1;
+    }
+    
+    # check whether $p2 is internal or external  (internal = on the left)
+    return point_is_on_left_of_segment($p2, $p1_segment)
+        || point_in_segment($p2, $p1_segment);
+}
+
 sub deg2rad {
     my ($degrees) = @_;
     return PI() * $degrees / 180;
@@ -264,4 +305,158 @@ sub perp {
     return $u->[X] * $v->[Y] - $u->[Y] * $v->[X];
 }
 
+sub polygon_points_visibility {
+    my ($polygon, $p1, $p2) = @_;
+    
+    my $our_line = [ $p1, $p2 ];
+    foreach my $line (polygon_lines($polygon)) {
+        my $intersection = line_intersection($our_line, $line, 1) or next;
+        next if grep points_coincide($intersection, $_), $p1, $p2;
+        return 0;
+    }
+    
+    return 1;
+}
+
+my $i = 0;
+sub line_intersection {
+    my ($line1, $line2, $require_crossing) = @_;
+    $require_crossing ||= 0;
+    
+    Slic3r::SVG::output(undef, "line_intersection_" . $i++ . ".svg",
+        lines => [ $line1, $line2 ],
+    ) if 0;
+    
+    my $intersection = _line_intersection(map @$_, @$line1, @$line2);
+    return (ref $intersection && $intersection->[1] == $require_crossing) 
+        ? $intersection->[0] 
+        : undef;
+}
+
+sub _line_intersection {
+  my ( $x0, $y0, $x1, $y1, $x2, $y2, $x3, $y3 );
+
+  if ( @_ == 8 ) {
+    ( $x0, $y0, $x1, $y1, $x2, $y2, $x3, $y3 ) = @_;
+
+    # The bounding boxes chop the lines into line segments.
+    # bounding_box() is defined later in this chapter.
+    my @box_a = bounding_box([ [$x0, $y0], [$x1, $y1] ]);
+    my @box_b = bounding_box([ [$x2, $y2], [$x3, $y3] ]);
+    
+    # Take this test away and the line segments are
+    # turned into lines going from infinite to another.
+    # bounding_box_intersect() defined later in this chapter.
+    return "out of bounding box" unless bounding_box_intersect( 2, @box_a, @box_b );
+  }
+  elsif ( @_ == 4 ) { # The parametric form.
+    $x0 = $x2 = 0;
+    ( $y0, $y2 ) = @_[ 1, 3 ];
+    # Need to multiply by 'enough' to get 'far enough'.
+    my $abs_y0 = abs $y0;
+    my $abs_y2 = abs $y2;
+    my $enough = 10 * ( $abs_y0 > $abs_y2 ? $abs_y0 : $abs_y2 );
+    $x1 = $x3 = $enough;
+    $y1 = $_[0] * $x1 + $y0;
+    $y3 = $_[2] * $x2 + $y2;
+  }
+
+  my ($x, $y);  # The as-yet-undetermined intersection point.
+
+  my $dy10 = $y1 - $y0; # dyPQ, dxPQ are the coordinate differences
+  my $dx10 = $x1 - $x0; # between the points P and Q.
+  my $dy32 = $y3 - $y2;
+  my $dx32 = $x3 - $x2;
+
+  my $dy10z = abs( $dy10 ) < epsilon; # Is the difference $dy10 "zero"?
+  my $dx10z = abs( $dx10 ) < epsilon;
+  my $dy32z = abs( $dy32 ) < epsilon;
+  my $dx32z = abs( $dx32 ) < epsilon;
+
+  my $dyx10;                            # The slopes.
+  my $dyx32;
+
+
+  $dyx10 = $dy10 / $dx10 unless $dx10z;
+  $dyx32 = $dy32 / $dx32 unless $dx32z;
+
+  # Now we know all differences and the slopes;
+  # we can detect horizontal/vertical special cases.
+  # E.g., slope = 0 means a horizontal line.
+
+  unless ( defined $dyx10 or defined $dyx32 ) {
+    return "parallel vertical";
+  }
+  elsif ( $dy10z and not $dy32z ) { # First line horizontal.
+    $y = $y0;
+    $x = $x2 + ( $y - $y2 ) * $dx32 / $dy32;
+  }
+  elsif ( not $dy10z and $dy32z ) { # Second line horizontal.
+    $y = $y2;
+    $x = $x0 + ( $y - $y0 ) * $dx10 / $dy10;
+  }
+  elsif ( $dx10z and not $dx32z ) { # First line vertical.
+    $x = $x0;
+    $y = $y2 + $dyx32 * ( $x - $x2 );
+  }
+  elsif ( not $dx10z and $dx32z ) { # Second line vertical.
+    $x = $x2;
+    $y = $y0 + $dyx10 * ( $x - $x0 );
+  }
+  elsif ( abs( $dyx10 - $dyx32 ) < epsilon ) {
+    # The slopes are suspiciously close to each other.
+    # Either we have parallel collinear or just parallel lines.
+
+    # The bounding box checks have already weeded the cases
+    # "parallel horizontal" and "parallel vertical" away.
+
+    my $ya = $y0 - $dyx10 * $x0;
+    my $yb = $y2 - $dyx32 * $x2;
+
+    return "parallel collinear" if abs( $ya - $yb ) < epsilon;
+    return "parallel";
+  }
+  else {
+    # None of the special cases matched.
+    # We have a "honest" line intersection.
+
+    $x = ($y2 - $y0 + $dyx10*$x0 - $dyx32*$x2)/($dyx10 - $dyx32);
+    $y = $y0 + $dyx10 * ($x - $x0);
+  }
+
+  my $h10 = $dx10 ? ($x - $x0) / $dx10 : ($dy10 ? ($y - $y0) / $dy10 : 1);
+  my $h32 = $dx32 ? ($x - $x2) / $dx32 : ($dy32 ? ($y - $y2) / $dy32 : 1);
+
+  return [[$x, $y], $h10 >= 0 && $h10 <= 1 && $h32 >= 0 && $h32 <= 1];
+}
+
+# 2D
+sub bounding_box {
+    my ($points) = @_;
+    
+    my @x = sort { $a <=> $b } map $_->[X], @$points;
+    my @y = sort { $a <=> $b } map $_->[Y], @$points;
+    
+    return ($x[0], $y[0], $x[-1], $y[-1]);
+}
+
+# bounding_box_intersect($d, @a, @b)
+#   Return true if the given bounding boxes @a and @b intersect
+#   in $d dimensions.  Used by line_intersection().
+sub bounding_box_intersect {
+    my ( $d, @bb ) = @_; # Number of dimensions and box coordinates.
+    my @aa = splice( @bb, 0, 2 * $d ); # The first box.
+    # (@bb is the second one.)
+    
+    # Must intersect in all dimensions.
+    for ( my $i_min = 0; $i_min < $d; $i_min++ ) {
+        my $i_max = $i_min + $d; # The index for the maximum.
+        return 0 if ( $aa[ $i_max ] + epsilon ) < $bb[ $i_min ];
+        return 0 if ( $bb[ $i_max ] + epsilon ) < $aa[ $i_min ];
+    }
+    
+    return 1;
+}
+
+
 1;
diff --git a/lib/Slic3r/SVG.pm b/lib/Slic3r/SVG.pm
index a8e91da93e..6ce81efaa2 100644
--- a/lib/Slic3r/SVG.pm
+++ b/lib/Slic3r/SVG.pm
@@ -17,106 +17,97 @@ sub svg {
     return SVG->new(width => $print->max_length * factor(), height => $print->max_length * factor());
 }
 
-sub output_points {
-    my ($print, $filename, $points, $red_points) = @_;
-    $red_points ||= [];
+sub output {
+    my ($print, $filename, %things) = @_;
     
     my $svg = svg($print);
-    my $g = $svg->group(
-        style => {
-            'stroke-width' => 2,
-            'stroke' => 'black',
-            'fill' => 'black',
-        },
-    );
-    foreach my $point (@$points) {
-        $g->circle(
-            cx      => $point->[X] * factor(),
-            cy      => $point->[Y] * factor(),
-            r       => 2,
-        );
+    
+    foreach my $type (qw(polygons polylines)) {
+        if ($things{$type}) {
+            my $method = $type eq 'polygons' ? 'polygon' : 'polyline';
+            my $g = $svg->group(
+                style => {
+                    'stroke-width' => 2,
+                    'stroke' => 'black',
+                    'fill' => 'none',
+                },
+            );
+            foreach my $polygon (@{$things{$type}}) {
+                my $path = $svg->get_path(
+                    'x' => [ map($_->[X] * factor(), @$polygon) ],
+                    'y' => [ map($_->[Y] * factor(), @$polygon) ],
+                    -type => 'polygon',
+                );
+                $g->$method(
+                    %$path,
+                );
+            }
+        }
     }
     
-    my $g2 = $svg->group(
-        style => {
-            'stroke-width' => 2,
-            'stroke' => 'red',
-            'fill' => 'red',
-        },
-    );
-    foreach my $point (@$red_points) {
-        $g2->circle(
-            cx      => $point->[X] * factor(),
-            cy      => $point->[Y] * factor(),
-            r       => 3,
-        );
+    foreach my $type (qw(points red_points)) {
+        if ($things{$type}) {
+            my ($colour, $r) = $type eq 'points' ? ('black', 2) : ('red', 3);
+            my $g = $svg->group(
+                style => {
+                    'stroke-width' => 2,
+                    'stroke' => 'black',
+                    'fill' => $colour,
+                },
+            );
+            foreach my $point (@{$things{$type}}) {
+                $g->circle(
+                    cx      => $point->[X] * factor(),
+                    cy      => $point->[Y] * factor(),
+                    r       => $r,
+                );
+            }
+        }
+    }
+    
+    foreach my $type (qw(lines red_lines)) {
+        if ($things{$type}) {
+            my ($colour) = $type eq 'lines' ? ('black') : ('red');
+            my $g = $svg->group(
+                style => {
+                    'stroke-width' => 2,
+                },
+            );
+            foreach my $line (@{$things{$type}}) {
+                $g->line(
+                    x1 => $line->[0][X] * factor(),
+                    y1 => $line->[0][Y] * factor(),
+                    x2 => $line->[1][X] * factor(),
+                    y2 => $line->[1][Y] * factor(),
+                    style => {
+                        'stroke' => $colour,
+                    },
+                );
+            }
+        }
     }
     
     write_svg($svg, $filename);
 }
 
+sub output_points {
+    my ($print, $filename, $points, $red_points) = @_;
+    return output($print, $filename, points => $points, red_points => $red_points);
+}
+
 sub output_polygons {
-    my ($print, $filename, $polygons, $type) = @_;
-    $type ||= 'polygon';
-    
-    my $svg = svg($print);
-    my $g = $svg->group(
-        style => {
-            'stroke-width' => 2,
-            'stroke' => 'black',
-            'fill' => 'none',
-        },
-    );
-    foreach my $polygon (@$polygons) {
-        my $path = $svg->get_path(
-            'x' => [ map($_->[X] * factor(), @$polygon) ],
-            'y' => [ map($_->[Y] * factor(), @$polygon) ],
-            -type => 'polygon',
-        );
-        $g->$type(
-            %$path,
-        );
-    }
-    
-    write_svg($svg, $filename);
+    my ($print, $filename, $polygons) = @_;
+    return output($print, $filename, polygons => $polygons);
 }
 
 sub output_polylines {
-    return output_polygons(@_, 'polyline');
+    my ($print, $filename, $polylines) = @_;
+    return output($print, $filename, polylines => $polylines);
 }
 
 sub output_lines {
     my ($print, $filename, $lines) = @_;
-    
-    my $svg = svg($print);
-    my $g = $svg->group(
-        style => {
-            'stroke-width' => 2,
-        },
-    );
-    
-    my $color = 'red';
-    my $draw_line = sub {
-        my ($line) = @_;
-        $g->line(
-            x1 => $line->[0][X] * factor(),
-            y1 => $line->[0][Y] * factor(),
-            x2 => $line->[1][X] * factor(),
-            y2 => $line->[1][Y] * factor(),
-            style => {
-                'stroke' => $color,
-            },
-        );
-    };
-    
-    my $last = pop @$lines;
-    foreach my $line (@$lines) {
-        $draw_line->($line);
-    }
-    $color = 'black';
-    $draw_line->($last);
-    
-    write_svg($svg, $filename);
+    return output($print, $filename, lines => $lines);
 }
 
 sub write_svg {
diff --git a/t/geometry.t b/t/geometry.t
new file mode 100644
index 0000000000..3d90186866
--- /dev/null
+++ b/t/geometry.t
@@ -0,0 +1,71 @@
+use Test::More;
+use strict;
+use warnings;
+
+plan tests => 4;
+
+BEGIN {
+    use FindBin;
+    use lib "$FindBin::Bin/../lib";
+}
+
+use Slic3r;
+
+#==========================================================
+
+my $line1 = [ [73.6310778185108/0.0000001, 371.74239268924/0.0000001], [73.6310778185108/0.0000001, 501.74239268924/0.0000001] ];
+my $line2 = [ [75/0.0000001, 437.9853/0.0000001], [62.7484/0.0000001, 440.4223/0.0000001] ];
+isnt Slic3r::Geometry::line_intersection($line1, $line2, 1), undef, 'line_intersection';
+
+#==========================================================
+
+my $polyline = [
+    [459190000, 5152739000], [147261000, 4612464000], [147261000, 3487535000], [339887000, 3153898000], 
+    [437497000, 3438430000], [454223000, 3522515000], [523621000, 3626378000], [627484000, 3695776000], 
+    [750000000, 3720147000], [872515000, 3695776000], [976378000, 3626378000], [1045776000, 3522515000], 
+    [1070147000, 3400000000], [1045776000, 3277484000], [976378000, 3173621000], [872515000, 3104223000], 
+    [827892000, 3095347000], [698461000, 2947261000], [2540810000, 2947261000], [2852739000, 3487535000], 
+    [2852739000, 4612464000], [2540810000, 5152739000],
+];
+
+# this points belongs to $polyline
+my $point = [2797980957.103410,3392691792.513960];
+
+is_deeply Slic3r::Geometry::polygon_segment_having_point($polyline, $point), 
+    [ [2540810000, 2947261000], [2852739000, 3487535000] ],
+    'polygon_segment_having_point';
+
+#==========================================================
+
+$point = [ 736310778.185108, 5017423926.8924 ];
+my $line = [ [627484000, 3695776000], [750000000, 3720147000] ];
+is Slic3r::Geometry::point_in_segment($point, $line), 0, 'point_in_segment';
+
+#==========================================================
+
+my $polygons = [
+    [ # contour, ccw
+        [459190000, 5152739000], [147261000, 4612464000], [147261000, 3487535000], [339887000, 3153898000], 
+        [437497000, 3438430000], [454223000, 3522515000], [523621000, 3626378000], [627484000, 3695776000], 
+        [750000000, 3720147000], [872515000, 3695776000], [976378000, 3626378000], [1045776000, 3522515000], 
+        [1070147000, 3400000000], [1045776000, 3277484000], [976378000, 3173621000], [872515000, 3104223000], 
+        [827892000, 3095347000], [698461000, 2947261000], [2540810000, 2947261000], [2852739000, 3487535000], 
+        [2852739000, 4612464000], [2540810000, 5152739000],
+
+    ],
+    [ # hole, cw
+        [750000000, 5020147000], [872515000, 4995776000], [976378000, 4926378000], [1045776000, 4822515000], 
+        [1070147000, 4700000000], [1045776000, 4577484000], [976378000, 4473621000], [872515000, 4404223000], 
+        [750000000, 4379853000], [627484000, 4404223000], [523621000, 4473621000], [454223000, 4577484000], 
+        [429853000, 4700000000], [454223000, 4822515000], [523621000, 4926378000], [627484000, 4995776000],
+    ],
+];
+
+my $points = [
+    [ 736310778.185108, 3717423926.892399788 ],
+    [ 736310778.185108, 5017423926.8924 ],
+];
+
+is Slic3r::Geometry::can_connect_points(@$points, $polygons), 0, 'can_connect_points';
+
+#==========================================================