--- a/python/prediction.py	Mon Jul 17 16:11:18 2017 -0400
+++ b/python/prediction.py	Mon Jul 17 17:56:52 2017 -0400
@@ -72,20 +72,30 @@
     to the ratio of the user instantaneous speed and the trajectory closest speed)'''
 
     def __init__(self, initialPosition, initialVelocity, prototypeTrajectory, constantSpeed = False, probability = 1.):
+        ''' prototypeTrajectory is a MovingObject'''
         self.prototypeTrajectory = prototypeTrajectory
         self.constantSpeed = constantSpeed
         self.probability = probability
         self.predictedPositions = {0: initialPosition}
-        self.closestPointIdx = prototypeTrajectory.getClosestPoint(initialPosition)
-        self.deltaPosition = prototypeTrajectory[self.closestPointIdx]-initialPosition
-        self.initialSpeed = initialVelocity.norm()
+        self.closestPointIdx = prototypeTrajectory.getPositions().getClosestPoint(initialPosition)
+        self.deltaPosition = initialPosition-prototypeTrajectory.getPositionAt(self.closestPointIdx)
+        self.initialSpeed = initialVelocity.norm2()
         #self.predictedSpeedOrientations = {0: moving.NormAngle(moving.NormAngle.fromPoint(initialVelocity).norm, findNearestParams(initialPosition,prototypeTrajectory)[1])}#moving.NormAngle.fromPoint(initialVelocity)}
     
     def predictPosition(self, nTimeSteps):
         if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys():
             if self.constantSpeed:
                 # calculate cumulative distance
-                pass
+                traj = self.prototypeTrajectory.getPositions()
+                traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx)
+                i = self.closestPointIdx
+                while i < traj.length() and traj.getCumulativeDistance(i) < traveledDistance:
+                    i += 1
+                if i == traj.length():
+                    v = self.prototypeTrajectory.getVelocityAt(-1)
+                    self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+v*((traveledDistance-traj.getCumulativeDistance(i-1))/v.norm2())
+                else:
+                    self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+(traj[i]-traj[i-1])*((traveledDistance-traj.getCumulativeDistance(i-1))/traj.getDistance(i-1))
             else: # see c++ code, calculate ratio
                 speedNorm= self.predictedSpeedOrientations[0].norm
                 instant=findNearestParams(self.predictedPositions[0],self.prototypeTrajectory)[0]