python路径规划算法可视化_基于粒子群算法的牙齿正畸路径规划方法python实现

这篇是基于粒子群算法的牙齿正畸路径规划研究的python实现，参考的是徐晓强等人的《基于改进粒子群算法的牙齿正畸路径规划方法》，与这篇文章的区别在于：

1.徐等的文章设计了一种改进的粒子群算法，此篇使用的是普通的粒子群算法。

2.徐等的文章是虽然是对三维进行建模，但是对二维的仿真，而此篇是直接做三维仿真。

3.徐等的文章利用matlab实现在基准函数进行了模型测试，此篇用python实现简单的路径规划仿真。

此篇涉及的知识点有：

1.粒子群算法：如何直观形象地理解粒子群算法？

2.obb包围盒实现：Python实现obb包围盒及包围框

3.python如何读取stl格式数据

主函数

import numpy as np

from sympy import *

from matplotlib import cm

import math

import pylab as pl

import scipy as sp

from stl import stl

from numpy.linalg import solve

import matplotlib.pyplot as plt

import mpl_toolkits.mplot3d as a3

import matplotlib.colors as colors

from mpl_toolkits.mplot3d import Axes3D

from orthodontic import Tooth_dot,Tooth_obb_box,Tooth_obb_plot,Tooth_obb_plot2,Orthodontic,Particle_generation

from orthodontic import PSO

fig = plt.figure()

ax = Axes3D(fig)

#加载牙齿的stl数据

stl_list=[0,1,2,3,4,5,6,7,8,9,10,11,12,13]

# stl_list=[13]

z=5#30个粒子

m=len(stl_list)#牙齿个数

n=5#七个阶段

iter_max=10

#通过随机平移和旋转生成z个粒子

pt=Particle_generation(z,m,n)

# print(pt)

#得到最好的粒子

best_pt=PSO(pt,z,m,n,iter_max)

# 获取牙齿的stl点坐标,理想位置

for k in range(n):

for i in range(0,1):

dot=Tooth_dot(stl_list[i])

b,x,ans,les,cos=Tooth_obb_box(dot)

# diag_b=b.diagonal()

# for i in range(3):

# print(x,math.acos(diag_b[i])*180/math.pi)

color_bar=['purple','y','orange','g','k','gray','purple']

#矫正位置

orth_x=best_pt[i,k,0:3]

orth_ang=best_pt[i,k,3:6]

orth_b,orth_ans=Orthodontic(b,cos,orth_ang,orth_x,les)

if k==0:

Tooth_obb_plot(ax,dot,orth_b,orth_x,orth_ans,'gp')

Tooth_obb_plot(ax,dot,orth_b,orth_x,orth_ans,color_bar[k])

if k==n-1:

Tooth_obb_plot2(ax,dot,b,x,ans,'rp')

plt.pause(0.000001)

plt.show()

#obb碰撞检测参考：http://www.doc8