# HG changeset patch
# User Nicolas Saunier <nicolas.saunier@polymtl.ca>
# Date 1343114849 14400
# Node ID 9281878ff19e6f0c448a5394ecaa8181966dde98
# Parent  dc1faa7287bd4a27c60c33e6d55755b2c174985c
untested collision/crossing computation

diff -r dc1faa7287bd -r 9281878ff19e python/extrapolation.py
--- a/python/extrapolation.py	Tue Jul 24 01:37:21 2012 -0400
+++ b/python/extrapolation.py	Tue Jul 24 03:27:29 2012 -0400
@@ -57,6 +57,50 @@
         return moving.NormAngle(self.accelerationDistribution(),self.steeringDistribution())
 
 
+class ExtrapolationParameters:
+    def __init__(self, name):
+        self.name = name
+
+def createExtrapolatedTrajectories(extrapolationParameters, initialPosition, initialVelocity, maxSpeed):
+    '''extrapolationParameters specific to each method (in name field)  '''
+    if extrapolationHypothesis.name == 'constant velocity':
+        return [ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, maxSpeed = maxSpeed)]
+    else:
+        print('Unknown extrapolation hypothesis')
+        return []
+
+def computeCrossingsCollisions(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon):
+    '''returns the lists of collision points and crossing zones '''
+    collisionPoints = []
+    TTCs = []
+    crossingZones = []
+    pPETs = []
+    for et1 in extrapolatedTrajectories1:
+        for et2 in extrapolatedTrajectories2:
+            
+            t = 1
+            p1 = et1.predictPosition(t)
+            p2 = et2.predictPosition(t)
+            while t <= timeHorizon and (p1-p2).norm() > collisionDistanceThreshold:
+                p1 = et1.predictPosition(t)
+                p2 = et2.predictPosition(t)
+                t += 1
+
+            if t <= timeHorizon:
+                TTCs.append(t) # todo store probability
+                collisionPoints.append((p1+p2).multiply(0.5))
+            else: # check if there is a crossing zone
+                for t1 in xrange(timeHorizon-1):
+                    for t2 in xrange(timeHorizon-1):
+                        cz = moving.segmentIntersection (et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1))
+                        if cz:
+                            crossingZones.append(cz)
+                            pPETs.append(abs(t1-t2))
+                            break
+    return collisionPoints, crossingZones, TTCs, pPETs
+    
+
+
 # Default values: to remove because we cannot tweak that from a script where the value may be different
 FPS= 25  # No. of frame per second (FPS) 
 vLimit= 25/FPS    #assume limit speed is 90km/hr = 25 m/sec