v5.9

c_cpp/.gitignore

@@ -0,0 +1,3 @@
+
+*.csv
+*/__pycache__

c_cpp/.vscode/c_cpp_properties.json

@@ -0,0 +1,20 @@
+{
+    "configurations": [
+        {
+            "name": "Win32",
+            "includePath": [
+                "${workspaceFolder}/**"
+            ],
+            "defines": [
+                "_DEBUG",
+                "UNICODE",
+                "_UNICODE"
+            ],
+            "compilerPath": "E:/mingu/bin/gcc.exe",
+            "cStandard": "c11",
+            "cppStandard": "c++17",
+            "intelliSenseMode": "windows-gcc-x64"
+        }
+    ],
+    "version": 4
+}

c_cpp/.vscode/launch.json

@@ -0,0 +1,29 @@
+{
+    // 使用 IntelliSense 了解相关属性。 
+    // 悬停以查看现有属性的描述。
+    // 欲了解更多信息，请访问: https://go.microsoft.com/fwlink/?linkid=830387
+    "version": "0.2.0",
+    "configurations": [
+        {
+            "name": "gcc.exe - 生成和调试活动文件",
+            "type": "cppdbg",
+            "request": "launch",
+            "program": "${fileDirname}\\${fileBasenameNoExtension}.exe",
+            "args": [],
+            "stopAtEntry": false,
+            "cwd": "${fileDirname}",
+            "environment": [],
+            "externalConsole": true,
+            "MIMode": "gdb",
+            "miDebuggerPath": "E:\\mingu\\bin\\gdb.exe",
+            "setupCommands": [
+                {
+                    "description": "为 gdb 启用整齐打印",
+                    "text": "-enable-pretty-printing",
+                    "ignoreFailures": true
+                }
+            ],
+            "preLaunchTask": "C/C++: gcc.exe 生成活动文件"
+        }
+    ]
+}

c_cpp/.vscode/settings.json

@@ -0,0 +1,6 @@
+{
+    "files.associations": {
+        "stdio.h": "c"
+    },
+    "python.formatting.provider": "black"
+}

c_cpp/.vscode/tasks.json

@@ -0,0 +1,37 @@
+{
+    "tasks": [
+        {
+            "type": "cppbuild",
+            "label": "C/C++: gcc.exe 生成活动文件",
+            "command": "E:\\mingu\\bin\\gcc.exe",
+            "args": [
+                "-fdiagnostics-color=always",
+                "-g",
+                "${file}",
+                "-o",
+                "${fileDirname}\\${fileBasenameNoExtension}.exe"
+            ],
+            "options": {
+                "cwd": "${fileDirname}"
+            },
+            "problemMatcher": [
+                "$gcc"
+            ],
+            "presentation": {
+                "panel": "new" //默认为“shared“表示共享，改成new之后每个进程创建新的端口
+            },
+            "group": {
+                "kind": "build",
+                "isDefault": true
+            },
+            "detail": "调试器生成的任务。"
+        },
+        {
+            "label": "Open Terminal",
+            "type": "shell",
+            "command": "osascript -e 'tell application \"Terminal\"\ndo script \"echo hello\"\nend tell'",
+            "problemMatcher": []
+        }
+    ],
+    "version": "2.0.0"
+}

c_cpp/BA.py

@@ -0,0 +1,625 @@
+import networkx as nx
+import numpy as np
+import random
+import math
+import numpy as np
+import matplotlib.pyplot as plt
+from mpl_toolkits import mplot3d
+
+ind = 10000
+n = 35
+memlen = 3
+state = np.zeros([n, ind + 1])
+initx = np.zeros(n)
+inity = np.zeros(n)
+initz = np.zeros(n)
+finalx = np.zeros(n)
+finaly = np.zeros(n)
+finalz = np.zeros(n)
+for i in range(0, n):
+    # initx[i] = random.random() * 100
+    # inity[i] = random.random() * 100
+    # initz[i] = random.random() * 100
+    # finalx[i] = random.random() * 100
+    # finaly[i] = random.random() * 100
+    # finalz[i] = random.random() * 100
+    # initx[i] = 20*random.random() + 30
+    # inity[i] = 20*random.random() + 30
+    # initz[i] = 0
+    # finalx[i] = 2+i*40/n
+    # finalz[i] = 75
+    # finaly[i] = 22-abs(i-16)*20/22
+    # initx[i] = 2 + i*40/n
+    # initz[i] = 75
+    # inity[i] = 22-abs(i-17)*20/17
+    
+    # finalx[i] = 5+3*int(i%7)
+    # finaly[i] = 60
+    # finalz[i] = 75+3*int(i/7)
+    
+    initx[i] = 15+3*int(i%7)
+    inity[i] = 80
+    initz[i] = 75+3*int(i/7)
+
+
+    psai = 2*math.pi*i/35
+    finalx[i] = 120+10*math.cos(psai)
+    finaly[i] = 80
+    finalz[i] = 50+10*math.sin(psai)
+   
+    # initx[i] = 20 + 8 * math.cos(math.pi * 2 * i / n)
+    # inity[i] = 80 + 8 * math.sin(math.pi * 2 * i / n)
+    # finalx[i] = 30 + i * 10 / n
+    # finaly[i] = 15 + i * 15 / n
+    # initx[i] = 30 + i * 10 / n
+    # inity[i] = 15 + i * 15 / n
+    # finalx[i] = 85-15*abs(i-7)/7
+    # finaly[i] = 60-40*i/n
+    # initx[i] = 10 + 2 * i * 11 / n
+    # inity[i] = 20  #+ 10 * math.sin(math.pi * 2 * i / n)
+    # finalx[i] = 50 - 2 * i * 11 / n
+    # finaly[i] = 65
+    # if i <10:
+    #     initx[i] = random.random()*30
+    # else:
+    #     initx[i] = random.random()*30 + 70
+    # inity[i] = random.random()*30
+    # finalx[i] = 50 + 10 * math.cos(math.pi * 2 * i / n)
+    # finaly[i] = 80 + 10 * math.sin(math.pi * 2 * i / n)
+
+
+
+#计算无人机到目标点距离，输入：无人机群的xy坐标，目标位置的xy坐标
+def getdis(x1, y1, z1,x2, y2,z2):
+    dis1 = np.zeros((n, n))
+    for i in range(0, n):
+        for j in range(0, n):
+            dis1[i, j] = math.sqrt((x1[i] - x2[j])**2 + (y1[i] - y2[j])**2+(z1[i] - z2[j])**2)
+    return dis1
+dis = getdis(initx,inity,initz,finalx,finaly,finalz)
+
+#计算无人机之间距离并判断连接关系，输入：无人机群的xy坐标
+def getcon(x1, y1,z1):
+    juli1 = np.zeros((n, n))
+    for i in range(0, n):
+        for j in range(0, n):
+            juli1[i, j] = math.sqrt((x1[i] - x1[j])**2 + (y1[i] - y1[j])**2+(z1[i] - z1[j])**2)
+    con = []
+    for i in range(0, n):
+        tmpcon = []
+        for j in range(0, n):
+            if (juli1[i][j] <= 1000 and i != j):
+                tmpcon.append(j)
+        con.append(tmpcon)
+    return con
+
+# for i in range(0, n):
+#     for j in range(0, n):
+#         dis[i, j] = math.sqrt((initx[i] - finalx[j])**2 +
+#                               (inity[i] - finaly[j])**2)
+#         juli[i, j] = math.sqrt(
+#             (initx[i] - initx[j])**2 + (inity[i] - inity[j])**2)  #建立距离存储表格
+mem = np.zeros([n, memlen])  #创建记忆存储表格
+# con = []
+# for i in range(0, n):
+#     tmpcon = []
+#     for j in range(0, n):
+#         if (juli[i][j] <= 150 and i != j):
+#             tmpcon.append(j)
+#     con.append(tmpcon)
+
+
+def gettasks(dis):
+    tasks = []
+    for i in range(0, n):
+        temptsk = []
+        for j in range(0, n):
+            if dis[i, j] <= 1400:
+                temptsk.append(j)
+        tasks.append(temptsk)
+    return tasks
+
+con = getcon(initx,inity,initz)
+#创建任务集：距离80以内才能作为可选任务
+tasks = gettasks(dis)
+
+for i in range(0, n):
+    state[i, 0] = int(random.random() * n)
+
+
+#定义个体寻找相同点的函数
+def findrep(s1, co1, repi, t):
+    count = 0
+    for vb in range(0, len(co1[repi])):
+        if s1[repi, t] == s1[co1[repi][vb], t] and repi != vb:
+            count = count + 1
+    return count
+
+
+#定义寻找list最优点位置程序
+def findmin(c):
+    listmin = min(c)
+    for r in range(0, len(c)):
+        if c[r] == listmin:
+            stra = r
+            break
+    return stra
+
+
+def memre(mem):
+    for q in range(1, memlen):
+        for p in range(0, n):
+            mem[p, memlen - q] = mem[p, memlen - 1 - q]
+
+
+def findrep(choice_place, con, i, t):
+    a1 = 0
+    for j in range(0, len(con[i])):
+        if choice_place == state[con[i][j], t]:
+            a1 = a1 + 1
+    return a1
+
+
+def calchoice(t):
+    if (t == 0):
+        choice = 0
+    else:
+        if (t < memlen):
+            choice = int(random.random() * t)
+        else:
+            choice = int(random.random() * memlen)
+    return choice
+
+
+costsum = np.zeros([n, ind])
+
+stop = np.zeros(n)
+
+
+# def distribute(tasks, dis, con):
+#     t = 0
+#     while t < ind:
+#         for i in range(0, n):
+#             costsum[i, t] = (findrep(state[i, t], con, i, t) * 1000 +
+#                              dis[i, int(state[i, t])])
+#         for p in range(1, memlen):
+#             for q in range(0, n):
+#                 mem[q, memlen - p] = mem[q, memlen - 1 - p]
+#         for i in range(0, n):
+#             if (stop[i] == 0):
+#                 cost = []
+#                 for k in range(0, len(tasks[i])):
+#                     cost.append(dis[i, tasks[i][k]] +
+#                                 1000 * findrep(tasks[i][k], con, i, t))
+#                 mem[i, 0] = tasks[i][findmin(cost)]
+#             else:
+#                 mem[i, 0] = state[i, t]
+#         for l in range(0, n):
+#             choice = calchoice(t)
+#             state[l, t + 1] = mem[l, choice]
+#         t = t + 1
+#     return state
+# #如果进行再分配，无人机即使避开了此次的重复选择，在选择新的位置时，仍然将选择其他无人机已经选择但是它不知道的位置。
+
+# state = distribute(tasks, dis, con)
+# np.savetxt('D:/ws0.csv', state, fmt="%d", delimiter=",")
+#开始计算博弈
+for t in range(0, ind):
+    memre(mem)#需要考虑顺序的影响吗
+    for i in range(0, n):
+        costsum[i, t] = (findrep(state[i, t], con,i,t) * 1000 +
+                         dis[i, int(state[i, t])])
+    for i in range(0, n):
+        cost = []
+        for k in range(0, len(tasks[i])):
+            cost.append(dis[i, tasks[i][k]] +
+                        1000 * findrep(tasks[i][k], con,i,t))  #存储每个选择的花费
+            #应该改成：首先生成一轮最优解，后续的每一轮依据之前的选择存入状态，用这个状态判断重复。
+            #经过如此完成多次的反思最终确定结果
+        #state[i, t] = findmin(cost)  #需要后续修改
+        mem[i, 0] = tasks[i][findmin(cost)]
+    for l in range(0, n):
+        if (t == 0):
+            choice = 0
+        else:
+            if (t < memlen):
+                choice = int(random.random() * t)
+            else:
+                choice = int(random.random() * memlen)
+        state[l, t + 1] = mem[l, choice]
+plt.rcParams['font.sans-serif'] = ['Microsoft YaHei']
+# plt.title("15架无人机视角下花费变化图")
+# plt.xlabel('迭代次数')
+# plt.ylabel("花费")
+# for i in range(0, n):
+#     for j in range(0, 500):
+#         plt.plot([j, j + 1], [costsum[i, j], costsum[i, j + 1]])
+# plt.show()
+np.savetxt('D:/ws0.csv', state, fmt="%d", delimiter=",")
+# np.savetxt('D:/ws3.csv', costsum, fmt="%f", delimiter=",")
+#画图，1-500次时无人机利润
+list = []
+for r in range(0, n):
+    list.append(state[r, ind - 1])
+list.sort()
+for i in range(0, len(list) - 1):
+    if (list[i]) == list[i + 1]:
+        print(list)
+        print("存在冲突")
+        break
+
+
+def getlength(x1, x2, y1, y2,z1,z2):
+    return math.sqrt((x1 - x2)**2 + (y1 - y2)**2 + (z1 - z2)**2)
+
+
+def gravi(x1, x2, y1, y2,z1,z2):
+    length = getlength(x1, x2, y1, y2,z1,z2)
+    fgra = 30 + length**2
+    fgrax = fgra * (x2 - x1) / length
+    fgray = fgra * (y2 - y1) / length
+    fgraz = fgra * (z2 - z1) / length
+    return [fgrax, fgray, fgraz]
+
+
+def repul(x1, x2, y1, y2, z1 , z2):
+    distance = getlength(x1, x2, y1, y2,z1,z2)
+    if (distance < 2):
+        frep = 20 * (2 / distance - 1)**2
+    else:
+        frep = 0
+    frepx =  frep * (x1 - x2) / (distance)
+    frepy =  frep * (y1 - y2) / (distance)
+    frepz =  frep * (z1 - z2) / (distance)
+    return [frepx, frepy, frepz]
+
+def vituralpoint1(x,y,z):
+    if getlength(x,22,y,45,z,30)<25 and y>(0.5*x+33.5):
+        fv1x = -40*abs(x-22)
+    else:
+        fv1x = 0
+    fv1y = 0.5*fv1x
+    fv1z = 0.5*fv1x
+    return [fv1x,fv1y,fv1z]
+
+
+position = np.zeros([3, n])
+destina = np.zeros([3, n])
+for i in range(0, n):
+    point = int(state[i, ind - 1])
+    destina[0, i] = finalx[point]
+    destina[1, i] = finaly[point]
+    destina[2, i] = finalz[point]
+    position[0, i] = initx[i]
+    position[1, i] = inity[i]
+    position[2, i] = initz[i]
+cntstep = 2000
+stop = np.zeros(n)
+hisplacex = np.zeros([n, cntstep])
+hisplacey = np.zeros([n, cntstep])
+hisplacez = np.zeros([n, cntstep])
+step = 0.5
+speedx = np.zeros([n,ind])
+speedy = np.zeros([n,ind])
+speedz = np.zeros([n,ind])
+#引入斥力场
+#返回横线与纵向受力，椭圆形力场，原直线为ax+by=c
+#那么距离=abs(ax+by+c)/math.sqrt(a**2+b**2)
+#修改x1,x2位置改变初始
+
+
+def cirfield(x, y, z, x0, y0, z0, r):
+    length1 = getlength(x,x0,y,y0,z,z0)
+    if length1 >= 1.2*r:
+        fr = 0
+    else:
+        fr = 3000*(1.2*r/length1 -1)
+    frx = fr * (x - x0)/length1
+    fry = fr * (y - y0)/length1
+    frz = fr * (z - z0)/length1
+    return [frx,fry,frz]
+
+
+def linefield(x0,y0,z0,x1,y1,z1,x,y,z):
+    flx = fly = flz = 0
+    pointnum = 80
+    xmid = np.zeros(pointnum)
+    ymid = np.zeros(pointnum)
+    zmid = np.zeros(pointnum)
+    for i in range(0,pointnum-1):
+        xmid[i] = (x1-x0)/pointnum*i+x0
+        ymid[i] = (y1-y0)/pointnum*i+y0
+        zmid[i] = (z1-z0)/pointnum*i+z0
+    for j in range(0,pointnum-1):
+        flx = flx+ ballfield(x,y,z,xmid[j],ymid[j],zmid[j],2)[0]
+        fly = fly+ ballfield(x,y,z,xmid[j],ymid[j],zmid[j],2)[1]
+        flz = flz+ ballfield(x,y,z,xmid[j],ymid[j],zmid[j],2)[2]
+    return [flx,fly,flz]
+
+def virpoi(x,y,z,x0,y0,z0):
+    destan1 = getlength(x,x0,y,y0,z,z0)
+    fx = (x0-x)/destan1
+    fy = (y0-y)/destan1
+    fz = (z0-z)/destan1
+    return [fx,fy,fz]
+
+
+def ballfield(x,y,z,x0,y0,z0,r1):
+    destan2 = getlength(x,x0,y,y0,z,z0)
+    if destan2 >1.5*r1:  
+        f = 0
+    else:
+        f = -8000*(1.5*r1/destan2-1)
+    fx = f*(x0-x)/destan2
+    fy = f*(y0-y)/destan2
+    fz = f*(z0-z)/destan2
+    return [fx,fy,fz]
+
+def surfield1(x,y,z):
+    #法向量为（a,b,c）
+    if(x + y < (60+2.5*math.sqrt(2))and y<(20) and z<60):
+        ff1 = 40*(5/(x+y-60)-1)
+    else:
+        ff1 = 0
+    fx = ff1/math.sqrt(2)
+    fy = fx
+    return [fx,fy]
+
+
+def surfield2(x,y,z):
+    if((0.5*x+y)<(40+2.5*math.sqrt(5)) and z<60 and x<40):
+        f2 = 50*(5/(0.5*x+y-40)-1)
+    else:
+        f2 = 0
+    fx = f2/math.sqrt(5)
+    fy = f2*2/math.sqrt(5)
+    return[fx,fy]
+
+def yuanzhu(x,y,z):
+    jl1 = math.sqrt((x-50)**2+(y-60)**2)
+    if(z<80 and jl1<15):
+        f = 1000*(15/jl1-1)
+    else:
+        f = 0
+    fx = f*(x-50)/jl1
+    fy = f*(y-60)/jl1
+    return[fx,fy]
+def getangel(v1, v2):
+
+    # 分别计算两个向量的模：
+    module_x = math.sqrt(v1[1]**2 + v1[0]**2 + v1[2]**2)
+    module_y = math.sqrt(v2[1]**2 + v2[0]**2 + v2[2]**2)
+
+    # 计算两个向量的点积
+    dot_value = v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]
+
+    # 计算夹角的cos值：
+    cos_theta = dot_value / (module_x * module_y)
+    return cos_theta
+
+
+def renewposi(force):
+    # if (force != 0):
+        deltax = position[0, i] - destina[0, i]
+        deltay = position[1, i] - destina[1, i]
+        deltaz = position[2, i] - destina[2, i]
+        line1 = [deltax, deltay, deltaz]
+        line2 = [forcex, forcey, forcez]
+        cosangle = getangel(line1, line2)
+        
+            
+        step = 0.3 * 2**(0.5*cosangle)
+        # if cosangle > 0.8:
+        #     position[0, i] = position[0, i] - step * forcey / force
+        #     position[1, i] = position[1, i] + step * forcex / force
+        # else:
+        position[0, i] = position[0, i] + step * forcex / force
+        position[1, i] = position[1, i] + step * forcey / force
+        position[2, i] = position[2, i] + step * forcez / force
+        speedx[i,jishu] = step * forcex / force
+        speedy[i,jishu] = step * forcey / force
+        speedz[i,jishu] = step * forcez / force
+    # else:
+    #     position[0, i] = position[0, i] + random.uniform(-0.05, 0.05)
+    #     position[1, i] = position[1, i] + random.uniform(-0.05, 0.05)
+
+
+#添加一个斥力源：中心在某处的圆形
+#另一种斥力源的思路：横线障碍，方向为：两个半圆斥力场与两个方向上直线型斥力场
+#思考如何处理中途的改进：每20步重新分配一次
+#在分配时如何处理已经到达的无人机？
+#在博弈时，发现对方已经到达目标位置，就让该无人机变换利润：提高他机，固定本位置为0，或者修改记忆！
+jishu = 0
+while (1):
+
+    for i in range(0, n):
+        forcelist = []
+        for m in range(0, n):
+            if (stop[m] == 0 and m != i):
+                forcelist.append(m)
+        
+        frepx1 =(0 + ballfield(position[0,i],position[1,i],position[2,i],55,80,65,7)[0]
+
+                # +linefield(25,45,35,50,position[0, i],position[1, i])[0]
+                # #+linefield(70,35,80,55,position[0, i],position[1, i])[0]
+                # +vituralpoint1(position[0, i],position[1, i])[0]
+                # +cirfield(position[0, i],position[1, i],65,40,5)[0]
+                )
+        frepy1 =(0 + ballfield(position[0,i],position[1,i],position[2,i],55,80,65,7)[1]
+
+                # +linefield(25,45,35,50,position[0, i],position[1, i])[1]
+                # #+linefield(70,35,80,55,position[0, i],position[1, i])[1]
+                # +vituralpoint1(position[0, i],position[1, i])[1]
+                # +cirfield(position[0, i],position[1, i],65,40,5)[1]
+                )
+        frepz1 =(0+ballfield(position[0,i],position[1,i],position[2,i],55,80,65,7)[2]
+                 )
+        for count in range(0, len(forcelist)):
+            listrep = repul(
+                position[0, i], position[0, forcelist[count]], position[1, i],
+                position[1, forcelist[count]], position[2, i], position[2, forcelist[count]])
+            frepx1 = frepx1 + listrep[0]
+            frepy1 = frepy1 + listrep[1]
+            frepz1 = frepz1 + listrep[2]
+        if stop[i] == 0:
+            listgra = gravi(position[0, i], destina[0, i],position[1, i], destina[1, i],position[2, i], destina[2, i])
+            forcex = frepx1 + listgra[0]
+            forcey = frepy1 + listgra[1]
+            forcez = frepz1 + listgra[2]
+            force = math.sqrt(forcex**2 + forcey**2 + forcez**2)
+            renewposi(force)
+            #一轮位置变化,一次0.5m，接下来判断是否应该停止
+            hisplacex[i, jishu] = position[0, i]
+            hisplacey[i, jishu] = position[1, i]
+            hisplacez[i, jishu] = position[2, i]
+        if getlength(position[0, i], destina[0, i], position[1, i],
+                     destina[1, i], position[2, i],destina[2, i]) <= 0.2:
+            stop[i] = 1
+            #到达目标点后将斥力变为0
+    if jishu %100 == 0 :
+        con_re = []
+        dis_re = np.zeros([n, n])
+        julire = np.zeros([n, n])
+        tasksre = []
+        for d in range(0, n):
+            for f in range(0, n):
+                dis_re[d, f] = math.sqrt((position[0, d] - destina[0, f])**2 +
+                                         (position[1, d] - destina[1, f])**2 + 
+                                         (position[2, d] - destina[2, f])**2)
+                julire[d, f] = math.sqrt((position[0, d] - position[0, f])**2 +
+                                         (position[1, d] - position[1, f])**2 +
+                                         (position[2, d] - position[2, f])**2)
+        tasksre = gettasks(dis_re)
+        for x in range(0, n):
+            tmpcon = []
+            for c in range(0, n):
+                if (julire[x][c] <= 80 and x != c):
+                    tmpcon.append(c)
+            con_re.append(tmpcon)
+        statere = np.zeros(n)
+        
+        for t2 in range(0, n):
+            statere[t2] = state[t2, ind - 1]
+        relist = []
+        recost = []
+        findsame = []
+        t1 = ind - 1
+        for r in range(0, n):
+            count = 0
+            for vb in range(0, len(con_re[r])):
+                if state[r, t1] == state[con_re[r][vb], t1] and r != vb:
+                    count = count + 1
+                    tmsame = []
+                    tmsame.append(r)
+                    tmsame.append(con_re[r][vb])
+                    findsame.append(tmsame)
+            if (count != 0 ):
+                relist.append(r)
+        renum = len(relist)
+        print(findsame)  #位置变化后，寻找到重复的无人机，对其重新分配
+        for t6 in range(0,int(len(findsame)/2)):
+            choicelist1 = []
+            tmpcost = []
+            for t7 in range(1,len(findsame[t6])):
+                rebianhao = findsame[t6][t7]
+                relist1 = []
+                for t8 in range(0,len(con_re[rebianhao])):
+                    if statere[con_re[rebianhao][t8]] not in relist1:
+                        relist1.append(statere[con_re[rebianhao][t8]])
+                for t9 in range(len(tasksre[rebianhao])):
+                    if tasksre[rebianhao][t9] not in relist1:
+                        choicelist1.append(tasksre[rebianhao][t9])
+                for t10 in range(0,len(choicelist1)):
+                    tmpcost.append(dis_re[rebianhao,choicelist1[t10]])
+                statere[rebianhao] = choicelist1[findmin(tmpcost)] 
+        liata = np.sort(statere)        
+        print(liata)
+        for t11 in range(0,n):
+            state[t11,t1] = statere[t11]
+        for b in range(0, int(len(findsame)/2)):
+            for b1 in range(0,len(findsame[b])):
+                point = int(statere[int(findsame[b][b1])])
+                stop[point] = 0
+                destina[0, findsame[b][b1]] = finalx[point]
+                destina[1, findsame[b][b1]] = finaly[point]
+                destina[2, findsame[b][b1]] = finalz[point]
+    tostop = 1
+    for cnt in range(0, n):
+        tostop = tostop * stop[cnt]
+    if tostop == 1 or jishu == cntstep-1:
+        print(jishu)
+        break
+    else:
+        jishu = jishu + 1
+#或许需要在第30步时再一次分配
+
+np.savetxt('D:/wsf1x.csv', hisplacex, fmt="%f", delimiter=",")
+np.savetxt('D:/wsf1y.csv', hisplacey, fmt="%f", delimiter=",")
+np.savetxt('D:/wsf1z.csv', hisplacez, fmt="%f", delimiter=",")
+np.savetxt('D:/speed1x.csv', speedx, fmt="%f", delimiter=",")
+np.savetxt('D:/speed1y.csv', speedy, fmt="%f", delimiter=",")
+np.savetxt('D:/speed1z.csv', speedz, fmt="%f", delimiter=",")
+placex = []
+placey = []
+placez = []
+for i in range(0, n):
+    for j in range(0, cntstep):
+        if (hisplacex[i, j]) != 0:
+            placex.append(hisplacex[i, j])
+            placey.append(hisplacey[i, j])
+            placez.append(hisplacez[i, j])
+fig = plt.figure(1)
+ax = plt.axes(projection='3d')
+# new_tick = np.linspace(0, 115, 24)
+# plt.xticks(new_tick)
+# plt.yticks(new_tick)
+# plt.title("无人机轨迹散点图")
+# plt.xlabel('x方向         单位：m')
+# plt.ylabel("y方向         单位：m")
+
+# plt.plot([25,35],[45,50])
+# # plt.plot([70,80],[35,55])
+# plt.scatter(initx, inity, c='blue', s=10)
+# plt.scatter(finalx, finaly, c='black', s=10)  #蓝色点为起点，黑色点为终点
+# plt.scatter(placex, placey, c='red', s=1)  #画出散点图，但是不表示顺序
+# theta = np.arange(0, 2 * np.pi, 0.01)
+# x = 65 + 5 * np.cos(theta)
+# y = 40 + 5 * np.sin(theta)
+# plt.plot(x, y)
+# #调用 ax.plot3D创建三维线图
+# x = 50+7.5*np.cos(theta)
+# y = 60+7.5*np.sin(theta)
+# z = np.linspace(0, 80, 1000)
+# theta = np.linspace(0,2*np.pi,30)
+# z,theta = np.meshgrid(z,theta)
+theta1 = np.linspace(0,2*np.pi,30)
+fai = np.linspace(0,2*np.pi,30)
+theta1,fai = np.meshgrid(theta1,fai)
+xr = 55+6*np.cos(theta1)*np.sin(fai)
+yr = 80+6*np.sin(theta1)*np.sin(fai)
+zr = 65+6*np.cos(fai)
+ax.plot_surface(xr, yr, zr,cmap='viridis', edgecolor='none')
+# ax.plot_surface(x, y, z,cmap='viridis', edgecolor='none')
+
+# x1 = np.linspace(0,40,100)
+# z1 = np.linspace(0,60,100)
+# x1,z1 = np.meshgrid(x1,z1)
+# y1 = -0.5*x1+40
+# ax.plot_surface(x1, y1, z1,cmap='viridis', edgecolor='none')
+# y2 = np.linspace(0,20,80)
+# y2,z1 = np.meshgrid(y2,z1)
+# x2 = 60-y2
+# ax.plot_surface(x2, y2, z1,cmap='viridis', edgecolor='none')
+# z3 = 60
+# x3 = np.linspace(0,40,100)
+# x3, z3 = np.meshgrid(x3,z3)
+# y3 = 40-0.5*x3
+# ax.plot_surface(x3, y3, z3,cmap='viridis', edgecolor='none')
+# ax.set_title('3D line plot')
+ax.plot3D(0,0,0)
+ax.plot3D(100,100,0)
+ax.scatter3D(placex,placey,placez,c='blue',s = 0.1)
+ax.scatter3D(initx,inity,initz,c='red',s = 0.5)
+ax.scatter3D(finalx,finaly,finalz,c='red',s = 0.5)
+
+plt.show()

c_cpp/draw.py

@@ -0,0 +1,22 @@
+import numpy as np
+import random
+import math
+import numpy as np
+import matplotlib.pyplot as plt
+cnt = np.zeros(500)
+for t in range(0,500):
+    cnt[t] = t
+xt = np.zeros(500)
+vx = np.loadtxt("D:/speed1x.csv",delimiter=",", dtype=float)
+vy = np.loadtxt("D:/speed1y.csv",delimiter=",", dtype=float)
+vz = np.loadtxt("D:/speed1z.csv",delimiter=",", dtype=float)
+plt.rcParams['font.sans-serif'] = ['Microsoft YaHei']
+plt.xlabel('时间')
+plt.ylabel('每个时间段x方向位移')
+for i in range(0,35):
+    xt = np.zeros(500)
+    for j in range(0,500):
+        xt[j] = vz[i,j]
+    plt.plot(cnt,xt)
+
+plt.show()

c_cpp/id_rsa.pub

@@ -0,0 +1 @@
+ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABgQD9iADhugWA20YFU9+3zLPUmXe3HEMiQCPBDOFguPKSFcn9nbgxIDENeyTEjaIMShISHw4pj5RQ0tKeSfBAD9DtEPIRQcCfWnwU4TtF2UEwn31pxd1VrAnlN9RShv0FQ8LKPgoZ8uge8EsVku6BawilbxLCi0xcctvIRyDccErxUoArwbEnWRAx6gitRUwvqx8iEIwGkEr4Woer022XCVbfTMI6X5mnRyrlcAg2iJyv5Tbz7s+ugl7ZMt7Tt2jUOvnl2BmxtWbxUjiSDiIhVM/P7J1nDwUczjICl6PI+2iaRu2GOUkQMSBY5Uf+dEfVH+6XAJ18KzE/ozPO9NT6vEypBP9D0W+T0eMXDaetTT4rakq9Mv6nFGJOIO6ZU0TmcpUoK0ZaTiVB/+YMFbiW+o9qcSG0i52b5wNNiyieaIN74gAoVTbG4rVz9Xa8ExccMnvzY+rQuW5JvIKtsJPSAqYKoBYHCgQmWv8/6YODFARdrNsiZSADGVr6D0cZ8Y4xE4k= 邮箱名称