diff trafficintelligence/moving.py @ 1094:c96388c696ac

adding functions for simulation, with contribution from student Lionel Nebot-Janvier, lionel.nebot-janvier@polymtl.ca
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Mon, 04 Feb 2019 10:08:23 -0500
parents 8734742c08c0
children 9a32d63bae3f
line wrap: on
line diff
--- a/trafficintelligence/moving.py	Sat Feb 02 09:33:26 2019 -0500
+++ b/trafficintelligence/moving.py	Mon Feb 04 10:08:23 2019 -0500
@@ -389,7 +389,7 @@
         else:
             return None
 
-    @staticmethod   
+    @staticmethod
     def midPoint(p1, p2):
         'Returns the middle of the segment [p1, p2]'
         return Point(0.5*p1.x+0.5*p2.x, 0.5*p1.y+0.5*p2.y)
@@ -421,11 +421,11 @@
             if(round(p1.x, 10) == round(p2.x, 10)):
                 p2.x += 0.0000000001
             if(round(p1.y, 10) == round(p2.y, 10)):
-                p2.y += 0.0000000001            
+                p2.y += 0.0000000001
             p1 = p2
 
 def ppldb2p(qx,qy, p0x,p0y, p1x,p1y):
-    ''' Point-projection (Q) on line defined by 2 points (P0,P1). 
+    ''' Point-projection (Q) on line defined by 2 points (P0,P1).
         http://cs.nyu.edu/~yap/classes/visual/03s/hw/h2/math.pdf
         '''
     if(p0x == p1x and p0y == p1y):
@@ -435,14 +435,14 @@
         # if(round(p0x, 10) == round(p1x, 10)):
         #     p1x += 0.0000000001
         # if(round(p0y, 10) == round(p1y, 10)):
-        #     p1y += 0.0000000001            
+        #     p1y += 0.0000000001
         #make the calculation
         Y = (-(qx)*(p0y-p1y)-(qy*(p0y-p1y)**2)/(p0x-p1x)+p0x**2*(p0y-p1y)/(p0x-p1x)-p0x*p1x*(p0y-p1y)/(p0x-p1x)-p0y*(p0x-p1x))/(p1x-p0x-(p0y-p1y)**2/(p0x-p1x))
         X = (-Y*(p1y-p0y)+qx*(p1x-p0x)+qy*(p1y-p0y))/(p1x-p0x)
     except ZeroDivisionError:
         print('Error: Division by zero in ppldb2p. Please report this error with the full traceback:')
         print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y))
-        import pdb; pdb.set_trace()  
+        import pdb; pdb.set_trace()
     return Point(X,Y)
 
 def getSYfromXY(p, alignments, goodEnoughAlignmentDistance = 0.5):
@@ -451,10 +451,10 @@
     
     Output:
     =======
-    [alignment index, 
-    subsegment leading point index, 
-    snapped point, 
-    subsegment distance, 
+    [alignment index,
+    subsegment leading point index,
+    snapped point,
+    subsegment distance,
     alignment distance,
     orthogonal point offset]
 
@@ -1049,6 +1049,11 @@
         lanes = [lane]*nPoints
         return CurvilinearTrajectory(S, Y, lanes)
 
+    def append(self,other):
+        '''adds positions of other to the curvilinear trajectory (in-place modification)'''
+        for p in other:
+            self.addPosition(p)
+
     @staticmethod
     def fromTrajectoryProjection(t, alignments, halfWidth = 3):
         ''' Add, for every object position, the class 'moving.CurvilinearTrajectory()'
@@ -1235,6 +1240,26 @@
     def updatePositions(self):
         inter, self.positions, self.velocities = MovingObject.aggregateTrajectories(self.features, self.getTimeInterval())
 
+    def updateCurvilinearPositions(self, method, changeOfAlignment, nextAlignment_idx, timeStep = None, time = None,
+                                   leaderVehicleCurvilinearPositionAtPrecedentTime = None, previousAlignmentId = None,
+                                   maxSpeed = None, acceleration = None, seed = None, delta = None):
+        import math
+
+        if method == 'newell':
+            self.curvilinearPositions.addPositionSYL(leaderVehicleCurvilinearPositionAtPrecedentTime - self.dn, 0, nextAlignment_idx)
+            if changeOfAlignment:
+                self.velocities.addPositionSYL((self.curvilinearPositions[-1][0]-self.curvilinearPositions[-2][0])/timeStep,
+                                                (self.curvilinearPositions[-1][1]-self.curvilinearPositions[-1][1])/timeStep,
+                                                (previousAlignmentId, nextAlignment_idx))
+            else:
+                self.velocities.addPositionSYL((self.curvilinearPositions[-1][0]-self.curvilinearPositions[-2][0])/timeStep,
+                                                (self.curvilinearPositions[-1][1]-self.curvilinearPositions[-1][1])/timeStep,
+                                                None)
+        elif method == 'constantSpeed':
+            random.seed(seed)
+            self.curvilinearPositions.addPositionSYL(self.curvilinearPositions[time-1][0] + self.desiredSpeed * timeStep,
+                                                     0,
+                                                     nextAlignment_idx)
     @staticmethod
     def concatenate(obj1, obj2, num = None, newFeatureNum = None, computePositions = False):
         '''Concatenates two objects, whether overlapping temporally or not