def test_nodes_on_obstacles():
scenario_3d_data = {
"size": 16,
"x": [4, 5, 6, 7, 4, 5, 6, 7, 4, 5, 6, 7, 4, 5, 6, 7],
"y": [6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6],
"z": [2, 2, 2, 2, 3, 3, 3, 3, 4, 4, 4, 4, 5, 5, 5, 5]
}
obstacle_3D_array = Model.create_obstacle_array(scenario_3d_data)
test_good_waypoint_array = [Node(5, 9, 2), Node(5, 0, 4)]
assert Model.nodes_on_obstacles(obstacle_3D_array,
test_good_waypoint_array) == False
test_error_waypoint_array = [Node(5, 9, 2), Node(6, 6, 5)]
Model.nodes_on_obstacles(obstacle_3D_array,
test_error_waypoint_array) == True
def __init__(self, scenario):
dimension = scenario["dimension"]
self.is_2d = Model.is_two_dimensional(dimension)
if "data" in scenario:
self.obstacle_array = Model.create_obstacle_array(scenario["data"])
else:
self.obstacle_array = []
self.num_obstacles = len(self.obstacle_array)
self.waypoint = scenario["waypoint"]
start = self.waypoint["start"]
self.start_node = Node(start["x"], start["y"]) if self.is_2d else Node(start["x"], start["y"], start["z"])
self.start_node.set_as_start_node()
stop = self.waypoint["stop"]
self.stop_node = Node(stop["x"], stop["y"]) if self.is_2d else Node(stop["x"], stop["y"], stop["z"])
self.last_node_key = str(self.stop_node)
self.allow_diagonal = bool(self.waypoint["allowDiagonal"]) if "allowDiagonal" in self.waypoint else False
model = Model(dimension, self.obstacle_array, self.waypoint, True)
self.Q = model.create_initial_Q(False)
self.open_set = dict()
self.open_set[str(self.start_node)] = self.Q.get(str(self.start_node))
if Model.nodes_on_obstacles(self.obstacle_array, [self.start_node, self.stop_node]):
message = "[Waypoint Error] start position or stop position is on the obstacle."
print(message)
self.message = message
self.boundary = scenario["boundary"] if "boundary" in scenario else None
if not self.is_2d:
self.z_ceil = int(self.boundary["zCeil"]) if (self.boundary and "zCeil" in self.boundary) else inf
self.z_floor = int(self.boundary["zFloor"]) if (self.boundary and "zFloor" in self.boundary) else -inf
# print("z_ceil: {}, z_floor: {}".format(self.z_ceil, self.z_floor))
if not Model.is_boundary_available(self.z_floor, self.start_node.z, self.z_ceil):
message = "[Boundary Error] start position is out of boundary."
print(message)
self.message = message
self.message = "[Ready] No Results."
def __init__(self, scenario, options=None):
self.dimension = scenario["dimension"]
self.is_2d = Model.is_two_dimensional(self.dimension)
if "data" in scenario:
self.obstacle_array = Model.create_obstacle_array(scenario["data"])
else:
self.obstacle_array = []
self.num_obstacles = len(self.obstacle_array)
self.waypoint = scenario["waypoint"]
start = self.waypoint["start"]
self.start_node = Node(start["x"], start["y"]) if self.is_2d else Node(
start["x"], start["y"], start["z"])
self.start_node.set_as_start_node()
stop = self.waypoint["stop"]
self.stop_node = Node(stop["x"], stop["y"]) if self.is_2d else Node(
stop["x"], stop["y"], stop["z"])
self.last_node_key = str(self.stop_node)
self.allow_diagonal = bool(
self.waypoint["allowDiagonal"]
) if "allowDiagonal" in self.waypoint else False
if options is None:
self.debug_mode = False
self.is_fast = True
else:
self.debug_mode = True if 'debug_mode' in options and options[
'debug_mode'] is True else False
self.is_fast = False if 'type' in options and options[
'type'] == 'original' else True
if self.debug_mode:
print("A* Path Finding (2D)") if self.is_2d else print(
"A* Path Finding (3D)")
model = Model(self.dimension, self.obstacle_array, self.waypoint,
self.debug_mode)
self.Q = model.create_initial_Q(self.is_fast)
self.open_set = dict()
self.open_set[str(self.start_node)] = self.Q.get(str(self.start_node))
self.message = "[Done] no results."
if Model.nodes_on_obstacles(self.obstacle_array,
[self.start_node, self.stop_node]):
message = "[Waypoint Error] start position or stop position is on the obstacle."
print(message)
self.message = message
self.boundary = scenario["boundary"] if "boundary" in scenario else None
if not self.is_2d:
self.z_ceil = int(self.boundary["zCeil"]) if (
self.boundary and "zCeil" in self.boundary) else inf
self.z_floor = int(self.boundary["zFloor"]) if (
self.boundary and "zFloor" in self.boundary) else -inf
# print("z_ceil: {}, z_floor: {}".format(self.z_ceil, self.z_floor))
if not Model.is_boundary_available(self.z_floor, self.start_node.z,
self.z_ceil):
message = "[Boundary Error] start position is out of boundary."
print(message)
self.message = message
grouping = scenario["grouping"] if "grouping" in scenario else None
# Only for integer type
# self.is_grouping = True if grouping is not None and "radius" in grouping and str(grouping["radius"]).isnumeric() else False
# For integer and float type
self.is_grouping = True if grouping is not None and "radius" in grouping and Tools.is_number(
str(grouping["radius"])) else False
self.group_radius = float(
grouping["radius"]) if self.is_grouping else 0
self.is_group_flat = True if self.is_2d or "boundary" in scenario else False
self.num_obstacles_in_start_group = 0
self.num_obstacles_in_stop_group = 0
if self.is_grouping:
grouping_style = 'circle' if self.is_group_flat else 'sphere'
print('[Grouping] radius', (self.group_radius + 0.6), 'of',
grouping_style)
for obstacle in self.obstacle_array:
if Tools.intersect(self.start_node, obstacle,
self.group_radius, self.is_group_flat):
# message = f'[Grouping Error] obstacle is in the start {grouping_style}'
# print(message)
self.num_obstacles_in_start_group = self.num_obstacles_in_start_group + 1
if Tools.intersect(self.stop_node, obstacle, self.group_radius,
self.is_group_flat):
# message = f'[Grouping Error] obstacle is in the stop {grouping_style}'
# print(message)
self.num_obstacles_in_stop_group = self.num_obstacles_in_stop_group + 1
if self.num_obstacles_in_start_group > 0:
print(f'[Grouping Error] {self.num_obstacles_in_start_group} obstacle is in the start {grouping_style}') if self.num_obstacles_in_start_group == 1 \
else print(f'[Grouping Error] {self.num_obstacles_in_start_group} obstacles are in the start {grouping_style}')
if self.num_obstacles_in_stop_group > 0:
print(f'[Grouping Error] {self.num_obstacles_in_stop_group} obstacle is in the stop {grouping_style}') if self.num_obstacles_in_stop_group == 1 \
else print(f'[Grouping Error] {self.num_obstacles_in_stop_group} obstacles are in the stop {grouping_style}')