Mercurial Hosting > traffic-intelligence
comparison trafficintelligence/moving.py @ 1214:01c24c1cdb70
implemented direct method
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Thu, 04 May 2023 22:30:32 -0400 |
parents | 3f2214125164 |
children | 5038c357b57f |
comparison
equal
deleted
inserted
replaced
1213:3f2214125164 | 1214:01c24c1cdb70 |
---|---|
396 return None | 396 return None |
397 else: | 397 else: |
398 return None | 398 return None |
399 | 399 |
400 @staticmethod | 400 @staticmethod |
401 def ttc_calculate(bbox_info_total_1, bbox_info_total_2): | 401 def timeToCollisionPoly(corners1, v1, corners2, v2): |
402 """ | 402 """ |
403 :param bbox_1: list: [bbox_1 array 4*2 , v_1_x (float), v_1_y (float)] | 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)] | 404 :param bbox_2: [bbox_2_x array 4*2, v_2_x (float), v_2_y (float)] |
405 :return: ttc | 405 :return: ttc |
406 """ | 406 """ |
407 from sympy import solve | 407 from sympy import solve |
408 from sympy.abc import t | |
408 def NewFourPoints(bbox, col_time, v_x, v_y): | 409 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 return [i + col_time * v_x for i in bbox[0]], [j + col_time * v_y for j in bbox[1]] |
410 | 411 |
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 v_1_x, v_1_y = v1.x, v1.y |
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 v_2_x, v_2_y = v2.x, v2.y |
413 x_bbox_1 = bbox_1[:, 0].tolist() | 414 x_bbox_1 = [p.x for p in corners1] |
414 y_bbox_1 = bbox_1[:, 1].tolist() | 415 y_bbox_1 = [p.y for p in corners1] |
415 bbox_1 = [x_bbox_1, y_bbox_1] | 416 bbox_1 = [x_bbox_1, y_bbox_1] |
416 x_bbox_2 = bbox_2[:, 0].tolist() | 417 x_bbox_2 = [p.x for p in corners2] |
417 y_bbox_2 = bbox_2[:, 1].tolist() | 418 y_bbox_2 = [p.y for p in corners2] |
418 bbox_2 = [x_bbox_2, y_bbox_2] | 419 bbox_2 = [x_bbox_2, y_bbox_2] |
419 t_total = [] | 420 t_total = [] |
420 line = [[0, 1], [1, 2], [2, 3], [3, 0]] | 421 line = [[0, 1], [1, 2], [2, 3], [3, 0]] |
421 for i in range(4): | 422 for i in range(4): |
422 for j in range(4): | 423 for j in range(4): |
482 if t_total: | 483 if t_total: |
483 collision_t_min = min(t_total) | 484 collision_t_min = min(t_total) |
484 # print(f'collision_time:') | 485 # print(f'collision_time:') |
485 # print(collision_t_min) | 486 # print(collision_t_min) |
486 else: | 487 else: |
487 collision_t_min = 1e7 | 488 collision_t_min = None |
488 # print(f'No collision') | 489 # print(f'No collision') |
489 return collision_t_min | 490 return collision_t_min |
490 | 491 |
491 @staticmethod | 492 @staticmethod |
492 def midPoint(p1, p2): | 493 def midPoint(p1, p2): |