diff python/prediction.py @ 937:b67a784beb69

work started on prototype prediction
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Mon, 17 Jul 2017 01:38:06 -0400
parents 063d1267585d
children a2f3f3ca241e
line wrap: on
line diff
--- a/python/prediction.py	Fri Jul 14 16:48:42 2017 -0400
+++ b/python/prediction.py	Mon Jul 17 01:38:06 2017 -0400
@@ -76,17 +76,16 @@
         self.constantSpeed = constantSpeed
         self.probability = probability
         self.predictedPositions = {0: initialPosition}
-        self.predictedSpeedOrientations = {0: moving.NormAngle(moving.NormAngle.fromPoint(initialVelocity).norm, findNearestParams(initialPosition,prototypeTrajectory)[1])}#moving.NormAngle.fromPoint(initialVelocity)}
+        self.closestPointIdx = prototypeTrajectory.getClosestPoint(initialPosition)
+        self.deltaPosition = prototypeTrajectory[self.closestPointIdx]-initialPosition
+        self.initialSpeed = initialVelocity.norm()
+        #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
-                speedNorm= self.predictedSpeedOrientations[0].norm #moving.NormAngle.fromPoint(initialVelocity).norm
-                anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototypeTrajectory)[1]
-                self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(speedNorm, anglePrototype)
-                self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None)
-            
+                pass
             else: # see c++ code, calculate ratio
                 speedNorm= self.predictedSpeedOrientations[0].norm
                 instant=findNearestParams(self.predictedPositions[0],self.prototypeTrajectory)[0]