TSP问题，飞行器多阵形模拟——三角形阵形

接上次一字排开阵形，现在介绍三角形阵形的实现

模拟tsp问题问题及实现方法思路参照上一篇博客    http://t.csdnimg.cn/Ky3Fq

      分析三角形阵形与一字排开阵形形状上差距较大，一字排开阵形只需关于路径对称，维持动态垂直就可以实现较为完善的效果，但是三角形由于要一直保持一个角在前的效果，且一个点在路径上，换言之就是组成阵形的点，要保持相对稳定，并需要按照三角形的形状排列红点，在每次移动时根据路径的方向调整它们的位置。

整体思路

先定义了一个列表来存储三个红点相对于三角形中心的位置偏移。然后，我们根据路径的方向调整每个红点的位置。我们使用2D旋转矩阵来旋转每个红点的位置，以便三角形始终与路径方向对齐。

1.定义三角形阵形（通过与中心的偏移构建）

# 定义三角阵形的间距
spacing = 0.02
triangle_offsets = [
    (0, 0),  # 中心
    (spacing, spacing),  # 右上
    (-spacing, spacing)  # 左上
]
red_dots = [plt.plot([], [], 'ro')[0] for _ in triangle_offsets]

2，垂直方向，路径方向（具体计算参照之前博客）

            # 计算路径的方向
            dx = x[j] - (x[j - 1] if j > 0 else cities[tour[i], 0])
            dy = y[j] - (y[j - 1] if j > 0 else cities[tour[i], 1])

            # 计算垂直方向
            norm = np.sqrt(dx * dx + dy * dy)
            dx_perp = -dy / norm
            dy_perp = dx / norm

3，调整红点阵形稳定，顶角的方向指向路径方向

通过循环用于调整红点的位置，(offset_x, offset_y）存储了了三角形的偏移量。
每一次循环中，
1） 通过将偏移量与垂直方向的增量dx_perp和dy_perp进行线性组合，将三角形进行旋转。这是通    过以下公式实现的：

   rotated_x = offset_x * dx_perp - offset_y * dy_perp
   rotated_y = offset_x * dy_perp + offset_y * dx_perp

2） 根据旋转后的坐标计算红点的新位置。红点的位置是通过将旋转后的坐标与中心点的坐标相加   得到的。

                dot_x = center_x + rotated_x
                dot_y = center_y + rotated_y

3）使用新的位置更新红点的数据。这是通过调用set_data方法来实现的，将红点的x坐标设置为[dot_x]，y坐标设置为[dot_y]。通过循环的迭代，将依次调整每个红点的位置，使其与旋转后的三角形匹配。

总体代码

# 调整红点的位置
            for k, (offset_x, offset_y) in enumerate(triangle_offsets):
                # 旋转三角形以匹配路径方向
                rotated_x = offset_x * dx_perp - offset_y * dy_perp
                rotated_y = offset_x * dy_perp + offset_y * dx_perp

                dot_x = center_x + rotated_x
                dot_y = center_y + rotated_y

                red_dots[k].set_data([dot_x], [dot_y])

实现效果展示

 

三角形阵形

总结而言，基本与一字排开阵形大同小异，因为主要思想就是维持阵形与路径同方向变化，不同阵形区别点就是不同阵形的构造不同，运动后怎么保持阵形不散。

整体实现代码，可复制粘贴，直接pycharm运行

import numpy as np
import matplotlib.pyplot as plt
from scipy.optimize import linear_sum_assignment

# 创建随机城市
num_cities = 5
cities = np.random.rand(num_cities, 2)

# 计算城市之间的距离
distances = np.sqrt(((cities[:, np.newaxis, :] - cities[np.newaxis, :, :]) ** 2).sum(axis=2))

# 使用线性分配解决TSP问题
row_ind, col_ind = linear_sum_assignment(distances)

# 添加起始城市到路径
tour = np.append(col_ind, col_ind[0])

# 创建图像和红点三角阵形
fig, ax = plt.subplots()

# 定义三角阵形的间距
spacing = 0.02
triangle_offsets = [
    (0, 0),  # 中心
    (spacing, spacing),  # 右上
    (-spacing, spacing)  # 左上
]
red_dots = [plt.plot([], [], 'ro')[0] for _ in triangle_offsets]

# 绘制初始的蓝色路径
ax.plot(cities[:, 0], cities[:, 1], 'bo')
ax.plot(cities[tour, 0], cities[tour, 1], 'b.-')

# 移动红点三角阵形并更新路径
for i in range(num_cities + 1):
    if i < num_cities:
        x = np.linspace(cities[tour[i], 0], cities[tour[i + 1], 0], 100)
        y = np.linspace(cities[tour[i], 1], cities[tour[i + 1], 1], 100)
        for j in range(100):
            center_x = x[j]
            center_y = y[j]

            # 计算路径的方向
            dx = x[j] - (x[j - 1] if j > 0 else cities[tour[i], 0])
            dy = y[j] - (y[j - 1] if j > 0 else cities[tour[i], 1])

            # 计算垂直方向
            norm = np.sqrt(dx * dx + dy * dy)
            dx_perp = -dy / norm
            dy_perp = dx / norm

            # 调整红点的位置
            for k, (offset_x, offset_y) in enumerate(triangle_offsets):
                # 旋转三角形以匹配路径方向
                rotated_x = offset_x * dx_perp - offset_y * dy_perp
                rotated_y = offset_x * dy_perp + offset_y * dx_perp

                dot_x = center_x + rotated_x
                dot_y = center_y + rotated_y

                red_dots[k].set_data([dot_x], [dot_y])

            plt.draw()
            plt.pause(0.01)

        # 更新路径
        ax.plot(cities[tour[i:i + 2], 0], cities[tour[i:i + 2], 1], 'r.-')

plt.show()