diff trafficintelligence/prediction.py @ 1210:1bad5f9b60de

work in progress
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Fri, 28 Apr 2023 17:03:39 -0400
parents 2064e52019db
children a095d4fbb2ea
line wrap: on
line diff
--- a/trafficintelligence/prediction.py	Wed Apr 26 23:36:32 2023 -0400
+++ b/trafficintelligence/prediction.py	Fri Apr 28 17:03:39 2023 -0400
@@ -170,6 +170,23 @@
         collision = (p1-p2).norm2() <= collisionDistanceThreshold
     return collision, t, p1, p2
 
+def computeCollisionTimePolygon(predictedTrajectories1, predictedTrajectories2, timeHorizon):
+    '''Computes the first instant 
+    at which two objects represented by a series of points (eg box)
+    Computes all the times including timeHorizon
+    
+    User has to check the first variable collision to know about a collision'''
+    t = 1
+    poly1 = moving.pointsToShapely([t1.predictPosition(t) for t1 in predictedTrajectories1])
+    poly2 = moving.pointsToShapely([t2.predictPosition(t) for t2 in predictedTrajectories2])
+    collision = poly1.overlaps(poly2)
+    while t < timeHorizon and not collision:
+        t += 1
+        poly1 = moving.pointsToShapely([t1.predictPosition(t) for t1 in predictedTrajectories1])
+        poly2 = moving.pointsToShapely([t2.predictPosition(t) for t2 in predictedTrajectories2])
+        collision = poly1.overlaps(poly2)
+    return collision, t, p1, p2
+
 def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon, printFigure = True):
     from matplotlib.pyplot import figure, axis, title, clf, savefig
     if printFigure: