From d5e7fdf687a377e411b40c70379995cbd651db24 Mon Sep 17 00:00:00 2001 From: changwu <19375188@buaa.edu.cn> Date: Tue, 9 May 2023 22:23:09 +0800 Subject: [PATCH] v5.9 --- c_cpp/.gitignore | 3 + c_cpp/.vscode/c_cpp_properties.json | 20 + c_cpp/.vscode/launch.json | 29 ++ c_cpp/.vscode/settings.json | 6 + c_cpp/.vscode/tasks.json | 37 ++ c_cpp/BA.py | 625 ++++++++++++++++++++++++++++ c_cpp/draw.py | 22 + c_cpp/id_rsa.pub | 1 + 8 files changed, 743 insertions(+) create mode 100644 c_cpp/.gitignore create mode 100644 c_cpp/.vscode/c_cpp_properties.json create mode 100644 c_cpp/.vscode/launch.json create mode 100644 c_cpp/.vscode/settings.json create mode 100644 c_cpp/.vscode/tasks.json create mode 100644 c_cpp/BA.py create mode 100644 c_cpp/draw.py create mode 100644 c_cpp/id_rsa.pub diff --git a/c_cpp/.gitignore b/c_cpp/.gitignore new file mode 100644 index 0000000..30a0bba --- /dev/null +++ b/c_cpp/.gitignore @@ -0,0 +1,3 @@ + +*.csv +*/__pycache__ diff --git a/c_cpp/.vscode/c_cpp_properties.json b/c_cpp/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..156ce75 --- /dev/null +++ b/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 +} \ No newline at end of file diff --git a/c_cpp/.vscode/launch.json b/c_cpp/.vscode/launch.json new file mode 100644 index 0000000..6737712 --- /dev/null +++ b/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 生成活动文件" + } + ] +} \ No newline at end of file diff --git a/c_cpp/.vscode/settings.json b/c_cpp/.vscode/settings.json new file mode 100644 index 0000000..e1c9f42 --- /dev/null +++ b/c_cpp/.vscode/settings.json @@ -0,0 +1,6 @@ +{ + "files.associations": { + "stdio.h": "c" + }, + "python.formatting.provider": "black" +} \ No newline at end of file diff --git a/c_cpp/.vscode/tasks.json b/c_cpp/.vscode/tasks.json new file mode 100644 index 0000000..6f78525 --- /dev/null +++ b/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" +} \ No newline at end of file diff --git a/c_cpp/BA.py b/c_cpp/BA.py new file mode 100644 index 0000000..8555ded --- /dev/null +++ b/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() \ No newline at end of file diff --git a/c_cpp/draw.py b/c_cpp/draw.py new file mode 100644 index 0000000..efeb815 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/c_cpp/id_rsa.pub b/c_cpp/id_rsa.pub new file mode 100644 index 0000000..89431ba --- /dev/null +++ b/c_cpp/id_rsa.pub @@ -0,0 +1 @@ +ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABgQD9iADhugWA20YFU9+3zLPUmXe3HEMiQCPBDOFguPKSFcn9nbgxIDENeyTEjaIMShISHw4pj5RQ0tKeSfBAD9DtEPIRQcCfWnwU4TtF2UEwn31pxd1VrAnlN9RShv0FQ8LKPgoZ8uge8EsVku6BawilbxLCi0xcctvIRyDccErxUoArwbEnWRAx6gitRUwvqx8iEIwGkEr4Woer022XCVbfTMI6X5mnRyrlcAg2iJyv5Tbz7s+ugl7ZMt7Tt2jUOvnl2BmxtWbxUjiSDiIhVM/P7J1nDwUczjICl6PI+2iaRu2GOUkQMSBY5Uf+dEfVH+6XAJ18KzE/ozPO9NT6vEypBP9D0W+T0eMXDaetTT4rakq9Mv6nFGJOIO6ZU0TmcpUoK0ZaTiVB/+YMFbiW+o9qcSG0i52b5wNNiyieaIN74gAoVTbG4rVz9Xa8ExccMnvzY+rQuW5JvIKtsJPSAqYKoBYHCgQmWv8/6YODFARdrNsiZSADGVr6D0cZ8Y4xE4k= 邮箱名称