5
|
1 #############################################################################
|
|
2 #
|
|
3 # Copyright (c) 2010 by Casey Duncan and contributors
|
|
4 # All Rights Reserved.
|
|
5 #
|
|
6 # This software is subject to the provisions of the MIT License
|
|
7 # A copy of the license should accompany this distribution.
|
|
8 # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
9 # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
10 # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
|
|
11 #
|
|
12 #############################################################################
|
|
13 """
|
|
14 **Grease collision detection systems**
|
|
15
|
|
16 Grease uses two-phase broad and narrow collision detection. *Broad-phase*
|
|
17 collision systems are used to efficiently identify pairs that may be colliding
|
|
18 without resorting to a brute-force check of all possible pairs. *Narrow-phase*
|
|
19 collision systems use the pairs generated by the broad-phase and perform more
|
|
20 precise collision tests to determine if a collision has actually occurred. The
|
|
21 narrow-phase system also calculates more details about each collision,
|
|
22 including collision point and normal vector for use in collision response.
|
|
23
|
|
24 A typical collision detection system consists of a narrow-phase system that
|
|
25 contains a broad-phased system. The narrow-phase system is usually the only
|
|
26
|
|
27 one that the application directly interacts with, though the application is
|
|
28 free to use the broad-phased system directly if desired. This could be
|
|
29 useful in cases where speed, rather than precision is paramount.
|
|
30
|
|
31 The narrow-phase system can be assigned handler objects to run after
|
|
32 collision detection. These can perform tasks like handling collision response
|
|
33 or dispatching collision events to application handlers.
|
|
34
|
|
35 Note that broad-phase systems can return false positives, though they should
|
|
36 never return false negatives. Do not assume that all pairs returned by a
|
|
37 broad-phase system are actually in collision.
|
|
38 """
|
|
39
|
|
40 __version__ = '$Id$'
|
|
41
|
|
42 from grease.geometry import Vec2d
|
|
43 from bisect import bisect_right
|
|
44
|
|
45
|
|
46 class Pair(tuple):
|
|
47 """Pair of entities in collision. This is an ordered sequence of two
|
|
48 entities, that compares and hashes unordered.
|
|
49
|
|
50 Also stores additional collision point and normal vectors
|
|
51 for each entity.
|
|
52
|
|
53 Sets of ``Pair`` objects are exposed in the ``collision_pairs``
|
|
54 attribute of collision systems to indicate the entity pairs in
|
|
55 collision.
|
|
56 """
|
|
57 info = None
|
|
58 """A sequence of (entity, collision point, collision normal)
|
|
59 for each entity in the pair
|
|
60 """
|
|
61
|
|
62 def __new__(cls, entity1, entity2, point=None, normal=None):
|
|
63 pair = tuple.__new__(cls, (entity1, entity2))
|
|
64 return pair
|
|
65
|
|
66 def __hash__(self):
|
|
67 return hash(self[0]) ^ hash(self[1])
|
|
68
|
|
69 def __eq__(self, other):
|
|
70 other = tuple(other)
|
|
71 return tuple(self) == other or (self[1], self[0]) == other
|
|
72
|
|
73 def __repr__(self):
|
|
74 return '%s%r' % (self.__class__.__name__, tuple(self))
|
|
75
|
|
76 def set_point_normal(self, point0, normal0, point1, normal1):
|
|
77 """Set the collision point and normal for both entities"""
|
|
78 self.info = (
|
|
79 (self[0], point0, normal0),
|
|
80 (self[1], point1, normal1),
|
|
81 )
|
|
82
|
|
83
|
|
84 class BroadSweepAndPrune(object):
|
|
85 """2D Broad-phase sweep and prune bounding box collision detector
|
|
86
|
|
87 This algorithm is efficient for collision detection between many
|
|
88 moving bodies. It has linear algorithmic complexity and takes
|
|
89 advantage of temporal coherence between frames. It also does
|
|
90 not suffer from bad worst-case performance (like RDC can).
|
|
91 Unlike spacial hashing, it does not need to be optimized for
|
|
92 specific space and body sizes.
|
|
93
|
|
94 Other algorithms may be more efficient for collision detection with
|
|
95 stationary bodies, bodies that are always evenly distributed, or ad-hoc
|
|
96 queries.
|
|
97
|
|
98 :param collision_component: Name of the collision component used by this
|
|
99 system, defaults to 'collision'. This component supplies each
|
|
100 entities' aabb and collision masks.
|
|
101 :type collision_component: str
|
|
102 """
|
|
103 world = None
|
|
104 """|World| object this system belongs to"""
|
|
105
|
|
106 collision_component = None
|
|
107 """Name of world's collision component used by this system"""
|
|
108
|
|
109 LEFT_ATTR = "left"
|
|
110 RIGHT_ATTR = "right"
|
|
111 TOP_ATTR = "top"
|
|
112 BOTTOM_ATTR = "bottom"
|
|
113
|
|
114 def __init__(self, collision_component='collision'):
|
|
115 self.collision_component = collision_component
|
|
116 self._by_x = None
|
|
117 self._by_y = None
|
|
118 self._collision_pairs = None
|
|
119
|
|
120 def set_world(self, world):
|
|
121 """Bind the system to a world"""
|
|
122 self.world = world
|
|
123
|
|
124 def step(self, dt):
|
|
125 """Update the system for this time step, updates and sorts the
|
|
126 axis arrays.
|
|
127 """
|
|
128 component = getattr(self.world.components, self.collision_component)
|
|
129 LEFT = self.LEFT_ATTR
|
|
130 RIGHT = self.RIGHT_ATTR
|
|
131 TOP = self.TOP_ATTR
|
|
132 BOTTOM = self.BOTTOM_ATTR
|
|
133 if self._by_x is None:
|
|
134 # Build axis lists from scratch
|
|
135 # Note we cache the box positions here
|
|
136 # so that we can perform hit tests efficiently
|
|
137 # it also isolates us from changes made to the
|
|
138 # box positions after we run
|
|
139 by_x = self._by_x = []
|
|
140 append_x = by_x.append
|
|
141 by_y = self._by_y = []
|
|
142 append_y = by_y.append
|
|
143 for data in component.itervalues():
|
|
144 append_x([data.aabb.left, LEFT, data])
|
|
145 append_x([data.aabb.right, RIGHT, data])
|
|
146 append_y([data.aabb.bottom, BOTTOM, data])
|
|
147 append_y([data.aabb.top, TOP, data])
|
|
148 else:
|
|
149 by_x = self._by_x
|
|
150 by_y = self._by_y
|
|
151 removed = []
|
|
152 for entry in by_x:
|
|
153 entry[0] = getattr(entry[2].aabb, entry[1])
|
|
154 for entry in by_y:
|
|
155 entry[0] = getattr(entry[2].aabb, entry[1])
|
|
156 # Removing entities is inefficient, but expected to be rare
|
|
157 if component.deleted_entities:
|
|
158 deleted_entities = component.deleted_entities
|
|
159 deleted_x = []
|
|
160 deleted_y = []
|
|
161 for i, (_, _, data) in enumerate(by_x):
|
|
162 if data.entity in deleted_entities:
|
|
163 deleted_x.append(i)
|
|
164 deleted_x.reverse()
|
|
165 for i in deleted_x:
|
|
166 del by_x[i]
|
|
167 for i, (_, _, data) in enumerate(by_y):
|
|
168 if data.entity in deleted_entities:
|
|
169 deleted_y.append(i)
|
|
170 deleted_y.reverse()
|
|
171 for i in deleted_y:
|
|
172 del by_y[i]
|
|
173 # Tack on new entities
|
|
174 for entity in component.new_entities:
|
|
175 data = component[entity]
|
|
176 by_x.append([data.aabb.left, LEFT, data])
|
|
177 by_x.append([data.aabb.right, RIGHT, data])
|
|
178 by_y.append([data.aabb.bottom, BOTTOM, data])
|
|
179 by_y.append([data.aabb.top, TOP, data])
|
|
180
|
|
181 # Tim-sort is highly efficient with mostly sorted lists.
|
|
182 # Because positions tend to change little each frame
|
|
183 # we take advantage of this here. Obviously things are
|
|
184 # less efficient with very fast moving, or teleporting entities
|
|
185 by_x.sort()
|
|
186 by_y.sort()
|
|
187 self._collision_pairs = None
|
|
188
|
|
189 @property
|
|
190 def collision_pairs(self):
|
|
191 """Set of candidate collision pairs for this timestep"""
|
|
192 if self._collision_pairs is None:
|
|
193 if self._by_x is None:
|
|
194 # Axis arrays not ready
|
|
195 return set()
|
|
196
|
|
197 LEFT = self.LEFT_ATTR
|
|
198 RIGHT = self.RIGHT_ATTR
|
|
199 TOP = self.TOP_ATTR
|
|
200 BOTTOM = self.BOTTOM_ATTR
|
|
201 # Build candidates overlapping along the x-axis
|
|
202 component = getattr(self.world.components, self.collision_component)
|
|
203 xoverlaps = set()
|
|
204 add_xoverlap = xoverlaps.add
|
|
205 discard_xoverlap = xoverlaps.discard
|
|
206 open = {}
|
|
207 for _, side, data in self._by_x:
|
|
208 if side is LEFT:
|
|
209 for open_entity, (from_mask, into_mask) in open.iteritems():
|
|
210 if data.from_mask & into_mask or from_mask & data.into_mask:
|
|
211 add_xoverlap(Pair(data.entity, open_entity))
|
|
212 open[data.entity] = (data.from_mask, data.into_mask)
|
|
213 elif side is RIGHT:
|
|
214 del open[data.entity]
|
|
215
|
|
216 if len(xoverlaps) <= 10 and len(xoverlaps)*4 < len(self._by_y):
|
|
217 # few candidates were found, so just scan the x overlap candidates
|
|
218 # along y. This requires an additional sort, but it should
|
|
219 # be cheaper than scanning everyone and its simpler
|
|
220 # than a separate brute-force check
|
|
221 entities = set([entity for entity, _ in xoverlaps]
|
|
222 + [entity for _, entity in xoverlaps])
|
|
223 by_y = []
|
|
224 for entity in entities:
|
|
225 data = component[entity]
|
|
226 # We can use tuples here, which are cheaper to create
|
|
227 by_y.append((data.aabb.bottom, BOTTOM, data))
|
|
228 by_y.append((data.aabb.top, TOP, data))
|
|
229 by_y.sort()
|
|
230 else:
|
|
231 by_y = self._by_y
|
|
232
|
|
233 # Now check the candidates along the y-axis
|
|
234 open = set()
|
|
235 add_open = open.add
|
|
236 discard_open = open.discard
|
|
237 self._collision_pairs = set()
|
|
238 add_pair = self._collision_pairs.add
|
|
239 for _, side, data in by_y:
|
|
240 if side is BOTTOM:
|
|
241 for open_entity in open:
|
|
242 pair = Pair(data.entity, open_entity)
|
|
243 if pair in xoverlaps:
|
|
244 discard_xoverlap(pair)
|
|
245 add_pair(pair)
|
|
246 if not xoverlaps:
|
|
247 # No more candidates, bail
|
|
248 return self._collision_pairs
|
|
249 add_open(data.entity)
|
|
250 elif side is TOP:
|
|
251 discard_open(data.entity)
|
|
252 return self._collision_pairs
|
|
253
|
|
254 def query_point(self, x_or_point, y=None, from_mask=0xffffffff):
|
|
255 """Hit test at the point specified.
|
|
256
|
|
257 :param x_or_point: x coordinate (float) or sequence of (x, y) floats.
|
|
258
|
|
259 :param y: y coordinate (float) if x is not a sequence
|
|
260
|
|
261 :param from_mask: Bit mask used to filter query results. This value
|
|
262 is bit ANDed with candidate entities' ``collision.into_mask``.
|
|
263 If the result is non-zero, then it is considered a hit. By
|
|
264 default all entities colliding with the input point are
|
|
265 returned.
|
|
266
|
|
267 :return: A set of entities where the point is inside their bounding
|
|
268 boxes as of the last time step.
|
|
269 """
|
|
270 if self._by_x is None:
|
|
271 # Axis arrays not ready
|
|
272 return set()
|
|
273 if y is None:
|
|
274 x, y = x_or_point
|
|
275 else:
|
|
276 x = x_or_point
|
|
277 LEFT = self.LEFT_ATTR
|
|
278 RIGHT = self.RIGHT_ATTR
|
|
279 TOP = self.TOP_ATTR
|
|
280 BOTTOM = self.BOTTOM_ATTR
|
|
281 x_index = bisect_right(self._by_x, [x])
|
|
282 x_hits = set()
|
|
283 add_x_hit = x_hits.add
|
|
284 discard_x_hit = x_hits.discard
|
|
285 if x_index <= len(self._by_x) // 2:
|
|
286 # closer to the left, scan from left to right
|
|
287 while (x == self._by_x[x_index][0]
|
|
288 and self._by_x[x_index][1] is LEFT
|
|
289 and x_index < len(self._by_x)):
|
|
290 # Ensure we hit on exact left edge matches
|
|
291 x_index += 1
|
|
292 for _, side, data in self._by_x[:x_index]:
|
|
293 if side is LEFT and from_mask & data.into_mask:
|
|
294 add_x_hit(data.entity)
|
|
295 else:
|
|
296 discard_x_hit(data.entity)
|
|
297 else:
|
|
298 # closer to the right
|
|
299 for _, side, data in reversed(self._by_x[x_index:]):
|
|
300 if side is RIGHT and from_mask & data.into_mask:
|
|
301 add_x_hit(data.entity)
|
|
302 else:
|
|
303 discard_x_hit(data.entity)
|
|
304 if not x_hits:
|
|
305 return x_hits
|
|
306
|
|
307 y_index = bisect_right(self._by_y, [y])
|
|
308 y_hits = set()
|
|
309 add_y_hit = y_hits.add
|
|
310 discard_y_hit = y_hits.discard
|
|
311 if y_index <= len(self._by_y) // 2:
|
|
312 # closer to the bottom
|
|
313 while (y == self._by_y[y_index][0]
|
|
314 and self._by_y[y_index][1] is BOTTOM
|
|
315 and y_index < len(self._by_y)):
|
|
316 # Ensure we hit on exact bottom edge matches
|
|
317 y_index += 1
|
|
318 for _, side, data in self._by_y[:y_index]:
|
|
319 if side is BOTTOM:
|
|
320 add_y_hit(data.entity)
|
|
321 else:
|
|
322 discard_y_hit(data.entity)
|
|
323 else:
|
|
324 # closer to the top
|
|
325 for _, side, data in reversed(self._by_y[y_index:]):
|
|
326 if side is TOP:
|
|
327 add_y_hit(data.entity)
|
|
328 else:
|
|
329 discard_y_hit(data.entity)
|
|
330 if y_hits:
|
|
331 return x_hits & y_hits
|
|
332 else:
|
|
333 return y_hits
|
|
334
|
|
335
|
|
336 class Circular(object):
|
|
337 """Basic narrow-phase collision detector which treats all entities as
|
|
338 circles with their radius defined in the collision component.
|
|
339
|
|
340 :param handlers: A sequence of collision handler functions that are invoked
|
|
341 after collision detection.
|
|
342 :type handlers: sequence of functions
|
|
343
|
|
344 :param collision_component: Name of collision component for this system,
|
|
345 defaults to 'collision'. This supplies each entity's collision
|
|
346 radius and masks.
|
|
347 :type collision_component: str
|
|
348
|
|
349 :param position_component: Name of position component for this system,
|
|
350 defaults to 'position'. This supplies each entity's position.
|
|
351 :type position_component: str
|
|
352
|
|
353 :param update_aabbs: If True (the default), then the entities'
|
|
354 `collision.aabb` fields will be updated using their position
|
|
355 and collision radius before invoking the broad phase system.
|
|
356 Set this False if another system updates the aabbs.
|
|
357 :type update_aabbs: bool
|
|
358
|
|
359 :param broad_phase: A broad-phase collision system to use as a source
|
|
360 for collision pairs. If not specified, a :class:`BroadSweepAndPrune`
|
|
361 system will be created automatically.
|
|
362 """
|
|
363 world = None
|
|
364 """|World| object this system belongs to"""
|
|
365
|
|
366 position_component = None
|
|
367 """Name of world's position component used by this system"""
|
|
368
|
|
369 collision_component = None
|
|
370 """Name of world's collision component used by this system"""
|
|
371
|
|
372 update_aabbs = True
|
|
373 """Flag to indicate whether the system updates the entities' `collision.aabb`
|
|
374 field before invoking the broad phase collision system
|
|
375 """
|
|
376
|
|
377 handlers = None
|
|
378 """A sequence of collision handler functions invoke after collision
|
|
379 detection
|
|
380 """
|
|
381
|
|
382 broad_phase = None
|
|
383 """Broad phase collision system used as a source for collision pairs"""
|
|
384
|
|
385 def __init__(self, handlers=(), position_component='position',
|
|
386 collision_component='collision', update_aabbs=True, broad_phase=None):
|
|
387 self.handlers = tuple(handlers)
|
|
388 if broad_phase is None:
|
|
389 broad_phase = BroadSweepAndPrune(collision_component)
|
|
390 self.collision_component = collision_component
|
|
391 self.position_component = position_component
|
|
392 self.update_aabbs = bool(update_aabbs)
|
|
393 self.broad_phase = broad_phase
|
|
394 self._collision_pairs = None
|
|
395
|
|
396 def set_world(self, world):
|
|
397 """Bind the system to a world"""
|
|
398 self.world = world
|
|
399 self.broad_phase.set_world(world)
|
|
400 for handler in self.handlers:
|
|
401 if hasattr(handler, 'set_world'):
|
|
402 handler.set_world(world)
|
|
403
|
|
404 def step(self, dt):
|
|
405 """Update the collision system for this time step and invoke
|
|
406 the handlers
|
|
407 """
|
|
408 if self.update_aabbs:
|
|
409 for position, collision in self.world.components.join(
|
|
410 self.position_component, self.collision_component):
|
|
411 aabb = collision.aabb
|
|
412 x, y = position.position
|
|
413 radius = collision.radius
|
|
414 aabb.left = x - radius
|
|
415 aabb.right = x + radius
|
|
416 aabb.bottom = y - radius
|
|
417 aabb.top = y + radius
|
|
418 self.broad_phase.step(dt)
|
|
419 self._collision_pairs = None
|
|
420 for handler in self.handlers:
|
|
421 handler(self)
|
|
422
|
|
423 @property
|
|
424 def collision_pairs(self):
|
|
425 """The set of entity pairs in collision in this timestep"""
|
|
426 if self._collision_pairs is None:
|
|
427 position = getattr(self.world.components, self.position_component)
|
|
428 collision = getattr(self.world.components, self.collision_component)
|
|
429 pairs = self._collision_pairs = set()
|
|
430 for pair in self.broad_phase.collision_pairs:
|
|
431 entity1, entity2 = pair
|
|
432 position1 = position[entity1].position
|
|
433 position2 = position[entity2].position
|
|
434 radius1 = collision[entity1].radius
|
|
435 radius2 = collision[entity2].radius
|
|
436 separation = position2 - position1
|
|
437 if separation.get_length_sqrd() <= (radius1 + radius2)**2:
|
|
438 normal = separation.normalized()
|
|
439 pair.set_point_normal(
|
|
440 normal * radius1 + position1, normal,
|
|
441 normal * -radius2 + position2, -normal)
|
|
442 pairs.add(pair)
|
|
443 return self._collision_pairs
|
|
444
|
|
445 def query_point(self, x_or_point, y=None, from_mask=0xffffffff):
|
|
446 """Hit test at the point specified.
|
|
447
|
|
448 :param x_or_point: x coordinate (float) or sequence of (x, y) floats.
|
|
449
|
|
450 :param y: y coordinate (float) if x is not a sequence
|
|
451
|
|
452 :param from_mask: Bit mask used to filter query results. This value
|
|
453 is bit ANDed with candidate entities' ``collision.into_mask``.
|
|
454 If the result is non-zero, then it is considered a hit. By
|
|
455 default all entities colliding with the input point are
|
|
456 returned.
|
|
457
|
|
458 :return: A set of entities where the point is inside their collision
|
|
459 radii as of the last time step.
|
|
460
|
|
461 """
|
|
462 if y is None:
|
|
463 point = Vec2d(x_or_point)
|
|
464 else:
|
|
465 point = Vec2d(x_or_point, y)
|
|
466 hits = set()
|
|
467 position = getattr(self.world.components, self.position_component)
|
|
468 collision = getattr(self.world.components, self.collision_component)
|
|
469 for entity in self.broad_phase.query_point(x_or_point, y, from_mask):
|
|
470 separation = point - position[entity].position
|
|
471 if separation.get_length_sqrd() <= collision[entity].radius**2:
|
|
472 hits.add(entity)
|
|
473 return hits
|
|
474
|
|
475
|
|
476 def dispatch_events(collision_system):
|
|
477 """Collision handler that dispatches `on_collide()` events to entities
|
|
478 marked for collision by the specified collision system. The `on_collide()`
|
|
479 event handler methods are defined by the application on the desired entity
|
|
480 classes. These methods should have the following signature::
|
|
481
|
|
482 def on_collide(self, other_entity, collision_point, collision_normal):
|
|
483 '''Handle A collision between this entity and `other_entity`
|
|
484
|
|
485 - other_entity (Entity): The other entity in collision with
|
|
486 `self`
|
|
487
|
|
488 - collision_point (Vec2d): The point on this entity (`self`)
|
|
489 where the collision occurred. Note this may be `None` for
|
|
490 some collision systems that do not report it.
|
|
491
|
|
492 - collision_normal (Vec2d): The normal vector at the point of
|
|
493 collision. As with `collision_point`, this may be None for
|
|
494 some collision systems.
|
|
495 '''
|
|
496
|
|
497 Note the arguments to `on_collide()` are always passed positionally, so you
|
|
498 can use different argument names than above if desired.
|
|
499
|
|
500 If a pair of entities are in collision, then the event will be dispatched
|
|
501 to both objects in arbitrary order if all of their collision masks align.
|
|
502 """
|
|
503 collision = getattr(collision_system.world.components,
|
|
504 collision_system.collision_component)
|
|
505 for pair in collision_system.collision_pairs:
|
|
506 entity1, entity2 = pair
|
|
507 if pair.info is not None:
|
|
508 args1, args2 = pair.info
|
|
509 else:
|
|
510 args1 = entity1, None, None
|
|
511 args2 = entity2, None, None
|
|
512 try:
|
|
513 on_collide = entity1.on_collide
|
|
514 masks_align = collision[entity2].from_mask & collision[entity1].into_mask
|
|
515 except (AttributeError, KeyError):
|
|
516 pass
|
|
517 else:
|
|
518 if masks_align:
|
|
519 on_collide(*args2)
|
|
520 try:
|
|
521 on_collide = entity2.on_collide
|
|
522 masks_align = collision[entity1].from_mask & collision[entity2].into_mask
|
|
523 except (AttributeError, KeyError):
|
|
524 pass
|
|
525 else:
|
|
526 if masks_align:
|
|
527 on_collide(*args1)
|
|
528
|