--- a/python/prediction.py	Thu Jul 11 00:17:25 2013 -0400
+++ b/python/prediction.py	Thu Jul 11 02:17:12 2013 -0400
@@ -63,6 +63,28 @@
     def getControl(self):
         return moving.NormAngle(self.accelerationDistribution(),self.steeringDistribution())
 
+class SafetyPoint(moving.Point):
+    '''Can represent a collision point or crossing zone 
+    with respective safety indicator, TTC or pPET'''
+    def __init__(self, p, probability = 1., indicator = -1):
+        self.x = p.x
+        self.y = p.y
+        self.probability = probability
+        self.indicator = indicator
+
+    def __str__(self):
+        return '{0} {1} {2} {3}'.format(self.x, self.y, self.probability, self.indicator)
+
+    @staticmethod
+    def save(out, points, predictionInstant, objNum1, objNum2):
+        for p in points:
+            out.write('{0} {1} {2} {3}\n'.format(objNum1, objNum2, predictionInstant, p))
+
+    @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'''
     t = 1
@@ -86,9 +108,7 @@
         return []
 
     def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, 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'''
+        '''returns the lists of collision points and crossing zones'''
         predictedTrajectories1 = self.generatePredictedTrajectories(obj1, currentInstant)
         predictedTrajectories2 = self.generatePredictedTrajectories(obj2, currentInstant)
 
@@ -288,27 +308,58 @@
                                                                          maxSpeed = self.maxSpeed))
         return predictedTrajectories
 
-class SafetyPoint(moving.Point):
-    '''Can represent a collision point or crossing zone 
-    with respective safety indicator, TTC or pPET'''
-    def __init__(self, p, probability = 1., indicator = -1):
-        self.x = p.x
-        self.y = p.y
-        self.probability = probability
-        self.indicator = indicator
+
+class CVDirectPredictionParameters(PredictionParameters):
+    '''Prediction parameters of prediction at constant velocity
+    using direct computation of the intersecting point'''
+    
+    def __init__(self, maxSpeed):
+        PredictionParameters.__init__(self, 'constant velocity (direct computation)', maxSpeed)
+
+    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)
 
-    def __str__(self):
-        return '{0} {1} {2} {3}'.format(self.x, self.y, self.probability, self.indicator)
+            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 not collisionTimeInterval.empty():
+                        collisionPoints = [SafetyPoint(intersection, 1., collisionTimeInterval.center())]
+                    else:
+                        crossingZones = [SafetyPoint(intersection, 1., timeInterval1.distance(timeInterval2))]
+    
+        if debug and intersection!= None:
+            from matplotlib.pyplot import plot, figure, axis, title
+            figure()
+            plot([p1.x, intersection.x], [p1.y, intersection.y], 'r')
+            plot([p2.x, intersection.x], [p2.y, intersection.y], 'b')
+            intersection.draw()            
+            obj1.draw('r')
+            obj2.draw('b')
+            title('instant {0}'.format(currentInstant))
+            axis('equal')
 
-    @staticmethod
-    def save(out, points, predictionInstant, objNum1, objNum2):
-        for p in points:
-            out.write('{0} {1} {2} {3}\n'.format(objNum1, objNum2, predictionInstant, p))
+        return collisionPoints, crossingZones
 
-    @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])
+
 
 
 ####