mosp.collide

1 # -*- coding: utf-8 -*- 2 """Classes and algorithms for collision detection.""" 3 4 from math import sqrt 5 import os 6 import struct 7 8 __author__ = "B. Henne, F. Ludwig, P. Tute" 9 __contact__ = "henne@dcsec.uni-hannover.de" 10 __copyright__ = "(c) 2010-2011, DCSec, Leibniz Universitaet Hannover, Germany" 11 __license__ = "GPLv3" 12 13

14 -class World(object):

15 """A set of all walkable elements in the simulated world, e. g. streets and 16 public places. 17 @author: F. Ludwig 18 @author: P. Tute 19 @author: B. Henne"""

20 - def __init__(self, grid_size=100):

21 """Initialize collision grid. 22 23 grid_size determines maximum collision region. Collision 24 range/region/radius must be lower or equal the grid_size. 25 World.obj is the set of walkable elements. 26 calculate_grid() must be called after all objects have been loaded.""" 27 self.grid_size = int(grid_size) 28 self.obj = set() 29 self.free_obj = set() 30 self.grid = {}

31

32 - def add(self, obj):

33 """Adds an object to the world.""" 34 self.obj.add(obj)

35

36 - def update(self, items):

37 """Updates an object of the world.""" 38 self.obj.update(items)

39

40 - def calculate_grid(self, cache_base_path=None):

41 """Calculates the collision grid of the World. 42 43 This method must be called after all world objects have been 44 added to the World. It calculate which objects are in which grid segment. 45 Calculation is done by colliding each object with each grid segment. 46 Determining a grid grid segment by coordinates is done by integer maths. 47 A grid segment is represented by its west and south border, e.g. 48 grid_size=50, p=(127.9; 33.2) => segment is x=100, y=0.""" 49 # setup boundaries if not done before 50 if not hasattr(self, 'start_x'): 51 # calculate grid boundary coordinates (min|max)* 52 min_x = min_y = float('inf') 53 max_x = max_y = 0 54 for obj in self.obj: 55 for point in obj.get_points(): 56 min_x = min(min_x, point[0]) 57 max_x = max(max_x, point[0]) 58 min_y = min(min_y, point[1]) 59 max_y = max(max_y, point[1]) 60 # calculate grid boundary min/max coordinates as (start|end)* 61 self.start_x = int(min_x) / self.grid_size * self.grid_size 62 self.start_y = int(min_y) / self.grid_size * self.grid_size 63 self.end_x = int(max_x) / self.grid_size * self.grid_size 64 self.end_y = int(max_y) / self.grid_size * self.grid_size 65 66 # setup grid cache path 67 cache_path = None 68 if cache_base_path: 69 cache_path = cache_base_path + '.grid' + str(self.grid_size) 70 71 # load grid from cache file if possible 72 if cache_path and os.path.exists(cache_path): 73 objs = {} 74 for obj in self.obj: 75 objs[obj.id] = obj 76 rect_data_fmt = struct.Struct('!III') 77 id_fmt = struct.Struct('!I') 78 cache = open(cache_path) 79 data = cache.read(rect_data_fmt.size) 80 while data: 81 x, y, count = rect_data_fmt.unpack(data) 82 self.grid.setdefault(x, {}) 83 rect = set() 84 for i in xrange(count): 85 rect.add(objs[id_fmt.unpack(cache.read(id_fmt.size))[0]]) 86 self.grid[x][y] = rect 87 data = cache.read(rect_data_fmt.size) 88 # if not cached, build collision grid from scratch 89 else: 90 # for each grid segment x,y 91 for x in xrange(self.start_x, self.end_x + 1, self.grid_size): 92 self.grid[x] = {} 93 for y in xrange(self.start_y, self.end_y + 1, self.grid_size): 94 # collide world with grid segment and get all objects contained in segment 95 rect = self.collide_rectangle(x, y, x + self.grid_size, y + self.grid_size) 96 self.grid[x][y] = rect 97 # store grid in cache file 98 if cache_path: 99 cache = open(cache_path, 'w') 100 for x in self.grid: 101 for y in self.grid[x]: 102 data = '' 103 for obj in self.grid[x][y]: 104 data += struct.pack('!I', obj.id) 105 cache.write(struct.pack('!III', x, y, len(self.grid[x][y])) + data) 106 cache.close()

107

108 - def collide_circle_impl0(self, x, y, radius):

109 """Checks all registered walkable objects for a collision with a 110 given circle. Simplest implementation, least performance.""" 111 re = set() 112 for obj in self.obj | self.free_obj: 113 if obj.collide_circle(x, y, radius): 114 re.add(obj) 115 return re

116

117 - def collide_circle_impl1(self, x, y, radius):

118 """Checks all registered walkable objects for collision that are 119 in grid segments which collide with the specified circle. 120 121 This method first determines all concerned grid segments and finally 122 only collides objects located in these segments. This implementation 123 uses sub/add/abs to determine neighbor segments by checking coordinates.""" 124 grid_size = self.grid_size 125 rect = self.grid 126 assert radius <= grid_size 127 128 # center of circle is in grid segment r_x (west), r_y (south) 129 # this segment is colliding the circle in any case 130 r_x = int(x) / grid_size * grid_size 131 r_y = int(y) / grid_size * grid_size 132 133 # set() pos contains all objects that have to be tested for collision 134 pos = rect.get(r_x, {}).get(r_y, set()) 135 pos = pos | self.free_obj 136 137 # check direct neighbor grid segments 138 top = bottom = left = right = False 139 # top 140 if abs(r_y - y) < radius: 141 # north grid segment is colliding 142 pos.update(rect.get(r_x, {}).get(r_y - grid_size, set())) 143 top = True 144 # bottom 145 if abs(r_y - y + grid_size) < radius: 146 # south grid segment is colliding 147 pos.update(rect.get(r_x, {}).get(r_y + grid_size, set())) 148 bottom = True 149 # left 150 if abs(r_x - x) < radius: 151 # west grid segment is colliding 152 pos.update(rect.get(r_x - grid_size, {}).get(r_y, set())) 153 left = True 154 # right 155 if abs(r_x - x + grid_size) < radius: 156 # east grid segment is colliding 157 pos.update(rect.get(r_x + grid_size, {}).get(r_y, set())) 158 right = True 159 160 # check corner neighbor grid segments 161 # top left 162 if left and top: 163 # north west grid segment is colliding 164 pos.update(rect.get(r_x - grid_size, {}).get(r_y - grid_size, set())) 165 # top right 166 if top and right: 167 # north east grid segment is colliding 168 pos.update(rect.get(r_x + grid_size, {}).get(r_y - grid_size, set())) 169 # bottom left 170 if bottom and left: 171 # south west grid segment is collding 172 pos.update(rect.get(r_x - grid_size, {}).get(r_y + grid_size, set())) 173 # bottom right 174 if bottom and right: 175 # south east grid segment is colliding 176 pos.update(rect.get(r_x + grid_size, {}).get(r_y + grid_size, set())) 177 178 # finally check all objects in concerned grid segments for collision 179 re2 = set() 180 for obj in pos: 181 if obj.collide_circle(x, y, radius): 182 re2.add(obj) 183 return re2

184

185 - def collide_circle_impl2(self, x, y, radius):

186 """Checks all registered walkable objects for collision that are 187 in grid segments which collide with the specified circle. 188 189 This method first determines all concerned grid segments and finally 190 only collides objects located in these segments. This implementation 191 uses Line.collide to determine neighbor segments by colliding circle 192 with segment boundaries.""" 193 194 grid_size = self.grid_size 195 rect = self.grid 196 assert radius <= grid_size 197 198 # center of circle is in grid segment r_x (west), r_y (south) 199 # this segment is colliding the circle in any case 200 r_x = int(x) / grid_size * grid_size 201 r_y = int(y) / grid_size * grid_size 202 203 # set() pos contains all objects that have to be tested for collision 204 pos = rect.get(r_x, {}).get(r_y, set()) 205 pos = pos | self.free_obj 206 207 # Lines are boundaries of segment r_x, r_y 208 top_line = Line(r_x, r_y, r_x + grid_size, r_y) 209 left_line = Line(r_x, r_y, r_x, r_y + grid_size) 210 right_line = Line(r_x + grid_size, r_y, r_x + grid_size, r_y + grid_size) 211 bottom_line = Line(r_x, r_y + grid_size, r_x + grid_size, r_y + grid_size) 212 # determine which of the boundary lines collide the given circle 213 top_line_col = top_line.collide_circle(x, y, radius) 214 left_line_col = left_line.collide_circle(x, y, radius) 215 right_line_col = right_line.collide_circle(x, y, radius) 216 bottom_line_col = bottom_line.collide_circle(x, y, radius) 217 218 # top left 219 if top_line_col and left_line_col: 220 # north west grid segment is colliding 221 pos.update(rect.get(r_x - grid_size, {}).get(r_y - grid_size, set())) 222 # top 223 if top_line_col: 224 # north grid segment is colliding 225 pos.update(rect.get(r_x, {}).get(r_y - grid_size, set())) 226 # top right 227 if top_line_col and right_line_col: 228 # north east grid segment is colliding 229 pos.update(rect.get(r_x + grid_size, {}).get(r_y - grid_size, set())) 230 # left 231 if left_line_col: 232 # west grid segment is colliding 233 pos.update(rect.get(r_x - grid_size, {}).get(r_y, set())) 234 # right 235 if right_line_col: 236 # east grid segment is colliding 237 pos.update(rect.get(r_x + grid_size, {}).get(r_y, set())) 238 # south east grid segment is colliding 239 if left_line_col and bottom_line_col: 240 # south west grid segment is colliding 241 pos.update(rect.get(r_x - grid_size, {}).get(r_y + grid_size, set())) 242 # bottom 243 if bottom_line_col: 244 # south grid segment is colliding 245 pos.update(rect.get(r_x, {}).get(r_y + grid_size, set())) 246 # bottom right 247 if right_line_col and bottom_line_col: 248 # south east grid segment is colliding 249 pos.update(rect.get(r_x + grid_size, {}).get(r_y + grid_size, set())) 250 251 # finally check all objects in concerned grid segments for collision 252 re2 = set() 253 for obj in pos: 254 if obj.collide_circle(x, y, radius): 255 re2.add(obj) 256 return re2

257 258 # select circle collision implementation, may be overridden by user 259 collide_circle = collide_circle_impl2 260 261 # TODO: Is THIS working? It is not, is it?

262 - def collide_polygon(self, corners):

263 """Checks all registered walkable objects for a collision with a given polygon. 264 @status: not implemented""" 265 raise NotImplementedError 266 re = set() 267 for obj in self: 268 if obj.collide_circle(corners): 269 re.add(obj) 270 return re

271

272 - def collide_rectangle(self, x_min, y_min, x_max, y_max):

273 """Checks all registered walkable objects for a collision with a given rectangle.""" 274 re = set() 275 for obj in self.obj | self.free_obj: 276 if obj.collide_rectangle(x_min, y_min, x_max, y_max): 277 re.add(obj) 278 return re

279 280

281 -class Rectangle(object):

282 """A collidable rectangle. Not fully implemented. 283 @status: not completely implemented 284 @author: P. Tute"""

285 - def __init__(self, x0, y0, x1, y1):

286 """initializes the collidable rectangle. 287 288 x0, y0 define south west point of rectangle, 289 x1, y1 define north east point of rectangle.""" 290 self.x0 = x0 291 self.y0 = y0 292 self.x1 = x1 293 self.y1 = y1

294

295 - def collide_circle(self, x, y, radius):

296 """Not implemented! Checks if this rectangle collides with the given circle. 297 @status: not implemented""" 298 raise NotImplementedError

299

300 - def collide_rectangle(self, x_min, y_min, x_max, y_max):

301 """Not implemented! Checks if this rectangle collides with the given rectangle. 302 @status: not implemented""" 303 raise NotImplementedError

304

305 - def collide_polygon(corners):

306 """Not implemented! Checks if this rectangle collides with the given polygon. 307 @status: not implemented""" 308 raise NotImplementedError

309 310

311 -class Line(object):

312 """A collidable line. 313 @author: P. Tute"""

314 - def __init__(self, x_start, y_start, x_end, y_end):

315 """initializes the collidable Line. 316 317 x_start, y_start define starting point, 318 x_end, y_end define ending point.""" 319 self.x_start = x_start 320 self.y_start = y_start 321 self.x_end = x_end 322 self.y_end = y_end

323

324 - def get_points(self):

325 """Yields the start and end point coordinates (x, y) of the Line.""" 326 yield self.x_start, self.y_start 327 yield self.x_end, self.y_end

328

329 - def closest_to_point(self, x, y):

330 """Finds the point on this line closest to a given point. 331 332 Based on http://www.alecjacobson.com/weblog/?p=1486""" 333 # create vector (x1, y1) from start to end of line 334 x1 = self.x_end - self.x_start 335 y1 = self.y_end - self.y_start 336 337 squared_dist = x1 ** 2 + y1 ** 2 338 339 # check if start- and endpoints are the same 340 if squared_dist == 0: 341 return self.x_start, self.y_start 342 343 # calculate vector from start of line to given point 344 start_to_point_x = x - self.x_start 345 start_to_point_y = y - self.y_start 346 347 # do some math to calculate, where the closest point lies 348 t = (start_to_point_x * x1 + start_to_point_y * y1) / float(squared_dist) 349 if t < 0.0: 350 # point lies "before" start of line 351 return self.x_start, self.y_start 352 elif t > 1.0: 353 # point lies "after" end of line 354 return self.x_end, self.y_end 355 else: 356 # point lies "between" end and start 357 re_x = self.x_start + t * x1 358 re_y = self.y_start + t * y1 359 return re_x, re_y

360 361 # def closest_to_point2(self, x, y): 362 # """Finds the point on this line closest to a given point.""" 363 # vx = self.x_start - x 364 # vy = self.y_start - y 365 # ux = self.x_end - self.x_start 366 # uy = self.y_end - self.y_start 367 # length = ux*ux+uy*uy 368 # det = (-vx * ux) + (-vy * uy) 369 # if (det < 0) or (det > length): 370 # # outside 371 # ux = self.x_end - x 372 # uy = self.y_end - y 373 # if (vx*vx + vy*vy) < (ux*ux + uy*uy): 374 # return self.x_start, self.y_start 375 # else: 376 # return self.x_end, self.y_end 377 # det_l = det/length 378 # return self.x_start+ux*det_l, self.y_start+uy*det_l 379 # 380 # def dist_to_point(self, x, y): 381 # """Calculates the distance between Line and point(x,y).""" 382 # vx = self.x_start - x 383 # vy = self.y_start - y 384 # ux = self.x_end - self.x_start 385 # uy = self.y_end - self.y_start 386 # length = ux*ux+uy*uy 387 # det = (-vx * ux) + (-vy * uy) 388 # if (det < 0) or (det > length): 389 # # outside 390 # ux = self.x_end - x 391 # uy = self.y_end - y 392 # return sqrt(min(vx*vx + vy*vy, ux*ux + uy*uy)) 393 # det = ux * vy - uy * vx 394 # return sqrt((det*det)/length) 395

396 - def collide_vertical_line(self, x, y1, y2):

397 """Checks if this line collides with a vertical line. 398 399 Returns a boolean and the intersection-coordinates.""" 400 # line is vertical -> no collision necessary 401 if self.x_start == self.x_end: 402 return False, 0, 0 403 # line completely left or right of other line 404 if (self.x_start < x and self.x_end < x or 405 self.x_start > x and self.x_end > x): 406 return False, 0, 0 407 # line over or under other line 408 bottom_y = min(y1, y2) 409 top_y = max(y1, y2) 410 if (self.y_start > top_y and self.y_end > top_y or 411 self.y_start < bottom_y and self.y_end < bottom_y): 412 return False, 0, 0 413 # lines collide...calculate y-value 414 if self.y_start == self.y_end: 415 # line is horizontal 416 return True, x, self.y_start 417 elif self.x_start < self.x_end: 418 p1 = [self.x_start, self.y_start] 419 p2 = [self.x_end, self.y_end] 420 else: 421 p1 = [self.x_end, self.y_end] 422 p2 = [self.x_start, self.y_start] 423 m = (p2[1] - p1[1]) / (p2[0] - p1[0]) 424 b = p1[1] - m * p1[0] 425 y = m * x + b 426 return True, x, y

427

428 - def collide_horizontal_line(self, x1, x2, y):

429 """Checks if this line collides with a horizontal line. 430 431 Returns a boolean and the intersection-coordinates.""" 432 # line is horizontal -> no collision necessary 433 if self.y_start == self.y_end: 434 return False, 0, 0 435 # line completely under or over other line 436 if (self.y_start < y and self.y_end < y or 437 self.y_start > y and self.y_end > y): 438 return False, 0, 0 439 # line left or right of other line 440 left_x = min(x1, x2) 441 right_x = max(x1, x2) 442 if (self.x_start < left_x and self.x_end < left_x or 443 self.x_start > right_x and self.x_end > right_x): 444 return False, 0, 0 445 446 # lines collide...calculate x-value 447 if self.x_start == self.x_end: 448 # line is vertical 449 return True, self.x_start, y 450 elif self.x_start < self.x_end: 451 p1 = [self.x_start, self.y_start] 452 p2 = [self.x_end, self.y_end] 453 else: 454 p1 = [self.x_end, self.y_end] 455 p2 = [self.x_start, self.y_start] 456 m = (p2[1] - p1[1]) / (p2[0] - p1[0]) 457 b = p1[1] - m * p1[0] 458 x = (y - b) / m 459 return True, x, y

460

461 - def collide_circle(self, x, y, radius):

462 """Checks if this line collides with the given circle.""" 463 #dist = self.dist_to_point(x, y) 464 #return dist <= radius 465 close_x, close_y = self.closest_to_point(x, y) 466 return sqrt((close_x - x) ** 2 + (close_y - y) ** 2) <= radius

467

468 - def collide_rectangle(self, x_min, y_min, x_max, y_max):

469 """Checks if this line collides with the given rectangle. 470 471 The algorithm by Cohen & Sutherland is used. 472 It only works for axis-aligned rectangles. 473 http://en.wikipedia.org/wiki/Cohen%E2%80%93Sutherland""" 474 INSIDE = 0 475 LEFT = 1 476 RIGHT = 2 477 BOTTOM = 4 478 TOP = 8 479 480 def calculate_code(x, y): 481 code = INSIDE 482 483 if x < x_min: 484 code |= LEFT 485 elif x > x_max: 486 code |= RIGHT 487 if y < y_min: 488 code |= BOTTOM 489 elif y > y_max: 490 code |= TOP 491 492 return code

493 494 x0 = self.x_start 495 y0 = self.y_start 496 x1 = self.x_end 497 y1 = self.y_end 498 499 code0 = calculate_code(x0, y0) 500 code1 = calculate_code(x1, y1) 501 502 while True: 503 if not code0 | code1: 504 return True 505 elif code0 & code1: 506 return False 507 else: 508 code = code0 if code0 else code1 509 if code & TOP: 510 x = (x0 + (x1 - x0) * 511 (y_max - y0) / (y1 - y0)) 512 y = y_max 513 elif code & BOTTOM: 514 x = (x0 + (x1 - x0) * 515 (y_min - y0) / (y1 - y0)) 516 y = y_min 517 elif code & RIGHT: 518 y = (y0 + (y1 - y0) * 519 (x_max - x0) / (x1 - x0)) 520 x = x_max 521 elif code & LEFT: 522 y = (y0 + (y1 - y0) * 523 (x_min - x0) / (x1 - x0)) 524 x = x_min 525 526 if code == code0: 527 x0 = x 528 y0 = y 529 code0 = calculate_code(x0, y0) 530 elif code == code1: 531 x1 = x 532 y1 = y 533 code1 = calculate_code(x1, y1)

534 535

536 - def collide_polygon(self, corners):

537 """Nothing Implemented! Checks if this line collides with the given polygon. 538 @status: not implemented""" 539 raise NotImplementedErrorss

540

541 - def __repr__(self):

542 return "<Line (%i,%i) to (%i, %i)>" % (self.x_start, self.y_start, self.x_end, self.y_end)

543 544

545 -class Polygon(object):

546 """A collidable polygon. Nothing implemented yet. 547 @status: not implemented 548 @author: P. Tute"""

549 - def collide_circle(self, x, y, radius):

550 """Checks if this polygon collides with the given circle. 551 @status: not implemented""" 552 raise NotImplementedError

553

554 - def collide_polygon(corners):

555 """Checks if this polygon collides with the given polygon. 556 @status: not implemented""" 557 raise NotImplementedError

558 559

560 -class Point(object):

561 """A collidable point. 562 @author: P. Tute"""

563 - def __init__(self, x, y):

564 """initializes the collidable point. 565 566 x and y are its coordinates.""" 567 self.x = x 568 self.y = y

569

570 - def collide_circle(self, x, y, radius):

571 """Checks if this point collides with the given circle.""" 572 x_dist = self.x - x 573 y_dist = self.y - y 574 dist = sqrt(x_dist ** 2 + y_dist ** 2) 575 return dist <= radius

576

577 - def collide_rectangle(self, x_min, y_min, x_max, y_max):

578 """Checks if this point collides with the given rectangle.""" 579 return (x_min <= self.x <= x_max and 580 y_min <= self.y <= y_max)

581

582 - def collide_polygon(corners):

583 """Checks if this point collides with the given polygon. 584 @status: not implemented""" 585 raise NotImplementedError

586

587 - def get_points(self):

588 """Yields the coordinates (x, y) of the point.""" 589 yield self.x, self.y

590 591

592 -class WorldPart(Rectangle, set):

593 """Empty, nothing implemented. 594 @status: not implemented 595 @author: F. Ludwig"""

596 - def __init__(self):

597 raise NotImplementedError

598

Source Code for Module mosp.collide