三次元版は、あまりないようでしたので、書いてみました。
細かい動作確認までは行っていませんが、以下の通りです。
最短経路を求める為、ダイクストラ法でも良かったかも
# coding: utf-8 import sys import numpy as np # cf. https://shapely.readthedocs.io/en/stable/manual.html from shapely.geometry import Polygon from shapely.geometry import MultiPolygon import matplotlib.pyplot as plt import mpl_toolkits.mplot3d.art3d as art3d max_seek_loop = 5000 def main(): # 壁,床,天井polygonのmesh化生成 my_meshs = MyMeshs() ( meshs_floor,meshs_ceil, meshs_wall_a,meshs_wall_b, meshs_wall_c,meshs_wall_d ) = my_meshs.init_floor_ceil_wall() # a-starによる経路探索 astar_start = [ 5, 0,5] astar_goal = [55,40,5] my_astar = Astar( astar_start, astar_goal, meshs_floor,meshs_ceil, meshs_wall_a,meshs_wall_b, meshs_wall_c,meshs_wall_d ) goal_node = now_node = my_astar.find_path() # parent_nodeをたどり、経路を収集 path_nodes = [] while now_node and now_node.parent_node: path_nodes.insert(0, now_node.pos ) global tmp_node now_node = now_node.parent_node path_nodes = np.array(path_nodes) # 最後に、3Dで表示 fig = plt.figure() ax = fig.add_subplot(111, projection='3d') ax.scatter3D(path_nodes[:,0], path_nodes[:,1], path_nodes[:,2], col or='red', s=50) for meshs in [meshs_ceil, meshs_wall_a, meshs_wall_b, meshs_wall_c, meshs_wall_d ]: for mesh in meshs: ax.scatter3D(mesh[:,0],mesh[:,1],mesh[:,2], color='gray', s=5) ax.add_collection3d( art3d.Poly3DCollection(meshs, facecolors='gray', linewidths=0.5, edgecolors='gray', alpha=.01) ) plt.show() #cf. https://stone-program.com/python/algorithm/a-star-introduction/ class Astar: # F = G + H # F: total距離 # G: startからnow_nodeの実移動距離 # H: now_nodeからgoalまでの仮距離. Heuristic def __init__(self, start_coord,goal_coord, meshs_floor,meshs_ceil, meshs_wall_a,meshs_wall_b, meshs_wall_c,meshs_wall_d ): self.open_list = AstarNodeList() self.close_list = AstarNodeList() self.nodes = [] # あえて床は探索経路から除く for meshs in [ meshs_ceil, #meshs_floor meshs_wall_a,meshs_wall_b, meshs_wall_c,meshs_wall_d ]: for mesh_coord in meshs: node = AstarNode(mesh_coord,start_coord,goal_coord ) self.nodes.append( node ) if node.hs_start == 0: self.start_node = node if node.hs_goal == 0: self.goal_node = node def find_next_node(self, edge_coord, except_pos ): for node in self.nodes: if node.has_equal_edge( edge_coord ) and \ node.pos != except_pos : return node def find_path(self): self.open_list.append( self.start_node ) self.start_node.fs = self.start_node.hs_goal i = 0 while True: i += 1 if i > max_seek_loop: print( "Reached MAX_SEEK_LOOP %d" % {max_seek_loop}, file=sys.stderr ) return None #Openリストが空になったら解なし if len( self.open_list.nodes ) == 0: print("There is no route.", file=sys.stderr ) return None # 総移動距離:Fが短い順にsort sorted_list = sorted(self.open_list.nodes, key=lambda x: x.fs ) now_node = sorted_list[0] # print( "NOW:",now_node.pos, now_node) self.open_list.remove( now_node) self.close_list.append(now_node) if now_node.hs_goal == 0: # ゴール到達 print( "You reached to GOAL", file=sys.stderr ) return now_node # F=G+H は G(実移動距離)=F-H に変形できることから以下 n_gs = now_node.fs - now_node.hs_goal for edge_coord in now_node.edges(): #移動先のノードがOpen,Closeのどちらのリストに #格納されているか、または新規ノードなのかを調べる next_node = self.open_list.find_next_node( edge_coord, now_node.pos ) if next_node: dist = now_node.distance_to_coord( next_node.pos ) if next_node.fs > n_gs + next_node.hs_goal + dist: next_node.fs = n_gs + next_node.hs_goal + dist next_node.parent_node = now_node continue next_node = self.close_list.find_next_node( edge_coord, now_node.pos ) if next_node: dist = now_node.distance_to_coord( next_node.pos ) if next_node.fs > n_gs + next_node.hs_goal + dist: next_node.fs = n_gs + next_node.hs_goal + dist next_node.parent_node = now_node self.open_list.append(next_node) self.closelist.remove(next_node) continue next_node = self.find_next_node( edge_coord, now_node.pos ) if not next_node: continue dist = now_node.distance_to_coord( next_node.pos ) next_node.fs = n_gs + next_node.hs_goal + dist next_node.parent_node = now_node self.open_list.append(next_node) class AstarNodeList: def __init__(self): self.nodes = [] def append(self, node): if isinstance(node, AstarNode): self.nodes.append(node) def remove(self, target_node): i = 0 for node in self.nodes: if target_node.pos == node.pos: self.nodes.pop(i) return i += 1 def find_next_node(self, edge_coord, except_pos ): for node in self.nodes: if node.has_equal_edge( edge_coord ) and \ node.pos != except_pos : return node class AstarNode: def __init__(self, org_mesh, start_coord, goal_coord ): self.mesh = org_mesh # 頂点座標 # 重心座標 self.pos = [ (org_mesh[0][0]+org_mesh[2][0])/2, (org_mesh[0][1]+org_mesh[2][1])/2, (org_mesh[0][2]+org_mesh[2][2])/2 ] self.hs_start = self.distance_to_coord( start_coord ) self.hs_goal = self.distance_to_coord( goal_coord ) # F = G + H # F: total距離 # G: startからnow_nodeの実移動距離 # H: now_nodeからgoalまでの仮距離. Heuristic self.fs = 0 self.owner_list = [] self.parent_node = None def edges(self): return [[self.mesh[0], self.mesh[1]], [self.mesh[1], self.mesh[2]], [self.mesh[2], self.mesh[3]], [self.mesh[3], self.mesh[0]] ] def has_equal_edge(self, edge_coord ): for edge in self.edges(): if list(edge[0]) == list(edge_coord[0]) and \ list(edge[1]) == list(edge_coord[1]): return True if list(edge[0]) == list(edge_coord[1]) and \ list(edge[1]) == list(edge_coord[0]): return True return False def distance_to_coord(self, other_coord ): dist = \ (self.pos[0] - other_coord[0])**2 + \ (self.pos[1] - other_coord[1])**2 + \ (self.pos[2] - other_coord[2])**2 return dist class MyMeshs: def __init__(self): pass def init_floor_ceil_wall(self): # 床,天井,壁のpolygonを Local座標で作成 (floor, ceil, wall_a, wall_b, wall_c, wall_d) = \ self.make_polygon_for_floor_ceil_wall() # メッシュに分割し、numpyのlistを返却 mesh_size = 10 meshs_floor = self.divide_by_mesh(floor,mesh_size) meshs_ceil = self.divide_by_mesh(ceil, mesh_size) meshs_wall_a = self.divide_by_mesh(wall_a, mesh_size) meshs_wall_b = self.divide_by_mesh(wall_b, mesh_size) meshs_wall_c = self.divide_by_mesh(wall_c, mesh_size) meshs_wall_d = self.divide_by_mesh(wall_d, mesh_size) # 絶対座標へ変換(配置) meshs_ceil = self.mesh_coords_to_abs( meshs_ceil, [ self.affines("z90") ] ) meshs_wall_a = self.mesh_coords_to_abs( meshs_wall_a, [ self.affines("rx90")] ) meshs_wall_b = self.mesh_coords_to_abs( meshs_wall_b, [ self.affines("rx90" ), self.affines("rz180")]) meshs_wall_c = self.mesh_coords_to_abs( meshs_wall_c, [ self.affines("rx90"), self.affines("rz90")] ) meshs_wall_d = self.mesh_coords_to_abs( meshs_wall_d, [ self.affines("rx90"), self.affines("rz270")] ) return ( meshs_floor, meshs_ceil, meshs_wall_a, meshs_wall_b, meshs_wall_c, meshs_wall_d ) # 回転や移動のaffine変換定義 # cf. https://org-technology.com/posts/rotational-transformation-matrix.html def affines(self, conv_key): affines_def = { # Z方向へ+40移動 "z90" : np.array([[ 1, 0, 0, 0], [ 0, 1, 0, 0], [ 0, 0, 1,40], [ 0, 0, 0, 1]] ), # X軸に90度回転 "rx90" : np.array([[ 1, 0, 0, 0], # 1, 0, 0 [ 0, 0,-1, 0], # 0,cos,-sin [ 0, 1, 0, 0], # 0 sin, cos [ 0, 0, 0, 1]] ), # Z軸に90度回転後、Y方向へ+40 "rz90" : np.array([[ 0, 1, 0, 0], # cos,sin,0 [-1, 0, 0,40], # -sin,cos,0 [ 0, 0, 1, 0], # 0 0, 1 [ 0, 0, 0, 1]] ), # Z軸に180度回転後、X方向へ+70、Y方向へ+40 "rz180" : np.array([[-1, 0, 0,70], # cos,sin,0 [ 0,-1, 0,40], # -sin,cos,0 [ 0, 0, 1, 0], # 0 0, 1 [ 0, 0, 0, 1]] ), # Z軸に270度回転後、X方向へ+70 "rz270" : np.array([[ 0,-1, 0,70], # cos,sin,0 [ 1, 0, 0, 0], # -sin,cos,0 [ 0, 0, 1, 0], # 0 0, 1 [ 0, 0, 0, 1]] ) } return affines_def[conv_key] def mesh_coords_to_abs( self, meshs, affines ): ret_meshs = [] for mesh in meshs: np_mesh = np.array( mesh ).T # affine変換の為、最終行に[1,1,1,1]を一時追加 np_mesh = np.insert(np_mesh, 3,[1,1,1,1],axis=0) # アフィン変換 for affine in affines: np_mesh = np.dot( affine , np_mesh ) # 先程、一時追加した[1,1,1,1]を削除し、転置 np_mesh = np.delete(np_mesh, 3,axis=0).T ret_meshs.append(np_mesh) return np.array( ret_meshs ) def divide_by_mesh(self, polygon,mesh_size): (min_x,y, max_x,max_y) = polygon.bounds x = min_x meshes = [] while y < max_y: while x < max_x: mesh = Polygon( [ [x, y+mesh_size],[x+mesh_size,y+mesh_size], [x+mesh_size,y ],[x, y]] ) intersects = polygon.intersection(mesh) if intersects.type == "Polygon": (m_min_x,m_min_y, m_max_x,m_max_y) = intersects.bounds mesh = np.array( [[m_min_x,m_min_y,0],[m_min_x,m_max_y,0], [m_max_x,m_max_y,0],[m_max_x,m_min_y,0]] ) meshes.append( mesh ) elif intersects.type == "MultiPolygon": for intersect in intersects.geoms: if intersect.type != "Polygon": continue (m_min_x,m_min_y, m_max_x,m_max_y) = intersect.bounds mesh = np.array( [[m_min_x,m_min_y,0],[m_min_x,m_max_y,0], [m_max_x,m_max_y,0],[m_max_x,m_min_y,0]] ) meshes.append( mesh ) x += mesh_size x = min_x y += mesh_size return meshes def make_polygon_for_floor_ceil_wall(self): # 床/天井 horizontal_shell =[ [ 0, 0],[ 0,40],[70,40],[70, 0],[0, 0] ] floor = Polygon( shell=horizontal_shell ) ceil = Polygon( shell=horizontal_shell ) # ┌─壁-B─┐ #壁-C 壁-D # └─壁-A─┘ wall_ab_shell =[ [ 0, 0],[ 0,40],[70,40],[70, 0],[60,0], [60,30],[40,30],[40, 0],[ 0, 0] ] wall_ab_holes =[[[10,10],[30,10],[30,30],[10,30],[10,10]] ] wall_a = Polygon( shell=wall_ab_shell, holes=wall_ab_holes ) wall_b = Polygon( shell=wall_ab_shell, holes=wall_ab_holes ) wall_cd_shell =[ [ 0, 0],[ 0,40],[40,40],[40, 0],[0, 0] ] wall_cd_holes =[[[ 0,10],[30,10],[30,30],[ 0,30],[0,10]] ] wall_c = Polygon( shell=wall_cd_shell, holes=wall_cd_holes ) wall_d = Polygon( shell=wall_cd_shell, holes=wall_cd_holes ) return (floor, ceil, wall_a, wall_b, wall_c, wall_d) if __name__ == '__main__': main()
↑こう書くと、↓こう表示されます
