def test_insert_balanced(self):
tree = DynamicAABB()
# By inserting same shape, tree will not balance based on surface
# checks, so we can check the height balancing
shape = AABB(Vector(0, 0), Vector(1, 1))
nodes = []
for i in range(128):
nodes.append(tree.add(StabObj(shape), shape.bbox()))
self.assertEqual(tree.get_height(), 5)
def test_dist(self):
a = Vector(2, 2)
b = Vector(6, 4)
rect = AABB(a, b)
self.assertEqual(rect.distance2(Vector(0, 0)), 8)
self.assertEqual(rect.distance2(a), 0)
self.assertEqual(rect.distance2(Vector(3, 3)), 0)
self.assertEqual(rect.distance2(Vector(4, 3)), 0)
self.assertEqual(rect.distance2(Vector(8, 5)), 5)
self.assertEqual(rect.distance2(Vector(1, -1)), 10)
self.assertEqual(rect.distance2(Vector(4, 1)), 1)
self.assertEqual(rect.distance2(Vector(9, 3)), 9)
self.assertEqual(rect.distance2(Vector(5, 5)), 1)
self.assertEqual(rect.distance2(Vector(0, 4)), 4)
self.assertEqual(rect.distance(Vector(0, 4)), 2)
self.assertEqual(rect.distance(Vector(9, 3)), 3.0)
def test_contains(self):
a = Vector(2, 2)
b = Vector(6, 4)
rect = AABB(a, b)
self.assertEqual(rect.contains(a), True)
self.assertEqual(rect.contains(b), True)
self.assertEqual(rect.contains(Vector(3, 3)), True)
self.assertEqual(rect.contains(Vector(-1, -1)), False)
self.assertEqual(rect.contains(Vector(213132534, -9843574398)), False)
def test_query_shape(self):
tree = self._create_tree()
# Check zero results
r = tree.query_shape(Circle(Vector(5, 5), 1))
r = self._get_shapes(r)
self.assertEqual(r, [])
# Check 1 result
r = tree.query_shape(Circle(Vector(-2, -2), 0.5))
r = self._get_shapes(r)
self.assertEqual(r, [self._shapes['aabb']])
# Check many results
r = tree.query_shape(AABB(Vector(-2, -2), Vector(2, 2)))
r = self._get_shapes(r)
self.assertEqual(set(r), set([
self._shapes['aabb'], self._shapes['circle'],
self._shapes['triangle']]))
def test_intersects_triangle(self):
rect = AABB(Vector(-2, -2), Vector(2, 2))
# Outside, separated by AC's normal
t = Triangle([Vector(2, 3), Vector(3, 2), Vector(3, 3)])
self.assertFalse(rect.intersects(t))
# Outside, separated by X axis
t = Triangle([Vector(-1, 3), Vector(0, 2.1), Vector(1, 3)])
self.assertFalse(rect.intersects(t))
# On border
t = Triangle([Vector(2, 3), Vector(2, 2), Vector(3, 2)])
self.assertTrue(rect.intersects(t))
# Overlap
t = Triangle([Vector(3, 0), Vector(3, 3), Vector(0, 3)])
self.assertTrue(rect.intersects(t))
def test_intersects_polygon(self):
rect = AABB(Vector(-2, -2), Vector(2, 2))
# Outside, separated by AC's normal
t = Polygon([Vector(2, 3), Vector(3, 2), Vector(4, 3), Vector(3, 4)])
self.assertFalse(rect.intersects(t))
# Outside, separated by X axis
t = Polygon([Vector(-1, 3), Vector(0, 2.1), Vector(1, 3),
Vector(0, 3.9)])
self.assertFalse(rect.intersects(t))
# On border
t = Polygon([Vector(1, 3), Vector(3, 1), Vector(5, 3), Vector(3, 5)])
self.assertTrue(rect.intersects(t))
# Overlap
t = Polygon([Vector(3, 0), Vector(3, 2), Vector(2, 3), Vector(0, 3)])
self.assertTrue(rect.intersects(t))
def main():
parser = get_parser()
args = parser.parse_args()
points = [[0, 5], [-1, 4], [-2, 1], [-2, 0], [-1, -3], [0, -5]]
poly = Polygon([Vector(*p) for p in points]).translate(Vector(0.5, 0))
shapes = {
"cirlce": Circle(Vector(0, 0), radius=2),
"polygon": poly,
"triangle": Triangle([Vector(-2, 2),
Vector(0, -2),
Vector(4, 4)]),
"aabb": AABB(Vector(-3.5, -2.5), Vector(1, 4))
}
shape = shapes[args.shape]
segments = []
for angle in range(360, step=1):
d = Vector.polar_deg(angle)
p = d * -10
t = shape.raycast(p, d)
p2 = p + d * t
segments.append(Segment(p, p2))
debug_draw(shape, *segments)
class TestDynamicAABB(ShapeTestCase):
_shapes = {
'triangle': Triangle([Vector(0, 0), Vector(1, 0), Vector(1, 1)]),
'circle': Circle(Vector(0, 0), radius=1),
'aabb': AABB(Vector(-2, -2), Vector(-1, -1)),
'poly': Polygon(list(reversed([
Vector(3, 3), Vector(3.5, 3.5),
Vector(4.5, 3.5), Vector(4, 3)]))),
}
def _create_tree(self):
tree = DynamicAABB()
for shape in self._shapes.values():
tree.add(StabObj(shape), shape.bbox())
return tree
def _get_shapes(self, objects):
return [x.shape for x in objects]
def _dump_tree(self, node):
if hasattr(node, '_root'):
# Actually passed tree object directly
node = node._root
result = []
if node.leaf:
result.append(node.obj.shape)
else:
result.append(self._dump_tree(node.left))
result.append(self._dump_tree(node.right))
return result
def _raycast_cb(self, obj, point, direction, max_distance):
# Callback to get the first hit of the ray
hit_dist = obj.shape.raycast(point, direction)
if hit_dist is not None:
if hit_dist < max_distance:
return hit_dist
return None
def test_query_shape(self):
tree = self._create_tree()
# Check zero results
r = tree.query_shape(Circle(Vector(5, 5), 1))
r = self._get_shapes(r)
self.assertEqual(r, [])
# Check 1 result
r = tree.query_shape(Circle(Vector(-2, -2), 0.5))
r = self._get_shapes(r)
self.assertEqual(r, [self._shapes['aabb']])
# Check many results
r = tree.query_shape(AABB(Vector(-2, -2), Vector(2, 2)))
r = self._get_shapes(r)
self.assertEqual(set(r), set([
self._shapes['aabb'], self._shapes['circle'],
self._shapes['triangle']]))
def test_raycast(self):
tree = self._create_tree()
# Void raycast (not in tree at all)
res = tree.raycast(Vector(3, -3), Vector(1, -1).unit(),
callback=self._raycast_cb)
self.assertEqual(res, None)
# Void raycast (in AABB's, but no object intersection)
res = tree.raycast(Vector(1, -1), Vector(1, -1).unit(),
callback=self._raycast_cb)
self.assertEqual(res, None)
# Raycast hitting many objects, returning AABB
p = Vector(-3, -3)
d = Vector(1, 1).unit()
res = tree.raycast(p, d, callback=self._raycast_cb)
self.assertEqual(res.shape, self._shapes['aabb'])
# Raycast hitting many objects, returning Circle
p = Vector(-0.5, -1.5)
d = Vector(1, 1).unit()
res = tree.raycast(p, d, callback=self._raycast_cb)
self.assertEqual(res.shape, self._shapes['circle'])
# Raycast hitting many objects, returning Triangle
p = Vector(2, 2)
d = Vector(-2, -3).unit()
res = tree.raycast(p, d, callback=self._raycast_cb)
self.assertEqual(res.shape, self._shapes['triangle'])
# Raycast hitting many objects, returning Polygon
p = Vector(4.5, 4)
d = Vector(-1, -1).unit()
res = tree.raycast(p, d, callback=self._raycast_cb)
self.assertEqual(res.shape, self._shapes['poly'])
def test_insert_big_and_small(self):
# This test assures the surface is used in inserts
# Lets setup a tree with 1 big object and 1 small object.
tree = DynamicAABB()
c1 = Circle(Vector(0, 0), 20)
c2 = Circle(Vector(20, 18), 1)
tree.add(StabObj(c2), c2.bbox())
tree.add(StabObj(c1), c1.bbox())
c3 = Circle(Vector(18, -19), 1)
tree.add(StabObj(c3), c3.bbox())
self.assertEqual(self._dump_tree(tree), [
[[c2], [c3]],
[c1],
])
def test_remove_one(self):
tree = DynamicAABB()
c1 = StabObj(Circle(Vector(0, 0), 1))
node_id = tree.add(c1, c1.shape.bbox())
self.assertTrue(tree._root)
tree.remove(node_id)
self.assertFalse(tree._root)
@pytest.mark.xfail
def test_insert_balanced(self):
tree = DynamicAABB()
# By inserting same shape, tree will not balance based on surface
# checks, so we can check the height balancing
shape = AABB(Vector(0, 0), Vector(1, 1))
nodes = []
for i in range(128):
nodes.append(tree.add(StabObj(shape), shape.bbox()))
self.assertEqual(tree.get_height(), 5)
def test_bbox(self):
rect = AABB(Vector(2, 2), Vector(6, 4))
self.assertEqual(rect, rect.bbox())
def test_intersects_aabb(self):
rect = AABB(Vector(2, 2), Vector(6, 4))
zr = Vector(0, 0)
self.assertEqual(rect.intersects(AABB(zr, Vector(1, 2))), None)
self.assertTrue(rect.intersects(AABB(zr, Vector(3, 2))))
def test_intersects_circle(self):
rect = AABB(Vector(2, 2), Vector(6, 4))
zr = Vector(0, 0)
self.assertEqual(rect.intersects(Circle(zr, 1)), None)
self.assertEqual(rect.intersects(Circle(Vector(6, 6.0001), 2)), None)
def raycast(self, point, direction, *, callback, max_distance=None):
""" Implementation taken directly from Box2D, as it's quite extensible
there and optimized.
This tree does not quite give as an easy way to get the `first`
hit effitiently, but we can iterate over all objects, that ray MAY
hit quite fast.
If we want to get the `first` hit, we can check each object for ray
intersection in callback and return a modified max_distance version,
that will limit the search area, with last found element being our
`first` hit. For example:
res = {'res': None}
def cb(obj, point, direction, max_distance, res=res):
ray_hit = RAY_TEST(obj, point, direction, max_distance)
if ray_hit is not None:
res['res'] = obj
_, hit_dist = ray_hit
return hit_dist
return None
tree.raycast(p, d, max_distance=1000, callback=cb)
If we need any element, just return 0.0 from callback, it will stop
traversing the tree.
Has no return value, use callback for that.
NOTE: I know, that this interface is very un-pythonic, but a generator
is even harder to work with, as it requires usage of `send` API for
shortening the distance.
"""
assert abs(direction.length2() - 1) < EPSILON
if self._root is None:
return None
v = direction.rotate_deg(90)
abs_v = Vector(math.fabs(v.x), math.fabs(v.y))
if max_distance is None:
max_distance = float("inf")
p2 = point + direction * max_distance
segment_aabb = AABB(min_vector(point, p2), max_vector(point, p2))
node_stack = [self._root]
last_result = None
while node_stack:
node = node_stack.pop()
# First check AABB
if not node.aabb.intersects(segment_aabb):
continue
# Separating axis for segment (Gino, p80).
# |dot(v, p1 - c)| > dot(|v|, h)
c = node.aabb.center
h = node.aabb.extents
separation = abs(v.dot(point - c)) - abs_v.dot(h)
if separation > 0:
continue
# Ok, now we know, this AABB intersects the ray
if node.leaf:
value = callback(node.obj, point, direction, max_distance)
if value is None:
continue
last_result = node.obj
if value == 0:
# Client has terminated the raycast
return last_result
if value > 0:
# Fixup the bounds of our AABB
max_distance = value
p2 = point + direction * max_distance
segment_aabb = AABB(
min_vector(point, p2), max_vector(point, p2))
else:
node_stack.append(node.left)
node_stack.append(node.right)
return last_result
def test_triangle_bbox(self):
t = Triangle([Vector(2, 2), Vector(4, 2), Vector(4, 4)])
self.assertEqual(t.bbox(), AABB(Vector(2, 2), Vector(4, 4)))
def test_circle_bbox(self):
c = Circle(Vector(5, 3), 2)
self.assertEqual(c.bbox(), AABB(Vector(3, 1), Vector(7, 5)))