SuperSlicer/lib/Slic3r/Geometry.pm

package Slic3r::Geometry;
use strict;
use warnings;

require Exporter;
our @ISA = qw(Exporter);
our @EXPORT_OK = qw(
    PI epsilon slope line_atan lines_parallel three_points_aligned
    line_point_belongs_to_segment points_coincide distance_between_points 
    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_along_segment polygon_segment_having_point polygon_has_subsegment
    polygon_has_vertex polyline_length can_connect_points deg2rad rad2deg
    rotate_points move_points remove_coinciding_points clip_segment_polygon
    sum_vectors multiply_vector subtract_vectors dot perp polygon_points_visibility
    line_intersection bounding_box bounding_box_intersect 
    clip_segment_complex_polygon longest_segment angle3points
);

use Slic3r::Geometry::DouglasPeucker ();
use XXX;

use constant PI => 4 * atan2(1, 1);
use constant A => 0;
use constant B => 1;
use constant X => 0;
use constant Y => 1;
our $parallel_degrees_limit = abs(deg2rad(3));

our $epsilon = 1E-4;
sub epsilon () { $epsilon }

sub slope {
    my ($line) = @_;
    return undef if abs($line->[B][X] - $line->[A][X]) < epsilon;  # line is vertical
    return ($line->[B][Y] - $line->[A][Y]) / ($line->[B][X] - $line->[A][X]);
}

sub line_atan {
    my ($line) = @_;
    return atan2($line->[B][Y] - $line->[A][Y], $line->[B][X] - $line->[A][X]);
}

sub lines_parallel {
    my ($line1, $line2) = @_;
    
    return abs(line_atan($line1) - line_atan($line2)) < $parallel_degrees_limit;
}

sub three_points_aligned {
    my ($p1, $p2, $p3) = @_;
    return lines_parallel([$p1, $p2], [$p2, $p3]);
}

# this subroutine checks whether a given point may belong to a given
# segment given the hypothesis that it belongs to the line containing
# the segment
sub line_point_belongs_to_segment {
    my ($point, $segment) = @_;
    
    #printf "   checking whether %f,%f may belong to segment %f,%f - %f,%f\n",
    #    @$point, map @$_, @$segment;
    
    my @segment_extents = (
        [ sort { $a <=> $b } map $_->[X], @$segment ],
        [ sort { $a <=> $b } map $_->[Y], @$segment ],
    );
    
    return 0 if $point->[X] < ($segment_extents[X][0] - epsilon) || $point->[X] > ($segment_extents[X][1] + epsilon);
    return 0 if $point->[Y] < ($segment_extents[Y][0] - epsilon) || $point->[Y] > ($segment_extents[Y][1] + epsilon);
    return 1;
}

sub points_coincide {
    my ($p1, $p2) = @_;
    return 1 if abs($p2->[X] - $p1->[X]) < epsilon && abs($p2->[Y] - $p1->[Y]) < epsilon;
    return 0;
}

sub distance_between_points {
    my ($p1, $p2) = @_;
    return sqrt((($p1->[X] - $p2->[X])**2) + ($p1->[Y] - $p2->[Y])**2);
}

sub line_length {
    my ($line) = @_;
    return distance_between_points(@$line[A, B]);
}

sub longest_segment {
    my (@lines) = @_;
    
    my ($longest, $maxlength);
    foreach my $line (@lines) {
        my $line_length = line_length($line);
        if (!defined $longest || $line_length > $maxlength) {
            $longest = $line;
            $maxlength = $line_length;
        }
    }
    return $longest;
}

sub midpoint {
    my ($line) = @_;
    return [ ($line->[B][X] + $line->[A][X]) / 2, ($line->[B][Y] + $line->[A][Y]) / 2 ];
}

sub point_in_polygon {
    my ($point, $polygon) = @_;
    
    my ($x, $y) = @$point;
    my @xy = map @$_, @$polygon;
    
    # Derived from the comp.graphics.algorithms FAQ,
    # courtesy of Wm. Randolph Franklin
    my $n = @xy / 2;                        # Number of points in polygon
    my @i = map { 2*$_ } 0..(@xy/2);        # The even indices of @xy
    my @x = map { $xy[$_]     } @i;         # Even indices: x-coordinates
    my @y = map { $xy[$_ + 1] } @i;         # Odd indices:  y-coordinates
    
    my ($i, $j);
    my $side = 0;                           # 0 = outside; 1 = inside
    for ($i = 0, $j = $n - 1; $i < $n; $j = $i++) {
        if (
            # If the y is between the (y-) borders...
            ($y[$i] <= $y && $y < $y[$j]) || ($y[$j] <= $y && $y < $y[$i])
            and
            # ...the (x,y) to infinity line crosses the edge
            # from the ith point to the jth point...
            ($x < ($x[$j] - $x[$i]) * ($y - $y[$i]) / ($y[$j] - $y[$i]) + $x[$i])
        ) {
            $side = not $side;  # Jump the fence
        }
    }
    
    # if point is not in polygon, let's check whether it belongs to the contour
    if (!$side && 0) {
        return 1 if polygon_segment_having_point($polygon, $point);
    }
    
    return $side;
}

sub point_in_segment {
    my ($point, $line) = @_;
    
    my ($x, $y) = @$point;
    my @line_x = sort { $a <=> $b } $line->[A][X], $line->[B][X];
    my @line_y = sort { $a <=> $b } $line->[A][Y], $line->[B][Y];
    
    # check whether the point is in the segment bounding box
    return 0 unless $x >= ($line_x[0] - epsilon) && $x <= ($line_x[1] + epsilon)
        && $y >= ($line_y[0] - epsilon) && $y <= ($line_y[1] + epsilon);
    
    # if line is vertical, check whether point's X is the same as the line
    if ($line->[A][X] == $line->[B][X]) {
        return 1 if abs($x - $line->[A][X]) < epsilon;
    }
    
    # calculate the Y in line at X of the point
    my $y3 = $line->[A][Y] + ($line->[B][Y] - $line->[A][Y])
        * ($x - $line->[A][X]) / ($line->[B][X] - $line->[A][X]);
    return abs($y3 - $y) < epsilon ? 1 : 0;
}

sub segment_in_segment {
    my ($needle, $haystack) = @_;
    
    # a segment is contained in another segment if its endpoints are contained
    return point_in_segment($needle->[A], $haystack) && point_in_segment($needle->[B], $haystack);
}

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 polyline_lines {
    my ($polygon) = @_;
    
    my @lines = ();
    my $last_point;
    foreach my $point (@$polygon) {
        push @lines, [ $last_point, $point ] if $last_point;
        $last_point = $point;
    }
    
    return @lines;
}

sub polygon_lines {
    my ($polygon) = @_;
    return polyline_lines([ @$polygon, $polygon->[0] ]);
}

sub nearest_point {
    my ($point, $points) = @_;
    
    my ($nearest_point, $distance);
    foreach my $p (@$points) {
        my $d = distance_between_points($point, $p);
        if (!defined $distance || $d < $distance) {
            $nearest_point = $p;
            $distance = $d;
            return $p if $distance < epsilon;
        }
    }
    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) = @_;
    
    my $point = [ @$p1 ];
    
    my $line_length = sqrt( (($p2->[X] - $p1->[X])**2) + (($p2->[Y] - $p1->[Y])**2) );
    for (X, Y) {
        if ($p1->[$_] != $p2->[$_]) {
            $point->[$_] = $p1->[$_] + ($p2->[$_] - $p1->[$_]) * $distance / $line_length;
        }
    }
    
    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;
}

# return true if the given segment is contained in any edge of the polygon
sub polygon_has_subsegment {
    my ($polygon, $segment) = @_;
    foreach my $line (polygon_lines($polygon)) {
        return 1 if segment_in_segment($segment, $line);
    }
    return 0;
}

sub polygon_has_vertex {
    my ($polygon, $point) = @_;
    foreach my $p (@$polygon) {
        return 1 if points_coincide($p, $point);
    }
    return 0;
}

sub polyline_length {
    my ($polyline) = @_;
    my $length = 0;
    $length += line_length($_) for polygon_lines($polyline);
    return $length;
}

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;
}

sub rad2deg {
    my ($rad) = @_;
    return $rad / PI() * 180;
}

sub rotate_points {
    my ($radians, $center, @points) = @_;
    $center ||= [0,0];
    return map {
        [
            $center->[X] + cos($radians) * ($_->[X] - $center->[X]) - sin($radians) * ($_->[Y] - $center->[Y]),
            $center->[Y] + cos($radians) * ($_->[Y] - $center->[Y]) + sin($radians) * ($_->[X] - $center->[X]),
        ]
    } @points;
}

sub move_points {
    my ($shift, @points) = @_;
    return map [ $shift->[X] + $_->[X], $shift->[Y] + $_->[Y] ], @points;
}

# preserves order
sub remove_coinciding_points {
    my ($points) = @_;
    
    my %p = map { sprintf('%f,%f', @$_) => "$_" } @$points;
    %p = reverse %p;
    @$points = grep $p{"$_"}, @$points;
}

# implementation of Liang-Barsky algorithm
# polygon must be convex and ccw
sub clip_segment_polygon {
    my ($line, $polygon) = @_;
    
    if (@$line == 1) {
        # the segment is a point, check for inclusion
        return point_in_polygon($line, $polygon);
    }
    
    my @V = (@$polygon, $polygon->[0]);
    my $tE = 0; # the maximum entering segment parameter
    my $tL = 1; # the minimum entering segment parameter
    my $dS = subtract_vectors($line->[B], $line->[A]); # the segment direction vector
    
    for (my $i = 0; $i < $#V; $i++) {   # process polygon edge V[i]V[Vi+1]
        my $e = subtract_vectors($V[$i+1], $V[$i]);
        my $N = perp($e, subtract_vectors($line->[A], $V[$i]));
        my $D = -perp($e, $dS);
        if (abs($D) < epsilon) {          # $line is nearly parallel to this edge
            ($N < 0) ? return : next;     # P0 outside this edge ? $line is outside : $line cannot cross edge, thus ignoring
        }
        
        my $t = $N / $D;
        if ($D < 0) { # $line is entering across this edge
            if ($t > $tE) {  # new max $tE
                $tE = $t;
                return if $tE > $tL;  # $line enters after leaving polygon?
            }
        } else { # $line is leaving across this edge
            if ($t < $tL) {  # new min $tL
                $tL = $t;
                return if $tL < $tE;  # $line leaves before entering polygon?
            }
        }
    }
    
    # $tE <= $tL implies that there is a valid intersection subsegment
    return [
        sum_vectors($line->[A], multiply_vector($dS, $tE)),  # = P(tE) = point where S enters polygon
        sum_vectors($line->[A], multiply_vector($dS, $tL)),  # = P(tE) = point where S enters polygon
    ];
}

sub sum_vectors {
    my ($v1, $v2) = @_;
    return [ $v1->[X] + $v2->[X], $v1->[Y] + $v2->[Y] ];
}

sub multiply_vector {
    my ($line, $scalar) = @_;
    return [ $line->[X] * $scalar, $line->[Y] * $scalar ];
}

sub subtract_vectors {
    my ($line2, $line1) = @_;
    return [ $line2->[X] - $line1->[X], $line2->[Y] - $line1->[Y] ];
}

# 2D dot product
sub dot {
    my ($u, $v) = @_;
    return $u->[X] * $v->[X] + $u->[Y] * $v->[Y];
}

# 2D perp product
sub perp {
    my ($u, $v) = @_;
    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;
}

sub line_intersection {
    my ($line1, $line2, $require_crossing) = @_;
    $require_crossing ||= 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;
}

sub clip_segment_complex_polygon {
    my ($line, $polygons) = @_;
    
    my @intersections = grep $_, map line_intersection($line, $_, 1), 
        map polygon_lines($_), @$polygons or return ();
    
    # this is not very elegant, however it works
    @intersections = sort { sprintf("%020f,%020f", @$a) cmp sprintf("%020f,%020f", @$b) } @intersections;
    
    shift(@intersections) if !grep(point_in_polygon($intersections[0], $_), @$polygons)
        && !grep(polygon_segment_having_point($_, $intersections[0]), @$polygons);
    
    # defensive programming
    ###die "Invalid intersections" if @intersections % 2 != 0;
    
    my @lines = ();
    while (@intersections) {
        # skip tangent points
        my @points = map shift @intersections, 1..2;
        next if !$points[1];
        next if points_coincide(@points);
        push @lines, [ @points ];
    }
    return [@lines];
}

sub angle3points {
    my ($p1, $p2, $p3) = @_;
    # p1 is the center
    
    my $angle = atan2($p2->[X] - $p1->[X], $p2->[Y] - $p1->[Y])
              - atan2($p3->[X] - $p1->[X], $p3->[Y] - $p1->[Y]);
    
    # we only want to return only positive angles
    return $angle <= 0 ? $angle + 2*PI() : $angle;
}

1;