模拟显示一个点图到另一个点图的变换

灵感来源：看到无人机表演组成字或者图，对路径规划有点兴趣，想简单通过python来展示从一个点图到另一个点图的变化

真正的无人机路径规划需要考虑的东西太多了，不想管那么多现实的限制条件，先从最简单的开始逐步添加条件。发文记下思路。

1. 实现最简单的三角形点图到正方形点图的变化 （每个点找离自己最近的点，最low的方式）
2. 由于每个点需要走的路径长度不同，显示变化过程中每一秒每个点的位置，直到所有点都达到目的地。点的数目有差异，对这些点需要设置初始位置。
3. 输入稍微复杂的点图，50~80个点，观察变化，改改bug （不知道从哪找点图的坐标。。自己写太nm累了）
4. 应用路径规划的算法（如果3的效果不错，就放弃这个，不然还要研究一些论文，选择哪种路径规划方案最优，不管怎么说，我觉得最low的方式也可以作为剪枝的标准）
5. 完成点图“？”到“？”的变换 （未定）

1完成了，代码

#!/usr/bin/python3
import matplotlib as mpl
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
tri = [(1, 1, 0),
       (3, 1, 0),
       (5, 1, 0),
       (2, 3, 0),
       (3, 5, 0),
       (4, 3, 0)
       ]
quar = [(1, 1, 0),
        (3, 1, 0),
        (5, 1, 0),
        (1, 3, 0),
        (1, 5, 0),
        (3, 5, 0),
        (5, 5, 0),
        (5, 3, 0)
        ]
arr1 = np.array(tri)
arr2 = np.array(quar)

xdata, ydata = [], []
dot, = ax.plot([], [], [], 'g.')


def distance(a,b):
    return np.linalg.norm(a-b)


def init():
    ax.set_xlim(0, 7)
    ax.set_ylim(0, 7)
    ax.set_zlim(-3, 3)
    return 1,


def update_dot(n, x, y, z, dot):
    # xdata.append(5*np.sin(n))
    # ydata.append(5*np.cos(n))
    dot.set_data(x, y)
    dot.set_3d_properties(z, 'z')
    if n % 2 == 1:
        dot.set_data(xdata, ydata)
        dot.set_3d_properties(zdata, 'z')
    return dot


x = arr1[0:6, 0]
y = arr1[0:6, 1]
z = arr1[0:6, 2]
xdata = arr2[0:8, 0]
ydata = arr2[0:8, 1]
zdata = arr2[0:8, 2]

T1 = np.copy(arr1)
T2 = np.copy(arr2)


dim = max(len(arr1), len(arr2))
T1 = np.pad(T1,((0,dim-len(arr1)),(0,0)),'constant',constant_values = (0,0)) #填充到同样的点坐标数量
T2 = np.pad(T2,((0,dim-len(arr2)),(0,0)),'constant',constant_values = (0,0))
print ("T1 = ",T1)
print ("T2 = ",T2)
max_dist = 0
Transform = np.zeros((8,2))

for i in range(0,dim):
    dist = 100
    flag = 0
    for j in range(0,dim):
        if np.any(Transform[0:dim,1]==j+1):
           continue
        tmp_dist = distance(T1[i],T2[j])
        print("i=", i,"j=",j,"tmp_dist=",tmp_dist)
        if tmp_dist < dist:
            dist = tmp_dist
            flag = j
    Transform[i] = [i+1,flag+1]
    print("i=", i,"flag=",flag,"dist=",dist)
    if dist > max_dist:
        max_dist = dist

print("max dist", max_dist)
print("Transform", Transform)

ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')

ani = FuncAnimation(fig, update_dot, frames=50, fargs=(x, y, z, dot), interval=500, init_func=init)
plt.show()