def best_path_to(current, p): """ p will be offset to 0.5 due to our use of truncation elsewhere """ p //= 1.0 p += 0.5 trial_angle = int(math.atan2(p.y - current.y, p.x - current.x) * point.RADS_TO_BIN) % 65520 for step in xrange(0, 65520, 1): # print t('trial'), step, trial = current + P.angle((trial_angle - step) % 65520) # print a(trial) if trial // 1 == p: break if get_point(trial, 2) != POINT.NO_GO_MASK: # print m(get_point(trial, 2)) break else: assert False, current return (trial_angle - step) % 65520
def best_path_to(current, p): """ p will be offset to 0.5 due to our use of truncation elsewhere """ p //= 1.0 p += 0.5 trial_angle = int( math.atan2(p.y - current.y, p.x - current.x) * point.RADS_TO_BIN) % 65520 for step in xrange(0, 65520, 1): # print t('trial'), step, trial = current + P.angle((trial_angle - step) % 65520) # print a(trial) if trial // 1 == p: break if get_point(trial, 2) != POINT.NO_GO_MASK: # print m(get_point(trial, 2)) break else: assert False, current return (trial_angle - step) % 65520
def path(surface, runs=10, cutter_diameter=20): d = surface.d get_point = surface.get_point set_point = surface.set_point #cutter radius in 1/1000 of an inch radius = cutter_size / 2 show_path = 1 show_heading = 1 show_scan = 1 show_debug = [] radius_sq = radius**2 initial_scan_yaw = 1 def clockwise_sorted(l): l = list(set(l)) l.sort( key=lambda p: (math.atan2(p.y, p.x) * point.RADS_TO_BIN) % 65520) return l def scanline_sorted(l): l = list(set(l)) l.sort(key=lambda p: (p.y, p.x)) return l def distance_sorted(current, l): l = list(set(l)) l.sort(key=lambda p: math.hypot(p.x - current.x, p.y - current.y)) return l cut_points = [] for angle in range(65520): for inwards in range(2): p = P.angle(angle) * (radius - (inwards / 2.0)) p = P(round(p.x), round(p.y)) cut_points.append(p) offset_points = [P(0, -1), P(-1, 0), P(1, 0), P(0, 1)] offset_points_solid = [ P(0, -1), P(-1, 0), P(1, 0), P(0, 1), P(-1, -1), P(-1, 1), P(1, -1), P(1, 1) ] offset_points_with_center = [P(0, -1), P(-1, 0), P(1, 0), P(0, 1)] no_go_trace_points = [] for cut_point in cut_points: for offset in offset_points_solid: p = cut_point + offset no_go_trace_points.append(p) scanner_points = cut_points + no_go_trace_points for point_list in [cut_points, no_go_trace_points]: point_list[:] = scanline_sorted(point_list) print offset_points_solid for point_list in [offset_points, offset_points_solid, scanner_points]: point_list[:] = clockwise_sorted(point_list) print offset_points_solid # for p in offset_points_solid: # print p, int(math.atan2(p.y, p.x) * point.RADS_TO_BIN % 65520) # return for run in xrange(1, runs + 1): print "Run {}\r".format(run), sys.stdout.flush() for f in [1, 3, 4, 5, 6, 7, 8]: for p in surface: set_point(p, 0x00000000, f) for p in surface: set_point(p, POINT.MATERIAL, 0) set_point(p, POINT.NO_BOUND, 2) offset = P(200, 250) size = (500, 200) tl = P(250, 250) br = P(800, 650) lines = [ (P(600 - radius, 250), P(600, 500)), (P(700 - radius, 250), P(700, 500)), ] print "Rendering path" for y in range(d.y): for x in range(d.x): if y > tl.y and y < br.y: if abs(x - tl.x) < 40 or abs(x - br.x) < 40: set_point(P(x, y), POINT.NO_GO) if x > tl.x and x < br.x: if abs(y - tl.y) < 40 or abs(y - br.y) < 40: set_point(P(x, y), POINT.NO_GO) circle_center = math.hypot(800 - x, 800 - y) if circle_center > 50 and circle_center < 80: set_point(P(x, y), POINT.NO_GO) for p1, p2 in lines: if y >= p1.y and y <= p2.y: if x >= p1.x and x <= p2.x: set_point(P(x, y), POINT.NO_GO) if x == 0 or x == d.x - 1 or y == 0 or y == d.y - 1: set_point(P(x, y), POINT.NO_GO) c = P(200, 800) for r in range(1, 50): for offset in offset_points_with_center: set_point(c + P.angle(0, r) + offset, POINT.NO_GO, 0) set_point(c + P.angle(65520 / 4, r), POINT.NO_GO, 0) print "Copying part rendering" for y in range(d.y): for x in range(d.x): p = P(x, y) if get_point(p) not in [POINT.NO_GO]: continue set_point(p, POINT.NO_GO, 2) print "Filling cutter radius around parts" for y in range(d.y): for x in range(d.x): p = P(x, y) if get_point(p, 0) != POINT.NO_GO: continue # interior points can be ignored re: radius checks for offset in offset_points: if get_point(p + offset, 0) != POINT.NO_GO: break else: continue for cut_point in cut_points: no_go_radius = p + cut_point if get_point(no_go_radius, 2) != POINT.NO_BOUND: continue set_point(no_go_radius, POINT.CANT_REACH, 2) for bound in offset_points: for rad in range(radius - 1): no_go_radius = p + bound * rad if get_point(no_go_radius, 2) != POINT.NO_BOUND: continue set_point(no_go_radius, POINT.CANT_REACH, 2) print "Tracing limit path" for y in range(d.y): for x in range(d.x): p = P(x, y) if get_point(p, 2) != POINT.CANT_REACH: continue for offset in offset_points: edge = p + offset if get_point(edge, 2) != POINT.NO_BOUND: continue set_point(edge, POINT.NO_GO_RADIUS, 2) print "Filling cutter radius around limit path" for y in range(d.y): for x in range(d.x): p = P(x, y) if get_point(p, 2) != POINT.NO_GO_RADIUS: continue for cut_point in cut_points: no_go_radius = p + cut_point if get_point(no_go_radius, 2) != POINT.CANT_REACH: continue set_point(no_go_radius, POINT.NO_GO_MASK, 2) for bound in offset_points: for rad in range(radius - 1): no_go_radius = p + bound * rad if get_point(no_go_radius, 2) != POINT.CANT_REACH: continue break try: def nextprev_radius_point(p, direction): """ direction is only needed to make single-pixel radius "rivers" tractable, to set a bias Note that the next and previous points may be the same (i.e. if the p is a deadend) """ # print (direction-1) / (65520/8) * (65520/8) if p is None: return None # if show_debug: # and math.hypot(current.x-755, current.y-710) > 5: # print t(tick), p, direction # import traceback # traceback.print_stack() # # os._exit(0) # # if p // 1 == P(754, 710): # show_debug.append(1) index = (((direction) / (65520 / 8)) + 1) % 8 no_go_offsets = offset_points_solid[ index:] + offset_points_solid[:index] if show_debug: print print t(tick), "radpoint ", p, a(direction), " " print t(tick), "offset_points_solid" for ii, offset in enumerate(offset_points_solid): print t(ii), offset print t(), "index", index, offset_points_solid[index] print t(tick), "no_go_mask_offsets" for ii, offset in enumerate(no_go_offsets): print t(ii), offset no_go_index = None for _no_go_index, neighbour in enumerate(no_go_offsets): bound = get_point(p + neighbour, 2) if bound == POINT.NO_GO_MASK: no_go_index = _no_go_index if show_debug: print no_go_index break if no_go_index is None: return None if show_debug: print t(), "no_go_index", no_go_index, no_go_offsets[ no_go_index], neighbour next_radius_offsets = (no_go_offsets[no_go_index:] + no_go_offsets[:no_go_index])[::-1] if show_debug: print t(tick), "next radius offsets" for ii, offset in enumerate(next_radius_offsets): print t(), ii, offset for next_radius_index, neighbour in enumerate( next_radius_offsets): bound = get_point(p + neighbour, 2) if show_debug: print 'Neighbour:', neighbour, m(bound) if bound == POINT.NO_GO_RADIUS: if show_debug: print t( ), "next_radius_index", next_radius_index, next_radius_offsets[ next_radius_index], neighbour print "Got it", p + neighbour break else: assert False if show_debug: print t('*****'), a(direction), p print t('search'), index, offset_points_solid[ index], p + offset_points_solid[index] print t('no-go'), no_go_index, offset_points_solid[ no_go_index], p + offset_points_solid[no_go_index] print t(), "next:", neighbour next_radius = p + neighbour next_radius_offsets = next_radius_offsets[::-1] # next_radius_offsets[next_radius_index:] + next_radius_offsets[next_radius_index:] for next_radius_index, neighbour in enumerate( next_radius_offsets): bound = get_point(p + neighbour, 2) if bound == POINT.NO_GO_RADIUS: break else: assert False if show_debug: print t(), "previous:", neighbour previous_radius = p + neighbour return next_radius, previous_radius def direct_path_to(current, p): """ p will be offset to 0.5 due to our use of truncation elsewhere """ p //= 1.0 p += 0.5 return int( math.atan2(p.y - current.y, p.x - current.x) * point.RADS_TO_BIN) % 65520 def best_path_to(current, p): """ p will be offset to 0.5 due to our use of truncation elsewhere """ p //= 1.0 p += 0.5 trial_angle = int( math.atan2(p.y - current.y, p.x - current.x) * point.RADS_TO_BIN) % 65520 for step in xrange(0, 65520, 1): # print t('trial'), step, trial = current + P.angle((trial_angle - step) % 65520) # print a(trial) if trial // 1 == p: break if get_point(trial, 2) != POINT.NO_GO_MASK: # print m(get_point(trial, 2)) break else: assert False, current return (trial_angle - step) % 65520 skip = 0 direction = 0 current = P(400.0, 500.0) last_bound = None current_bound = None feed_step = 1 working = P(0.0, 0.0) old_current = current old_direction = direction jogging_target = None jogging_direction = direction last_on_radius = None last_on_direction = None last_off_radius = None last_off_direction = None prior_last_on = None cuts = 0 no_cuts_for = 0 total_cuts = 0 total_cuts_for_path = 0 total_distance = 0 recent_off_radius = None last_offs = {} last_cuts = {} last_ons = {} tick = 0 timer = 0.00 while True: if show_debug: print tick += 1 skip += 1 scan_angle = direction # scan_angle += initial_scan_yaw if jogging_target is None: last_checked = None for step in xrange(65520): scan_angle = direction + step scan_point = current + P.angle(scan_angle, radius + 1) // 1 if scan_point == last_checked: continue last_checked = scan_point material = get_point(scan_point) if material in [POINT.MATERIAL]: direction = scan_angle break else: scan_angle = direction working = current + P.angle(scan_angle + step, radius + 1) material = get_point(working) last_checked = None if last_checked is not None and material == POINT.MATERIAL: for step in xrange(65520): scan_angle = direction - step scan_point = current + P.angle( scan_angle, radius + 1) material = get_point(scan_point) if show_scan: set_point( current + P.angle(scan_angle, radius), 0x88444444, 3) if material in [POINT.REMOVED]: if show_debug: print t(tick), "scan 1", m(material) direction = scan_angle break else: if show_debug: print t(tick), "Scan fail 1" elif last_checked is not None and current_bound not in [ POINT.NO_GO_RADIUS ]: for step in xrange(65520): # TODO most of these loops can be massively improved by seeking the angle by bisection instead of stepping like this scan_angle = direction - step scan_point = current + P.angle( scan_angle, radius + 1) material = get_point(scan_point) if show_scan: set_point( current + P.angle(scan_angle, radius), 0xffff0000, 3) if material not in [POINT.REMOVED]: direction -= step direction %= 65520 if show_debug: print t(tick), "scan 2", m(material) break # material = get_point(current + P.angle(scan_angle, radius+2)) # if show_scan: # set_point(current + P.angle(scan_angle, radius), 0xffffffff, 3) # time.sleep(0.01) # if material not in [POINT.REMOVED]: # direction -= step # direction %= 65520 # if show_debug: # print t(tick), "scan 2a", m(material) # break else: no_cuts_for = radius * 8 if show_debug or True: print t( tick ), "Scan fail 2", cuts, last_cuts, no_cuts_for, len( last_ons), len(last_offs) if no_cuts_for > radius: print t(tick), "No cut fail 2" no_cuts_for = radius * 8 # TODO We shouldn't get here nearly as often, we're dropping the last cut at intersections with existing material cuts (not at a bound) direction %= 65520 off_node = None on_radius = nextprev_radius_point( current, direct_path_to(old_current, current) if old_current else direction) next_bound = get_point(current + P.angle(direction), 2) # if current.x > 747 and current.x < 780 and current.y > 708 and current.y < 718: # print t(tick), "corner", current, on_radius, a(direction) # timer = 0.05 # # print # # print # # # # print nextprev_radius_point(P(752, 711), 350*65520/360) # # print nextprev_radius_point(P(753, 711), 350*65520/360) # # # # print # # print # # else: # # if timer: # # return # timer = 0 if (on_radius and get_point(on_radius[0], 5)) or ( not on_radius and no_cuts_for > radius): jog_charge = 0 if not last_offs: closest = math.hypot(d.x, d.y) jogging_target = None for y in range(0, d.y, radius / 2): for x in range(0, d.x, radius / 2): proposal = P(x, y) if get_point(proposal, 0) != POINT.MATERIAL: continue if get_point(proposal, 2) != POINT.NO_BOUND: continue distance = math.hypot( current.x - x, current.y - y) if distance <= closest and distance > radius: closest = distance jogging_target = proposal jogging_direction = direct_path_to( current, jogging_target) if not jogging_target: raise NothingToDo print t(tick), "Long jog" if show_debug: print t( ), "jogging3 from {} to {} ({})".format( current, jogging_target, a(jogging_direction)) else: # if not just_set and get_point(on_radius[0], 5) and last_offs and no_cuts_for > radius / 2: targets = distance_sorted(current, last_offs) # targets = last_offs.keys() jogging_target = targets[0] jogging_direction = last_offs.pop( jogging_target, None) set_point(jogging_target, 0xff0000ff, 4) if show_debug: print t( ), "jogging2 from {} to {} ({})".format( current, jogging_target, a(jogging_direction)) recent_off_radius = None if on_radius: # offs = get_point(on_radius[-1], 4) # ons = get_point(on_radius[-1], 5) if show_debug: print t(tick), "on rad", a(direction), current print t( ), "next", on_radius[0], "", "prev", on_radius[-1] print t(), "current bound", m( current_bound), "", "next (trial) bound", m( next_bound) # if get_point(current, 4): # if show_debug: # print t("***"), "current and last:", current, old_current # # we've been at this radius point before, so we can potentially jog # if last_offs and no_cuts_for > 0: # jogging_target = distance_sorted(current, last_offs)[0] # jogging_direction = last_offs.pop(jogging_target) # set_point(jogging_target, 0xff0000ff, 4) # # if show_debug or True: # print t(), "jogging1 from {} to {} ({})".format(current, jogging_target, a(jogging_direction)) if last_bound == POINT.NO_BOUND and not nextprev_radius_point( old_current, old_direction): if show_debug: print t(), "add on rad mark", on_radius[-1] last_ons[on_radius[-1]] = direction set_point(on_radius[-1], 0xffffffff, 5) if next_bound in [ POINT.NO_GO_MASK, POINT.NO_GO_RADIUS ] or last_checked == None: if show_debug: print t('clamping'), current, on_radius[0], a( direction) direction = best_path_to(current, on_radius[0]) if show_debug: print t(), "no go clamp", a(direction) print t(), m( get_point(current + P.angle(direction), 2)) print t( ), "next point", current + P.angle(direction) print t(), "new_angle", P.angle(direction) elif next_bound == POINT.NO_BOUND and not nextprev_radius_point( current + P.angle(direction), direction): last_on = nextprev_radius_point( on_radius[0], direction) last_dir = best_path_to(last_on[-1], last_on[0]) if show_debug: print t(), "add off rad mark" if show_debug: print t(), "from {}: {}".format( current, repr(on_radius)) print t(), " so {}: {}".format( on_radius[-1], repr(last_on)), a(last_dir) off_node = last_on[0], last_dir last_offs[last_on[0]] = last_dir set_point(last_on[0], 0xffccccff, 4) if show_debug: print print t(tick), "loc:", current print t(tick), "dir", P.angle(direction), a(direction) print t(tick), "old:", old_current print t(tick), "oldv", P.angle(old_direction), a( old_direction) print t(tick), "mat", m(current_bound) if jogging_target is None and ( old_current is None or current // 1 != old_current // 1): next_bound = get_point((current) + P.angle(direction), 2) last_cuts = cuts cuts = 0 for cut_point in cut_points: # was = set_point(old_current + cut_point, POINT.REMOVED, 0) was = set_point(current + cut_point, POINT.REMOVED, 0) if was == POINT.NO_GO: assert False elif was == POINT.REMOVED: continue cuts += 1 total_cuts_for_path += cuts total_cuts += cuts if cuts: no_cuts_for = 0 else: no_cuts_for += 1 if show_debug: print t( tick ), "cuts", cuts, "previous", last_cuts, "# since cut", no_cuts_for # if was_jogging or no_cuts_for > 1: # if no_cuts_for > radius: # assert False # if prior_last_on and math.hypot(prior_last_on.x - current.x, prior_last_on.y - current.y) < 1.5: # no_cuts_for = radius * 8 # print t(tick), "ping" # else: # for last_on in last_ons: # if last_on == last_on_radius: # continue # if current_bound != POINT.NO_GO_RADIUS: # continue # if math.hypot(last_on.x - current.x, last_on.y - current.y) < 1.5: # no_cuts_for = radius * 8 # last_ons.remove(last_on) # set_point(last_on_radius, 0xff88880, 4) # # prior_last_on = last_on # break if show_path: if jogging_target: if not skip % 3 or True: set_point( current + P.angle(direction + 65520 / 4, 1), 0x6666ff66, 7) set_point( current + P.angle(direction + 65520 / 4, -1), 0x6666ff66, 7) # for offset in offset_points: # set_point(current + offset + offset, 0xff226622, 7) elif no_cuts_for > 0: set_point(current, 0xcc999999, 1) else: set_point(current, 0xccffffff, 1) direction %= 65520 if show_heading and not skip % 3 and not jogging_target: for r in range(1, int(radius / 2)): set_point(current + P.angle(direction + 65520 / 4, r), 0x88888800, 6) last_bound = current_bound old_current = current old_direction = direction if jogging_target is not None: distance = math.hypot(current.x - jogging_target.x, current.y - jogging_target.y) # timer = 0.00 # if distance < 10 and not timer: # timer = 0.05 # # if distance < 2: if distance > 1: direction = direct_path_to(current, jogging_target) else: current = jogging_target direction = jogging_direction jogging_target = None jogging_direction = None no_cuts_for = 0 cuts = 0 total_cuts_for_path = 0 total_distance += distance old_current = None # no_cuts_for = -radius # direction = jogging_direction # current = jogging_target # old_current = current # cuts = 1 # total_cuts_for_path = 0 # current_bound = get_point(current, 2) vector = P.angle(direction) if current == old_current: #if jogging cause the current location to snap to the destination if show_debug: print t(tick), current, a(direction) current += P.angle(direction) total_distance += 1 if show_debug: print t(tick), current, a(direction) current_bound = get_point(current, 2) next_bound = get_point(current + P.angle(direction), 2) # if current_bound in [POINT.NO_GO_RADIUS]: # last_on_radius = current // 1.0 # if jogging_target is None and current_bound in [POINT.NO_GO_MASK]: # print t(tick), current, jogging_target # print type(direction), type(last_off_direction) # print t(tick), "In forbidden area", last_off_radius, m(current_bound), a(direction) # # assert False if not skip % 10: skip = 0 if timer: time.sleep(timer) except NothingToDo: failed_points = 0 for y in range(d.y): for x in range(d.x): p = P(x, y) if get_point(p) != POINT.MATERIAL: continue if get_point(p, 2) == POINT.CANT_REACH: continue set_point(p, 0xff4444ff) set_point(p, 0x00000000, 1) set_point(p, 0x00000000, 2) failed_points += 1 print " Total cuts: {:>8,.0f}".format(total_cuts) print "Total distance: {:>8,.0f}".format(total_distance) print " Efficiency: {:>8,.0f}%".format( 100 * total_cuts / total_distance / (radius * 2)) if failed_points: print Exception( "Failed to cut {} reachable points".format(failed_points))
def io_keyrelease_right(self): self.cursor_move -= P(1, 0)
def path(surface, runs=10, cutter_diameter=20): d = surface.d get_point = surface.get_point set_point = surface.set_point #cutter radius in 1/1000 of an inch radius = cutter_size / 2 show_path = 1 show_heading = 1 show_scan = 1 show_debug = [] radius_sq = radius ** 2 initial_scan_yaw = 1 def clockwise_sorted(l): l = list(set(l)) l.sort(key=lambda p: (math.atan2(p.y, p.x) * point.RADS_TO_BIN) % 65520) return l def scanline_sorted(l): l = list(set(l)) l.sort(key=lambda p: (p.y, p.x)) return l def distance_sorted(current, l): l = list(set(l)) l.sort(key=lambda p: math.hypot(p.x - current.x, p.y - current.y)) return l cut_points = [] for angle in range(65520): for inwards in range(2): p = P.angle(angle) * (radius - (inwards / 2.0)) p = P(round(p.x), round(p.y)) cut_points.append(p) offset_points = [P(0, -1), P(-1, 0), P(1, 0), P(0, 1)] offset_points_solid = [P(0, -1), P(-1, 0), P(1, 0), P(0, 1), P(-1, -1), P(-1, 1), P(1, -1), P(1, 1)] offset_points_with_center = [P(0, -1), P(-1, 0), P(1, 0), P(0, 1)] no_go_trace_points = [] for cut_point in cut_points: for offset in offset_points_solid: p = cut_point + offset no_go_trace_points.append(p) scanner_points = cut_points + no_go_trace_points for point_list in [cut_points, no_go_trace_points]: point_list[:] = scanline_sorted(point_list) print offset_points_solid for point_list in [offset_points, offset_points_solid, scanner_points]: point_list[:] = clockwise_sorted(point_list) print offset_points_solid # for p in offset_points_solid: # print p, int(math.atan2(p.y, p.x) * point.RADS_TO_BIN % 65520) # return for run in xrange(1, runs+1): print "Run {}\r".format(run), sys.stdout.flush() for f in [1, 3, 4, 5, 6, 7, 8]: for p in surface: set_point(p, 0x00000000, f) for p in surface: set_point(p, POINT.MATERIAL, 0) set_point(p, POINT.NO_BOUND, 2) offset = P(200, 250) size = (500, 200) tl = P(250, 250) br = P(800, 650) lines = [ (P(600-radius, 250), P(600, 500)), (P(700-radius, 250), P(700, 500)), ] print "Rendering path" for y in range(d.y): for x in range(d.x): if y > tl.y and y < br.y: if abs(x-tl.x) < 40 or abs(x-br.x) < 40: set_point(P(x, y), POINT.NO_GO) if x > tl.x and x < br.x: if abs(y-tl.y) < 40 or abs(y-br.y) < 40: set_point(P(x, y), POINT.NO_GO) circle_center = math.hypot(800-x, 800-y) if circle_center > 50 and circle_center < 80: set_point(P(x, y), POINT.NO_GO) for p1, p2 in lines: if y >= p1.y and y <= p2.y: if x >= p1.x and x <= p2.x: set_point(P(x, y), POINT.NO_GO) if x == 0 or x == d.x - 1 or y == 0 or y == d.y - 1: set_point(P(x, y), POINT.NO_GO) c = P(200, 800) for r in range(1, 50): for offset in offset_points_with_center: set_point(c + P.angle(0, r) + offset, POINT.NO_GO, 0) set_point(c + P.angle(65520/4, r), POINT.NO_GO, 0) print "Copying part rendering" for y in range(d.y): for x in range(d.x): p = P(x, y) if get_point(p) not in [POINT.NO_GO]: continue set_point(p, POINT.NO_GO, 2) print "Filling cutter radius around parts" for y in range(d.y): for x in range(d.x): p = P(x, y) if get_point(p, 0) != POINT.NO_GO: continue # interior points can be ignored re: radius checks for offset in offset_points: if get_point(p+offset, 0) != POINT.NO_GO: break else: continue for cut_point in cut_points: no_go_radius = p + cut_point if get_point(no_go_radius, 2) != POINT.NO_BOUND: continue set_point(no_go_radius, POINT.CANT_REACH, 2) for bound in offset_points: for rad in range(radius-1): no_go_radius = p + bound * rad if get_point(no_go_radius, 2) != POINT.NO_BOUND: continue set_point(no_go_radius, POINT.CANT_REACH, 2) print "Tracing limit path" for y in range(d.y): for x in range(d.x): p = P(x, y) if get_point(p, 2) != POINT.CANT_REACH: continue for offset in offset_points: edge = p + offset if get_point(edge, 2) != POINT.NO_BOUND: continue set_point(edge, POINT.NO_GO_RADIUS, 2) print "Filling cutter radius around limit path" for y in range(d.y): for x in range(d.x): p = P(x, y) if get_point(p, 2) != POINT.NO_GO_RADIUS: continue for cut_point in cut_points: no_go_radius = p + cut_point if get_point(no_go_radius, 2) != POINT.CANT_REACH: continue set_point(no_go_radius, POINT.NO_GO_MASK, 2) for bound in offset_points: for rad in range(radius-1): no_go_radius = p + bound * rad if get_point(no_go_radius, 2) != POINT.CANT_REACH: continue break try: def nextprev_radius_point(p, direction): """ direction is only needed to make single-pixel radius "rivers" tractable, to set a bias Note that the next and previous points may be the same (i.e. if the p is a deadend) """ # print (direction-1) / (65520/8) * (65520/8) if p is None: return None # if show_debug: # and math.hypot(current.x-755, current.y-710) > 5: # print t(tick), p, direction # import traceback # traceback.print_stack() # # os._exit(0) # # if p // 1 == P(754, 710): # show_debug.append(1) index = (((direction) / (65520/8))+1) % 8 no_go_offsets = offset_points_solid[index:] + offset_points_solid[:index] if show_debug: print print t(tick), "radpoint ", p, a(direction), " " print t(tick), "offset_points_solid" for ii, offset in enumerate(offset_points_solid): print t(ii), offset print t(), "index", index, offset_points_solid[index] print t(tick), "no_go_mask_offsets" for ii, offset in enumerate(no_go_offsets): print t(ii), offset no_go_index = None for _no_go_index, neighbour in enumerate(no_go_offsets): bound = get_point(p + neighbour, 2) if bound == POINT.NO_GO_MASK: no_go_index = _no_go_index if show_debug: print no_go_index break if no_go_index is None: return None if show_debug: print t(), "no_go_index", no_go_index, no_go_offsets[no_go_index], neighbour next_radius_offsets = (no_go_offsets[no_go_index:] + no_go_offsets[:no_go_index])[::-1] if show_debug: print t(tick), "next radius offsets" for ii, offset in enumerate(next_radius_offsets): print t(), ii, offset for next_radius_index, neighbour in enumerate(next_radius_offsets): bound = get_point(p + neighbour, 2) if show_debug: print 'Neighbour:', neighbour, m(bound) if bound == POINT.NO_GO_RADIUS: if show_debug: print t(), "next_radius_index", next_radius_index, next_radius_offsets[next_radius_index], neighbour print "Got it", p + neighbour break else: assert False if show_debug: print t('*****'), a(direction), p print t('search'), index, offset_points_solid[index], p + offset_points_solid[index] print t('no-go'), no_go_index, offset_points_solid[no_go_index], p + offset_points_solid[no_go_index] print t(), "next:", neighbour next_radius = p + neighbour next_radius_offsets = next_radius_offsets[::-1] # next_radius_offsets[next_radius_index:] + next_radius_offsets[next_radius_index:] for next_radius_index, neighbour in enumerate(next_radius_offsets): bound = get_point(p + neighbour, 2) if bound == POINT.NO_GO_RADIUS: break else: assert False if show_debug: print t(), "previous:", neighbour previous_radius = p + neighbour return next_radius, previous_radius def direct_path_to(current, p): """ p will be offset to 0.5 due to our use of truncation elsewhere """ p //= 1.0 p += 0.5 return int(math.atan2(p.y - current.y, p.x - current.x) * point.RADS_TO_BIN) % 65520 def best_path_to(current, p): """ p will be offset to 0.5 due to our use of truncation elsewhere """ p //= 1.0 p += 0.5 trial_angle = int(math.atan2(p.y - current.y, p.x - current.x) * point.RADS_TO_BIN) % 65520 for step in xrange(0, 65520, 1): # print t('trial'), step, trial = current + P.angle((trial_angle - step) % 65520) # print a(trial) if trial // 1 == p: break if get_point(trial, 2) != POINT.NO_GO_MASK: # print m(get_point(trial, 2)) break else: assert False, current return (trial_angle - step) % 65520 skip = 0 direction = 0 current = P(400.0, 500.0) last_bound = None current_bound = None feed_step = 1 working = P(0.0, 0.0) old_current = current old_direction = direction jogging_target = None jogging_direction = direction last_on_radius = None last_on_direction = None last_off_radius = None last_off_direction = None prior_last_on = None cuts = 0 no_cuts_for = 0 total_cuts = 0 total_cuts_for_path = 0 total_distance = 0 recent_off_radius = None last_offs = {} last_cuts = {} last_ons = {} tick = 0 timer = 0.00 while True: if show_debug: print tick += 1 skip += 1 scan_angle = direction # scan_angle += initial_scan_yaw if jogging_target is None: last_checked = None for step in xrange(65520): scan_angle = direction + step scan_point = current + P.angle(scan_angle, radius+1) // 1 if scan_point == last_checked: continue last_checked = scan_point material = get_point(scan_point) if material in [POINT.MATERIAL]: direction = scan_angle break else: scan_angle = direction working = current + P.angle(scan_angle + step, radius + 1) material = get_point(working) last_checked = None if last_checked is not None and material == POINT.MATERIAL: for step in xrange(65520): scan_angle = direction - step scan_point = current + P.angle(scan_angle, radius+1) material = get_point(scan_point) if show_scan: set_point(current + P.angle(scan_angle, radius), 0x88444444, 3) if material in [POINT.REMOVED]: if show_debug: print t(tick), "scan 1", m(material) direction = scan_angle break else: if show_debug: print t(tick), "Scan fail 1" elif last_checked is not None and current_bound not in [POINT.NO_GO_RADIUS]: for step in xrange(65520): # TODO most of these loops can be massively improved by seeking the angle by bisection instead of stepping like this scan_angle = direction - step scan_point = current + P.angle(scan_angle, radius+1) material = get_point(scan_point) if show_scan: set_point(current + P.angle(scan_angle, radius), 0xffff0000, 3) if material not in [POINT.REMOVED]: direction -= step direction %= 65520 if show_debug: print t(tick), "scan 2", m(material) break # material = get_point(current + P.angle(scan_angle, radius+2)) # if show_scan: # set_point(current + P.angle(scan_angle, radius), 0xffffffff, 3) # time.sleep(0.01) # if material not in [POINT.REMOVED]: # direction -= step # direction %= 65520 # if show_debug: # print t(tick), "scan 2a", m(material) # break else: no_cuts_for = radius * 8 if show_debug or True: print t(tick), "Scan fail 2", cuts, last_cuts, no_cuts_for, len(last_ons), len(last_offs) if no_cuts_for > radius: print t(tick), "No cut fail 2" no_cuts_for = radius * 8 # TODO We shouldn't get here nearly as often, we're dropping the last cut at intersections with existing material cuts (not at a bound) direction %= 65520 off_node = None on_radius = nextprev_radius_point(current, direct_path_to(old_current, current) if old_current else direction) next_bound = get_point(current + P.angle(direction), 2) # if current.x > 747 and current.x < 780 and current.y > 708 and current.y < 718: # print t(tick), "corner", current, on_radius, a(direction) # timer = 0.05 # # print # # print # # # # print nextprev_radius_point(P(752, 711), 350*65520/360) # # print nextprev_radius_point(P(753, 711), 350*65520/360) # # # # print # # print # # else: # # if timer: # # return # timer = 0 if (on_radius and get_point(on_radius[0], 5)) or (not on_radius and no_cuts_for > radius): jog_charge = 0 if not last_offs: closest = math.hypot(d.x, d.y) jogging_target = None for y in range(0, d.y, radius / 2): for x in range(0, d.x, radius / 2): proposal = P(x, y) if get_point(proposal, 0) != POINT.MATERIAL: continue if get_point(proposal, 2) != POINT.NO_BOUND: continue distance = math.hypot(current.x-x, current.y-y) if distance <= closest and distance > radius: closest = distance jogging_target = proposal jogging_direction = direct_path_to(current, jogging_target) if not jogging_target: raise NothingToDo print t(tick), "Long jog" if show_debug: print t(), "jogging3 from {} to {} ({})".format(current, jogging_target, a(jogging_direction)) else: # if not just_set and get_point(on_radius[0], 5) and last_offs and no_cuts_for > radius / 2: targets = distance_sorted(current, last_offs) # targets = last_offs.keys() jogging_target = targets[0] jogging_direction = last_offs.pop(jogging_target, None) set_point(jogging_target, 0xff0000ff, 4) if show_debug: print t(), "jogging2 from {} to {} ({})".format(current, jogging_target, a(jogging_direction)) recent_off_radius = None if on_radius: # offs = get_point(on_radius[-1], 4) # ons = get_point(on_radius[-1], 5) if show_debug: print t(tick), "on rad", a(direction), current print t(), "next", on_radius[0], "", "prev", on_radius[-1] print t(), "current bound", m(current_bound), "", "next (trial) bound", m(next_bound) # if get_point(current, 4): # if show_debug: # print t("***"), "current and last:", current, old_current # # we've been at this radius point before, so we can potentially jog # if last_offs and no_cuts_for > 0: # jogging_target = distance_sorted(current, last_offs)[0] # jogging_direction = last_offs.pop(jogging_target) # set_point(jogging_target, 0xff0000ff, 4) # # if show_debug or True: # print t(), "jogging1 from {} to {} ({})".format(current, jogging_target, a(jogging_direction)) if last_bound == POINT.NO_BOUND and not nextprev_radius_point(old_current, old_direction): if show_debug: print t(), "add on rad mark", on_radius[-1] last_ons[on_radius[-1]] = direction set_point(on_radius[-1], 0xffffffff, 5) if next_bound in [POINT.NO_GO_MASK, POINT.NO_GO_RADIUS] or last_checked == None: if show_debug: print t('clamping'), current, on_radius[0], a(direction) direction = best_path_to(current, on_radius[0]) if show_debug: print t(), "no go clamp", a(direction) print t(), m(get_point(current + P.angle(direction), 2)) print t(), "next point", current + P.angle(direction) print t(), "new_angle", P.angle(direction) elif next_bound == POINT.NO_BOUND and not nextprev_radius_point(current + P.angle(direction), direction): last_on = nextprev_radius_point(on_radius[0], direction) last_dir = best_path_to(last_on[-1], last_on[0]) if show_debug: print t(), "add off rad mark" if show_debug: print t(), "from {}: {}".format(current, repr(on_radius)) print t(), " so {}: {}".format(on_radius[-1], repr(last_on)), a(last_dir) off_node = last_on[0], last_dir last_offs[last_on[0]] = last_dir set_point(last_on[0], 0xffccccff, 4) if show_debug: print print t(tick), "loc:", current print t(tick), "dir", P.angle(direction), a(direction) print t(tick), "old:", old_current print t(tick), "oldv", P.angle(old_direction), a(old_direction) print t(tick), "mat", m(current_bound) if jogging_target is None and (old_current is None or current // 1 != old_current // 1): next_bound = get_point((current) + P.angle(direction), 2) last_cuts = cuts cuts = 0 for cut_point in cut_points: # was = set_point(old_current + cut_point, POINT.REMOVED, 0) was = set_point(current + cut_point, POINT.REMOVED, 0) if was == POINT.NO_GO: assert False elif was == POINT.REMOVED: continue cuts += 1 total_cuts_for_path += cuts total_cuts += cuts if cuts: no_cuts_for = 0 else: no_cuts_for += 1 if show_debug: print t(tick), "cuts", cuts, "previous", last_cuts, "# since cut", no_cuts_for # if was_jogging or no_cuts_for > 1: # if no_cuts_for > radius: # assert False # if prior_last_on and math.hypot(prior_last_on.x - current.x, prior_last_on.y - current.y) < 1.5: # no_cuts_for = radius * 8 # print t(tick), "ping" # else: # for last_on in last_ons: # if last_on == last_on_radius: # continue # if current_bound != POINT.NO_GO_RADIUS: # continue # if math.hypot(last_on.x - current.x, last_on.y - current.y) < 1.5: # no_cuts_for = radius * 8 # last_ons.remove(last_on) # set_point(last_on_radius, 0xff88880, 4) # # prior_last_on = last_on # break if show_path: if jogging_target: if not skip % 3 or True: set_point(current + P.angle(direction + 65520/4, 1), 0x6666ff66, 7) set_point(current + P.angle(direction + 65520/4, -1), 0x6666ff66, 7) # for offset in offset_points: # set_point(current + offset + offset, 0xff226622, 7) elif no_cuts_for > 0: set_point(current, 0xcc999999, 1) else: set_point(current, 0xccffffff, 1) direction %= 65520 if show_heading and not skip % 3 and not jogging_target: for r in range(1, int(radius / 2)): set_point(current + P.angle(direction + 65520/4, r), 0x88888800, 6) last_bound = current_bound old_current = current old_direction = direction if jogging_target is not None: distance = math.hypot(current.x - jogging_target.x, current.y - jogging_target.y) # timer = 0.00 # if distance < 10 and not timer: # timer = 0.05 # # if distance < 2: if distance > 1: direction = direct_path_to(current, jogging_target) else: current = jogging_target direction = jogging_direction jogging_target = None jogging_direction = None no_cuts_for = 0 cuts = 0 total_cuts_for_path = 0 total_distance += distance old_current = None # no_cuts_for = -radius # direction = jogging_direction # current = jogging_target # old_current = current # cuts = 1 # total_cuts_for_path = 0 # current_bound = get_point(current, 2) vector = P.angle(direction) if current == old_current: #if jogging cause the current location to snap to the destination if show_debug: print t(tick), current, a(direction) current += P.angle(direction) total_distance += 1 if show_debug: print t(tick), current, a(direction) current_bound = get_point(current, 2) next_bound = get_point(current + P.angle(direction), 2) # if current_bound in [POINT.NO_GO_RADIUS]: # last_on_radius = current // 1.0 # if jogging_target is None and current_bound in [POINT.NO_GO_MASK]: # print t(tick), current, jogging_target # print type(direction), type(last_off_direction) # print t(tick), "In forbidden area", last_off_radius, m(current_bound), a(direction) # # assert False if not skip % 10: skip = 0 if timer: time.sleep(timer) except NothingToDo: failed_points = 0 for y in range(d.y): for x in range(d.x): p = P(x, y) if get_point(p) != POINT.MATERIAL: continue if get_point(p, 2) == POINT.CANT_REACH: continue set_point(p, 0xff4444ff) set_point(p, 0x00000000, 1) set_point(p, 0x00000000, 2) failed_points += 1 print " Total cuts: {:>8,.0f}".format(total_cuts) print "Total distance: {:>8,.0f}".format(total_distance) print " Efficiency: {:>8,.0f}%".format(100*total_cuts / total_distance / (radius * 2)) if failed_points: print Exception("Failed to cut {} reachable points".format(failed_points))
def io_keyrelease_down(self): self.cursor_move -= P(0, 1)
def io_keyrelease_left(self): self.cursor_move -= P(-1, 0)
def io_key_right(self): self.cursor_move += P(1, 0)
def io_keyrelease_up(self): self.cursor_move -= P(0, -1)
def io_key_left(self): self.cursor_move += P(-1, 0)
def io_key_down(self): self.cursor_move += P(0, 1)
def io_key_up(self): self.cursor_move += P(0, -1)
def io_mousedown(self, event): if event.button == sdl.BUTTON_LEFT: self.cursor = P(event.x, event.y)
def __init__(self): self.frame = 0 self.surface = point.Surface() d = self.d = self.surface.d self.data = self.surface.data self.keys_down = set() self.cursor = P(0, 0) self.cursor_move = P(0, 0) self.cursor_surface = 2 self.show_material = True self.show_bounds = True self.show_paths = True self.show_jogs = True self.show_headings = True self.show_scans = True self.show_path_meets_bound = True self.show_path_leaves_bound = True sdl.init(sdl.INIT_EVERYTHING) sdl.ttf.init() sdl.setHint("render_scale_quality", "linear") self.window = sdl.createWindow("Test", sdl.WINDOWPOS_UNDEFINED, sdl.WINDOWPOS_UNDEFINED, d.x, d.y, sdl.WINDOW_RESIZABLE) self.render = self.window.createRenderer(-1, sdl.RENDERER_ACCELERATED) # self.render = self.window.createRenderer(-1, sdl.RENDERER_TARGETTEXTURE) # self.frame_texture = self.render.createTexture(sdl.PIXELFORMAT_ARGB8888, sdl.TEXTUREACCESS_TARGET, d.x, d.y) # self.overlay_texture = self.render.createTexture(sdl.PIXELFORMAT_ARGB8888, sdl.TEXTUREACCESS_TARGET, d.x, d.y) self.animation_textures = [] for frame in range(9): texture = self.render.createTexture(sdl.PIXELFORMAT_ARGB8888, sdl.TEXTUREACCESS_STATIC, d.x, d.y) self.animation_textures.append(texture) self.animation_textures[0].setTextureBlendMode(sdl.BLENDMODE_NONE) self.animation_textures[1].setTextureBlendMode(sdl.BLENDMODE_BLEND) self.animation_textures[2].setTextureBlendMode(sdl.BLENDMODE_BLEND) self.animation_textures[3].setTextureBlendMode(sdl.BLENDMODE_BLEND) self.animation_textures[4].setTextureBlendMode(sdl.BLENDMODE_BLEND) self.animation_textures[5].setTextureBlendMode(sdl.BLENDMODE_BLEND) self.animation_textures[6].setTextureBlendMode(sdl.BLENDMODE_BLEND) self.animation_textures[7].setTextureBlendMode(sdl.BLENDMODE_BLEND) self.animation_textures[8].setTextureBlendMode(sdl.BLENDMODE_BLEND) self.render.setRenderTarget(sdl.ffi.NULL) self._io_event = sdl.Event() self.event_map = { sdl.KEYDOWN: (self.io_keydown, lambda e: e.key), sdl.KEYUP: (self.io_keyup, lambda e: e.key), sdl.MOUSEMOTION: (self.io_mousemotion, lambda e: e.motion), sdl.MOUSEBUTTONDOWN: (self.io_mousedown, lambda e: e.button), sdl.MOUSEBUTTONUP: (self.io_mouseup, lambda e: e.button), sdl.MOUSEWHEEL: (self.io_mousewheel, lambda e: e.wheel), sdl.QUIT: (self.io_quit, lambda e: e.quit), sdl.SYSWMEVENT: (self.io_syswm, lambda e: e.syswm), sdl.WINDOWEVENT: (self.io_window, lambda e: e.window), }