diff trafficintelligence/prediction.py @ 1214:01c24c1cdb70

implemented direct method
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Thu, 04 May 2023 22:30:32 -0400
parents af329f3330ba
children 1b472cddf9b1
line wrap: on
line diff
--- 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):