diff 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
line wrap: on
line diff
--- a/python/moving.py	Tue Feb 17 02:21:31 2015 +0100
+++ b/python/moving.py	Tue Feb 17 03:55:55 2015 +0100
@@ -582,7 +582,7 @@
 def segmentLineIntersection(p1, p2, p3, p4):
     '''Indicates if the line going through p1 and p2 intersects inside p3, p4'''
     inter = intersection(p1, p2, p3, p4)
-    if inter != None and utils.inBetween(p3.x, p4.x, inter.x) and utils.inBetween(p3.y, p4.y, inter.y)):
+    if inter != None and utils.inBetween(p3.x, p4.x, inter.x) and utils.inBetween(p3.y, p4.y, inter.y):
         return inter
     else:
         return None
@@ -812,6 +812,7 @@
         intersects with the segment of extremities p1 and p2 
         the list is empty if there is no crossing'''
         indices = []
+        intersections = []
 
         for i in xrange(self.length()-1):
             q1=self.__getitem__(i)
@@ -825,15 +826,16 @@
                 else:
                     ratio = 0
                 indices.append(i+ratio)
+                intersections.append(p)
         return indices
 
     def getLineIntersections(self, p1, p2):
-         '''Returns a list of the indices at which the trajectory 
+        '''Returns a list of the indices at which the trajectory 
         intersects with the segment of extremities p1 and p2 
         the list is empty if there is no crossing'''
         indices = []
         intersections = []
-
+        
         for i in xrange(self.length()-1):
             q1=self.__getitem__(i)
             q2=self.__getitem__(i+1)
@@ -1169,9 +1171,29 @@
         '''Returns the instant(s)
         at which the object passes from one side of the segment to the other
         empty list if there is no crossing'''
-        indices = self.positions.getIntersections(p1, p2)
+        indices, intersections = self.positions.getIntersections(p1, p2)
         return [t+self.getFirstInstant() for t in indices]
 
+    @staticmethod
+    def computePET(obj1, obj2, collisionDistanceThreshold):
+        '''Post-encroachment time based on distance threshold'''
+        #for i in xrange(int(obj1.length())-1):
+        #    for j in xrange(int(obj2.length())-1):
+        #        inter = segmentIntersection(obj1.getPositionAt(i), obj1.getPositionAt(i+1), obj2.getPositionAt(i), obj2.getPositionAt(i+1))
+        from scipy.spatial.distance import cdist
+        from numpy import zeros
+        positions1 = [p.astuple() for p in obj1.getPositions()]
+        positions2 = [p.astuple() for p in obj2.getPositions()]
+        pets = zeros((int(obj1.length()), int(obj2.length())))
+        for i,t1 in enumerate(obj1.getTimeInterval()):
+            for j,t2 in enumerate(obj2.getTimeInterval()):
+                pets[i,j] = abs(t1-t2)
+        distances = cdist(positions1, positions2, metric = 'euclidean')
+        if distances.min() <= collisionDistanceThreshold:
+            return pets[distances <= collisionDistanceThreshold].min()
+        else:
+            return None
+
     def predictPosition(self, instant, nTimeSteps, externalAcceleration = Point(0,0)):
         '''Predicts the position of object at instant+deltaT, 
         at constant speed'''