def test_obstacle_collision(config, spec, obstacles):
for i in range(len(config.lengths)):
p = config.points[i]
q = config.points[i + 1]
for o in obstacles:
if not test_bounding_box(p, q, (o.x1, o.y1), (o.x2, o.y2)):
continue
for e in o.edges:
if test_line_collision((p, q), e):
return False
return True
def test_self_collision(config):
if len(config.lengths) < 3:
return True
for i in range(len(config.lengths) - 1):
p1 = config.points[i]
q1 = config.points[i + 1]
for j in range(i + 2, len(config.lengths)):
p2 = config.points[j]
q2 = config.points[j + 1]
if test_line_collision((p1, q1), (p2, q2)):
return False
return True
def test_obstacle_collision(config, spec, obstacles):
# return true for pass, false for fail
for i in range(spec.num_segments):
p = config.points[i]
q = config.points[i + 1]
for o in obstacles:
# bounding box check
if not test_bounding_box(p, q, (o.x1, o.y1), (o.x2, o.y2)):
continue
# full edge check
for e in o.edges:
if test_line_collision((p, q), e):
# collision between robot segment and obstacle edge
return False
return True
def test_collision_between_two_parts(config1, config2):
point = config1.points[0]
lines1 = []
for i in range(1, len(config1.points)):
lines1.append((point, config1.points[i]))
point = config1.points[i]
point = config2.points[0]
lines2 = []
for i in range(1, len(config2.points)):
lines2.append((point, config2.points[i]))
point = config2.points[i]
for l1 in lines1:
for l2 in lines2:
if test_line_collision(l1, l2):
return False
return True
def custom_test_obstacle_collision(config: robot_config.RobotConfig,
spec: problem_spec.ProblemSpec, obstacles):
# return true for pass, false for fail
for i in range(spec.num_segments):
p = config.points[i]
q = config.points[i + 1]
for o in obstacles:
line_start_x = p[0]
line_end_x = q[0]
line_start_y = p[1]
line_end_y = q[1]
for j in range(12):
delta_x = (line_end_x - line_start_x) / (j + 1)
delta_y = (line_end_y - line_start_y) / (j + 1)
if j == 0:
delta_x = 0
delta_y = 0
elif j == 11:
delta_x = (line_end_x - line_start_x)
delta_y = (line_end_y - line_start_y)
x = line_start_x + delta_x
y = line_start_y + delta_y
if (o.x1 == o.x2):
if (y < o.y2 + COLLISION_TOLERANCE) and (
y > o.y1 - COLLISION_TOLERANCE):
return False
elif (o.y1 == o.y2):
if (x < o.x2 + COLLISION_TOLERANCE) and (
x > o.x1 - COLLISION_TOLERANCE):
return False
elif (x<o.x2 +COLLISION_TOLERANCE) and (x>o.x1 -COLLISION_TOLERANCE) and\
(y<o.y2 +COLLISION_TOLERANCE) and (y>o.y1 -COLLISION_TOLERANCE):
return False
# bounding box check
if not custom_test_bounding_box(p, q, (o.x1, o.y1), (o.x2, o.y2)):
continue
# full edge check
for e in o.edges:
if tester.test_line_collision((p, q), e):
# collision between robot segment and obstacle edge
return False
return True
def test_self_collision(self, config, spec):
# return true for pass, false for fail
if spec.num_segments < 3:
# collision impossible with less than 3 segments
return True
# do full check
for i in range(spec.num_segments - 1):
p1 = config.points[i]
q1 = config.points[i + 1]
for j in range(i + 2, spec.num_segments):
p2 = config.points[j]
q2 = config.points[j + 1]
if test_line_collision((p1, q1), (p2, q2)):
return False
return True
