changeset 1214:01c24c1cdb70

implemented direct method
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Thu, 04 May 2023 22:30:32 -0400
parents 3f2214125164
children 1b472cddf9b1
files trafficintelligence/moving.py trafficintelligence/prediction.py
diffstat 2 files changed, 26 insertions(+), 14 deletions(-) [+]
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
 
--- a/trafficintelligence/prediction.py	Wed May 03 14:58:26 2023 -0400
+++ b/trafficintelligence/prediction.py	Thu May 04 22:30:32 2023 -0400
@@ -605,24 +605,35 @@
                     pass # compute pPET
         return collisionPoints, crossingZones
 
-class CVRectPredictionParameters(PredictionParameters):
+class CVExactPolyPredictionParameters(PredictionParameters):
     '''Prediction parameters of prediction at constant velocity
     for objects represented by boxes (bird eye view boxes)
     Warning: the computed time to collision may be higher than timeHorizon (not used)'''
     
     def __init__(self):
-        PredictionParameters.__init__(self, 'constant velocity for rectangles', None)
+        PredictionParameters.__init__(self, 'constant velocity for polygon representation (direct exact)', None)
 
     def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs):
         'TODO compute pPET'
         collisionPoints = []
         crossingZones = []
 
-        # first test if there is a collision (test if movement is parallel to sides of vehicle)
-        # test if vehicle aligned with y axis, otherwise, compute slope
-        
+        if self.useCurvilinear:
+            pass # Lionel
+        else:
+            poly1 = obj1.getBoundingPolygon(currentInstant)
+            poly2 = obj2.getBoundingPolygon(currentInstant)
+            v1 = obj1.getVelocityAtInstant(currentInstant)
+            v2 = obj2.getVelocityAtInstant(currentInstant)
 
-        # compute approximate time of arrival
+            if not moving.Point.parallel(v1, v2):
+                ttc = moving.Point.timeToCollisionPoly(poly1, v1, poly2, v2)
+                if ttc is not None:
+                    collisionPoints = [SafetyPoint((obj1.getPositionAtInstant(currentInstant)+(v1*ttc)+obj2.getPositionAtInstant(currentInstant)+(v2*ttc))*0.5, 1., ttc)]
+                else:
+                    pass # compute pPET
+        return collisionPoints, crossingZones
+
 
 class PrototypePredictionParameters(PredictionParameters):
     def __init__(self, prototypes, nPredictedTrajectories, pointSimilarityDistance, minSimilarity, lcssMetric = 'cityblock', minFeatureTime = 10, constantSpeed = False, useFeatures = True):