def test_triangle(self):
triang = pycrcc.Triangle(0, 0, 10, 0, 4, 5)
circ1 = pycrcc.Circle(2.5, 6, 7)
self.assertEqual(triang.collide(circ1), 0)
circ2 = pycrcc.Circle(2.5, 5, 7)
self.assertEqual(triang.collide(circ2), 1)
obb3 = pycrcc.RectOBB(1, 2, 0.6, 9, 3)
self.assertEqual(triang.collide(obb3), 0)
obb4 = pycrcc.RectOBB(1, 2, 0.4, 9, 3)
self.assertEqual(triang.collide(obb4), 1)
triang = pycrcc.Triangle(0, 0, 10, 0, 4, 5)
triang2 = pycrcc.Triangle(10, 2, 2, 2, 5, 5)
obb5 = pycrcc.RectOBB(1, 1.3, 0.6, 10.8, 0.8)
self.assertEqual(triang.collide(obb5), 0)
self.assertEqual(triang2.collide(obb5), 0)
obb6 = pycrcc.RectOBB(1, 1.3, 0.6, 10.6, 0.6)
self.assertEqual(triang.collide(obb6), 1)
obb7 = pycrcc.RectOBB(1, 1.3, 0.6, 10.8, 0.6)
self.assertEqual(triang.collide(obb7), 1)
self.assertEqual(triang2.collide(obb7), 0)
obb8 = pycrcc.RectOBB(1, 1.3, 0.6, 10.6, 1)
self.assertEqual(triang.collide(obb8), 0)
self.assertEqual(triang2.collide(obb8), 1)
def test_OBBCircle(self):
obb1 = pycrcc.RectOBB(1, 2, 0.2, 9, 10)
obb2 = pycrcc.RectOBB(1, 2, 0.1, 9, 10)
circ = pycrcc.Circle(2.5, 6, 7)
self.assertEqual(obb1.collide(circ), 0)
self.assertEqual(obb2.collide(circ), 1)
def test_AABBCircle(self):
aabb = pycrcc.RectAABB(2, 3, 3, 1.8)
aabb2 = pycrcc.RectAABB(2, 3, 3, 1.7)
circ = pycrcc.Circle(2.5, 6, 7)
self.assertEqual(aabb.collide(circ), 1)
self.assertEqual(aabb2.collide(circ), 0)
def test_cc(self):
aabb = pycrcc.RectAABB(2, 3, 3, 1.8)
aabb2 = pycrcc.RectAABB(2, 3, 3, 1.7)
circ = pycrcc.Circle(2.5, 6, 7)
cc = pycrcc.CollisionChecker()
cc.add_collision_object(aabb)
obb1 = pycrcc.RectOBB(2, 1, 1.5, 6.0, 0)
obb2 = pycrcc.RectOBB(2, 1, 1.5, 6.0, 20)
self.assertEqual(cc.collide(circ), 1)
tvo1 = pycrcc.TimeVariantCollisionObject(1)
tvo1.append_obstacle(pycrcc.RectOBB(2, 1, 0.0, 2.0, 5)) # time step 1
tvo1.append_obstacle(pycrcc.RectOBB(2, 1, 0.0, 2.5, 5)) # time step 2
tvo1.append_obstacle(pycrcc.RectOBB(2, 1, 0.0, 3, 5)) # time step 3
tvo1.append_obstacle(pycrcc.RectOBB(2, 1, 0.0, 3.5, 5)) # time step 4
tvo1.append_obstacle(pycrcc.RectOBB(2, 1, 0.0, 4, 5)) # time step 5
tvo1.append_obstacle(pycrcc.RectOBB(2, 1, 0.0, 4.5, 5)) # time step 6
tvo1.append_obstacle(pycrcc.RectOBB(2, 1, 0.0, 5, 5)) # time step 7
tvo1.append_obstacle(pycrcc.RectOBB(2, 1, 0.0, 5.5, 5)) # time step 8
# Time variant obstacle that starts at time 4
tvo2 = pycrcc.TimeVariantCollisionObject(4)
tvo2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 0)) # time step 4
tvo2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 2)) # time step 5
tvo2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 3)) # time step 6
tvo2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 4)) # time step 7
tvo2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 5)) # time step 8
tvo2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 6)) # time step 9
tvo2.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 7)) # time step 10
cc = pycrcc.CollisionChecker()
cc.add_collision_object(tvo2)
obb1 = pycrcc.RectOBB(2, 1, 1.5, 6.0, 0)
obb2 = pycrcc.RectOBB(2, 1, 1.5, 6.0, 20)
self.assertEqual(tvo2.collide(obb1), 1)
self.assertEqual(cc.collide(obb1), 1)
self.assertEqual(cc.collide(obb2), 0)
cc2 = pycrcc.CollisionChecker()
cc2.add_collision_object(obb2)
self.assertEqual(cc2.collide(tvo2), 0)
cc2.add_collision_object(obb1)
self.assertEqual(cc2.collide(tvo2), 1)
cc3 = pycrcc.CollisionChecker()
cc3.add_collision_object(tvo1)
self.assertEqual(cc3.collide(tvo2), 1)
tvo3 = pycrcc.TimeVariantCollisionObject(12)
tvo3.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 0))
tvo3.append_obstacle(pycrcc.RectOBB(2, 1, 1.5, 6.0, 2))
cc4 = pycrcc.CollisionChecker()
cc4.add_collision_object(tvo2)
cc4.add_collision_object(pycrcc.RectOBB(2, 1, 1.5, 6.0, 0))
self.assertEqual(cc4.collide(tvo3), 1)
def create_collision_object_circle(circle,
params=None,
collision_object_func=None):
return pycrcc.Circle(circle.radius, circle.center[0], circle.center[1])
def create_random_sphere(self):
center = self.generate_random_vector()
rad = abs(self.generate_random_xvalue())
return pycrcc.Circle(rad, center[0], center[1])
def triangulate(bounds, vertices, input_triangles, output_triangles, params):
"""Fill the scenario with triangles using the Triangle Library by Jonathan Richard Shewchuk:
https://www.cs.cmu.edu/~quake/triangle.html
To use it, Triangle is called from the wrapper library "triangle"
Step 1: Write the vertices and edges of the lane boundaries
Step 2: Call Triangle
Step 3: Read the triangles, construct them as collision objects, remove triangles that are in the road
"""
#steiner_ratio: determines the maximum amount of steiner points based on the number of vertices of the input
#eg: steiner_ratio = 0.1, 100 vertices => 0.1*100 = 10 steiner points allowed at most
steiner_ratio = params.get('steiner_ratio', 0.1)
steiner_max = (len(bounds) + len(vertices)) * steiner_ratio
# Triangle call string, default options used:
# - q quality mesh generation with minimal 20 degrees for each triangle
# - S no additional steiner points
# - c the points are enclosed by their convex hull, by default the option is only enabled if single vertices without
# edges are given
# for further options see
# http://dzhelil.info/triangle/index.html#api and https://www.cs.cmu.edu/~quake/triangle.switch.html
# eg. call_options: 'S1000q25'
defaul_calloptions = 'qS' + str(steiner_max)
# Build triangles in the convex hull defined by the vertices, if vertices without edges are given as input
if vertices:
defaul_calloptions += 'c'
call_options = 'p' + params.get('call_options' , defaul_calloptions)
#Parameter that determines if the hole areas are written to file
#Can be problematic when using section triangle representation
input_holes = params.get('input_holes', False)
#radius of the circles that are used for filtering triangles in the road
#value of 0 leads to a point
#a bigger radius can be used if some triangles between lanelets are not filtered
#a small radius can be used if small triangles near the road are filtered unwarrented
filter_radius = params.get('filter_radius', 0)
# Make sets for nodes, segments, holes
# The sets eliminate all duplicate elements, so each node/segment/hole is written to file only once.
nodeset = set()
segset = set()
holeset = set()
def addnode(vertex):
# The required format for nodes is: x, y
# The index is added to each entry after all nodes have been added.
nodeset.add((*vertex,))
#Input all individual vertices, that don't have an edge
for v in vertices:
addnode(v)
def addseg(v1, v2):
# The required format for segments is: v1_index, v2_index
# The frozenset makes sure that there are no duplicate edges with different order of vertices, ie. (v,w) and (w,v).
t = tuple([tuple([*v1]), tuple([*v2])]) #wrapping vertices for frozenset
segset.add(frozenset(t))
def addedge(v1, v2):
addnode(v1)
addnode(v2)
addseg(v1, v2)
# Input all edges from the lanelet boundaries
for bound in bounds:
addedge(*bound)
def addhole(v1, v2, v3):
# The required format for holes is: x, y
# A hole marks an area where no triangles should be generated.
# In this case, each input triangles defines a hole area, so that the road is not triangulated.
center = (v1 + v2 + v3) / 3
holeset.add((*center,))
# Input holes from input_triangles
if input_holes:
for t in input_triangles.unpack():
vertices = t.vertices()
vertices = [np.array(v) for v in vertices]
addhole(*vertices)
# Convert each set so that they apply to the required format
def map_segment(entry, vertices):
v1, v2 = entry
v1_index = [i for i, entry in enumerate(vertices) if entry == v1][0]
v2_index = [i for i, entry in enumerate(vertices) if entry == v2][0]
return (v1_index, v2_index)
vertices = [node for node in nodeset]
#the if condition eliminates edges that have two identical vertices as endpoints
segset = set([map_segment(entry, vertices) for entry in segset if len(entry) == 2])
vertices = np.array(vertices)
segments = np.array([seg for seg in segset])
holes = np.array([hole for hole in holeset])
if(holes):
output = triangle.triangulate({'vertices': vertices, 'segments': segments, 'holes': holes}, call_options)
else:
output = triangle.triangulate({'vertices': vertices, 'segments': segments}, call_options)
vertices = output.get('vertices')
tris = output.get('triangles')
for t in tris:
points = [vertices[index] for index in t]
a, b, c = (points[0], points[1], points[2])
collision_triangle = pycrcc.Triangle(*a, *b, *c)
# remove problem spots:
middle = (a + b + c) / 3
if filter_radius == 0:
middle_point = pycrcc.Point(*middle)
else:
middle_point = pycrcc.Circle(filter_radius, *middle)
# uncomment for drawing middle points
# draw_object(middle_point, draw_params={'collision': {'circle': {'facecolor': '#1f78b4', 'zorder': 30, 'edgecolor' :'#000000'}}})
if not input_triangles or not middle_point.collide(input_triangles):
output_triangles.add_shape(collision_triangle)