search_tree-CSDN博客

本文链接：https://blog.csdn.net/mvpboy123/article/details/135989424

该代码段描述了一个名为`defsearch_tree`的函数，用于使用BFSTree算法判断输入点是否接近现有中心线。它通过广度优先搜索计算距离并优化节点选择，当找到最邻近值后，根据角度进行进一步优化。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

def search_tree(self, root: TreeNode, point, gap_dis = 4,judge_seed= “seed”,appen_point_list= None):
‘’’
BFS tree, determine whether the current input point is close to the existing centerline
:param root:
:param point:
:return:
‘’’
queue = []
queue.append(root)
print(“point strat”,point)
if judge_seed == “seed”:
while queue:
vertex = queue.pop(0)
point = np.array(point)
dis_all = np.linalg.norm(point - vertex.value, axis=1)
dis = dis_all.min()
index = dis_all.argmin()
if dis < 4:
return vertex, index
nodes = vertex.child_list
for w in nodes:
queue.append(w)
return None
else:
all_point = []
all_point.append(root)
min_all_point = []
min_index_list = []
while queue: #先将所有的点append到list
vertex = queue.pop(0)
nodes = vertex.child_list
for w in nodes:
queue.append(w)
all_point.append(w)
for i in range(len(all_point)): #循环找到最小的
vertex = all_point[i]
point = np.array(point)
dis_all = np.linalg.norm(point - vertex.value, axis=1)
dis = dis_all.min()
index = dis_all.argmin()
min_all_point.append(dis)
min_index_list.append(index)
min_diatance = min(min_all_point) # 获取最小值
vertex_index = min_all_point.index(min_diatance) # 获取最小值位置
vertex2 = all_point[vertex_index]
if min_diatance < gap_dis :
index2 = min_index_list[vertex_index]
p0 = np.array([vertex2.value[index2][0], vertex2.value[index2][1], vertex2.value[index2][2]])
dis_all2 = np.linalg.norm(p0 - point)
if self.diatance is None:
self.diatance = min_diatance
self.vertex = vertex2
self.index = min_index_list[vertex_index]
elif min_diatance <= self.diatance:
self.diatance = min_diatance
self.vertex = vertex2
self.index = min_index_list[vertex_index]
print(point,“point min distance***************************”,self.diatance)
else:#进入这个分支代表已经找到最临近值，再次基础上，在算一下角度找个最优。
print(dis_all2,“vs”,min_diatance)
p1 = np.array([self.vertex.value[self.index][0], self.vertex.value[self.index][1], self.vertex.value[self.index][2]])
p2 = np.array([point[0],point[1],point[2]])
p3 = np.array([appen_point_list[-1][0],appen_point_list[-1][1],appen_point_list[-1][2]])
p4 = np.array([appen_point_list[-2][0],appen_point_list[-2][1],appen_point_list[-2][2]])
vector1 = p2 - p1
vector2 = p3 - p2
curr_angle = get_angle(vector1,vector2)
print(p1,p2,p3)
print(“curr_angle”,curr_angle)
if curr_angle > 100 and self.index > 1:
for k in range(self.index,-1,-1):
p0 = np.array([self.vertex.value[k][0], self.vertex.value[k][1], self.vertex.value[k][2]])
vector3 = p3 - p0
vector4 = p4 - p3
print(p1,p2,p3,p0)
curr_angle2 = get_angle(vector3,vector4)
dis_all2 = np.linalg.norm(p0 - p3)
# print(p0,p2)
dis2 = dis_all2.min()
print(“dis2”,k,dis2,curr_angle,curr_angle2)
print(“next”)
for k in range(self.index,self.index+100):
p0 = np.array([self.vertex.value[k][0], self.vertex.value[k][1], self.vertex.value[k][2]])
vector3 = p3 - p0
vector4 = p4 - p3
print(p1,p2,p3,p0)
curr_angle = get_angle(vector3,vector4)
dis_all2 = np.linalg.norm(p0 - p3)
# print(p0,p2)
dis2 = dis_all2.min()
print(“dis2”,k,dis2,curr_angle)
print(“next”)

                self.diatance = None
                return self.vertex, self.index
        elif self.diatance is  None:
             return None
        else:
            self.diatance = None
            print("1111ok")
            return self.vertex, self.index