def optimize_trajectory(states_groups):
	trajectory = None
	
	
	
	if states_groups:
		def generate_initial_state_node(state):
			state_node = \
				_StateNode(
					state                  = state,
					predecessor_state_node = None,
					segment                = None,
					estimation             = 0.0
				)
				
			return state_node
			
		open_states_nodes = \
			[generate_initial_state_node(initial_state) \
				for initial_state \
				in states_groups[0]]
				
				
		for states_group in states_groups[1:]:
			open_states_nodes = \
				optimize_transition(
					open_states_nodes,
					states_group
				)
				
				
				
		if open_states_nodes:
			best_goal_state_node_filter = \
				Filter(
					key = (lambda goal_state_node: goal_state_node.estimation)
				)
				
			best_goal_state_node_filter.check_elements(open_states_nodes)
			
			
			trajectory = []
			state_node = best_goal_state_node_filter.minimal_element
			
			while state_node.predecessor_state_node is not None:
				trajectory.insert(0, state_node.segment)
				state_node = state_node.predecessor_state_node
				
				
				
	return trajectory
def optimize_transition(predecessor_states_nodes, successor_states):
	successor_states_nodes = []
	
	
	for successor_state in successor_states:
		successor_state_node_filter = \
			Filter(
				key = (lambda state_node: state_node.estimation)
			)
			
			
		for predecessor_state_node in predecessor_states_nodes:
			predecessor_state      = predecessor_state_node.state
			predecessor_estimation = predecessor_state_node.estimation
			
			
			computing_result = \
				compute_shortest_segment(
					successor_state.predecessor_polygon,
					predecessor_state.successor_geometry_state,
					successor_state.predecessor_geometry_state,
					factors
				)
				
			if computing_result is not None:
				segment_length, segment = computing_result
				estimation              = \
					segment_length \
						+ predecessor_estimation
						
				successor_state_node = \
					_StateNode(
						state                  = successor_state,
						predecessor_state_node = predecessor_state_node,
						segment                = segment,
						estimation             = estimation
					)
					
				successor_state_node_filter.check_element(successor_state_node)
				
				
		if successor_state_node_filter.contains_element:
			successor_states_nodes.append(
				successor_state_node_filter.minimal_element
			)
			
			
	return successor_states_nodes
Пример #3
0
	def __init__(self, vertices, *args, **kwargs):
		super().__init__(*args, **kwargs)
		
		
		self.__vertices = list(vertices)
		
		
		# Проверка вершин (должно быть не менее 3 вершин)
		if len(self.__vertices) < 3:
			raise Exception() #!!!!!
			
			
		# Проверка вершин (должны образовывать выпуклый многоугольник)
		# и вычисление ориентации полигона - направления обхода вершин
		# (положительное значение соответствует обходу против часовой стрелки)
		area_filter = Filter()
		
		
		last_vector = self.__vertices[-1] - self.__vertices[-2]
		last_vertex = self.__vertices[-1]
		
		for vertex in self.__vertices:
			first_vector  = last_vector
			second_vector = vertex - last_vertex
			
			
			area = \
				first_vector.real * second_vector.imag \
					- first_vector.imag * second_vector.real
					
			if area == 0.0:
				raise Exception() #!!!!!
			else:
				area_filter.check_element(area)
				
				
			last_vector = second_vector
			last_vertex = vertex
			
			
		if area_filter.minimal_element < 0.0 < area_filter.maximal_element:
			raise Exception() #!!!!!
		else:
			self.__orientation = \
				area_filter.maximal_element \
					/ abs(area_filter.maximal_element)