Mercurial Hosting > traffic-intelligence
comparison python/moving.py @ 631:2d1d33ae1c69
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) |