Mercurial > parpg-source
annotate bGrease/collision.py @ 41:ff3e395abf91
Renamed grease to bGrease (Basic Grease) to get rid of conflicts with an already installed grease.
| author | KarstenBock@gmx.net |
|---|---|
| date | Mon, 05 Sep 2011 15:00:34 +0200 |
| parents | grease/collision.py@bc88f7d5ca8b |
| children | e856b604b650 |
| rev | line source |
|---|---|
| 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 | |
|
41
ff3e395abf91
Renamed grease to bGrease (Basic Grease) to get rid of conflicts with an already installed grease.
KarstenBock@gmx.net
parents:
5
diff
changeset
|
42 from bGrease.geometry import Vec2d |
| 5 | 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 |