def match(self, x, y, w, h, timeout=15): def rect_in_rect(cap): (xc1, yc1), im = cap hc, wc = im.shape[:2] xr1, yr1 = x, y xr2, yr2 = x + w, y + h xc2, yc2 = xc1 + wc, yc1 + hc return (xc1 <= xr1 <= xr2 <= xc2 and yc1 <= yr1 <= yr2 <= yc2) if self.pos is None: self.capture(x, y) matches = [cap for cap in self.caps if rect_in_rect(cap)] # !!! alpha handicap !!! # return matches start = time.time() while not len(matches) and (time.time() - start) < timeout: # calculate suitable trajectory to target ww, hh = self.dims ori = Vector(*self.pos) dst = Vector(x, y) - Vector(ww / 2, hh / 2) vec = dst - ori δ = vec.normalise() * (ww / 2) xx, yy = ori + (vec if vec.r() < δ.r() else δ) print(ori, dst, (x, y), δ) new = self.capture(xx, yy) if new is None: break elif rect_in_rect(new): matches.append(new) return matches
def draw(self, canvas): end = Vector(self.direction.x * 1000, self.direction.y * 1000) if self.hit is not None: end = Vector(self.hit.x, self.hit.y) line = Line(self.start, end) line.set_color(self.color) line.draw(canvas)
def polygon(self): """return a polygon object""" a = self.origin b = a + Vector.from_polar(self.width, self.angle) c = b + Vector.from_polar(self.height, self.angle + 0.5*pi) d = a + Vector.from_polar(self.height, self.angle + 0.5*pi) return Polygon(a, b, c, d)
def polygon(self): """return a polygon object""" a = self.origin b = a + Vector.from_polar(self.width, self.angle) c = b + Vector.from_polar(self.height, self.angle + 0.5 * pi) d = a + Vector.from_polar(self.height, self.angle + 0.5 * pi) return Polygon(a, b, c, d)
def cast_rays(self, polylines): self.rays = [] angle = 2 * math.pi / self.num_rays direction = Vector(0, -1) for i in range(self.num_rays - 1): x = direction.x * math.cos(angle) - direction.y * math.sin(angle) y = direction.x * math.sin(angle) + direction.y * math.cos(angle) direction = Vector(x, y) ray = Ray(self.position, direction) self.rays.append(ray) ray.cast(polylines)
def inbounds(self, im): boundc = cv2.perspectiveTransform(im['bounds'], im['h']) bounds = HomographyTest.cv2vec(boundc) xmin, xmax = sorted([x for x,_ in bounds])[1:3] ymin, ymax = sorted([y for _,y in bounds])[1:3] inbs = [Vector(xmin, ymin), Vector(xmax, ymin), Vector(xmax, ymax), Vector(xmin, ymax)] poly = Polygon(*bounds) if not all(poly.contains(inb) for inb in inbs): inbs = None return inbs, (int(xmax-xmin), int(ymax-ymin))
def mutate(self): childparams, childpoly = self.params, self.poly if random.random() < 1 / 2: childparams = [ round(random.gauss(1, 0.1) * p, 4) for p in self.params ] else: if len(self.poly.exterior.coords) < 5 or ( len(self.poly.exterior.coords) < 15 and random.random() < 1 / 2): #add a point #find the edge of the polygon nearest the new point and add the point there mindist = float('inf') pt_new = Vector( [round(random.uniform(-1 / 3, 1), 3) for i in range(2)]) for i in range(-1, len(self.poly.exterior.coords) - 2): pt_left, pt_center, pt_right = [ Vector(self.poly.exterior.coords[1:][i + k]) for k in (-1, 0, 1) ] v_left, v_right, v_new = [ pt - pt_center for pt in (pt_left, pt_right, pt_new) ] if (v_left.unit() + v_right.unit()).norm() > 10**( -4 ): #if the vectors are pointed nearly precisely opposite each other, ignore them if v_new.dot(v_right) > 0 and (pt_new - pt_right).dot( pt_center - pt_right) > 0: dist = v_new.reject(v_right).norm() edge = i + 1 else: dist = v_new.norm() edge = i + bool( v_new.reject(v_left.unit() + v_right.unit()).dot(v_right) > 0) if dist < mindist: mindist = dist nearedge = edge childpoly = Polygon(self.poly.exterior.coords[:nearedge + 1] + [pt_new.val] + self.poly.exterior.coords[nearedge + 1:-1]) else: #remove a point pt = random.randint(0, len(self.poly.exterior.coords) - 2) childpoly = Polygon(self.poly.exterior.coords[:pt] + self.poly.exterior.coords[pt + 1:-1]) while not (childpoly.is_valid and childpoly.is_simple): pt = random.randint(0, len(self.poly.exterior.coords) - 2) childpoly = Polygon(self.poly.exterior.coords[:pt] + self.poly.exterior.coords[pt + 1:-1]) return self.__class__(childparams, childpoly, self.wls)
def generate_rects(self): """generate a list of rectangles of the cameras size covering the given polygon; the rectangles will intersect by the specified stitching amount TODO: eliminate any rectangles which are not in the given polygon""" _, camera = self.backend prec = camera.precision() stitch = self.min_stitch * prec width, height = camera.resolution() width *= prec height *= prec wid = width - 2 * stitch hei = height - 2 * stitch bounding = self.polygon.rectangle(camera.orientation()) o = bounding.origin cols = int(ceil(bounding.width / wid)) rows = int(ceil(bounding.height / hei)) return [ i for s in [[( (i, j), Rectangle(o + Vector(j * wid - stitch, i * hei - stitch), 0, width, height), None) for j in (reversed(range(cols)) if i % 2 else range(cols))] for i in range(rows)] for i in s ]
def calsu(self, contact: vec.Vector, normal: vec.Vector): #distance from center of mass to the collision contact rP = contact - self.position #vertex velocity before the collision velocity = self.velocity + (vec.Vector(0, 0, self.angular).cross(rP)) e = 0.8 #maybe this could be parameter impulse = -(e + 1) * (velocity.dot(normal) / ( 1/self.mass + (rP.cross(normal).magnitude()**2)/self.inertia )) self.velocity += normal.scale(impulse/self.mass) self.angular += rP.cross(normal).scale(impulse/self.inertia).z
def status(self): """return information on: * calibration * idle? * position * attached """ return Status(ready=False, idle=False, calibrated=False, position=Vector(0, 0), attached=False)
class Car: def __init__(self, position): self.position = Vector(position.x, position.y) self.speed = 20 self.num_rays = 20 self.rays = [] def set_num_rays(self, amount): self.num_rays = amount def move(self, direction): direction = direction.normalized() self.position.x += direction.x * self.speed self.position.y += direction.y * self.speed def cast_rays(self, polylines): self.rays = [] angle = 2 * math.pi / self.num_rays direction = Vector(0, -1) for i in range(self.num_rays - 1): x = direction.x * math.cos(angle) - direction.y * math.sin(angle) y = direction.x * math.sin(angle) + direction.y * math.cos(angle) direction = Vector(x, y) ray = Ray(self.position, direction) self.rays.append(ray) ray.cast(polylines) def draw(self, canvas): self.position.set_radius(5) self.position.set_color("#FF0000") self.position.draw(canvas) for ray in self.rays: ray.draw(canvas) canvas.update_idletasks() canvas.update()
def read_map(path): polylines = [] with open(path) as json_file: json_data = json.load(json_file) for line in json_data['polylines'].split('\n'): if line == '': break pl = Polyline([]) coords_str = line.split(sep=':') coords_n = [] for c in coords_str: xy = c.split(sep=',') coords_n.append([int(xy[0]), int(xy[1])]) for i in range(len(coords_n) - 1): pl.add_line( Line(Vector(coords_n[i][0], coords_n[i][1]), Vector(coords_n[i + 1][0], coords_n[i + 1][1]))) polylines.append(pl) return Map(Car(Vector(json_data['start'][0], json_data['start'][1])), polylines)
def status(self): """return information on: * ready? * calibration * idle? * position * what head is attached """ return Status(ready=False, idle=False, calibrated=False, position=Vector(0, 0), attachment=None)
def cast(self, polylines): min_dist = float('inf') self.hit = None for pl in polylines: for line in pl.lines: x1 = line.start.x y1 = line.start.y x2 = line.end.x y2 = line.end.y x3 = self.start.x y3 = self.start.y x4 = self.direction.x * 1000 y4 = self.direction.y * 1000 den = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4) if den == 0: continue t = ((x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)) / den u = -((x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3)) / den if 0 < t < 1 and u > 0: point = Vector(0, 0) point.x = x1 + t * (x2 - x1) point.y = y1 + t * (y2 - y1) if self.hit is None: self.hit = point min_dist = self.start.dist(point) else: dist = self.start.dist(point) if dist < min_dist: min_dist = dist self.hit = point return self.hit
def rectangle(self, angle=0): """return the smallest bounding rectangle angle - the orientation of the rectangle, given in anticlockwise radians""" points = [point.rotate(angle) for point in self.points] x_max = max(x for x, _ in points) x_min = min(x for x, _ in points) y_max = max(y for _, y in points) y_min = min(y for _, y in points) origin = Vector(x_min, y_min).rotate(-angle) width = x_max - x_min height = y_max - y_min return Rectangle(origin, -angle, width, height)
def render_dir_deri(): vector_input = request.args.get('vector') vec_list = filter(None, re.split(r"[\s,]", vector_input)) to_real_vector = [] for vec in vec_list: to_real_vector.append(float(vec)) dir_deri = None try: if len(vec_list) > 0 and len(vec_list) == len(f.getVariables()): dir_deri = f.getDirectionalDerivative( Vector(to_real_vector)).toString() elif len(vec_list) > 0 and len(vec_list) != len(f.getVariables()): raise InvalidVectorInput() except InvalidVectorInput as e: dir_deri = str(e) return jsonify(dir_deri=dir_deri)
#!/usr/bin/env from hw.shapeoko import Shapeoko, Axes from lib.vector import Vector from math import pi import time import glob if __name__ == '__main__': """very simple circle drawing test (should instead use G2/G3 to draw arcs); connects to the first serial port it finds, calibrates it and then moves in a full circle with 1deg resolution""" com = glob.glob('/dev/ttyACM*')[0] print(com) stage = Shapeoko(com) time.sleep(2) stage.home([Axes.X, Axes.Y, Axes.Z]) origin = Vector(100, 100) radius = 80 d2r = lambda deg: deg * pi / 180 move = lambda v: stage.move([v.x(), v.y(), None]) move(origin) for deg in range(0, 360, 1): point = origin + Vector.from_polar(radius, d2r(deg)) move(point) stage.close()
def cv2vec(cv): return [Vector(*b[0]) for b in cv]
from hw.shapeoko import Shapeoko, Axes from lib.vector import Vector from math import pi import time import glob if __name__ == '__main__': """very simple circle drawing test (should instead use G2/G3 to draw arcs); connects to the first serial port it finds, calibrates it and then moves in a full circle with 1deg resolution""" com = glob.glob('/dev/ttyACM*')[0] print(com) stage = Shapeoko(com) time.sleep(2) stage.home([Axes.X, Axes.Y, Axes.Z]) origin = Vector(100,100) radius = 80 d2r = lambda deg: deg * pi / 180 move = lambda v: stage.move([v.x(), v.y(), None]) move(origin) for deg in range(0, 360, 1): point = origin + Vector.from_polar(radius, d2r(deg)) move(point) stage.close()
def bounds(self): """return bounding polygon, ideally a rectangle""" from .canvas import Rectangle return Rectangle(Vector(0, 0), Vector(1, 0), 1, 1)
coords_n = [] for c in coords_str: xy = c.split(sep=',') coords_n.append([int(xy[0]), int(xy[1])]) for i in range(len(coords_n) - 1): pl.add_line( Line(Vector(coords_n[i][0], coords_n[i][1]), Vector(coords_n[i + 1][0], coords_n[i + 1][1]))) polylines.append(pl) return Map(Car(Vector(json_data['start'][0], json_data['start'][1])), polylines) if __name__ == '__main__': setup() map = read_map('maps/map_name.json') map.car.set_num_rays(50) map.car.cast_rays(map.polylines) map.draw(canvas) while True: canvas.delete('all') map.car.move( Vector(rd.random() - rd.random(), rd.random() - rd.random())) map.car.cast_rays(map.polylines) map.draw(canvas) time.sleep(.5)
continue shapeoko = XY(dev, bounds) break print(shapeoko.status()) ε = shapeoko.register(XY.Empty()) ν = shapeoko.register(Needle()) shapeoko.head = ε shapeoko.head = ν ws = Workspace(shapeoko) ws.optimise_queue(False) up = 15 down = 0 done = threading.Event() ws.enqueue(ν, [Vector(0, 0)], lambda _: done.set(), { 'down': up, 'up': up }, {}) done.wait() input("Ready? ") while True: ws.enqueue(ν, [Vector(0, 0)], lambda _: None, { 'down': down, 'up': up }, {}) ws.enqueue(ν, [Vector(0.05, 0.05)], lambda _: None, { 'down': down, 'up': up }, {})
def centroid(self): """return the centroid in polygon coordinates""" origin = Vector(0, 0) return sum(self.points, origin) / len(self.points)
def from_bench(self, point): """convert device coordinates to pixels""" x, y = self.rectangle.from_bench(point) return Vector(int(x * self.data.cols / self.rectangle.width), int(y * self.data.rows / self.rectangle.height))
def to_bench(self, coord): """convert pixels to device coordinates""" x, y = coord return self.rectangle.to_bench( Vector(x * self.rectangle.width / self.data.cols, y * self.rectangle.height / self.data.rows))
def __init__(self, position): self.position = Vector(position.x, position.y) self.speed = 20 self.num_rays = 20 self.rays = []
dev = devs[int(choice)] except: continue shapeoko = XY(dev, bounds) break print(shapeoko.status()) ε = shapeoko.register(XY.Empty()) shapeoko.head = ε ws = Workspace(shapeoko) # test queuing ws.optimise_queue(True) ws.pause() imcb = lambda im:im.show() ws.enqueue(ε, [Vector(0,0)], lambda _:print(0), {}, {}) ws.enqueue(ε, [Vector(0.05,0.05)], lambda _:print(1), {}, {}) ws.enqueue(ε, [Vector(0.06,0.05)], lambda _:print(2), {}, {}) ws.enqueue(ε, [Vector(0.05,0.04)], lambda _:print(3), {}, {}) ws.enqueue(ε, [Vector(0.08,0.08)], lambda _:print(4), {}, {}) ws.enqueue(ε, [Vector(0.05,0.05)], lambda _:print(5), {}, {}) ws.enqueue(ε, [Vector(0.02,0.05)], lambda _:print(6), {}, {}) ws.play() time.sleep(2) ws.wait() num = input("How many random coordinates? ") random.seed() uni = random.uniform rcoord = lambda:Vector(uni(0,0.1), uni(0,0.1))
def getNormal(self, edge: vec.Vector) -> vec.Vector: normal = vec.Vector(-edge.y, edge.x, 0).normalize() if edge.cross(normal).z > 0: return normal.scale(-1.0) return normal
continue shapeoko = XY(dev, bounds) break print(shapeoko.status()) ε = shapeoko.register(XY.Empty()) ν = shapeoko.register(Needle()) shapeoko.head = ε shapeoko.head = ν ws = Workspace(shapeoko) ws.optimise_queue(False) down = 0 up = 15 done = threading.Event() ws.enqueue(ν, [Vector(0, 0)], lambda _: done.set(), { 'down': up, 'up': up }, {}) done.wait() input("Ready? ") while True: ws.enqueue(ν, [ Vector(0, 0), Vector(0, 0.05), Vector(0.05, 0.05), Vector(0.05, 0) ], lambda _: None, { 'down': down, 'up': down