--- a/python/extrapolation.py	Mon Jul 16 04:57:35 2012 -0400
+++ b/python/extrapolation.py	Tue Jul 17 00:15:42 2012 -0400
@@ -1,166 +1,190 @@
 #! /usr/bin/env python
 '''Library for moving object extrapolation hypotheses'''
 
-import sys
+import moving 
+
+class ExtrapolatedTrajectory:
+    '''Class for extrapolated trajectories with lazy evaluation
+    if the predicted position has not been already computed, compute it
+
+    it should also have a probability'''
+    def predictPosition(self, nTimeSteps):
+        return None
 
-import moving 
+class ExtrapolatedTrajectoryConstant(ExtrapolatedTrajectory):
+    '''Extrapolated trajectory at constant speed or acceleration
+    TODO add limits if acceleration
+    TODO generalize by passing a series of velocities/accelerations'''
+
+    def __init__(self, initialPosition, initialVelocity, initialAccleration = 0, probability = 1):
+        self.initialPosition = initialPosition
+        self.initialVelocity = initialVelocity
+        self.initialAccleration = initialAccleration
+        self.probability = probability
+        self.predictedPositions = {}
+        self.predictedVelocities = {}
+
+    def predictPosition(self, nTimeSteps):
+        if not nTimeSteps in self.predictedPositions.keys():
+            self.predictedPositions[nTimeSteps] = moving.Point.predictPosition(nTimeSteps, self.initialPosition, self.initialVelocity, self.initialAcceleration)
+        return self.predictedPositions[nTimeSteps]
 
 # Default values: to remove because we cannot tweak that from a script where the value may be different
 FPS= 25  # No. of frame per second (FPS) 
-vLimit= 25/FPS	#assume limit speed is 90km/hr = 25 m/sec
-deltaT= FPS*5	# extrapolatation time Horizon = 5 second	
-			
+vLimit= 25/FPS    #assume limit speed is 90km/hr = 25 m/sec
+deltaT= FPS*5    # extrapolatation time Horizon = 5 second    
+            
 def motion (position, velocity, acceleration):
-	''' extrapolation hypothesis: constant acceleration'''
-	from math import atan2,cos,sin
-	vInit= velocity
-	vInitial= velocity.norm2()
-	theta= atan2(velocity.y,velocity.x)
-	vFinal= vInitial+acceleration
-	
-	if acceleration<= 0:
-		v= max(0,vFinal)
-		velocity= moving.Point(v* cos(theta),v* sin(theta))
-		position= position+ (velocity+vInit). multiply(0.5)
-	else:
-		v= min(vLimit,vFinal)
-		velocity= moving.Point(v* cos(theta),v* sin(theta))
-		position= position+ (velocity+vInit). multiply(0.5)
-	return(position,velocity)	
+    ''' extrapolation hypothesis: constant acceleration'''
+    from math import atan2,cos,sin
+    vInit= velocity
+    vInitial= velocity.norm2()
+    theta= atan2(velocity.y,velocity.x)
+    vFinal= vInitial+acceleration
+    
+    if acceleration<= 0:
+        v= max(0,vFinal)
+        velocity= moving.Point(v* cos(theta),v* sin(theta))
+        position= position+ (velocity+vInit). multiply(0.5)
+    else:
+        v= min(vLimit,vFinal)
+        velocity= moving.Point(v* cos(theta),v* sin(theta))
+        position= position+ (velocity+vInit). multiply(0.5)
+    return(position,velocity)    
 
 def motionPET (position, velocity, acceleration, deltaT):
-	''' extrapolation hypothesis: constant acceleration for calculating pPET '''
-	from math import atan2,cos,sin,fabs
-	vInit= velocity
-	vInitial= velocity.norm2()
-	theta= atan2(velocity.y,velocity.x)
-	vFinal= vInitial+acceleration * deltaT
-	if acceleration< 0:
-		if vFinal> 0:	
-			velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta))
-			position= position+ (vInit+ velocity). multiply(0.5*deltaT)
-		else:
-			T= fabs(vInitial/acceleration)
-			position= position + vInit. multiply(0.5*T)
-	elif acceleration> 0 :
-		if vFinal<= vLimit:
-			velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta))
-			position= position+ (vInit+ velocity). multiply(0.5*deltaT)
-		else:
-			time1= fabs((vLimit-vInitial)/acceleration)
-			velocity= moving.Point(vLimit* cos(theta),vLimit* sin(theta))
-			position= (position+ (velocity+vInit). multiply(0.5*time1)) + (velocity.multiply (deltaT-time1))
-	elif acceleration == 0:
-		position= position + velocity. multiply(deltaT)
-	
-	return position
+    ''' extrapolation hypothesis: constant acceleration for calculating pPET '''
+    from math import atan2,cos,sin,fabs
+    vInit= velocity
+    vInitial= velocity.norm2()
+    theta= atan2(velocity.y,velocity.x)
+    vFinal= vInitial+acceleration * deltaT
+    if acceleration< 0:
+        if vFinal> 0:    
+            velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta))
+            position= position+ (vInit+ velocity). multiply(0.5*deltaT)
+        else:
+            T= fabs(vInitial/acceleration)
+            position= position + vInit. multiply(0.5*T)
+    elif acceleration> 0 :
+        if vFinal<= vLimit:
+            velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta))
+            position= position+ (vInit+ velocity). multiply(0.5*deltaT)
+        else:
+            time1= fabs((vLimit-vInitial)/acceleration)
+            velocity= moving.Point(vLimit* cos(theta),vLimit* sin(theta))
+            position= (position+ (velocity+vInit). multiply(0.5*time1)) + (velocity.multiply (deltaT-time1))
+    elif acceleration == 0:
+        position= position + velocity. multiply(deltaT)
+    
+    return position
 
 def timePET (position, velocity, acceleration, intersectedPoint ):
-	''' extrapolation hypothesis: constant acceleration for calculating pPET '''
-	from math import atan2,cos,sin,fabs
-	vInit= velocity
-	vInitial= velocity.norm2()
-	theta= atan2(velocity.y,velocity.x)
-	vFinal= vInitial+acceleration * deltaT
-	if acceleration< 0:
-		if vFinal> 0:	
-			velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta))
-			time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x)))
-		else:
-			time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x)))
-	elif acceleration> 0 :
-		if vFinal<= vLimit:
-			velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta))
-			time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x)))
-		else:
-			time1= fabs((vLimit-vInitial)/acceleration) 
-			velocity= moving.Point(vLimit* cos(theta),vLimit* sin(theta))
-			time2= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x)))
-			if time2<=time1:
-				time= time2
-			else:
-				position2= (position+ (velocity+vInit). multiply(0.5*time1)) 
-				time= time1+fabs((intersectedPoint.x-position2.x)/( velocity.x))
-	elif acceleration == 0:
-		time= fabs((intersectedPoint.x-position.x)/(velocity.x))
-	
-	return time
-	
+    ''' extrapolation hypothesis: constant acceleration for calculating pPET '''
+    from math import atan2,cos,sin,fabs
+    vInit= velocity
+    vInitial= velocity.norm2()
+    theta= atan2(velocity.y,velocity.x)
+    vFinal= vInitial+acceleration * deltaT
+    if acceleration< 0:
+        if vFinal> 0:    
+            velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta))
+            time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x)))
+        else:
+            time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x)))
+    elif acceleration> 0 :
+        if vFinal<= vLimit:
+            velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta))
+            time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x)))
+        else:
+            time1= fabs((vLimit-vInitial)/acceleration) 
+            velocity= moving.Point(vLimit* cos(theta),vLimit* sin(theta))
+            time2= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x)))
+            if time2<=time1:
+                time= time2
+            else:
+                position2= (position+ (velocity+vInit). multiply(0.5*time1)) 
+                time= time1+fabs((intersectedPoint.x-position2.x)/( velocity.x))
+    elif acceleration == 0:
+        time= fabs((intersectedPoint.x-position.x)/(velocity.x))
+    
+    return time
+    
 def motionSteering (position, velocity, deltaTheta, deltaT ):
-	''' extrapolation hypothesis: steering with deltaTheta'''
-	from math import atan2,cos,sin
-	vInitial= velocity.norm2()
-	theta= atan2(velocity.y,velocity.x)
-	newTheta= theta + deltaTheta
-	velocity= moving.Point(vInitial* cos(newTheta),vInitial* sin(newTheta))
-	position= position+ (velocity). multiply(deltaT)
-	return position	
-	
+    ''' extrapolation hypothesis: steering with deltaTheta'''
+    from math import atan2,cos,sin
+    vInitial= velocity.norm2()
+    theta= atan2(velocity.y,velocity.x)
+    newTheta= theta + deltaTheta
+    velocity= moving.Point(vInitial* cos(newTheta),vInitial* sin(newTheta))
+    position= position+ (velocity). multiply(deltaT)
+    return position    
+    
 def MonteCarlo(movingObject1,movingObject2, instant):
-	''' Monte Carlo Simulation :  estimate the probability of collision'''
-	from random import uniform
-	from math import pow, sqrt, sin, cos,atan2		
-	N=1000
-	ProbOfCollision = 0
-	for n in range (1, N):
-		# acceleration limit	
-		acc1 = uniform(-0.040444,0)
-		acc2 = uniform(-0.040444,0)	
-		p1= movingObject1.getPositionAtInstant(instant)
-		p2= movingObject2.getPositionAtInstant(instant)
-		v1= movingObject1.getVelocityAtInstant(instant)
-		v2= movingObject2.getVelocityAtInstant(instant)
-		distance= (p1-p2).norm2()
-		distanceThreshold= 1.8	
-		t=1				
-		while distance > distanceThreshold and t <= deltaT:
-			# Extrapolation position
-			(p1,v1) = motion(p1,v1,acc1)
-			(p2,v2) = motion(p2,v2,acc2)
-			distance= (p1-p2).norm2()
-			if distance <=distanceThreshold:
-				ProbOfCollision= ProbOfCollision+1
-			t+=1
-	POC= float(ProbOfCollision)/N
-	return POC
-	
+    ''' Monte Carlo Simulation :  estimate the probability of collision'''
+    from random import uniform
+    from math import pow, sqrt, sin, cos,atan2        
+    N=1000
+    ProbOfCollision = 0
+    for n in range (1, N):
+        # acceleration limit    
+        acc1 = uniform(-0.040444,0)
+        acc2 = uniform(-0.040444,0)    
+        p1= movingObject1.getPositionAtInstant(instant)
+        p2= movingObject2.getPositionAtInstant(instant)
+        v1= movingObject1.getVelocityAtInstant(instant)
+        v2= movingObject2.getVelocityAtInstant(instant)
+        distance= (p1-p2).norm2()
+        distanceThreshold= 1.8    
+        t=1                
+        while distance > distanceThreshold and t <= deltaT:
+            # Extrapolation position
+            (p1,v1) = motion(p1,v1,acc1)
+            (p2,v2) = motion(p2,v2,acc2)
+            distance= (p1-p2).norm2()
+            if distance <=distanceThreshold:
+                ProbOfCollision= ProbOfCollision+1
+            t+=1
+    POC= float(ProbOfCollision)/N
+    return POC
+    
 def velocitySteering(velocity,steering):
-	from math import atan2,cos,sin
-	vInitial= velocity.norm2()
-	theta= atan2(velocity.y,velocity.x)
-	newTheta= theta + steering
-	v= moving.Point(vInitial* cos(newTheta),vInitial* sin(newTheta))
-	return v
+    from math import atan2,cos,sin
+    vInitial= velocity.norm2()
+    theta= atan2(velocity.y,velocity.x)
+    newTheta= theta + steering
+    v= moving.Point(vInitial* cos(newTheta),vInitial* sin(newTheta))
+    return v
 
 def MonteCarloSteering(movingObject1,movingObject2, instant,steering1,steering2):
-	''' Monte Carlo Simulation :  estimate the probability of collision in case of steering'''
-	from random import uniform
-	from math import pow, sqrt, sin, cos,atan2		
-	N=1000
-	L= 2.4
-	ProbOfCollision = 0
-	for n in range (1, N):
-		# acceleration limit	
-		acc1 = uniform(-0.040444,0)
-		acc2 = uniform(-0.040444,0)	
-		p1= movingObject1.getPositionAtInstant(instant)
-		p2= movingObject2.getPositionAtInstant(instant)
-		vInit1= movingObject1.getVelocityAtInstant(instant)
-		v1= velocitySteering (vInit1,steering1)
-		vInit2= movingObject2.getVelocityAtInstant(instant)
-		v2= velocitySteering (vInit2,steering2)
-		distance= (p1-p2).norm2()
-		distanceThreshold= 1.8	
-		t=1				
-		while distance > distanceThreshold and t <= deltaT:
-			# Extrapolation position
-			(p1,v1) = motion(p1,v1,acc1)
-			(p2,v2) = motion(p2,v2,acc2)
-			distance= (p1-p2).norm2()
-			if distance <=distanceThreshold:
-				ProbOfCollision= ProbOfCollision+1
-			t+=1
-	POC= float(ProbOfCollision)/N
-	return POC
+    ''' Monte Carlo Simulation :  estimate the probability of collision in case of steering'''
+    from random import uniform
+    from math import pow, sqrt, sin, cos,atan2        
+    N=1000
+    L= 2.4
+    ProbOfCollision = 0
+    for n in range (1, N):
+        # acceleration limit    
+        acc1 = uniform(-0.040444,0)
+        acc2 = uniform(-0.040444,0)    
+        p1= movingObject1.getPositionAtInstant(instant)
+        p2= movingObject2.getPositionAtInstant(instant)
+        vInit1= movingObject1.getVelocityAtInstant(instant)
+        v1= velocitySteering (vInit1,steering1)
+        vInit2= movingObject2.getVelocityAtInstant(instant)
+        v2= velocitySteering (vInit2,steering2)
+        distance= (p1-p2).norm2()
+        distanceThreshold= 1.8    
+        t=1                
+        while distance > distanceThreshold and t <= deltaT:
+            # Extrapolation position
+            (p1,v1) = motion(p1,v1,acc1)
+            (p2,v2) = motion(p2,v2,acc2)
+            distance= (p1-p2).norm2()
+            if distance <=distanceThreshold:
+                ProbOfCollision= ProbOfCollision+1
+            t+=1
+    POC= float(ProbOfCollision)/N
+    return POC