Mercurial > hg > nsaunier > traffic-intelligence

comparison trafficintelligence/moving.py @ 1214:01c24c1cdb70

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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):