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_r_max_angle_sheep(herd_point_a, g_mean, all_sheep,
'clockwise')
gt_dist_a = la.norm(far_a - g_mean)
far_b = common.find_r_max_angle_sheep(herd_point_b, g_mean, all_sheep,
'anticlockwise')
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, all_sheep, speed, app_dist):
speeds = speed * 1.5
herd_point = herd.position2point()
g_mean = np.array([np.mean(all_sheep[:, 0]), np.mean(all_sheep[:, 1])])
far_a = common.find_r_max_angle_sheep(herd_point, g_mean, all_sheep,
'clockwise')
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()