--- a/python/extrapolation.py	Sat Jul 21 00:50:42 2012 -0400
+++ b/python/extrapolation.py	Mon Jul 23 02:50:26 2012 -0400
@@ -1,7 +1,8 @@
 #! /usr/bin/env python
 '''Library for moving object extrapolation hypotheses'''
 
-import moving 
+import moving
+import math
 
 class ExtrapolatedTrajectory:
     '''Class for extrapolated trajectories with lazy evaluation
@@ -16,19 +17,34 @@
     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
+    def __init__(self, initialPosition, initialVelocity, control = moving.NormAngle(0,0), probability = 1, maxSpeed = None):
+        self.control = control
+        self.maxSpeed = maxSpeed
         self.probability = probability
-        self.predictedPositions = {}
-        self.predictedVelocities = {}
+        self.predictedPositions = {0: initialPosition}
+        self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)}
 
     def predictPosition(self, nTimeSteps):
-        if not nTimeSteps in self.predictedPositions.keys():
-            self.predictedPositions[nTimeSteps] = moving.predictPosition(nTimeSteps, self.initialPosition, self.initialVelocity, self.initialAcceleration)
+        if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys():
+            self.predictedPositions[nTimeSteps], self.predictedspeedOrientations[nTimeSteps] = predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedspeedOrientations[nTimeSteps-1], self.control, maxSpeed)
         return self.predictedPositions[nTimeSteps]
 
+class ExtrapolatedTrajectoryAdaptation(ExtrapolatedTrajectoryConstant):
+    '''Random small adaptation of vehicle control '''
+    def __init__(self, initialPosition, initialVelocity, accelerationBounds, steeringBounds, probability = 1, maxSpeed = None):
+        self.accelerationBounds = accelerationBounds
+        self.steeringBounds = steeringBounds
+        self.maxSpeed = maxSpeed
+        self.probability = probability
+        self.predictedPositions = {0: initialPosition}
+        self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)}
+
+    def predictPosition(self, nTimeSteps):
+        # draw acceleration and steering
+        # TODO should the predictPosition be in the base class ??
+        self.control = moving.NormAngle(0,0)
+        return ExtrapolatedTrajectoryConstant.predictPosition(self, 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