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

--- a/python/moving.py	Tue Jul 24 03:27:29 2012 -0400
+++ b/python/moving.py	Tue Jul 24 12:37:47 2012 -0400
@@ -1,10 +1,10 @@
 #! /usr/bin/env python
 '''Libraries for moving objects, trajectories...'''
 
-import utils;
-import cvutils;
+import utils
+import cvutils
 
-from math import sqrt;
+from math import sqrt
 
 #from shapely.geometry import Polygon
 
@@ -295,25 +295,31 @@
     from numpy import matrix
     from numpy.linalg import linalg, det
 
-    dp1 = p2-p1#[s1[0][1]-s1[0][0], s1[1][1]-s1[1][0]]
-    dp2 = p4-p3#[s2[0][1]-s2[0][0], s2[1][1]-s2[1][0]]
-
-    A = matrix([[dp1.y, -dp1.x],
-                [dp2.y, -dp2.x]])
-    B = matrix([[dp1.y*p1.x-dp1.x*p1.y],
-                [dp2.y*p3.x-dp2.x*p3.y]])
-
-    if linalg.det(A) == 0:#crossProduct(ds1, ds2) == 0:
+    if (Interval(p1.x,p2.x, True).intersection(Interval(p3.x,p4.x, True)).empty()
+        or Interval(p1.y,p2.y, True).intersection(Interval(p3.y,p4.y, True)).empty()):
         return None
     else:
-        intersection = linalg.solve(A,B)
-        if (utils.inBetween(p1.x, p2.x, intersection[0,0])
-            and utils.inBetween(p3.x, p4.x, intersection[0,0])
-            and utils.inBetween(p1.y, p2.y, intersection[1,0])
-            and utils.inBetween(p3.y, p4.y, intersection[1,0])):
-            return Point(intersection[0,0], intersection[1,0])
+        dp1 = p2-p1#[s1[0][1]-s1[0][0], s1[1][1]-s1[1][0]]
+        dp2 = p4-p3#[s2[0][1]-s2[0][0], s2[1][1]-s2[1][0]]
+
+        A = matrix([[dp1.y, -dp1.x],
+                    [dp2.y, -dp2.x]])
+        B = matrix([[dp1.y*p1.x-dp1.x*p1.y],
+                    [dp2.y*p3.x-dp2.x*p3.y]])
+
+        if linalg.det(A) == 0:#crossProduct(ds1, ds2) == 0:
+            return None
         else:
-            return None
+            intersection = linalg.solve(A,B)
+            if (utils.inBetween(p1.x, p2.x, intersection[0,0])
+                and utils.inBetween(p3.x, p4.x, intersection[0,0])
+                and utils.inBetween(p1.y, p2.y, intersection[1,0])
+                and utils.inBetween(p3.y, p4.y, intersection[1,0])):
+                return Point(intersection[0,0], intersection[1,0])
+            else:
+                return None
+
+# TODO: implement a better algorithm for intersections of sets of segments http://en.wikipedia.org/wiki/Line_segment_intersection
 
 class Trajectory:
     '''Class for trajectories
@@ -408,11 +414,11 @@
     
     def xBounds(self):
         # look for function that does min and max in one pass
-        return [min(self.getXCoordinates()), max(self.getXCoordinates())]
+        return Interval(min(self.getXCoordinates()), max(self.getXCoordinates()))
     
     def yBounds(self):
         # look for function that does min and max in one pass
-        return [min(self.getYCoordinates()), max(self.getYCoordinates())]
+        return Interval(min(self.getYCoordinates()), max(self.getYCoordinates()))
     
     def add(self, traj2):
         '''Returns a new trajectory of the same length'''