diff --git a/FlatCAMDraw.py b/FlatCAMDraw.py index cd2648d7..886cd5f4 100644 --- a/FlatCAMDraw.py +++ b/FlatCAMDraw.py @@ -1,5 +1,6 @@ from PyQt4 import QtGui, QtCore, Qt import FlatCAMApp +from camlib import * from shapely.geometry import Polygon, LineString, Point, LinearRing from shapely.geometry import MultiPoint, MultiPolygon @@ -10,7 +11,8 @@ from shapely.wkt import loads as sloads from shapely.wkt import dumps as sdumps from shapely.geometry.base import BaseGeometry -from numpy import arctan2, Inf, array, sqrt, pi, ceil, sin, cos +from numpy import arctan2, Inf, array, sqrt, pi, ceil, sin, cos, sign, dot +from numpy.linalg import solve from mpl_toolkits.axes_grid.anchored_artists import AnchoredDrawingArea @@ -32,8 +34,14 @@ class DrawTool(object): self.geometry = None def click(self, point): + """ + :param point: [x, y] Coordinate pair. + """ return "" + def on_key(self, key): + return None + def utility_geometry(self, data=None): return None @@ -79,11 +87,183 @@ class FCCircle(FCShapeTool): def make(self): p1 = self.points[0] p2 = self.points[1] - radius = sqrt((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2) + radius = distance(p1, p2) self.geometry = Point(p1).buffer(radius) self.complete = True +class FCArc(FCShapeTool): + def __init__(self, draw_app): + DrawTool.__init__(self, draw_app) + self.start_msg = "Click on CENTER ..." + + # Direction of rotation between point 1 and 2. + # 'cw' or 'ccw'. Switch direction by hitting the + # 'o' key. + self.direction = "cw" + + # Mode + # C12 = Center, p1, p2 + # 12C = p1, p2, Center + # 132 = p1, p3, p2 + self.mode = "c12" # Center, p1, p2 + + self.steps_per_circ = 55 + + def click(self, point): + self.points.append(point) + + if len(self.points) == 1: + return "Click on 1st point ..." + + if len(self.points) == 2: + return "Click on 2nd point to complete ..." + + if len(self.points) == 3: + self.make() + return "Done." + + return "" + + def on_key(self, key): + if key == 'o': + self.direction = 'cw' if self.direction == 'ccw' else 'ccw' + return 'Direction: ' + self.direction.upper() + + if key == 'p': + if self.mode == 'c12': + self.mode = '12c' + elif self.mode == '12c': + self.mode = '132' + else: + self.mode = 'c12' + return 'Mode: ' + self.mode + + def utility_geometry(self, data=None): + if len(self.points) == 1: # Show the radius + center = self.points[0] + p1 = data + + return LineString([center, p1]) + + if len(self.points) == 2: # Show the arc + + if self.mode == 'c12': + center = self.points[0] + p1 = self.points[1] + p2 = data + + radius = sqrt((center[0] - p1[0]) ** 2 + (center[1] - p1[1]) ** 2) + startangle = arctan2(p1[1] - center[1], p1[0] - center[0]) + stopangle = arctan2(p2[1] - center[1], p2[0] - center[0]) + + elif self.mode == '132': + p1 = array(self.points[0]) + p3 = array(self.points[1]) + p2 = array(data) + + center, radius, t = three_point_circle(p1, p2, p3) + direction = 'cw' if sign(t) > 0 else 'ccw' + + startangle = arctan2(p1[1] - center[1], p1[0] - center[0]) + stopangle = arctan2(p3[1] - center[1], p3[0] - center[0]) + + return [LineString(arc(center, radius, startangle, stopangle, + direction, self.steps_per_circ)), + Point(center), Point(p1), Point(p3)] + + else: # '12c' + p1 = array(self.points[0]) + p2 = array(self.points[1]) + + # Midpoint + a = (p1 + p2) / 2.0 + + # Parallel vector + c = p2 - p1 + + # Perpendicular vector + b = dot(c, array([[0, -1], [1, 0]], dtype=float32)) + b /= norm(b) + + # Distance + t = distance(data, a) + + # Which side? Cross product with c. + side = data[0] * c[1] - data[1] * c[0] + t *= sign(side) + + # Center = a + bt + center = a + b * t + + radius = norm(center - p1) + startangle = arctan2(p1[1] - center[1], p1[0] - center[0]) + stopangle = arctan2(p2[1] - center[1], p2[0] - center[0]) + + return [LineString(arc(center, radius, startangle, stopangle, + self.direction, self.steps_per_circ)), + Point(center)] + + return None + + def make(self): + + if self.mode == 'c12': + center = self.points[0] + p1 = self.points[1] + p2 = self.points[2] + + radius = distance(center, p1) + startangle = arctan2(p1[1] - center[1], p1[0] - center[0]) + stopangle = arctan2(p2[1] - center[1], p2[0] - center[0]) + self.geometry = LineString(arc(center, radius, startangle, stopangle, + self.direction, self.steps_per_circ)) + + elif self.mode == '132': + p1 = array(self.points[0]) + p3 = array(self.points[1]) + p2 = array(self.points[2]) + + center, radius, t = three_point_circle(p1, p2, p3) + direction = 'cw' if sign(t) > 0 else 'ccw' + + startangle = arctan2(p1[1] - center[1], p1[0] - center[0]) + stopangle = arctan2(p3[1] - center[1], p3[0] - center[0]) + + self.geometry = LineString(arc(center, radius, startangle, stopangle, + direction, self.steps_per_circ)) + + else: # self.mode == '12c' + p1 = array(self.points[0]) + p2 = array(self.points[1]) + + # Midpoint + a = (p1 + p2) / 2.0 + + # Parallel vector + c = p2 - p1 + + # Perpendicular vector + b = dot(c, array([[0, -1], [1, 0]], dtype=float32)) + + # Distance + t = distance(self.points[2], p1) + + # Which side? Cross product with c. + side = self.points[2][0] * c[1] - self.points[2][1] * c[0] + t *= sign(side) + + # Center = a + bt + center = a + b * t + + radius = norm(center - p1) + startangle = arctan2(p1[1] - center[1], p1[0] - center[0]) + stopangle = arctan2(p2[1] - center[1], p2[0] - center[0]) + self.geometry = LineString(arc(center, radius, startangle, stopangle, + self.direction, self.steps_per_circ)) + self.complete = True + + class FCRectangle(FCShapeTool): """ Resulting type: Polygon @@ -277,6 +457,7 @@ class FlatCAMDraw(QtCore.QObject): self.app.ui.addToolBar(self.drawing_toolbar) self.select_btn = self.drawing_toolbar.addAction(QtGui.QIcon('share/pointer32.png'), 'Select') self.add_circle_btn = self.drawing_toolbar.addAction(QtGui.QIcon('share/circle32.png'), 'Add Circle') + self.add_arc_btn = self.drawing_toolbar.addAction(QtGui.QIcon('share/arc32.png'), 'Add Arc') self.add_rectangle_btn = self.drawing_toolbar.addAction(QtGui.QIcon('share/rectangle32.png'), 'Add Rectangle') self.add_polygon_btn = self.drawing_toolbar.addAction(QtGui.QIcon('share/polygon32.png'), 'Add Polygon') self.add_path_btn = self.drawing_toolbar.addAction(QtGui.QIcon('share/path32.png'), 'Add Path') @@ -320,6 +501,8 @@ class FlatCAMDraw(QtCore.QObject): "constructor": FCSelect}, "circle": {"button": self.add_circle_btn, "constructor": FCCircle}, + "arc": {"button": self.add_arc_btn, + "constructor": FCArc}, "rectangle": {"button": self.add_rectangle_btn, "constructor": FCRectangle}, "polygon": {"button": self.add_polygon_btn, @@ -620,6 +803,11 @@ class FlatCAMDraw(QtCore.QObject): if event.key == 'k': self.corner_snap_btn.trigger() + ### Propagate to tool + response = self.active_tool.on_key(event.key) + if response is not None: + self.app.info(response) + def on_canvas_key_release(self, event): self.key = None @@ -671,6 +859,12 @@ class FlatCAMDraw(QtCore.QObject): plot_elements.append(element) continue + if type(geo) == Point: + x, y = geo.coords.xy + element, = self.axes.plot(x, y, 'bo', linewidth=linewidth, animated=animated) + plot_elements.append(element) + continue + return plot_elements # self.canvas.auto_adjust_axes() @@ -823,4 +1017,8 @@ class FlatCAMDraw(QtCore.QObject): def distance(pt1, pt2): - return sqrt((pt1[0] - pt2[0]) ** 2 + (pt1[1] - pt2[1]) ** 2) \ No newline at end of file + return sqrt((pt1[0] - pt2[0]) ** 2 + (pt1[1] - pt2[1]) ** 2) + + +def mag(vec): + return sqrt(vec[0] ** 2 + vec[1] ** 2) \ No newline at end of file diff --git a/camlib.py b/camlib.py index 9a94ea4b..3da969a0 100644 --- a/camlib.py +++ b/camlib.py @@ -8,7 +8,9 @@ #from __future__ import division import traceback -from numpy import arctan2, Inf, array, sqrt, pi, ceil, sin, cos +from numpy import arctan2, Inf, array, sqrt, pi, ceil, sin, cos, dot, float32, \ + transpose +from numpy.linalg import solve, norm from matplotlib.figure import Figure import re @@ -2738,26 +2740,33 @@ def arc(center, radius, start, stop, direction, steps_per_circ): da_sign = {"cw": -1.0, "ccw": 1.0} points = [] if direction == "ccw" and stop <= start: - stop += 2*pi + stop += 2 * pi if direction == "cw" and stop >= start: - stop -= 2*pi + stop -= 2 * pi angle = abs(stop - start) #angle = stop-start - steps = max([int(ceil(angle/(2*pi)*steps_per_circ)), 2]) - delta_angle = da_sign[direction]*angle*1.0/steps - for i in range(steps+1): - theta = start + delta_angle*i - points.append((center[0]+radius*cos(theta), center[1]+radius*sin(theta))) + steps = max([int(ceil(angle / (2 * pi) * steps_per_circ)), 2]) + delta_angle = da_sign[direction] * angle * 1.0 / steps + for i in range(steps + 1): + theta = start + delta_angle * i + points.append((center[0] + radius * cos(theta), center[1] + radius * sin(theta))) return points +def arc2(p1, p2, center, direction, steps_per_circ): + r = sqrt((center[0] - p1[0]) ** 2 + (center[1] - p1[1]) ** 2) + start = arctan2(p1[1] - center[1], p1[0] - center[0]) + stop = arctan2(p2[1] - center[1], p2[0] - center[0]) + return arc(center, r, start, stop, direction, steps_per_circ) + + def arc_angle(start, stop, direction): if direction == "ccw" and stop <= start: - stop += 2*pi + stop += 2 * pi if direction == "cw" and stop >= start: - stop -= 2*pi + stop -= 2 * pi angle = abs(stop - start) return angle @@ -3108,4 +3117,35 @@ def autolist(obj): _ = iter(obj) return obj except TypeError: - return [obj] \ No newline at end of file + return [obj] + + +def three_point_circle(p1, p2, p3): + """ + Computes the center and radius of a circle from + 3 points on its circumference. + + :param p1: Point 1 + :param p2: Point 2 + :param p3: Point 3 + :return: center, radius + """ + # Midpoints + a1 = (p1 + p2) / 2.0 + a2 = (p2 + p3) / 2.0 + + # Normals + b1 = dot((p2 - p1), array([[0, -1], [1, 0]], dtype=float32)) + b2 = dot((p3 - p2), array([[0, 1], [-1, 0]], dtype=float32)) + + # Params + T = solve(transpose(array([-b1, b2])), a1 - a2) + print T + + # Center + center = a1 + b1 * T[0] + + # Radius + radius = norm(center - p1) + + return center, radius, T[0]