# HG changeset patch
# User Nicolas Saunier <nicolas.saunier@polymtl.ca>
# Date 1500399977 14400
# Node ID d8ab183a7351dc2579c1e4a904894e25f9fba48d
# Parent  a2f3f3ca241ef2e8955b8d27219e2b9293237633
verified motion prediction with prototypes at constant speed (test needed)

diff -r a2f3f3ca241e -r d8ab183a7351 python/prediction.py
--- a/python/prediction.py	Mon Jul 17 17:56:52 2017 -0400
+++ b/python/prediction.py	Tue Jul 18 13:46:17 2017 -0400
@@ -71,38 +71,42 @@
     (applying a constant ratio equal 
     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
+    def __init__(self, initialPosition, initialVelocity, prototype, constantSpeed = False, probability = 1.):
+        ''' prototype is a MovingObject
+
+        Prediction at constant speed will not work for unrealistic trajectories 
+        that do not follow a slowly changing velocity (eg moving object trajectories, 
+        but is good for realistic motion (eg features)'''
+        self.prototype = prototype
         self.constantSpeed = constantSpeed
         self.probability = probability
         self.predictedPositions = {0: initialPosition}
-        self.closestPointIdx = prototypeTrajectory.getPositions().getClosestPoint(initialPosition)
-        self.deltaPosition = initialPosition-prototypeTrajectory.getPositionAt(self.closestPointIdx)
+        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,prototypeTrajectory)[1])}#moving.NormAngle.fromPoint(initialVelocity)}
+        #self.predictedSpeedOrientations = {0: moving.NormAngle(moving.NormAngle.fromPoint(initialVelocity).norm, findNearestParams(initialPosition,prototype)[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
-                traj = self.prototypeTrajectory.getPositions()
+                traj = self.prototype.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)
+                    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.prototypeTrajectory)[0]
-                prototypeSpeeds= self.prototypeTrajectory.getSpeeds()[instant:]
+                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.prototypeTrajectory)[1]
+                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)                
@@ -279,7 +283,7 @@
             collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
 
             if collision:
-                collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t))
+                collisionPoints.append(SafetyPoint((p1+p2)*0.5, et1.probability*et2.probability, t))
             elif computeCZ: # check if there is a crossing zone
                 # TODO? zone should be around the points at which the traj are the closest
                 # look for CZ at different times, otherwise it would be a collision
@@ -516,7 +520,7 @@
         p1 = obj1.getPositionAtInstant(currentInstant)
         p2 = obj2.getPositionAtInstant(currentInstant)
         if (p1-p2).norm2() <= collisionDistanceThreshold:
-            collisionPoints = [SafetyPoint((p1+p1).multiply(0.5), 1., 0.)]
+            collisionPoints = [SafetyPoint((p1+p1)*0.5, 1., 0.)]
         else:
             v1 = obj1.getVelocityAtInstant(currentInstant)
             v2 = obj2.getVelocityAtInstant(currentInstant)
diff -r a2f3f3ca241e -r d8ab183a7351 python/tests/moving.txt
--- a/python/tests/moving.txt	Mon Jul 17 17:56:52 2017 -0400
+++ b/python/tests/moving.txt	Tue Jul 18 13:46:17 2017 -0400
@@ -55,7 +55,7 @@
 (2.000000,-3.000000)
 >>> -Point(1,2)
 (-1.000000,-2.000000)
->>> Point(1,2).multiply(0.5)
+>>> Point(1,2)*0.5
 (0.500000,1.000000)
 
 >>> Point(3,2).norm2Squared()
diff -r a2f3f3ca241e -r d8ab183a7351 python/utils.py
--- a/python/utils.py	Mon Jul 17 17:56:52 2017 -0400
+++ b/python/utils.py	Tue Jul 18 13:46:17 2017 -0400
@@ -836,14 +836,11 @@
 # plotting section
 #########################
 
-def plotPolygon(poly, options = ''):
+def plotPolygon(poly, options = '', **kwargs):
     'Plots shapely polygon poly'
-    from numpy.core.multiarray import array
     from matplotlib.pyplot import plot
-    from shapely.geometry import Polygon
-
-    tmp = array(poly.exterior)
-    plot(tmp[:,0], tmp[:,1], options)
+    x,y = poly.exterior.xy
+    plot(x, y, options, **kwargs)
 
 def stepPlot(X, firstX, lastX, initialCount = 0, increment = 1):
     '''for each value in X, increment by increment the initial count