--- a/trafficintelligence/moving.py	Wed Apr 26 23:36:32 2023 -0400
+++ b/trafficintelligence/moving.py	Fri Apr 28 17:03:39 2023 -0400
@@ -5,7 +5,7 @@
 from math import sqrt, atan2, cos, sin
 
 from numpy import median, mean, array, arange, zeros, ones, hypot, NaN, std, floor, ceil, float32, argwhere, minimum,  issubdtype, integer as npinteger
-from matplotlib.pyplot import plot, text
+from matplotlib.pyplot import plot, text, arrow
 from scipy.stats import scoreatpercentile
 from scipy.spatial.distance import cdist
 from scipy.signal import savgol_filter
@@ -430,6 +430,10 @@
         return [Point(Point.dot(originalE1, p), Point.dot(originalE2, p)) for p in [frontLeft, frontRight, rearRight, rearLeft]]
 
 if shapelyAvailable:
+    def pointsToShapely(points):
+        'Returns shapely polygon'
+        return Polygon([p.astuple() for p in points])
+    
     def pointsInPolygon(points, polygon):
         '''Optimized tests of a series of points within (Shapely) polygon (not prepared)'''
         if type(polygon) == PreparedGeometry:
@@ -1233,6 +1237,17 @@
         return indices
 
 ##################
+# Geometry
+##################
+
+class Geometry(object):
+    '''Generic class for outline and size of object '''
+    
+
+class CarGeometry(Geometry):
+    '''Default car geometry as rectangle '''
+    
+##################
 # Moving Objects
 ##################
 
@@ -1656,10 +1671,14 @@
         else:
             self.positions.plotOnWorldImage(nPixelsPerUnitDistance, options, withOrigin, timeStep, None, **kwargs)
 
-    def plotOutlineAtInstant(self, t, options = '', **kwargs):
+    def plotOutlineAtInstant(self, t, options = '', withVelocity = False, velocityMultiply = 5, arrowWidth=0.1, **kwargs):
         if self.hasFeatures():
             points = [f.getPositionAtInstant(t) for f in self.getFeatures()]
             Point.plotAll(points, True, options, **kwargs)
+        if withVelocity:
+            p = self.getPositionAtInstant(t)
+            v = self.getVelocityAtInstant(t)*velocityMultiply
+            arrow(p.x, p.y, v.x, v.y, width=arrowWidth)
 
     def play(self, videoFilename, homography = None, undistort = False, intrinsicCameraMatrix = None, distortionCoefficients = None, undistortedImageMultiplication = 1.):
         cvutils.displayTrajectories(videoFilename, [self], homography = homography, firstFrameNum = self.getFirstInstant(), lastFrameNumArg = self.getLastInstant(), undistort = undistort, intrinsicCameraMatrix = intrinsicCameraMatrix, distortionCoefficients = distortionCoefficients, undistortedImageMultiplication = undistortedImageMultiplication)
@@ -1809,14 +1828,21 @@
                         relativePositions[(i,j)] = Point(median(xj-xi), median(yj-yi))
                         relativePositions[(j,i)] = -relativePositions[(i,j)]
 
-    def computeBoundingPolygon(self, instant):
+    def computeBoundingPolygon(self, t, shape = 'raw'):
         '''Returns a bounding box for the feature positions at instant
-        bounding box format is a list of points (4 in this case for a rectangle)
-
-        TODO add method argument if using different methods/shapes'''
+        bounding box format is a list of points
+        shape is the type of output shape: 
+         - raw means the list of feature positions
+         - rect means the bounding rectangle aligned with velocity'''
         if self.hasFeatures():
-            positions = [f.getPositionAtInstant(instant) for f in self.getFeatures() if f.existsAtInstant(instant)]
-            return Point.boundingRectangle(positions, self.getVelocityAtInstant(instant))
+            positions = [f.getPositionAtInstant(t) for f in self.getFeatures() if f.existsAtInstant(t)]
+            if shape == 'rect':
+                return Point.boundingRectangle(positions, self.getVelocityAtInstant(t))
+            elif shape == 'raw':
+                return positions
+            else:
+                print('Unknown shape')
+                return None
         else:
             print('Object {} has no features'.format(self.getNum()))
             return None

--- 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: