def calc_rotation(self, pnt): """ Calc the rotation of the triangle with another point""" triangle_line = distance(self.center, self.top) point_line = distance(self.center, pnt) aux_line = distance(pnt, self.top) if point_line == 0 or triangle_line == 0: return 0 value = math.acos((point_line**2 - aux_line**2 + triangle_line**2) / (2 * point_line * triangle_line)) * 180 / math.pi m = slope(self.top, pnt) b = offset_rect(self.top, m) dx, dy = delta_x_y(m, b, self.center) orientation = up_or_down(dx, dy, self.center, m) return value * is_clockwise(m, orientation, self.top, pnt) * -1
def calc_rotation(self, pnt): self.point = """ Calc the rotation of the triangle with another point""" center = self.pos top = (self.pos[0], self.pos[1] + self.separation) triangle_line = distance(self.pos, top) point_line = distance(self.pos, pnt) aux_line = distance(pnt, top) if point_line == 0 or triangle_line == 0: return 0 value = math.acos((point_line**2 - aux_line**2 + triangle_line**2) / (2 * point_line * triangle_line)) * 180 / math.pi m = slope(top, pnt) b = offset_rect(top, m) dx, dy = delta_x_y(m, b, center) orientation = up_or_down(dx, dy, center, m) return value * is_clockwise(m, orientation, top, pnt) * -1
def calc_top_center(self): """ Return the center and the top point of the triangle """ distance1 = distance(self.contours[0][0], self.contours[1][0]) distance2 = distance(self.contours[1][0], self.contours[2][0]) distance3 = distance(self.contours[0][0], self.contours[2][0]) minor = min(distance1, distance2, distance3) center = top = (-1, -1) if minor == distance1: top = self.contours[2][0] center = between_pt(self.contours[0][0], self.contours[1][0]) elif minor == distance2: top = self.contours[0][0] center = between_pt(self.contours[2][0], self.contours[1][0]) elif minor == distance3: top = self.contours[1][0] center = between_pt(self.contours[0][0], self.contours[2][0]) top = (top[0], top[1]) return top, center
def get_close_unknown_position(self, pos): if self.new_pos[0] == -1 or distance(pos, self.new_pos) < 30: new_pos = (-1, -1) min_dist = 9999 contours, _ = cv2.findContours( cv2.cvtColor(self.area, cv2.COLOR_BGR2GRAY), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_TC89_L1) for cnt in contours: area = cv2.contourArea(cnt) approx = cv2.convexHull(cnt) max_rand = len(cnt) - 1 rand = random.randint(0, max_rand) x = cnt[rand][0][0] y = cnt[rand][0][1] dist = min(distance(pos, (x, y)), min_dist) if dist < min_dist: new_pos = (x, y) min_dist = dist self.new_pos = new_pos return self.new_pos
def update(self, status, boxes, agent, ants: List): ''' update status of the claw ''' # closed if (self.status == b'\x02') and (status == b'\x01'): self.status = b'\x01' self.separation -= 1 return boxes elif self.status == b'\x01': if self.separation > 10: self.separation -= 1 for box in boxes: distance_to_box = distance(box.pos, self.pos) if distance_to_box < 80 or (box.leader is not None and distance_to_box < 120): self.status = b'\x04' self.box_id = box.id agent.send_box_data(box) agent.con.writeCharacteristic(agent.chars.claw, b'\x04', withResponse=True) agent.destination = (50, 50) agent.con.writeCharacteristic(agent.chars.com, b'\x3422', withResponse=True) if box.leader is None: box.leader = self.agent else: self.leader = box.leader for ant in ants: if ant.color == box.leader: ant.con.writeCharacteristic(ant.chars.claw, b'\x06', withResponse=True) self.leader_pos = ant.triangle.center break elif distance_to_box < 160: agent.destination = box.pos agent.box_found = True return boxes if self.status == b'\x04': boxes = self.update_box_pos(boxes) return boxes