--- a/python/extrapolation.py	Wed Jul 25 22:06:51 2012 -0400
+++ b/python/extrapolation.py	Thu Jul 26 03:54:41 2012 -0400
@@ -62,24 +62,33 @@
     def getControl(self):
         return moving.NormAngle(self.accelerationDistribution(),self.steeringDistribution())
 
-
 class ExtrapolationParameters:
     def __init__(self, name):
         self.name = name
 
-def createExtrapolatedTrajectories(extrapolationParameters, initialPosition, initialVelocity):
+def createExtrapolatedTrajectories(extrapolationParameters, obj, instant):
     '''extrapolationParameters specific to each method (in name field)  '''
     if extrapolationParameters.name == 'constant velocity':
-        return [ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, maxSpeed = extrapolationParameters.maxSpeed)]
+        return [ExtrapolatedTrajectoryConstant(obj.getPositionAtInstant(instant), obj.getVelocityAtInstant(instant), 
+                                               maxSpeed = extrapolationParameters.maxSpeed)]
     elif extrapolationParameters.name == 'normal adaptation':
-        # generate several and with the right distribution
         extrapolatedTrajectories = []
         for i in xrange(extrapolationParameters.nExtrapolatedTrajectories):
-            extrapolatedTrajectories.append(ExtrapolatedTrajectoryNormalAdaptation(initialPosition, initialVelocity, 
+            extrapolatedTrajectories.append(ExtrapolatedTrajectoryNormalAdaptation(obj.getPositionAtInstant(instant), 
+                                                                                   obj.getVelocityAtInstant(instant), 
                                                                                    extrapolationParameters.accelerationDistribution, 
                                                                                    extrapolationParameters.steeringDistribution, 
                                                                                    maxSpeed = extrapolationParameters.maxSpeed))
         return extrapolatedTrajectories
+    elif extrapolationParameters.name == 'pointset':
+        extrapolatedTrajectories = []
+        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):
+            extrapolatedTrajectories.append(ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, 
+                                                                           maxSpeed = extrapolationParameters.maxSpeed))
+        return extrapolatedTrajectories
     else:
         print('Unknown extrapolation hypothesis')
         return []
@@ -135,15 +144,19 @@
                     t1 += 1                        
     return collisionPoints, crossingZones
     
-def computeCrossingsCollisions(obj1, obj2, extrapolationParameters, collisionDistanceThreshold, timeHorizon, outCP, outCZ, debug = False):
+def computeCrossingsCollisions(obj1, obj2, extrapolationParameters, collisionDistanceThreshold, timeHorizon, outCP, outCZ, debug = False, timeInterval = None):
     collisionPoints={}
     crossingZones={}
     #TTCs = {}
     #pPETs = {}
-    commonTimeInterval = obj1.commonTimeInterval(obj2)
+    if timeInterval:
+        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
-        extrapolatedTrajectories1 = createExtrapolatedTrajectories(extrapolationParameters, obj1.getPositionAtInstant(i), obj1.getVelocityAtInstant(i))
-        extrapolatedTrajectories2 = createExtrapolatedTrajectories(extrapolationParameters, obj2.getPositionAtInstant(i), obj2.getVelocityAtInstant(i))
+        print(obj1.num, obj2.num, i)
+        extrapolatedTrajectories1 = createExtrapolatedTrajectories(extrapolationParameters, obj1, i)
+        extrapolatedTrajectories2 = createExtrapolatedTrajectories(extrapolationParameters, obj2, i)
 
         collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon)
         # if len(collisionPoints[i]) > 0: