# HG changeset patch
# User Nicolas Saunier <nicolas.saunier@polymtl.ca>
# Date 1500415276 14400
# Node ID ab13aaf41432b6a33f477918ef99db5dc8753106
# Parent  c5191acb025fe8c693ed83cc67d949c079283648
implemented motion prediction with prototypes at constant ratio, with tests

diff -r c5191acb025f -r ab13aaf41432 python/prediction.py
--- a/python/prediction.py	Tue Jul 18 16:21:39 2017 -0400
+++ b/python/prediction.py	Tue Jul 18 18:01:16 2017 -0400
@@ -50,14 +50,6 @@
 
     def getControl(self):
         return self.control
-        
-def findNearestParams(initialPosition, prototypeTrajectory):
-    ''' nearest parameters are the index of minDistance and the orientation  '''
-    distances=[]
-    for position in prototypeTrajectory.positions:
-        distances.append(moving.Point.distanceNorm2(initialPosition, position))
-    minDistanceIndex= np.argmin(distances)
-    return minDistanceIndex, moving.NormAngle.fromPoint(prototypeTrajectory.velocities[minDistanceIndex]).angle
 
 class PredictedTrajectoryPrototype(PredictedTrajectory):
     '''Predicted trajectory that follows a prototype trajectory
@@ -84,36 +76,34 @@
         self.closestPointIdx = prototype.getPositions().getClosestPoint(initialPosition)
         self.deltaPosition = initialPosition-prototype.getPositionAt(self.closestPointIdx)
         self.initialSpeed = initialVelocity.norm2()
-        #self.predictedSpeedOrientations = {0: moving.NormAngle(moving.NormAngle.fromPoint(initialVelocity).norm, findNearestParams(initialPosition,prototype)[1])}#moving.NormAngle.fromPoint(initialVelocity)}
+        if not constantSpeed:
+            self.ratio = self.initialSpeed/prototype.getVelocityAt(self.closestPointIdx).norm2()
     
     def predictPosition(self, nTimeSteps):
         if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys():
             if self.constantSpeed:
                 # calculate cumulative distance
                 traj = self.prototype.getPositions()
+                trajLength = traj.length()
                 traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx)
                 i = self.closestPointIdx
-                while i < traj.length() and traj.getCumulativeDistance(i) < traveledDistance:
+                while i < trajLength and traj.getCumulativeDistance(i) < traveledDistance:
                     i += 1
-                if i == traj.length():
+                if i == trajLength:
                     v = self.prototype.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.prototype)[0]
-                prototypeSpeeds= self.prototype.getSpeeds()[instant:]
-                ratio=float(speedNorm)/prototypeSpeeds[0]
-                resampledSpeeds=[sp*ratio for sp in prototypeSpeeds]
-                anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototype)[1]
-                if nTimeSteps<len(resampledSpeeds):
-                    self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[nTimeSteps], anglePrototype)
-                    self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None)                
+            else:
+                traj = self.prototype.getPositions()
+                trajLength = traj.length()
+                nSteps = self.ratio*nTimeSteps+self.closestPointIdx
+                i = int(np.floor(nSteps))
+                if nSteps < trajLength-1:
+                    self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i]+(traj[i+1]-traj[i])*(nSteps-i)
                 else:
-                    self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[-1], anglePrototype)
-                    self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None)
-          
+                    v = self.prototype.getVelocityAt(-1)
+                    self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[-1]+v*(nSteps-trajLength+1)
         return self.predictedPositions[nTimeSteps]
 
 class PredictedTrajectoryRandomControl(PredictedTrajectory):
diff -r c5191acb025f -r ab13aaf41432 python/tests/prediction.txt
--- a/python/tests/prediction.txt	Tue Jul 18 16:21:39 2017 -0400
+++ b/python/tests/prediction.txt	Tue Jul 18 18:01:16 2017 -0400
@@ -1,5 +1,5 @@
 >>> from prediction import *
->>> import moving, storage
+>>> import moving, storage, utils
 >>> from numpy import absolute, array
 
 >>> et = PredictedTrajectoryConstant(moving.Point(0,0), moving.Point(1,0))
@@ -57,3 +57,15 @@
 >>> (absolute(array(traj.distances) - et.initialSpeed) < 1e-5).all()
 True
 
+>>> et = PredictedTrajectoryPrototype(proto.getPositionAt(10)+moving.Point(0.6, 0.6), proto.getVelocityAt(10)*0.7, proto, False)
+>>> absolute(et.initialSpeed - proto.getVelocityAt(10).norm2()*0.7) < 1e-5
+True
+>>> proto = moving.MovingObject.generate(1, moving.Point(-5.,0.), moving.Point(1.,0.), moving.TimeInterval(0,10))
+>>> et = PredictedTrajectoryPrototype(proto.getPositionAt(0)+moving.Point(0., 1.), proto.getVelocityAt(0)*0.5, proto, False)
+>>> for t in xrange(int(proto.length()/0.5)): x=et.predictPosition(t)
+>>> et.predictPosition(10) # doctest:+ELLIPSIS
+(0.0...,1.0...)
+>>> et.predictPosition(12) # doctest:+ELLIPSIS
+(1.0...,1.0...)
+
+