--- a/python/prediction.py	Thu Apr 18 15:29:33 2013 -0400
+++ b/python/prediction.py	Fri Dec 05 12:13:53 2014 -0500
@@ -30,14 +30,14 @@
     def getPredictedSpeeds(self):
         return [so.norm for so in self.predictedSpeedOrientations.values()]
 
-    def draw(self, options = '', withOrigin = False, timeStep = 1, **kwargs):
-        self.getPredictedTrajectory().draw(options, withOrigin, timeStep, **kwargs)
+    def plot(self, options = '', withOrigin = False, timeStep = 1, **kwargs):
+        self.getPredictedTrajectory().plot(options, withOrigin, timeStep, **kwargs)
 
 class PredictedTrajectoryConstant(PredictedTrajectory):
     '''Predicted trajectory at constant speed or acceleration
     TODO generalize by passing a series of velocities/accelerations'''
 
-    def __init__(self, initialPosition, initialVelocity, control = moving.NormAngle(0,0), probability = 1, maxSpeed = None):
+    def __init__(self, initialPosition, initialVelocity, control = moving.NormAngle(0,0), probability = 1., maxSpeed = None):
         self.control = control
         self.maxSpeed = maxSpeed
         self.probability = probability
@@ -47,9 +47,37 @@
     def getControl(self):
         return self.control
 
-class PredictedTrajectoryNormalAdaptation(PredictedTrajectory):
-    '''Random small adaptation of vehicle control '''
-    def __init__(self, initialPosition, initialVelocity, accelerationDistribution, steeringDistribution, probability = 1, maxSpeed = None):
+class PredictedTrajectoryPrototype(PredictedTrajectory):
+    '''Predicted trajectory that follows a prototype trajectory
+    The prototype is in the format of a moving.Trajectory: it could be
+    1. an observed trajectory (extracted from video)
+    2. a generic polyline (eg the road centerline) that a vehicle is supposed to follow
+
+    Prediction can be done
+    1. at constant speed (the instantaneous user speed)
+    2. following the trajectory path, at the speed of the user
+    (applying a constant ratio equal 
+    to the ratio of the user instantaneous speed and the trajectory closest speed)'''
+
+    def __init__(self, initialPosition, initialVelocity, prototypeTrajectory, constantSpeed = True, probability = 1.):
+        self.prototypeTrajectory = prototypeTrajectory
+        self.constantSpeed = constantSpeed
+        self.probability = probability
+        self.predictedPositions = {0: initialPosition}
+        self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)}
+
+    def predictPosition(self, nTimeSteps):
+        if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys():
+            if constantSpeed:
+                # calculate cumulative distance
+                pass
+            else: # see c++ code, calculate ratio
+                pass
+        return self.predictedPositions[nTimeSteps]
+
+class PredictedTrajectoryRandomControl(PredictedTrajectory):
+    '''Random vehicle control: suitable for normal adaptation'''
+    def __init__(self, initialPosition, initialVelocity, accelerationDistribution, steeringDistribution, probability = 1., maxSpeed = None):
         '''Constructor
         accelerationDistribution and steeringDistribution are distributions 
         that return random numbers drawn from them'''
@@ -63,101 +91,6 @@
     def getControl(self):
         return moving.NormAngle(self.accelerationDistribution(),self.steeringDistribution())
 
-class PredictionParameters:
-    def __init__(self, name, maxSpeed):
-        self.name = name
-        self.maxSpeed = maxSpeed
-
-    def __str__(self):
-        return '{0} {1}'.format(self.name, self.maxSpeed)
-
-class ConstantPredictionParameters(PredictionParameters):
-    def __init__(self, maxSpeed):
-        PredictionParameters.__init__(self, 'constant velocity', maxSpeed)
-
-    def generatePredictedTrajectories(self, obj, instant):
-        return [PredictedTrajectoryConstant(obj.getPositionAtInstant(instant), obj.getVelocityAtInstant(instant), 
-                                               maxSpeed = self.maxSpeed)]
-
-class NormalAdaptationPredictionParameters(PredictionParameters):
-    def __init__(self, maxSpeed, nPredictedTrajectories, maxAcceleration, maxSteering):
-        PredictionParameters.__init__(self, 'normal adaptation', maxSpeed)
-        self.nPredictedTrajectories = nPredictedTrajectories
-        self.maxAcceleration = maxAcceleration
-        self.maxSteering = maxSteering
-        self.accelerationDistribution = lambda: random.triangular(-self.maxAcceleration, 
-                                                                   self.maxAcceleration, 0.)
-        self.steeringDistribution = lambda: random.triangular(-self.maxSteering, 
-                                                               self.maxSteering, 0.)
-        
-    def __str__(self):
-        return PredictionParameters.__str__(self)+' {0} {1} {2}'.format(self.nPredictedTrajectories, 
-                                                                           self.maxAcceleration, 
-                                                                           self.maxSteering)
-
-    def generatePredictedTrajectories(self, obj, instant):
-        predictedTrajectories = []
-        for i in xrange(self.nPredictedTrajectories):
-            predictedTrajectories.append(PredictedTrajectoryNormalAdaptation(obj.getPositionAtInstant(instant), 
-                                                                                   obj.getVelocityAtInstant(instant), 
-                                                                                   self.accelerationDistribution, 
-                                                                                   self.steeringDistribution, 
-                                                                                   maxSpeed = self.maxSpeed))
-        return predictedTrajectories
-
-class PointSetPredictionParameters(PredictionParameters):
-    # todo generate several trajectories with normal adaptatoins from each position (feature)
-    def __init__(self, maxSpeed):
-        PredictionParameters.__init__(self, 'point set', maxSpeed)
-    
-    def generatePredictedTrajectories(self, obj, instant):
-        predictedTrajectories = []        
-        features = [f for f in obj.features if f.existsAtInstant(instant)]
-        positions = [f.getPositionAtInstant(instant) for f in features]
-        velocities = [f.getVelocityAtInstant(instant) for f in features]
-        for initialPosition,initialVelocity in zip(positions, velocities):
-            predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition, initialVelocity, 
-                                                                           maxSpeed = self.maxSpeed))
-        return predictedTrajectories
-
-class EvasiveActionPredictionParameters(PredictionParameters):
-    def __init__(self, maxSpeed, nPredictedTrajectories, minAcceleration, maxAcceleration, maxSteering, useFeatures = False):
-        if useFeatures:
-            name = 'point set evasive action'
-        else:
-            name = 'evasive action'
-        PredictionParameters.__init__(self, name, maxSpeed)
-        self.nPredictedTrajectories = nPredictedTrajectories
-        self.minAcceleration = minAcceleration
-        self.maxAcceleration = maxAcceleration
-        self.maxSteering = maxSteering
-        self.useFeatures = useFeatures
-        self.accelerationDistribution = lambda: random.triangular(self.minAcceleration, 
-                                                                  self.maxAcceleration, 0.)
-        self.steeringDistribution = lambda: random.triangular(-self.maxSteering, 
-                                                               self.maxSteering, 0.)
-
-    def __str__(self):
-        return PredictionParameters.__str__(self)+' {0} {1} {2} {3}'.format(self.nPredictedTrajectories, self.minAcceleration, self.maxAcceleration, self.maxSteering)
-
-    def generatePredictedTrajectories(self, obj, instant):
-        predictedTrajectories = []
-        if self.useFeatures:
-            features = [f for f in obj.features if f.existsAtInstant(instant)]
-            positions = [f.getPositionAtInstant(instant) for f in features]
-            velocities = [f.getVelocityAtInstant(instant) for f in features]
-        else:
-            positions = [obj.getPositionAtInstant(instant)]
-            velocities = [obj.getVelocityAtInstant(instant)]
-        for i in xrange(self.nPredictedTrajectories):
-            for initialPosition,initialVelocity in zip(positions, velocities):
-                predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition, 
-                                                                               initialVelocity, 
-                                                                               moving.NormAngle(self.accelerationDistribution(), 
-                                                                                                self.steeringDistribution()), 
-                                                                               maxSpeed = self.maxSpeed))
-        return predictedTrajectories
-
 class SafetyPoint(moving.Point):
     '''Can represent a collision point or crossing zone 
     with respective safety indicator, TTC or pPET'''
@@ -175,9 +108,10 @@
         for p in points:
             out.write('{0} {1} {2} {3}\n'.format(objNum1, objNum2, predictionInstant, p))
 
-def computeExpectedIndicator(points):
-    from numpy import sum
-    return sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points])
+    @staticmethod
+    def computeExpectedIndicator(points):
+        from numpy import sum
+        return sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points])
 
 def computeCollisionTime(predictedTrajectory1, predictedTrajectory2, collisionDistanceThreshold, timeHorizon):
     '''Computes the first instant at which two predicted trajectories are within some distance threshold'''
@@ -190,19 +124,34 @@
         t += 1
     return t, p1, p2
 
-def computeCrossingsCollisionsAtInstant(currentInstant, obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):
-    '''returns the lists of collision points and crossing zones
-    
-    Check: Predicting all the points together, as if they represent the whole vehicle'''
-    predictedTrajectories1 = predictionParameters.generatePredictedTrajectories(obj1, currentInstant)
-    predictedTrajectories2 = predictionParameters.generatePredictedTrajectories(obj2, currentInstant)
+def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon):
+    from matplotlib.pyplot import figure, axis, title, close, savefig
+    figure()
+    for et in predictedTrajectories1:
+        et.predictPosition(timeHorizon)
+        et.plot('rx')
+
+    for et in predictedTrajectories2:
+        et.predictPosition(timeHorizon)
+        et.plot('bx')
+    obj1.plot('r')
+    obj2.plot('b')
+    title('instant {0}'.format(currentInstant))
+    axis('equal')
+    savefig('predicted-trajectories-t-{0}.png'.format(currentInstant))
+    close()
+
+def computeCrossingsCollisionsAtInstant(predictionParams, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):
+    '''returns the lists of collision points and crossing zones'''
+    predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant)
+    predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant)
 
     collisionPoints = []
     crossingZones = []
     for et1 in predictedTrajectories1:
         for et2 in predictedTrajectories2:
             t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
-            
+
             if t <= timeHorizon:
                 collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t))
             elif computeCZ: # check if there is a crossing zone
@@ -217,29 +166,17 @@
                         #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:
+                        if cz != None:
                             crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2)))
                         t2 += 1
                     t1 += 1                        
 
     if debug:
-        from matplotlib.pyplot import figure, axis, title
-        figure()
-        for et in predictedTrajectories1:
-            et.predictPosition(timeHorizon)
-            et.draw('rx')
+        savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon)
 
-        for et in predictedTrajectories2:
-            et.predictPosition(timeHorizon)
-            et.draw('bx')
-        obj1.draw('r')
-        obj2.draw('b')
-        title('instant {0}'.format(i))
-        axis('equal')
+    return currentInstant, collisionPoints, crossingZones
 
-    return collisionPoints, crossingZones
-    
-def computeCrossingsCollisions(obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None):
+def computeCrossingsCollisions(predictionParams, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1):
     '''Computes all crossing and collision points at each common instant for two road users. '''
     collisionPoints={}
     crossingZones={}
@@ -247,49 +184,253 @@
         commonTimeInterval = timeInterval
     else:
         commonTimeInterval = obj1.commonTimeInterval(obj2)
-    for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
-        collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(i, obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ, debug)
-
-    return collisionPoints, crossingZones
- 
-def computeCollisionProbability(obj1, obj2, predictionParameters, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None):
-    '''Computes only collision probabilities
-    Returns for each instant the collision probability and number of samples drawn'''
-    collisionProbabilities = {}
-    if timeInterval:
-        commonTimeInterval = timeInterval
+    if nProcesses == 1:
+        for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
+            i, cp, cz = computeCrossingsCollisionsAtInstant(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)
+            if len(cp) != 0:
+                collisionPoints[i] = cp
+            if len(cz) != 0:
+                 crossingZones[i] = cz
     else:
-        commonTimeInterval = obj1.commonTimeInterval(obj2)
-    for i in list(commonTimeInterval)[:-1]:
-        nCollisions = 0
-        print(obj1.num, obj2.num, i)
-        predictedTrajectories1 = predictionParameters.generatePredictedTrajectories(obj1, i)
-        predictedTrajectories2 = predictionParameters.generatePredictedTrajectories(obj2, i)
-        for et1 in predictedTrajectories1:
-            for et2 in predictedTrajectories2:
-                t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
-                if t <= timeHorizon:
-                    nCollisions += 1
-        # take into account probabilities ??
-        nSamples = float(len(predictedTrajectories1)*len(predictedTrajectories2))
-        collisionProbabilities[i] = [nSamples, float(nCollisions)/nSamples]
+        from multiprocessing import Pool
+        pool = Pool(processes = nProcesses)
+        jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)) for i in list(commonTimeInterval)[:-1]]
+        #results = [j.get() for j in jobs]
+        #results.sort()
+        for j in jobs:
+            i, cp, cz = j.get()
+            #if len(cp) != 0 or len(cz) != 0:
+            if len(cp) != 0:
+                collisionPoints[i] = cp
+            if len(cz) != 0:
+                crossingZones[i] = cz
+        pool.close()
+    return collisionPoints, crossingZones
+
+class PredictionParameters:
+    def __init__(self, name, maxSpeed):
+        self.name = name
+        self.maxSpeed = maxSpeed
+
+    def __str__(self):
+        return '{0} {1}'.format(self.name, self.maxSpeed)
+
+    def generatePredictedTrajectories(self, obj, instant):
+        return []
+
+    def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):
+        return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)
+
+    def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1):
+        return computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, timeInterval, nProcesses)
+
+    def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None):
+        '''Computes only collision probabilities
+        Returns for each instant the collision probability and number of samples drawn'''
+        collisionProbabilities = {}
+        if timeInterval:
+            commonTimeInterval = timeInterval
+        else:
+            commonTimeInterval = obj1.commonTimeInterval(obj2)
+        for i in list(commonTimeInterval)[:-1]:
+            nCollisions = 0
+            predictedTrajectories1 = self.generatePredictedTrajectories(obj1, i)
+            predictedTrajectories2 = self.generatePredictedTrajectories(obj2, i)
+            for et1 in predictedTrajectories1:
+                for et2 in predictedTrajectories2:
+                    t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
+                    if t <= timeHorizon:
+                        nCollisions += 1
+            # take into account probabilities ??
+            nSamples = float(len(predictedTrajectories1)*len(predictedTrajectories2))
+            collisionProbabilities[i] = [nSamples, float(nCollisions)/nSamples]
+
+            if debug:
+                savePredictedTrajectoriesFigure(i, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon)
+
+        return collisionProbabilities
+
+class ConstantPredictionParameters(PredictionParameters):
+    def __init__(self, maxSpeed):
+        PredictionParameters.__init__(self, 'constant velocity', maxSpeed)
+
+    def generatePredictedTrajectories(self, obj, instant):
+        return [PredictedTrajectoryConstant(obj.getPositionAtInstant(instant), obj.getVelocityAtInstant(instant), 
+                                               maxSpeed = self.maxSpeed)]
+
+class NormalAdaptationPredictionParameters(PredictionParameters):
+    def __init__(self, maxSpeed, nPredictedTrajectories, accelerationDistribution, steeringDistribution, useFeatures = False):
+        '''An example of acceleration and steering distributions is
+        lambda: random.triangular(-self.maxAcceleration, self.maxAcceleration, 0.)
+        '''
+        if useFeatures:
+            name = 'point set normal adaptation'
+        else:
+            name = 'normal adaptation'
+        PredictionParameters.__init__(self, name, maxSpeed)
+        self.nPredictedTrajectories = nPredictedTrajectories
+        self.useFeatures = useFeatures
+        self.accelerationDistribution = accelerationDistribution
+        self.steeringDistribution = steeringDistribution
+        
+    def __str__(self):
+        return PredictionParameters.__str__(self)+' {0} {1} {2}'.format(self.nPredictedTrajectories, 
+                                                                        self.maxAcceleration, 
+                                                                        self.maxSteering)
 
-        if debug:
-            from matplotlib.pyplot import figure, axis, title
+    def generatePredictedTrajectories(self, obj, instant):
+        predictedTrajectories = []
+        if self.useFeatures:
+            features = [f for f in obj.features if f.existsAtInstant(instant)]
+            positions = [f.getPositionAtInstant(instant) for f in features]
+            velocities = [f.getVelocityAtInstant(instant) for f in features]
+        else:
+            positions = [obj.getPositionAtInstant(instant)]
+            velocities = [obj.getVelocityAtInstant(instant)]
+        for i in xrange(self.nPredictedTrajectories):
+            for initialPosition,initialVelocity in zip(positions, velocities):
+                predictedTrajectories.append(PredictedTrajectoryRandomControl(initialPosition, 
+                                                                              initialVelocity, 
+                                                                              self.accelerationDistribution, 
+                                                                              self.steeringDistribution, 
+                                                                              maxSpeed = self.maxSpeed))
+        return predictedTrajectories
+
+class PointSetPredictionParameters(PredictionParameters):
+    # todo generate several trajectories with normal adaptatoins from each position (feature)
+    def __init__(self, maxSpeed):
+        PredictionParameters.__init__(self, 'point set', maxSpeed)
+        #self.nPredictedTrajectories = nPredictedTrajectories
+    
+    def generatePredictedTrajectories(self, obj, instant):
+        predictedTrajectories = []        
+        features = [f for f in obj.features if f.existsAtInstant(instant)]
+        positions = [f.getPositionAtInstant(instant) for f in features]
+        velocities = [f.getVelocityAtInstant(instant) for f in features]
+        #for i in xrange(self.nPredictedTrajectories):
+        for initialPosition,initialVelocity in zip(positions, velocities):
+            predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition, initialVelocity, 
+                                                                     maxSpeed = self.maxSpeed))
+        return predictedTrajectories
+
+class EvasiveActionPredictionParameters(PredictionParameters):
+    def __init__(self, maxSpeed, nPredictedTrajectories, accelerationDistribution, steeringDistribution, useFeatures = False):
+        '''Suggested acceleration distribution may not be symmetric, eg
+        lambda: random.triangular(self.minAcceleration, self.maxAcceleration, 0.)'''
+
+        if useFeatures:
+            name = 'point set evasive action'
+        else:
+            name = 'evasive action'
+        PredictionParameters.__init__(self, name, maxSpeed)
+        self.nPredictedTrajectories = nPredictedTrajectories
+        self.useFeatures = useFeatures
+        self.accelerationDistribution = accelerationDistribution
+        self.steeringDistribution = steeringDistribution
+
+    def __str__(self):
+        return PredictionParameters.__str__(self)+' {0} {1} {2} {3}'.format(self.nPredictedTrajectories, self.minAcceleration, self.maxAcceleration, self.maxSteering)
+
+    def generatePredictedTrajectories(self, obj, instant):
+        predictedTrajectories = []
+        if self.useFeatures:
+            features = [f for f in obj.features if f.existsAtInstant(instant)]
+            positions = [f.getPositionAtInstant(instant) for f in features]
+            velocities = [f.getVelocityAtInstant(instant) for f in features]
+        else:
+            positions = [obj.getPositionAtInstant(instant)]
+            velocities = [obj.getVelocityAtInstant(instant)]
+        for i in xrange(self.nPredictedTrajectories):
+            for initialPosition,initialVelocity in zip(positions, velocities):
+                predictedTrajectories.append(PredictedTrajectoryConstant(initialPosition, 
+                                                                         initialVelocity, 
+                                                                         moving.NormAngle(self.accelerationDistribution(), 
+                                                                                          self.steeringDistribution()), 
+                                                                         maxSpeed = self.maxSpeed))
+        return predictedTrajectories
+
+
+class CVDirectPredictionParameters(PredictionParameters):
+    '''Prediction parameters of prediction at constant velocity
+    using direct computation of the intersecting point'''
+    
+    def __init__(self):
+        PredictionParameters.__init__(self, 'constant velocity (direct computation)', None)
+
+    def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):
+        collisionPoints = []
+        crossingZones = []
+
+        p1 = obj1.getPositionAtInstant(currentInstant)
+        p2 = obj2.getPositionAtInstant(currentInstant)
+        if (p1-p2).norm2() <= collisionDistanceThreshold:
+            collisionPoints = [SafetyPoint((p1+p1).multiply(0.5), 1., 0.)]
+        else:
+            v1 = obj1.getVelocityAtInstant(currentInstant)
+            v2 = obj2.getVelocityAtInstant(currentInstant)
+            intersection = moving.intersection(p1, p2, v1, v2)
+
+            if intersection != None:
+                dp1 = intersection-p1
+                dp2 = intersection-p2
+                if moving.Point.dot(dp1, v1) > 0 and moving.Point.dot(dp2, v2) > 0: # if the road users are moving towards the intersection
+                    dist1 = dp1.norm2()
+                    dist2 = dp2.norm2()
+                    s1 = v1.norm2()
+                    s2 = v2.norm2()
+                    halfCollisionDistanceThreshold = collisionDistanceThreshold/2.
+                    timeInterval1 = moving.TimeInterval(max(0,dist1-halfCollisionDistanceThreshold)/s1, (dist1+halfCollisionDistanceThreshold)/s1)
+                    timeInterval2 = moving.TimeInterval(max(0,dist2-halfCollisionDistanceThreshold)/s2, (dist2+halfCollisionDistanceThreshold)/s2)
+                    collisionTimeInterval = moving.TimeInterval.intersection(timeInterval1, timeInterval2)
+                    if computeCZ and collisionTimeInterval.empty():
+                        crossingZones = [SafetyPoint(intersection, 1., timeInterval1.distance(timeInterval2))]
+                    else:
+                        collisionPoints = [SafetyPoint(intersection, 1., collisionTimeInterval.center())]
+    
+        if debug and intersection!= None:
+            from matplotlib.pyplot import plot, figure, axis, title
             figure()
-            for et in predictedTrajectories1:
-                et.predictPosition(timeHorizon)
-                et.draw('rx')
-                
-            for et in predictedTrajectories2:
-                et.predictPosition(timeHorizon)
-                et.draw('bx')
-            obj1.draw('r')
-            obj2.draw('b')
-            title('instant {0}'.format(i))
+            plot([p1.x, intersection.x], [p1.y, intersection.y], 'r')
+            plot([p2.x, intersection.x], [p2.y, intersection.y], 'b')
+            intersection.plot()            
+            obj1.plot('r')
+            obj2.plot('b')
+            title('instant {0}'.format(currentInstant))
             axis('equal')
 
-    return collisionProbabilities
+        return collisionPoints, crossingZones
+
+class CVExactPredictionParameters(PredictionParameters):
+    '''Prediction parameters of prediction at constant velocity
+    using direct computation of the intersecting point (solving for the equation'''
+    
+    def __init__(self):
+        PredictionParameters.__init__(self, 'constant velocity (direct exact computation)', None)
+
+    def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):
+        'TODO add collision point coordinates, compute pPET'
+        #collisionPoints = []
+        #crossingZones = []
+
+        p1 = obj1.getPositionAtInstant(currentInstant)
+        p2 = obj2.getPositionAtInstant(currentInstant)
+        v1 = obj1.getVelocityAtInstant(currentInstant)
+        v2 = obj2.getVelocityAtInstant(currentInstant)
+        intersection = moving.intersection(p1, p2, v1, v2)
+
+        if intersection != None:
+            ttc = moving.Point.timeToCollision(p1, p2, v1, v2, collisionDistanceThreshold)
+            if ttc:
+                return [SafetyPoint(intersection, 1., ttc)], [] # (p1+v1.multiply(ttc)+p2+v2.multiply(ttc)).multiply(0.5)
+            else:
+                return [],[]
+
+####
+# Other Methods
+####
+
+
+
 
 
 if __name__ == "__main__":