def test_vector_x_y(self):
u = Vector(4, 3)
self.assertEqual(u.x, 4)
self.assertEqual(u.y, 3)
u = Vector(3.4, 7.8)
self.assertEqual(u.x, 3.4)
self.assertEqual(u.y, 7.8)
def test_vector_sub(self):
u = Vector(2, 5)
v = Vector(1, 2)
self.assertEqual(u - v, Vector(1, 3))
self.assertEqual(v - u, Vector(-1, -3))
with self.assertRaises(TypeError):
u - 1
def test_vector_mul(self):
u = Vector(2, 5)
self.assertEqual(u * 2, Vector(4, 10))
u = Vector(1, 3)
self.assertEqual(u * 3, Vector(3, 9))
with self.assertRaises(TypeError):
u * u
def test_vector_add(self):
u = Vector(2, 1)
v = Vector(0, 1)
self.assertEqual(u + v, Vector(2, 2))
self.assertEqual(v + u, Vector(2, 2))
with self.assertRaises(TypeError):
u + 1
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_circle_circle_intersection(self):
c = Circle(Vector(5, 3), 2)
# Full containment
self.assertTrue(c.intersects(Circle(Vector(5, 3), 1)))
# Intersection 2 points
self.assertTrue(c.intersects(Circle(Vector(2, 3), 2)))
# Intersection 1 point
self.assertTrue(c.intersects(Circle(Vector(2, 3), 1)))
# No intersection
self.assertFalse(c.intersects(Circle(Vector(0, 0), 1)))
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 shape_from_config(shape_conf):
if shape_conf['type'] == "circle":
x, y = shape_conf['center']
return Circle(Vector(x, y), shape_conf['radius'])
if shape_conf['type'] == "polygon":
points = []
for x, y in shape_conf['points']:
points.append(Vector(x, y))
return classify_polygon(points)
else:
raise ValueError(shape_conf['type'])
def test_vector_eq(self):
self.assertEqual(Vector(2, 2), Vector(2, 2))
self.assertEqual(Vector(0, 0), Vector(0, 0))
self.assertNotEqual(Vector(2, 1), Vector(1, 2))
# Low prescision error
self.assertEqual(Vector(0.7071067811865475, 0.7071067811865475),
Vector(0.7071067811865476, 0.7071067811865475))
# Wrong types
self.assertNotEqual(Vector(2, 2), 1)
def load_props(world, world_map):
props = []
for prop_conf in world_map['props']:
shape = shape_from_config(prop_conf['shape'])
x, y = prop_conf['position']
prop = SimpleProp(shape=shape, position=Vector(x, y))
props.append(prop)
for x in range(100):
x = random.randint(50, 100)
y = random.randint(50, 100)
c = Circle(Vector(0, 0), random.randint(2, 5))
prop = SimpleProp(shape=c, position=Vector(x, y))
props.append(prop)
return props
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)
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']]))
class Character(Actor):
shape = Circle(Vector(0, 0), CHARACTER_RADIUS)
def __init__(self, world, position=Vector(0, 0)):
self._world = world
self._position = position
self.movement = ActorMovement()
def tick(self, dt):
move = self._get_movement_vector(dt)
# Move character position
self._apply_collision(move * (dt * CHARACTER_SPEED))
def _get_movement_vector(self, dt):
movement = self.movement
# Rotate forward vector
if movement.rotation:
speed = CHARACTER_ROTATION * movement.rotation
ang = movement.forward.angle + dt * speed
movement.forward = Vector.polar(ang)
# Determime movement vector
if movement.movement:
move = movement.forward * movement.movement
# Apply strafing to movement vector
if movement.strafe:
move = move.rotate_deg(45 * movement.strafe *
movement.movement)
elif movement.strafe:
move = movement.forward.rotate_deg(movement.strafe * 90)
else:
move = Vector(0, 0)
return move
def _apply_collision(self, move):
""" Perform movement by described vector, but check for collision with
other objects.
"""
new_position = self._position + move
# Check if move is legal
manifolds = self._world.query_props_intersection(
new_position, self.shape)
# Invalid move
li = len(manifolds)
if li == 1:
# We can resolve contact for 1 object
new_move = self._resolve_movement(manifolds[0], move)
new_position = self._position + new_move
elif li > 1:
# For 2 and more collisions we can't perform movement
new_position = self._position
self._position = new_position
# Moving circle problem. Tunneling?
# BVH/BSP interface for queries. Implement 2 at least
# Movement should be locked and only after resolved
def _resolve_movement(self, manifold, move):
correction = manifold.normal * manifold.depth
return move + correction
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_incircle(self):
a = Vector(0, 0)
b = Vector(2, 0)
c = Vector(1, 1)
self.assertGreater(incircle(a, b, c, Vector(1, 0)), 0)
self.assertGreater(incircle(a, b, c, Vector(1, 0.9999999991999)), 0)
self.assertLess(incircle(a, b, c, Vector(-11, 0)), 0)
self.assertEqual(incircle(a, b, c, Vector(1, 1)), 0)
self.assertEqual(incircle(a, b, c, Vector(1, -1)), 0)
def __init__(self, world_map):
self._props = DynamicAABB()
for prop in load_props(self, world_map):
aabb = prop.shape.bbox().translate(prop.position)
self._props.add(prop, aabb)
self._actors = [Character(self, position=Vector(0, 0))]
self._tick_period = 0.03125 # ~30 fps simulation
self._timer = 0
self._reminder = 0
def _get_movement_vector(self, dt):
movement = self.movement
# Rotate forward vector
if movement.rotation:
speed = CHARACTER_ROTATION * movement.rotation
ang = movement.forward.angle + dt * speed
movement.forward = Vector.polar(ang)
# Determime movement vector
if movement.movement:
move = movement.forward * movement.movement
# Apply strafing to movement vector
if movement.strafe:
move = move.rotate_deg(45 * movement.strafe *
movement.movement)
elif movement.strafe:
move = movement.forward.rotate_deg(movement.strafe * 90)
else:
move = Vector(0, 0)
return move
def test_triangle_distance(self):
t = Triangle([Vector(2, 2), Vector(4, 2), Vector(4, 4)])
# Inside triangle
self.assertEqual(t.distance(Vector(3.5, 3)), -1)
# On edge
self.assertEqual(t.distance(Vector(3, 2)), 0)
# Exactly vertex
self.assertEqual(t.distance(Vector(2, 2)), 0)
# Outside in a vertex voronoi region
self.assertEqual(t.distance(Vector(-2, -1)), 5)
# Outside in a edge voronnoi region
self.assertEqual(t.distance(Vector(3, 0)), 2)
def decode(cls, data, offset=0, _short=_SHORT, _vector=_VECTORF):
# Unpack size
[points_len] = _short.unpack_from(data, offset)
offset += _short.size
# Unpack points vectors
points = []
for _ in range(points_len):
p_x, p_y = _vector.unpack_from(data, offset)
offset += _vector.size
points.append(Vector(p_x, p_y))
return cls(Polygon(points))
def test_triangle_contains(self):
t = Triangle([Vector(2, 2), Vector(4, 2), Vector(4, 4)])
# Inside triangle
self.assertTrue(t.contains(Vector(3.5, 3)))
# On edge
self.assertTrue(t.contains(Vector(3, 2)))
# Exactly vertex
self.assertTrue(t.contains(Vector(2, 2)))
# Outside
self.assertFalse(t.contains(Vector(2, 1)))
def test_prop(self):
pos = Vector(112.225, -843.8)
cr = Circle(Vector(-1, -1), 12)
points = [[0, 5], [-1, 4], [-2, 1], [-2, 0], [-1, -3], [0, -5]]
poly = Polygon([Vector(*p) for p in points])
render_data = b"\x01\x02\x03"
# Check encode/decode cirlce
propc = Prop(3, pos, cr, render_data)
propc_data = bytearray(propc.size)
propc.encode(propc_data)
self.assertBinaryEqual(propc_data, [
b'\x00\x00\x00\x00\x00\x00\x00\x03', # 8 byte prop_id
struct.pack("!dd", pos.x, pos.y), # 16 byte position as doubles
b'\x00', # 1 byte shape type
struct.pack("!ff", -1, -1), # 8 byte center as floats
struct.pack("!f", 12), # 4 byte radius as float
b'\x00\x03', # render data length
render_data
])
self.assertEqual(Prop.decode(propc_data), propc)
# Check encode/decode polygon
propc = Prop(4, pos, poly, render_data)
propc_data = bytearray(propc.size)
propc.encode(propc_data)
self.assertBinaryEqual(bytes(propc_data), [
b'\x00\x00\x00\x00\x00\x00\x00\x04', # 8 byte prop_id
struct.pack("!dd", pos.x, pos.y), # 16 byte position as doubles
b'\x01', # 1 byte shape type
b'\x00\x06', # 6 vertices
] + [struct.pack("!ff", *point) for point in points] + [
b'\x00\x03', # render data length
render_data
])
self.assertEqual(Prop.decode(propc_data), propc)
def test_seg_distance(self):
# An easily visualized test involving a segment parallel to the X axis
a1 = Vector(-2, 1)
b1 = Vector(4, 1)
c1 = Vector(0, 6)
c2 = Vector(7, 5)
# Points in close proximity to the segment for high precision checks
c3 = Vector(-2.01, 1)
c4 = Vector(0, 0.999999998)
c5 = Vector(4.0000000001, 0.99999999998)
self.assertEqual(seg_distance(a1, b1, c1), 5)
self.assertEqual(seg_distance(a1, b1, c2), 5)
self.assertAlmostEqual(seg_distance(a1, b1, c3), 0.01)
self.assertAlmostEqual(seg_distance(a1, b1, c4), 0.000000002)
self.assertGreater(seg_distance(a1, b1, c5), 0)
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)
def __init__(self, forward=Vector.polar(0)):
# One of:
# * 0 - no movement
# * 1 - movement forward
# * -1 - movement backward
self.movement = 0
# One of:
# * 0 - no rotation
# * 1 - rotation right
# * -1 - rotation left
self.rotation = 0
# One of:
# * 0 - no strafe
# * 1 - strafe right
# * -1 - strafe left
self.strafe = 0
# Forward unit vector
self.forward = forward
def decode(cls, data, offset=0, _short=_SHORT):
# Unpack static part
struct_base = cls.struct_base
prop_id, posx, posy = struct_base.unpack_from(data, offset)
offset += struct_base.size
# Unpack shape
# char shape_type; // Circle - 0, Polygon - 1
# SHAPE shape; // depending on shape_type
shape_t = data[offset]
offset += 1
if shape_t == CircleShape.shape_type:
shape = CircleShape.decode(data, offset)
elif shape_t == PolygonShape.shape_type:
shape = PolygonShape.decode(data, offset)
offset += shape.size
# Unpack render_data
# short render_data_len;
# char render_data[]; // Depending on render_data_len
[render_len] = _short.unpack_from(data, offset)
offset += _short.size
render_data = bytes(data[offset:offset + render_len])
return super().__new__(cls, prop_id, Vector(posx, posy), shape,
render_data)
def decode(cls, data, offset=0):
c_x, c_y, radius = cls.struct.unpack_from(data, offset)
return cls(Circle(Vector(c_x, c_y), radius))
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 __init__(self, world, position=Vector(0, 0)):
self._world = world
self._position = position
self.movement = ActorMovement()
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 __init__(self, world, position=Vector(0, 0)):
self._world = world
self._position = position