diff 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
line wrap: on
line diff
--- a/trafficintelligence/moving.py	Wed May 03 14:58:26 2023 -0400
+++ b/trafficintelligence/moving.py	Thu May 04 22:30:32 2023 -0400
@@ -398,23 +398,24 @@
             return None
 
     @staticmethod
-    def ttc_calculate(bbox_info_total_1, bbox_info_total_2):
+    def timeToCollisionPoly(corners1, v1, corners2, v2):
         """
         :param bbox_1: list: [bbox_1 array 4*2 , v_1_x (float), v_1_y (float)]
         :param bbox_2:  [bbox_2_x array 4*2, v_2_x (float), v_2_y (float)]
         :return: ttc
         """
         from sympy import solve
+        from sympy.abc import t
         def NewFourPoints(bbox, col_time, v_x, v_y):
             return [i + col_time * v_x for i in bbox[0]], [j + col_time * v_y for j in bbox[1]]
 
-        bbox_1, v_1_x, v_1_y = bbox_info_total_1[0], bbox_info_total_1[1], bbox_info_total_1[2]
-        bbox_2, v_2_x, v_2_y = bbox_info_total_2[0], bbox_info_total_2[1], bbox_info_total_2[2]
-        x_bbox_1 = bbox_1[:, 0].tolist()
-        y_bbox_1 = bbox_1[:, 1].tolist()
+        v_1_x, v_1_y = v1.x, v1.y
+        v_2_x, v_2_y = v2.x, v2.y
+        x_bbox_1 = [p.x for p in corners1]
+        y_bbox_1 = [p.y for p in corners1]
         bbox_1 = [x_bbox_1, y_bbox_1]
-        x_bbox_2 = bbox_2[:, 0].tolist()
-        y_bbox_2 = bbox_2[:, 1].tolist()
+        x_bbox_2 = [p.x for p in corners2]
+        y_bbox_2 = [p.y for p in corners2]
         bbox_2 = [x_bbox_2, y_bbox_2]
         t_total = []
         line = [[0, 1], [1, 2], [2, 3], [3, 0]]
@@ -484,7 +485,7 @@
             # print(f'collision_time:')
             # print(collision_t_min)
         else:
-            collision_t_min = 1e7
+            collision_t_min = None
             # print(f'No collision')
         return collision_t_min