def collecting(herd_a, herd_b, all_sheep, speed, app_dist): # A-最远距离 # B-最大角度 speeds = speed * 1.5 herd_point_a = herd_a.position2point() herd_point_b = herd_b.position2point() g_mean = np.array([np.mean(all_sheep[:, 0]), np.mean(all_sheep[:, 1])]) far_a = common.find_farthest_sheep(all_sheep) gt_dist_a = la.norm(far_a - g_mean) far_b = common.find_max_angle_sheep(herd_point_b, all_sheep) gt_dist_b = la.norm(far_b - g_mean) pc_a = (far_a - g_mean) / gt_dist_a * app_dist + far_a rd_a = (pc_a - herd_point_a) / la.norm(pc_a - herd_point_a) * speeds pc_b = (far_b - g_mean) / gt_dist_b * app_dist + far_b rd_b = (pc_b - herd_point_b) / la.norm(pc_a - herd_point_b) * speeds herd_a.x = rd_a[0] herd_a.y = rd_a[1] herd_b.x = rd_b[0] herd_b.y = rd_b[1] herd_a.draw() herd_b.draw()
def collecting(herd_a, herd_b, all_sheep, speed, app_dist, left, right, target): # A-左半边 最远距离 # B-右半边 方向性最大角度 speeds = speed * 1.5 herd_point_a = herd_a.position2point() herd_point_b = herd_b.position2point() g_mean_a = np.array([np.mean(left[:, 0]), np.mean(left[:, 1])]) g_mean_b = np.array([np.mean(right[:, 0]), np.mean(right[:, 1])]) far_a = common.find_farthest_sheep(left) gt_dist_a = la.norm(far_a - g_mean_a) far_b = common.find_directed_max_angle_sheep(herd_point_b, g_mean_b, all_sheep, target) gt_dist_b = la.norm(far_b - g_mean_b) pc_a = (far_a - g_mean_a) / gt_dist_a * app_dist + far_a rd_a = (pc_a - herd_point_a) / la.norm(pc_a - herd_point_a) * speeds pc_b = (far_b - g_mean_b) / gt_dist_b * app_dist + far_b rd_b = (pc_b - herd_point_b) / la.norm(pc_a - herd_point_b) * speeds herd_a.x = rd_a[0] herd_a.y = rd_a[1] herd_b.x = rd_b[0] herd_b.y = rd_b[1] herd_a.draw() herd_b.draw()
def collecting(herd, all_sheep, speed, app_dist): speeds = speed * 2 herd_point = herd.position2point() g_mean = np.array([np.mean(all_sheep[:, 0]), np.mean(all_sheep[:, 1])]) far_a = common.find_farthest_sheep(all_sheep) gt_dist_a = la.norm(far_a - g_mean) pc_a = (far_a - g_mean) / gt_dist_a * app_dist + far_a rd_a = (pc_a - herd_point) / la.norm(pc_a - herd_point) * speeds herd.x = rd_a[0] herd.y = rd_a[1] herd.draw()
def find(self, all_sheep, target, herd_point): return common.find_farthest_sheep(all_sheep)