Mercurial > hg > nsaunier > traffic-intelligence
comparison trafficintelligence/moving.py @ 1213:3f2214125164
work in progress
| author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
|---|---|
| date | Wed, 03 May 2023 14:58:26 -0400 |
| parents | af329f3330ba |
| children | 01c24c1cdb70 |
comparison
equal
deleted
inserted
replaced
| 1212:af329f3330ba | 1213:3f2214125164 |
|---|---|
| 394 return ttc2 | 394 return ttc2 |
| 395 else: # ttc1 < 0 and ttc2 < 0: | 395 else: # ttc1 < 0 and ttc2 < 0: |
| 396 return None | 396 return None |
| 397 else: | 397 else: |
| 398 return None | 398 return None |
| 399 | |
| 400 @staticmethod | |
| 401 def ttc_calculate(bbox_info_total_1, bbox_info_total_2): | |
| 402 """ | |
| 403 :param bbox_1: list: [bbox_1 array 4*2 , v_1_x (float), v_1_y (float)] | |
| 404 :param bbox_2: [bbox_2_x array 4*2, v_2_x (float), v_2_y (float)] | |
| 405 :return: ttc | |
| 406 """ | |
| 407 from sympy import solve | |
| 408 def NewFourPoints(bbox, col_time, v_x, v_y): | |
| 409 return [i + col_time * v_x for i in bbox[0]], [j + col_time * v_y for j in bbox[1]] | |
| 410 | |
| 411 bbox_1, v_1_x, v_1_y = bbox_info_total_1[0], bbox_info_total_1[1], bbox_info_total_1[2] | |
| 412 bbox_2, v_2_x, v_2_y = bbox_info_total_2[0], bbox_info_total_2[1], bbox_info_total_2[2] | |
| 413 x_bbox_1 = bbox_1[:, 0].tolist() | |
| 414 y_bbox_1 = bbox_1[:, 1].tolist() | |
| 415 bbox_1 = [x_bbox_1, y_bbox_1] | |
| 416 x_bbox_2 = bbox_2[:, 0].tolist() | |
| 417 y_bbox_2 = bbox_2[:, 1].tolist() | |
| 418 bbox_2 = [x_bbox_2, y_bbox_2] | |
| 419 t_total = [] | |
| 420 line = [[0, 1], [1, 2], [2, 3], [3, 0]] | |
| 421 for i in range(4): | |
| 422 for j in range(4): | |
| 423 p0_x, p0_y = x_bbox_2[i], y_bbox_2[i] | |
| 424 p1_x, p1_y = x_bbox_1[line[j][0]], y_bbox_1[line[j][0]] | |
| 425 p2_x, p2_y = x_bbox_1[line[j][1]], y_bbox_1[line[j][1]] | |
| 426 f = (p2_x - p1_x) * (p0_y - p1_y) - (p0_x - p1_x) * (p2_y - p1_y) | |
| 427 collision_t = solve(f, t, set=True, dict=True) | |
| 428 if collision_t and collision_t[0][t] >= 0: | |
| 429 collision_t = collision_t[0][t] | |
| 430 print(f'collision_t: {collision_t} >>>>>>>>>') | |
| 431 x_bbox_1_new, y_bbox_1_new = NewFourPoints(bbox_1, collision_t, v_1_x, v_1_y) | |
| 432 x_bbox_2_new, y_bbox_2_new = NewFourPoints(bbox_2, collision_t, v_2_x, v_2_y) | |
| 433 p0_x, p0_y = x_bbox_2_new[i], y_bbox_2_new[i] | |
| 434 p1_x, p1_y = x_bbox_1_new[line[j][0]], y_bbox_1_new[line[j][0]] | |
| 435 p2_x, p2_y = x_bbox_1_new[line[j][1]], y_bbox_1_new[line[j][1]] | |
| 436 min_x, max_x = min(p1_x, p2_x), max(p1_x, p2_x) | |
| 437 min_y, max_y = min(p1_y, p2_y), max(p1_y, p2_y) | |
| 438 if min_x == max_x: | |
| 439 if min_y <= p0_y <= max_y: | |
| 440 t_total.append(collision_t) | |
| 441 | |
| 442 elif min_y == max_y: | |
| 443 if min_x <= p0_x <= max_x: | |
| 444 t_total.append(collision_t) | |
| 445 | |
| 446 else: | |
| 447 if min_x <= p0_x <= max_x or min_y <= p0_y <= max_y: | |
| 448 t_total.append(collision_t) | |
| 449 x_temps = x_bbox_2 | |
| 450 x_bbox_2 = x_bbox_1 | |
| 451 x_bbox_1 = x_temps | |
| 452 # print(f'Change bounding box1 and box2 >>>>>>>>>>>>>>>>>>>\n\n\n') | |
| 453 for i in range(4): | |
| 454 for j in range(4): | |
| 455 p0_x, p0_y = x_bbox_2[i], y_bbox_2[i] | |
| 456 p1_x, p1_y = x_bbox_1[line[j][0]], y_bbox_1[line[j][0]] | |
| 457 p2_x, p2_y = x_bbox_1[line[j][1]], y_bbox_1[line[j][1]] | |
| 458 f = (p2_x - p1_x) * (p0_y - p1_y) - (p0_x - p1_x) * (p2_y - p1_y) | |
| 459 collision_t = solve(f, t, set=True, dict=True) | |
| 460 if collision_t and collision_t[0][t] >= 0: | |
| 461 collision_t = collision_t[0][t] | |
| 462 # print(f'collision_t: {collision_t} >>>>>>>>>') | |
| 463 x_bbox_1_new, y_bbox_1_new = NewFourPoints(bbox_1, collision_t) | |
| 464 x_bbox_2_new, y_bbox_2_new = NewFourPoints(bbox_2, collision_t) | |
| 465 p0_x, p0_y = x_bbox_2_new[i], y_bbox_2_new[i] | |
| 466 p1_x, p1_y = x_bbox_1_new[line[j][0]], y_bbox_1_new[line[j][0]] | |
| 467 p2_x, p2_y = x_bbox_1_new[line[j][1]], y_bbox_1_new[line[j][1]] | |
| 468 min_x, max_x = min(p1_x, p2_x), max(p1_x, p2_x) | |
| 469 min_y, max_y = min(p1_y, p2_y), max(p1_y, p2_y) | |
| 470 if min_x == max_x: | |
| 471 if min_y <= p0_y <= max_y: | |
| 472 t_total.append(collision_t) | |
| 473 | |
| 474 elif min_y == max_y: | |
| 475 if min_x <= p0_x <= max_x: | |
| 476 t_total.append(collision_t) | |
| 477 | |
| 478 else: | |
| 479 if min_x <= p0_x <= max_x or min_y <= p0_y <= max_y: | |
| 480 t_total.append(collision_t) | |
| 481 | |
| 482 if t_total: | |
| 483 collision_t_min = min(t_total) | |
| 484 # print(f'collision_time:') | |
| 485 # print(collision_t_min) | |
| 486 else: | |
| 487 collision_t_min = 1e7 | |
| 488 # print(f'No collision') | |
| 489 return collision_t_min | |
| 399 | 490 |
| 400 @staticmethod | 491 @staticmethod |
| 401 def midPoint(p1, p2): | 492 def midPoint(p1, p2): |
| 402 'Returns the middle of the segment [p1, p2]' | 493 'Returns the middle of the segment [p1, p2]' |
| 403 return Point(0.5*p1.x+0.5*p2.x, 0.5*p1.y+0.5*p2.y) | 494 return Point(0.5*p1.x+0.5*p2.x, 0.5*p1.y+0.5*p2.y) |