--- a/python/extrapolation.py	Tue Jul 24 03:27:29 2012 -0400
+++ b/python/extrapolation.py	Tue Jul 24 12:37:47 2012 -0400
@@ -10,6 +10,13 @@
 
     it should also have a probability'''
 
+    def __init__(self):
+        self.probability = 0.
+        self.predictedPositions = {}
+        self.predictedSpeedOrientations = {}
+        self.collisionPoints = {}
+        self.crossingZones = {}
+
     def predictPosition(self, nTimeSteps):
         if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys():
             self.predictPosition(nTimeSteps-1)
@@ -63,41 +70,60 @@
 
 def createExtrapolatedTrajectories(extrapolationParameters, initialPosition, initialVelocity, maxSpeed):
     '''extrapolationParameters specific to each method (in name field)  '''
-    if extrapolationHypothesis.name == 'constant velocity':
+    if extrapolationParameters.name == 'constant velocity':
         return [ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, maxSpeed = maxSpeed)]
     else:
         print('Unknown extrapolation hypothesis')
         return []
 
+class CollisionPoint(moving.Point):
+    def __init__(self, p, probability = 1., TTC = -1):
+        self.x = p.x
+        self.y = p.y
+        self.probability = probability
+        self.TTC = TTC
+
+class CrossingZone(moving.Point):
+    def __init__(self, p, probability = 1., pPET = -1):
+        self.x = p.x
+        self.y = p.y
+        self.probability = probability
+        self.pPET = pPET
+
 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:
+            while t <= timeHorizon and (p1-p2).norm2() > 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))
+                collisionPoints.append(CollisionPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t))
             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))
+                # TODO? zone should be around the points at which the traj are the closest
+                # look for CZ at different times, otherwise it would be a collision
+                # an approximation would be to look for close points at different times, ie the complementary of collision points
+                cz = None
+                t1 = 0
+                while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1
+                    t2 = 0
+                    while not cz and t2 < timeHorizon:
+                        #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold:
+                        #    cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5)
+                        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
+                            crossingZones.append(CrossingZone(cz, et1.probability*et2.probability, abs(t1-t2)))
+                        t2 += 1
+                    t1 += 1                        
+    return collisionPoints, crossingZones