# HG changeset patch
# User Nicolas Saunier <nicolas.saunier@polymtl.ca>
# Date 1557173788 14400
# Node ID 6bbcd94337326ab894a43c94a7648f3198d1eeda
# Parent  6e8ab471ebd437838e3a607ad8afd84bc15f34ec
formatting and addition of one method to CurvilinearTrajectory

diff -r 6e8ab471ebd4 -r 6bbcd9433732 trafficintelligence/moving.py
--- a/trafficintelligence/moving.py	Tue Apr 16 11:58:23 2019 -0400
+++ b/trafficintelligence/moving.py	Mon May 06 16:16:28 2019 -0400
@@ -273,7 +273,7 @@
 
     def angle(self):
         return atan2(self.y, self.x)
-        
+
     def norm2Squared(self):
         '''2-norm distance (Euclidean distance)'''
         return self.x**2+self.y**2
@@ -284,7 +284,7 @@
 
     def norm1(self):
         return abs(self.x)+abs(self.y)
-    
+
     def normMax(self):
         return max(abs(self.x),abs(self.y))
 
@@ -346,7 +346,7 @@
     @staticmethod
     def parallel(p1, p2):
         return Point.cross(p1, p2) == 0.
-    
+
     @staticmethod
     def cosine(p1, p2):
         return Point.dot(p1,p2)/(p1.norm2()*p2.norm2())
@@ -465,8 +465,9 @@
     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()
-    return Point(X,Y)
+        import pdb;
+        pdb.set_trace()
+    return Point(X, Y)
 
 def getSYfromXY(p, alignments, goodEnoughAlignmentDistance = 0.5):
     ''' Snap a point p to its nearest subsegment of it's nearest alignment (from the list alignments).
@@ -541,14 +542,14 @@
         print('Curvilinear point {} is past the end of the alignement'.format((s, y, alignmentNum)))
         return None
 
-    
+
 class NormAngle(object):
     '''Alternate encoding of a point, by its norm and orientation'''
 
     def __init__(self, norm, angle):
         self.norm = norm
         self.angle = angle
-    
+
     @staticmethod
     def fromPoint(p):
         norm = p.norm2()
@@ -598,7 +599,7 @@
     def plot(self, options = '', **kwargs):
         plot([self.position.x, self.position.x+self.velocity.x], [self.position.y, self.position.y+self.velocity.y], options, **kwargs)
         self.position.plot(options+'x', **kwargs)
-    
+
     @staticmethod
     def similar(f1, f2, maxDistance2, maxDeltavelocity2):
         return (f1.position-f2.position).norm2Squared()<maxDistance2 and (f1.velocity-f2.velocity).norm2Squared()<maxDeltavelocity2
@@ -625,7 +626,7 @@
 #                 [dp2.y, -dp2.x]])
 #     B = matrix([[dp1.y*p1.x-dp1.x*p1.y],
 #                 [dp2.y*p2.x-dp2.x*p2.y]])
-    
+
 #     if linalg.det(A) == 0:
 #         return None
 #     else:
@@ -655,7 +656,7 @@
         return inter
     else:
         return None
-        
+
 
 class Trajectory(object):
     '''Class for trajectories: temporal sequence of positions
@@ -792,15 +793,15 @@
 
     def asArray(self):
         return array(self.positions)
-    
+
     def xBounds(self):
         # look for function that does min and max in one pass
         return Interval(min(self.getXCoordinates()), max(self.getXCoordinates()))
-    
+
     def yBounds(self):
         # look for function that does min and max in one pass
         return Interval(min(self.getYCoordinates()), max(self.getYCoordinates()))
-    
+
     def add(self, traj2):
         '''Returns a new trajectory of the same length'''
         if self.length() != traj2.length():
@@ -951,7 +952,7 @@
         Returns an empty list if there is no crossing'''
         indices = []
         intersections = []
-        
+
         for i in range(self.length()-1):
             q1=self.__getitem__(i)
             q2=self.__getitem__(i+1)
@@ -1058,7 +1059,7 @@
             self.lanes = [None]*int(self.length())
         else:
             self.lanes = lanes
-        
+
     @staticmethod
     def generate(s, v, nPoints, lane, y = 0):
         '''s is initial position, v is velocity
@@ -1134,7 +1135,7 @@
 
     def getSCoordinates(self):
         return self.getXCoordinates()
-    
+
     def getLanes(self):
         return self.lanes
 
@@ -1164,6 +1165,10 @@
         'Adds position in the point format for curvilinear of list with 3 values'
         self.addPositionSYL(p[0], p[1], p[2])
 
+    def duplicateLastPosition(self):
+        super(CurvilinearTrajectory, self).duplicateLastPosition()
+        self.lanes.append(self.lanes[-1])
+
     def setPosition(self, i, s, y, lane):
         self.setPositionXY(i, s, y)
         if i < self.__len__():
@@ -1215,7 +1220,7 @@
     def predict(self, hog):
         return userType2Num['car']
 carClassifier = CarClassifier()
-    
+
 class MovingObject(STObject, VideoFilenameAddable):
     '''Class for moving objects: a spatio-temporal object
     with a trajectory and a geometry (constant volume over time)
@@ -1235,7 +1240,7 @@
         self.setNObjects(nObjects) # a feature has None for nObjects
         self.features = None
         # compute bounding polygon from trajectory
-        
+
     @staticmethod
     def croppedTimeInterval(obj, value, after = True):
         newTimeInterval = TimeInterval(obj.getFirstInstant(), min(value, obj.getLastInstant())) if after else TimeInterval(max(obj.getFirstInstant(), value), obj.getLastInstant())
@@ -1519,7 +1524,7 @@
                 print('Object {} does not exist at {}'.format(self.getNum(), t))
         else:
             print('Object {} has no curvilinear positions'.format(self.getNum()))
-            
+
     def setUserType(self, userType):
         self.userType = userType
 
@@ -1532,7 +1537,7 @@
         else:
             print('Number of objects represented by object {} must be greater or equal to 1 ({})'.format(self.getNum(), nObjects))
             self.nObjects = None
-        
+
     def setFeatures(self, features, featuresOrdered = False):
         '''Sets the features in the features field based on featureNumbers
         if not all features are loaded from 0, one needs to renumber in a dict'''
@@ -1591,7 +1596,7 @@
         if speeds is None:
             speeds = self.getSpeeds(nInstantsIgnoredAtEnds)
         return savgol_filter(speeds, window_length, polyorder, 1, delta, axis, mode, cval)
-        
+
     def getSpeedIndicator(self):
         from indicators import SeverityIndicator
         return SeverityIndicator('Speed', {t:self.getVelocityAtInstant(t).norm2() for t in self.getTimeInterval()})
@@ -1622,10 +1627,10 @@
 
     def getXCoordinates(self):
         return self.positions.getXCoordinates()
-    
+
     def getYCoordinates(self):
         return self.positions.getYCoordinates()
-    
+
     def plot(self, options = '', withOrigin = False, timeStep = 1, withFeatures = False, withIds = False, **kwargs):
         if withIds:
             objNum = self.getNum()
@@ -1690,7 +1695,7 @@
         positions1 = [f.getPositionAtInstant(instant1).astuple() for f in obj1.features if f.existsAtInstant(instant1)]
         positions2 = [f.getPositionAtInstant(instant2).astuple() for f in obj2.features if f.existsAtInstant(instant2)]
         return cdist(positions1, positions2, metric = 'euclidean')
-        
+
     @staticmethod
     def minDistance(obj1, obj2, instant1, instant2 = None):
         return MovingObject.distances(obj1, obj2, instant1, instant2).min()
@@ -1714,11 +1719,11 @@
         else:
             print('Load features to compute a maximum size')
             return None
-    
+
     def setRoutes(self, startRouteID, endRouteID):
         self.startRouteID = startRouteID
         self.endRouteID = endRouteID
-           
+
     def getInstantsCrossingLane(self, p1, p2):
         '''Returns the instant(s)
         at which the object passes from one side of the segment to the other
@@ -1734,7 +1739,7 @@
                 lcss.similarities(proto.getMovingObject().getPositions().asArray().T, self.getPositions().asArray().T)
                 similarities = lcss.similarityTable[-1, :-1].astype(float)
                 self.prototypeSimilarities.append(similarities/minimum(arange(1., len(similarities)+1), proto.getMovingObject().length()*ones(len(similarities))))
-    
+
     @staticmethod
     def computePET(obj1, obj2, collisionDistanceThreshold):
         '''Post-encroachment time based on distance threshold
@@ -1811,7 +1816,7 @@
         '''Classifies slow and fast road users
         slow: non-motorized -> pedestrians
         fast: motorized -> cars
-        
+
         aggregationFunc can be any function that can be applied to a vector of speeds, including percentile:
         aggregationFunc = lambda x: percentile(x, percentileFactor) # where percentileFactor is 85 for 85th percentile'''
         speeds = self.getSpeeds(nInstantsIgnoredAtEnds)
@@ -1970,7 +1975,7 @@
         return self.filename == p2.filename and self.num == p2.num and self.trajectoryType == p2.trajectoryType
     def __hash__(self):
         return hash((self.filename, self.num, self.trajectoryType))
-    
+
 ##################
 # Annotations
 ##################
@@ -2100,7 +2105,7 @@
             for a,o in matches.items():
                 gtMatches[a.getNum()][t] = o.getNum()
                 toMatches[o.getNum()][t] = a.getNum()
-        
+
         # compute metrics elements
         ct += len(matches)
         mt += nGTs-len(matches)
@@ -2123,7 +2128,7 @@
                 print('{} {}'.format(type(mm), mm.getNum()))
         # some object mismatches may appear twice
         mme += len(set(mismatches))
-        
+
     if ct > 0:
         motp = dist/ct
     else:
diff -r 6e8ab471ebd4 -r 6bbcd9433732 trafficintelligence/tests/moving.txt
--- a/trafficintelligence/tests/moving.txt	Tue Apr 16 11:58:23 2019 -0400
+++ b/trafficintelligence/tests/moving.txt	Mon May 06 16:16:28 2019 -0400
@@ -232,6 +232,11 @@
 >>> o.interpolateCurvilinearPositions(10.7) # doctest:+ELLIPSIS
 [14.09999..., 0.69999..., 'a']
 
+>>> t1 = CurvilinearTrajectory.generate(3, 1., 10, 'b')
+>>> t1.duplicateLastPosition()
+>>> t1[-1] == t1[-2]
+True
+
 >>> a = Trajectory.generate(Point(0.,0.), Point(10.,0.), 4)
 >>> t = Trajectory.generate(Point(0.1,-1.), Point(1.,0.), 22)
 >>> prepareAlignments([a])