Mercurial > hg > nsaunier > traffic-intelligence

comparison python/moving.py @ 631:2d1d33ae1c69

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
major work on pPET and pet, issues remain for pPET computed with predicted trajectories
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Tue, 17 Feb 2015 03:55:55 +0100
parents 69a98f84f3eb
children f410c8fb07b7
comparison
equal deleted inserted replaced
630:69a98f84f3eb 631:2d1d33ae1c69
580 return None 580 return None
581 581
582 def segmentLineIntersection(p1, p2, p3, p4): 582 def segmentLineIntersection(p1, p2, p3, p4):
583 '''Indicates if the line going through p1 and p2 intersects inside p3, p4''' 583 '''Indicates if the line going through p1 and p2 intersects inside p3, p4'''
584 inter = intersection(p1, p2, p3, p4) 584 inter = intersection(p1, p2, p3, p4)
585 if inter != None and utils.inBetween(p3.x, p4.x, inter.x) and utils.inBetween(p3.y, p4.y, inter.y)): 585 if inter != None and utils.inBetween(p3.x, p4.x, inter.x) and utils.inBetween(p3.y, p4.y, inter.y):
586 return inter 586 return inter
587 else: 587 else:
588 return None 588 return None
589 589
590 590
810 def getIntersections(self, p1, p2): 810 def getIntersections(self, p1, p2):
811 '''Returns a list of the indices at which the trajectory 811 '''Returns a list of the indices at which the trajectory
812 intersects with the segment of extremities p1 and p2 812 intersects with the segment of extremities p1 and p2
813 the list is empty if there is no crossing''' 813 the list is empty if there is no crossing'''
814 indices = [] 814 indices = []
815 intersections = []
815 816
816 for i in xrange(self.length()-1): 817 for i in xrange(self.length()-1):
817 q1=self.__getitem__(i) 818 q1=self.__getitem__(i)
818 q2=self.__getitem__(i+1) 819 q2=self.__getitem__(i+1)
819 p = utils.segmentIntersection(q1, q2, p1, p2) 820 p = utils.segmentIntersection(q1, q2, p1, p2)
823 elif q1.y != q2.y: 824 elif q1.y != q2.y:
824 ratio = (p.y-q1.y)/(q2.y-q1.y) 825 ratio = (p.y-q1.y)/(q2.y-q1.y)
825 else: 826 else:
826 ratio = 0 827 ratio = 0
827 indices.append(i+ratio) 828 indices.append(i+ratio)
829 intersections.append(p)
828 return indices 830 return indices
829 831
830 def getLineIntersections(self, p1, p2): 832 def getLineIntersections(self, p1, p2):
831 '''Returns a list of the indices at which the trajectory 833 '''Returns a list of the indices at which the trajectory
832 intersects with the segment of extremities p1 and p2 834 intersects with the segment of extremities p1 and p2
833 the list is empty if there is no crossing''' 835 the list is empty if there is no crossing'''
834 indices = [] 836 indices = []
835 intersections = [] 837 intersections = []
836 838
837 for i in xrange(self.length()-1): 839 for i in xrange(self.length()-1):
838 q1=self.__getitem__(i) 840 q1=self.__getitem__(i)
839 q2=self.__getitem__(i+1) 841 q2=self.__getitem__(i+1)
840 p = utils.segmentLineIntersection(p1, p2, q1, q2) 842 p = utils.segmentLineIntersection(p1, p2, q1, q2)
841 if p != None: 843 if p != None:
1167 1169
1168 def getInstantsCrossingLane(self, p1, p2): 1170 def getInstantsCrossingLane(self, p1, p2):
1169 '''Returns the instant(s) 1171 '''Returns the instant(s)
1170 at which the object passes from one side of the segment to the other 1172 at which the object passes from one side of the segment to the other
1171 empty list if there is no crossing''' 1173 empty list if there is no crossing'''
1172 indices = self.positions.getIntersections(p1, p2) 1174 indices, intersections = self.positions.getIntersections(p1, p2)
1173 return [t+self.getFirstInstant() for t in indices] 1175 return [t+self.getFirstInstant() for t in indices]
1176
1177 @staticmethod
1178 def computePET(obj1, obj2, collisionDistanceThreshold):
1179 '''Post-encroachment time based on distance threshold'''
1180 #for i in xrange(int(obj1.length())-1):
1181 # for j in xrange(int(obj2.length())-1):
1182 # inter = segmentIntersection(obj1.getPositionAt(i), obj1.getPositionAt(i+1), obj2.getPositionAt(i), obj2.getPositionAt(i+1))
1183 from scipy.spatial.distance import cdist
1184 from numpy import zeros
1185 positions1 = [p.astuple() for p in obj1.getPositions()]
1186 positions2 = [p.astuple() for p in obj2.getPositions()]
1187 pets = zeros((int(obj1.length()), int(obj2.length())))
1188 for i,t1 in enumerate(obj1.getTimeInterval()):
1189 for j,t2 in enumerate(obj2.getTimeInterval()):
1190 pets[i,j] = abs(t1-t2)
1191 distances = cdist(positions1, positions2, metric = 'euclidean')
1192 if distances.min() <= collisionDistanceThreshold:
1193 return pets[distances <= collisionDistanceThreshold].min()
1194 else:
1195 return None
1174 1196
1175 def predictPosition(self, instant, nTimeSteps, externalAcceleration = Point(0,0)): 1197 def predictPosition(self, instant, nTimeSteps, externalAcceleration = Point(0,0)):
1176 '''Predicts the position of object at instant+deltaT, 1198 '''Predicts the position of object at instant+deltaT,
1177 at constant speed''' 1199 at constant speed'''
1178 return predictPositionNoLimit(nTimeSteps, self.getPositionAtInstant(instant), self.getVelocityAtInstant(instant), externalAcceleration) 1200 return predictPositionNoLimit(nTimeSteps, self.getPositionAtInstant(instant), self.getVelocityAtInstant(instant), externalAcceleration)