def test_unique():
tree = AABBTree()
aabb1 = AABB([(0, 1)])
aabb2 = AABB([(0, 1)])
aabb3 = AABB([(0, 1)])
tree.add(aabb1, 'box 1')
tree.add(aabb2, 'box 2')
vals = tree.overlap_values(aabb3, unique=True)
assert len(vals) == 1
vals = tree.overlap_values(aabb3, unique=False)
assert len(vals) == 2
assert 'box 1' in vals
assert 'box 2' in vals
class WayContainerAABBTree:
def __init__(self, d_max=0):
self.d_max = d_max
self.data = AABBTree()
def __del__(self):
pass
def insert(self, element):
a, b = element.get_axis_aligned_bounding_box()
aabb = AABB([(a[0], b[0]), (a[1], b[1])])
self.data.add(aabb, element)
def find_near_candidates(self, lat_lon, d_max):
if not math.isfinite(lat_lon[0]) or not math.isfinite(lat_lon[1]):
return []
# transfer bounding box of +/- d_max (in meter) to a +/- d_lon and d_lat
# (approximate, but very good for d_max << circumference earth)
d_lat, d_lon = LocalMap.get_scale_at(lat_lon[0], lat_lon[1])
d_lat *= d_max
d_lon *= d_max
# define an axis-aligned bounding box (in lat/lon) around the queried point lat_lon
bb = AABB([(lat_lon[0] - d_lat, lat_lon[0] + d_lat),
(lat_lon[1] - d_lon, lat_lon[1] + d_lon)])
# and query all overlapping bounding boxes of ways
candidates = self.data.overlap_values(bb)
return candidates
@staticmethod
def axis_aligned_bounding_boxes_overlap(a1, b1, a2, b2):
return np.all(a1 < b2) and np.all(a2 < b1)
def test_overlap_values():
aabbs = standard_aabbs()
values = ['value 1', 3.14, None, None]
aabb5 = AABB([(-3, 3.1), (-3, 3)])
aabb6 = AABB([(0, 1), (5, 6)])
aabb7 = AABB([(6.5, 6.5), (5.5, 5.5)])
for indices in itertools.permutations(range(4)):
tree = AABBTree()
for i in indices:
tree.add(aabbs[i], values[i])
vals5 = tree.overlap_values(aabb5)
assert len(vals5) == 2
for val in ('value 1', 3.14):
assert val in vals5
assert tree.overlap_values(aabb6) == []
assert tree.overlap_values(aabb7) == []
assert AABBTree(aabb5).overlap_values(aabb7) == []
def test_return_the_origin_pass_in_value():
class Foo:
pass
tree = AABBTree()
value_set = {Foo() for _ in range(10)}
for value in value_set:
tree.add(AABB([(0, 1), (0, 1)]), value=value)
retrieved_value_set = set(
tree.overlap_values(AABB([(0, 2), (0, 2)]), unique=False))
assert retrieved_value_set == value_set
class Collision_detection_AABB(Collision_Detection):
def CheckCollisions(self , rects):
self.tree = AABBTree()
d = {i:rects[i] for i in range(len(rects))}
j = 0
for i in rects:
aabb = AABB([(i.x, i.x+i.w),(i.y, i.y+i.h)])
self.tree.add(aabb, j)
j += 1
s = set()
for i in rects:
a = self.tree.overlap_values(AABB([(i.x, i.x+i.w), (i.y, i.y+i.h)]))
s = s.union({frozenset([i,d[j]]) for j in a if i != d[j]})
return s
class RoadContainerAABBtree:
def __init__(self, d_max=0):
self.d_max = d_max
self.data = AABBTree()
def __del__(self):
pass
def insert(self, element):
a, b = element.get_axis_aligned_bounding_box()
e = AABB([(a[0], b[0]), (a[1], b[1])])
self.data.add(e, element)
def find_near(self, x, direction):
# find candidates, exclude only those which are safe to exclude
candidates = self.find_near_candidates(x, self.d_max)
# then enumerate all candidates an do precise search
dist_x_list = []
dist_dir_list = []
x_projected_list = []
way_direction_list = []
for r in candidates:
dist_x, x_projected, dist_dir, way_direction = r.distance_of_point(
x, direction)
dist_x_list.append(dist_x)
dist_dir_list.append(dist_dir)
x_projected_list.append(x_projected)
way_direction_list.append(way_direction)
return candidates, dist_x_list, x_projected_list, dist_dir_list, way_direction_list
def find_near_candidates(self, x, d_max):
if not math.isfinite(x[0]) or not math.isfinite(x[1]):
return []
# the point x and a square environment
bb = AABB([(x[0] - d_max, x[0] + d_max), (x[1] - d_max, x[1] + d_max)])
candidates = self.data.overlap_values(bb)
return candidates
def axis_aligned_bounding_boxes_overlap(self, a1, b1, a2, b2):
return np.all(a1 < b2) and np.all(a2 < b1)
def endpoint_statistics(path_to_svg):
'''
Given:
path_to_svg: A path to an SVG file.
Normalizes by the svg's long edge as defined by its viewBox.
Ignores <svg> width or height attributes.
'''
global_scale = 1.0
try:
doc = Document(path_to_svg)
flatpaths = doc.flatten_all_paths()
paths = [path for (path, _, _) in flatpaths]
except:
global_scale = get_global_scale(doc.tree)
## Let's truly fail if there are transform nodes we can't handle.
# try: global_scale = get_global_scale( doc.tree )
# except: print( "WARNING: There are transforms, but flatten_all_paths() failed. Falling back to unflattened paths and ignoring transforms.", file = sys.stderr )
paths, _ = svg2paths(path_to_svg)
## First pass: Gather all endpoints, path index, segment index, t value
endpoints = [] # a copy of point coordinations
endpoints_p = [
] # real points, we will do the snapping by changing points in this list
endpoint_addresses = []
for path_index, path in enumerate(paths):
for seg_index, seg in enumerate(path):
for t in (0, 1):
pt = seg.point(t)
endpoints.append((pt.real, pt.imag))
endpoint_addresses.append((path_index, seg_index, t))
print("Creating spatial data structures:")
## Point-point queries.
dist_finder = scipy.spatial.cKDTree(endpoints)
## Build an axis-aligned bounding box tree for the segments.
bbtree = AABBTree() # but, why?
# for path_index, path in tqdm( enumerate( paths ), total = len( paths ), ncols = 50 ):
for path_index, path in enumerate(paths):
for seg_index, seg in enumerate(path):
xmin, xmax, ymin, ymax = seg.bbox(
) # record bbox of each segmentation?
bbtree.add(AABB([(xmin, xmax), (ymin, ymax)]),
(path_index, seg_index, seg))
# Second pass: Gather all minimum distances
print("Finding minimum distances:")
minimum_distances = []
for i, (pt, (path_index, seg_index,
t)) in enumerate(zip(endpoints, endpoint_addresses)):
## 1. Find the minimum distance to any other endpoints
## Find two closest points, since the point itself is in dist_finder with distance 0.
mindist, closest_pt_indices = dist_finder.query([pt], k=2)
## These come back as 1-by-2 matrices.
mindist = mindist[0]
closest_pt_indices = closest_pt_indices[0]
## If we didn't find 2 points, then pt is the only point in this file.
## There is no point element in SVG, so that should never happen.
assert len(closest_pt_indices) == 2
## If there are two or more other points identical to pt,
## then pt might not actually be one of the two returned, but both distances
## should be zero.
assert i in closest_pt_indices or (mindist < eps).all()
assert min(mindist) <= eps
## The larger distance corresponds to the point that is not pt.
mindist = max(mindist)
## If we already found the minimum distance is 0, then there's no point also
## searching for T-junctions.
if mindist < eps:
minimum_distances.append(mindist)
continue
## 2. Find the closest point on any other paths (T-junction).
## We are looking for any segments closer than mindist to pt.
# why? why mindist?
query = AABB([(pt[0] - mindist, pt[0] + mindist),
(pt[1] - mindist, pt[1] + mindist)])
for other_path_index, other_seg_index, seg in bbtree.overlap_values(
query):
## Don't compare the point with its own segment.
if other_path_index == path_index and other_seg_index == seg_index:
continue
## Optimization: If the distance to the bounding box is larger
## than mindist, skip it.
## This is still relevant, because mindist will shrink as we iterate over
## the results of our AABB tree query.
# why? this is also not reasonable to me
xmin, xmax, ymin, ymax = seg.bbox()
if (pt[0] < xmin - mindist or pt[0] > xmax + mindist
or pt[1] < ymin - mindist or pt[1] > ymin + mindist):
continue
## Get the point to segment distance.
dist_to_other_path = distance_point_to_segment(pt, seg)
## Keep it if it's smaller.
if mindist is None or dist_to_other_path < mindist:
mindist = dist_to_other_path
## Terminate early if the minimum distance found already is 0
if mindist < eps: break
## Accumulate the minimum distance
minimum_distances.append(mindist)
minimum_distances = global_scale * asfarray(minimum_distances)
## Divide by long edge.
if 'viewBox' in doc.root.attrib:
import re
_, _, width, height = [
float(v)
for v in re.split('[ ,]+', doc.root.attrib['viewBox'].strip())
]
long_edge = max(width, height)
print("Normalizing by long edge:", long_edge)
minimum_distances /= long_edge
elif "width" in doc.root.attrib and "height" in doc.root.attrib:
width = doc.root.attrib["width"].strip().strip("px")
height = doc.root.attrib["height"].strip().strip("px")
long_edge = max(float(width), float(height))
print("Normalizing by long edge:", long_edge)
minimum_distances /= long_edge
else:
print(
"WARNING: No viewBox found in <svg>. Not normalizing by long edge."
)
print("Done")
return minimum_distances
class Collision:
def __init__(self, objects=None):
self.tree = AABBTree()
if objects:
for item in objects:
if item["type"] == "CUBE":
self.add_object(item["pos"],
(item["scale"][0] / 2, item["scale"][1] /
2, item["scale"][2] / 2), item["goal"])
elif item["type"] == "SPHERE":
self.add_object(item["pos"], item["scale"], item["goal"])
# Returns AABB object with coorect values based on a point and offset
def get_aabb(self, point, bound):
return AABB([(point.x - bound[0], point.x + bound[0]),
(point.y - bound[1], point.y + bound[1]),
(point.z - bound[2], point.z + bound[2])])
# Add object with position and scale to the tree
# Sets the position as the value returned in case of collision
def add_object(self, position, scale, goal):
self.tree.add(self.get_aabb(position, scale), {
"pos": position,
"scale": scale,
"goal": goal
})
# Makes checking for collision on point easier
def point_collision(self, point, bound):
return self.tree.does_overlap(self.get_aabb(point, bound))
# Returns the point object of collided objects
def collision_objects(self, point, bound):
return self.tree.overlap_values(self.get_aabb(point, bound))
# checks if player is between bounds of object on an axis with respect to size
def is_between(self, player, obj, axis):
return obj["pos"][axis] - obj["scale"][axis] - player["scale"][
axis] < player["pos"][axis] < obj["pos"][axis] + obj["scale"][
axis] + player["scale"][axis]
# Finds which side of a cube the player is touching
def get_colliding_face(self, player, obj):
# Zeroes the axis of the plane on the object the player is touching
return (1 if self.is_between(player, obj, 0) else 0,
1 if self.is_between(player, obj, 1) else 0,
1 if self.is_between(player, obj, 2) else 0)
# Returns surface vector based on player direction
def get_surface_vector(self, player, obj):
directions = self.get_colliding_face(player, obj)
# Returns a normalized vector that represents the direction of the surface
# Direction on the non zeroed axes based on player movement
return Vector(player["direction"].x * directions[0],
player["direction"].y * directions[1],
player["direction"].z * directions[2]).normalize()
# Returns slide vector for player on collided object
def get_slide_vector(self, player, obj):
surface_vector = self.get_surface_vector(player, obj)
return surface_vector * player["direction"].dot(surface_vector)
# Returns motion vector of player
def move(self, player):
# collision member set to advoid key error
player["collision"] = []
# If no collision return player directly
if not self.point_collision(player["newpos"], player["scale"]):
return player
else:
# If collision, get slide vector for each object collided with
for item in self.collision_objects(player["newpos"],
player["scale"]):
player["direction"] = self.get_slide_vector(player, item)
player["collision"].append(
self.get_colliding_face(player, item))
if item["goal"]:
player["collision"].append((0, 0, 0))
return player
