--- a/python/extrapolation.py	Mon Jul 23 12:35:35 2012 -0400
+++ b/python/extrapolation.py	Mon Jul 23 12:35:45 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

--- a/python/moving.py	Mon Jul 23 12:35:35 2012 -0400
+++ b/python/moving.py	Mon Jul 23 12:35:45 2012 -0400
@@ -228,10 +228,43 @@
         from matplotlib.pyplot import scatter
         scatter([p.x for p in points],[p.y for p in points], c=color)
 
+class NormAngle:
+    'alternate encoding of a point, by its norm and orientation'
 
-def predictPosition(nTimeSteps, initialPosition, initialVelocity, initialAcceleration = Point(0,0)):
+    def __init__(self, norm, angle):
+        self.norm = norm
+        self.angle = angle
+    
+    @staticmethod
+    def fromPoint(p):
+        from math import atan2
+        norm = p.norm2()
+        if norm > 0:
+            angle = atan2(p.x, p.y)
+        return NormAngle(norm, angle)
+
+    def __add__(self, other):
+        'a norm cannot become negative'
+        return NormAngle(max(self.norm+other.norm, 0), self.orientation+other.orientation)
+
+    def getPoint(self):
+        from math import cos, sin
+        return Point(self.norm*cos(self.orientation), self.norm*sin(self.orientation))
+
+
+def predictPositionNoLimit(nTimeSteps, initialPosition, initialVelocity, initialAcceleration = Point(0,0)):
     '''Predicts the position in nTimeSteps at constant speed/acceleration'''
-    return initialPosition+initialVelocity.multiply(nTimeSteps) + initialAcceleration.multiply(nTimeSteps**2)
+    return initialVelocity + initialAcceleration.multiply(nTimeSteps),initialPosition+initialVelocity.multiply(nTimeSteps) + initialAcceleration.multiply(nTimeSteps**2*0.5)
+
+def predictPosition(position, speedOrientation, control, maxSpeed = None):
+    '''Predicts the position (moving.Point) at the next time step with given control input (deltaSpeed, deltaTheta)
+    speedOrientation is the other encoding of velocity, (speed, orientation)
+    speedOrientation and control are NormAngle'''
+    predictedSpeedTheta = speedOrientation+control
+    if maxSpeed:
+         predictedSpeedTheta.norm = min(predictedSpeedTheta.norm, maxSpeed)
+    predictedPosition = position+predictedSpeedTheta.getPoint()
+    return predictedPosition, predictedSpeedTheta
 
 
 class FlowVector: