author.py

#    This is a component of emc2
#    Copyright 2007 Jeff Epler <jepler@unpythonic.net>
#
#    This program is free software; you can redistribute it and/or modify
#    it under the terms of the GNU General Public License as published by
#    the Free Software Foundation; either version 2 of the License, or
#    (at your option) any later version.
#
#    This program is distributed in the hope that it will be useful,
#    but WITHOUT ANY WARRANTY; without even the implied warranty of
#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#    GNU General Public License for more details.
#
#    You should have received a copy of the GNU General Public License
#    along with this program; if not, write to the Free Software
#    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
#
#    Modified by Frank Tkalcevic (frank@franksworkshop.com.au) to be used as part
#    of the g1tog23 conversion program.
#
#    Modified by thelongrunsmoke (thelongrunsmoke@gmail.com) to be used with py3.x.
#

import math
import os
import sys


def log(*args):
    if False:
        for arg in args:
            print(arg)


def dist_lseg(l1, l2, p):
    """
    Compute the 3D distance from the line segment l1..l2 to the point p.
    """
    x0, y0, z0, a0, f0, e0 = l1
    xa, ya, za, aa, fa, ea = l2
    xi, yi, zi, ai, fi, ei = p

    dx = xa - x0
    dy = ya - y0
    dz = za - z0
    d2 = dx * dx + dy * dy + dz * dz

    if d2 == 0:
        return 0

    t = (dx * (xi - x0) + dy * (yi - y0) + dz * (zi - z0)) / d2
    if t < 0:
        t = 0
    if t > 1:
        t = 1
    dist2 = (xi - x0 - t * dx) ** 2 + (yi - y0 - t * dy) ** 2 + (zi - z0 - t * dz) ** 2

    return dist2 ** .5


def rad1(x1, y1, x2, y2, x3, y3):
    x12 = x1 - x2
    y12 = y1 - y2
    x23 = x2 - x3
    y23 = y2 - y3
    x31 = x3 - x1
    y31 = y3 - y1

    den = abs(x12 * y23 - x23 * y12)
    if abs(den) < 1e-5:
        return sys.maxsize

    math.hypot(x12, y12) * math.hypot(x23, y23) * math.hypot(x31, y31) / 2 / den
    return math.hypot(x12, y12) * math.hypot(x23, y23) * math.hypot(x31, y31) / 2 / den


class Point:
    def __init__(self, x, y):
        self.x = x
        self.y = y

    def __str__(self): return "<%f,%f>" % (self.x, self.y)

    def __sub__(self, other):
        return Point(self.x - other.x, self.y - other.y)

    def __add__(self, other):
        return Point(self.x + other.x, self.y + other.y)

    def __mul__(self, other):
        return Point(self.x * other, self.y * other)

    __rmul__ = __mul__

    def cross(self, other):
        return self.x * other.y - self.y * other.x

    def dot(self, other):
        return self.x * other.x + self.y * other.y

    def mag(self):
        return math.hypot(self.x, self.y)

    def mag2(self):
        return self.x ** 2 + self.y ** 2


def cent1(x1, y1, x2, y2, x3, y3):
    p1 = Point(x1, y1)
    p2 = Point(x2, y2)
    p3 = Point(x3, y3)

    den = abs((p1 - p2).cross(p2 - p3))
    if abs(den) < 1e-5:
        return sys.maxsize, sys.maxsize

    alpha = (p2 - p3).mag2() * (p1 - p2).dot(p1 - p3) / 2 / den / den
    beta = (p1 - p3).mag2() * (p2 - p1).dot(p2 - p3) / 2 / den / den
    gamma = (p1 - p2).mag2() * (p3 - p1).dot(p3 - p2) / 2 / den / den

    pc = alpha * p1 + beta * p2 + gamma * p3
    return pc.x, pc.y


def arc_center(plane, p1, p2, p3):
    x1, y1, z1, a1, f1, e1 = p1
    x2, y2, z2, a2, f2, e2 = p2
    x3, y3, z3, a3, f3, e3 = p3

    if plane == 17:
        return cent1(x1, y1, x2, y2, x3, y3)
    if plane == 18:
        return cent1(x1, z1, x2, z2, x3, z3)
    if plane == 19:
        return cent1(y1, z1, y2, z2, y3, z3)


def arc_rad(plane, p1, p2, p3):
    if plane is None:
        return sys.maxsize

    x1, y1, z1, a1, f1, e1 = p1
    x2, y2, z2, a2, f2, e2 = p2
    x3, y3, z3, a3, f3, e3 = p3

    if plane == 17:
        return rad1(x1, y1, x2, y2, x3, y3)
    if plane == 18:
        return rad1(x1, z1, x2, z2, x3, z3)
    if plane == 19:
        return rad1(y1, z1, y2, z2, y3, z3)
    return None, 0


def get_pts(plane, x_y_z_a_f_e):
    x, y, z, a, f, e = x_y_z_a_f_e
    if plane == 17:
        return x, y
    if plane == 18:
        return x, z
    if plane == 19:
        return y, z


def one_quadrant(plane, c, p1, p2, p3):
    xc, yc = c
    x1, y1 = get_pts(plane, p1)
    x2, y2 = get_pts(plane, p2)
    x3, y3 = get_pts(plane, p3)
    log(";one_quadrant=", [xc, yc], [x1, y1], [x2, y2], [x3, y3])

    def sign(x):
        if abs(x) < 1e-5:
            return 0
        if x < 0:
            return -1
        return 1

    signs = {(sign(x1 - xc), sign(y1 - yc)), (sign(x2 - xc), sign(y2 - yc)), (sign(x3 - xc), sign(y3 - yc))}

    log(";signs=", signs)
    if len(signs) == 1:
        return True

    if (1, 1) in signs:
        signs.discard((1, 0))
        signs.discard((0, 1))
    if (1, -1) in signs:
        signs.discard((1, 0))
        signs.discard((0, -1))
    if (-1, 1) in signs:
        signs.discard((-1, 0))
        signs.discard((0, 1))
    if (-1, -1) in signs:
        signs.discard((-1, 0))
        signs.discard((0, -1))

    if len(signs) == 1:
        return True

    log("; signs return None ", signs)


def make2pi(theta):
    while theta < 0:
        theta = theta + 2 * math.pi
    while theta > 2 * math.pi:
        theta = theta - 2 * math.pi
    return theta


# return true if CCW
def arc_dir(plane, c, p1, p2, p3):
    xc, yc = c
    x1, y1 = get_pts(plane, p1)
    x2, y2 = get_pts(plane, p2)
    x3, y3 = get_pts(plane, p3)

    theta_start = make2pi(math.atan2(y1 - yc, x1 - xc))
    theta_mid = make2pi(math.atan2(y2 - yc, x2 - xc))
    theta_end = make2pi(math.atan2(y3 - yc, x3 - xc))
    log(";arc_dir", theta_start, theta_mid, theta_end)

    return is_arc_ccw(theta_start, theta_mid, theta_end)


# test 0.5, 0.6, 1, CCW = CCW
# test 1, 0.6, 0.5, CW = CW
# test 5, 5.5, 0.5, CCW = CCW
# test 5, 0.1, 0.5, CCW = CW
# test 0.5, 5.5, 5, CW = CCW
# test 0.5, 0.1, 5.5, CW = CW
# test 0.5, 1, 5, CCW = CCW
# test 5, 1, 0.5, CW = CW
# test 5.5, 0.5, 5, CCW = CW
# test 5.5, 5.6, 5, CCW = CCW
# test 5, 4, 6, CW = CW
# test 5, 6, 5.5, CW = CCW


def is_arc_ccw(start, mid, end):
    if start < end:
        if start < mid < end:
            ret = True
        else:
            ret = False
    else:
        if end < mid < start:
            ret = False
        else:
            ret = True

    # log( ";is_arc_ccw ", "CCW" if ret else "CW" )
    return ret


def test_arcs():
    def test(s, m, e, ccw):
        is_ccw = is_arc_ccw(s, m, e)
        if is_ccw != ccw:
            log("test " + str(s) + ", " + str(m) + ", " + str(e) + ", " + ("CCW" if ccw else "CW") + " = " + (
                "CCW" if is_ccw else "CW"))
            os.abort()

    test(.5, .6, 1, True)
    test(1, .6, .5, False)
    test(5, 5.5, .5, True)
    test(5, .1, .5, True)
    test(.5, 5.5, 5, False)
    test(.5, .1, 5.5, False)
    test(.5, 1, 5, True)
    test(5, 1, .5, False)
    test(5.5, .5, 5, True)
    test(5.5, 5.6, 5, True)
    test(5, 4, 6, False)
    test(5, 6, 5.5, False)


def chord_length(plane, c, p1, p2, p3):
    xc, yc = c
    x1, y1 = get_pts(plane, p1)
    x2, y2 = get_pts(plane, p3)
    r = math.hypot(y1 - yc, x1 - xc)

    theta_start = make2pi(math.atan2(y1 - yc, x1 - xc))
    theta_end = make2pi(math.atan2(y2 - yc, x2 - xc))

    if arc_dir(plane, c, p1, p2, p3):
        # CCW
        angle = make2pi(theta_end - theta_start)
    else:
        angle = make2pi(theta_start - theta_end)

    return angle * r


def arc_fmt(plane, c1, c2, p1):
    x, y, z, a, f, e = p1
    if plane == 17:
        return "I%.4f J%.4f" % (c1 - x, c2 - y)
    if plane == 18:
        return "I%.4f K%.4f" % (c1 - x, c2 - z)
    if plane == 19:
        return "J%.4f K%.4f" % (c1 - y, c2 - z)


def douglas(st, tolerance=.001, length_tolerance=0.005, plane=None):
    """
    Perform Douglas-Peucker simplification on the path 'st' with the specified
    tolerance.  The '_first' argument is for internal use only.

    The Douglas-Peucker simplification algorithm finds a subset of the input points
    whose path is never more than 'tolerance' away from the original input path.

    If 'plane' is specified as 17, 18, or 19, it may find helical arcs in the given
    plane in addition to lines.  Note that if there is movement in the plane
    perpendicular to the arc, it will be distorted, so 'plane' should usually
    be specified only when there is only movement on 2 axes
    """

    test_arcs()

    if len(st) == 1:
        path = [["G1", st[0], None]]
        return path

    # To build fuller arcs, we will use brute force, appending one
    # point at a time until we no longer have an arc/line
    path = []
    last_path = []
    new_path = []

    log("; len(st)=", len(st))
    for p in st:
        log(";", p)

    for i_st, point_st in enumerate(st):

        path.append(point_st)

        log(";path")
        for n in path:
            log(";\t", n)

        if i_st == 0:
            continue

        l1 = path[0]
        l2 = path[-1]

        worst_dist = 0
        min_rad = sys.maxsize
        max_arc = -1
        path_length = 0  # remember the path length

        ps = path[0]
        pe = path[-1]

        for i, p in enumerate(path):

            if i > 0:
                path_length = path_length + math.hypot(p[0] - path[i - 1][0], p[1] - path[i - 1][1])

            if p is l1 or p is l2:
                continue
            dist = dist_lseg(l1, l2, p)
            # log( "dist", i, p, dist )
            if dist > worst_dist:
                worst_dist = dist
                rad = arc_rad(plane, ps, p, pe)
                log(";rad", rad, max_arc, min_rad)
                if rad < min_rad:
                    max_arc = i
                    min_rad = rad

        worst_arc_dist = 0
        log(";min_rad=", min_rad)
        if min_rad != sys.maxsize:
            c1, c2 = arc_center(plane, ps, path[max_arc], pe)
            log(";arc center", c1, c2)
            is_one_quadrant = one_quadrant(plane, (c1, c2), ps, path[max_arc], pe)
            log(";is_one_quadrant=", is_one_quadrant)
            if True:  # is_one_quadrant:
                for i, (x, y, z, a, f, e) in enumerate(path):
                    if plane == 17:
                        dist = abs(math.hypot(c1 - x, c2 - y) - min_rad)
                    elif plane == 18:
                        dist = abs(math.hypot(c1 - x, c2 - z) - min_rad)
                    elif plane == 19:
                        dist = abs(math.hypot(c1 - y, c2 - z) - min_rad)
                    else:
                        dist = sys.maxsize
                    log(";wad", dist, worst_arc_dist)
                    if dist > worst_arc_dist:
                        worst_arc_dist = dist

                    # mx = (x + lx) / 2
                    # my = (y + ly) / 2
                    # mz = (z + lz) / 2
                    # if plane == 17:
                    #     dist = abs(math.hypot(c1 - mx, c2 - my) - min_rad)
                    # elif plane == 18:
                    #     dist = abs(math.hypot(c1 - mx, c2 - mz) - min_rad)
                    # elif plane == 19:
                    #     dist = abs(math.hypot(c1 - my, c2 - mz) - min_rad)
                    # else:
                    #     dist = sys.maxsize
                    # if dist > worst_arc_dist: worst_arc_dist = dist

                    # lx, ly, lz = x, y, z
            else:
                worst_arc_dist = sys.maxsize

        else:
            worst_arc_dist = sys.maxsize

        log(";worst\t", worst_arc_dist, worst_dist)
        if worst_arc_dist != sys.maxsize:
            log(";douglas", len(st), "\n;\t", path[0], "\n;\t", path[max_arc], "\n;\t", path[-1])
            log(";\t", worst_arc_dist, worst_dist)
            log(";\t", c1, c2)
            log(";chord_length", chord_length(plane, (c1, c2), ps, path[max_arc], pe))
            log(";path_length", path_length)
        if float(worst_arc_dist) < tolerance and worst_arc_dist < worst_dist and abs(
                        chord_length(plane, (c1, c2), ps, path[max_arc], pe) - path_length) < (
                    len(path) - 1) * length_tolerance:
            log("; arc")
            log(";chord_length", chord_length(plane, (c1, c2), ps, path[max_arc], pe))
            log(";path_length", path_length)
            log(";  (len(path)-1)*length_tolerance = ", (len(path) - 1) * length_tolerance)
            log("; abs(chord_length(plane,(c1,c2),ps,path[max_arc], pe) - path_length) = ",
                abs(chord_length(plane, (c1, c2), ps, path[max_arc], pe) - path_length))
            ccw = arc_dir(plane, (c1, c2), ps, path[max_arc], pe)
            # if plane == 18: ccw = not ccw # wtf?
            last_path = [["G1", ps, None]]
            if ccw:
                last_path.append(["G3", path[-1], arc_fmt(plane, c1, c2, ps)])
            else:
                last_path.append(["G2", path[-1], arc_fmt(plane, c1, c2, ps)])
        elif worst_dist > tolerance:
            log("; broke last")
            for p in last_path:
                new_path.append(p)
            last_path = []
            path = [point_st]
        else:
            log("; line")
            last_path = [["G1", path[0], None], ["G1", path[-1], None]]

    if len(last_path) > 0:
        for p in last_path:
            new_path.append(p)
    elif len(path) > 0:
        new_path.append(["G1", path[0], None])

    return new_path