diff python/moving.py @ 887:e2452abba0e7

added option to compute PET in safety analysis script, and save in database
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Wed, 22 Mar 2017 10:44:24 -0400
parents 8ba82b371eea
children ff92801e5c54
line wrap: on
line diff
--- a/python/moving.py	Tue Mar 21 17:51:38 2017 -0400
+++ b/python/moving.py	Wed Mar 22 10:44:24 2017 -0400
@@ -5,7 +5,7 @@
 from base import VideoFilenameAddable
 
 from math import sqrt, atan2, cos, sin
-from numpy import median, array, zeros, hypot, NaN, std, floor, float32
+from numpy import median, array, zeros, hypot, NaN, std, floor, float32, argwhere
 from matplotlib.pyplot import plot, text
 from scipy.stats import scoreatpercentile
 from scipy.spatial.distance import cdist
@@ -1426,19 +1426,25 @@
     def computePET(obj1, obj2, collisionDistanceThreshold):
         '''Post-encroachment time based on distance threshold
 
-        Returns the smallest time difference when the object positions are within collisionDistanceThreshold'''
+        Returns the smallest time difference when the object positions are within collisionDistanceThreshold
+        and the instants at which each object is passing through its corresponding position'''
         positions1 = [p.astuple() for p in obj1.getPositions()]
         positions2 = [p.astuple() for p in obj2.getPositions()]
-        pets = zeros((int(obj1.length()), int(obj2.length())))
+        n1 = len(positions1)
+        n2 = len(positions2)
+        pets = zeros((n1, n2))
         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:
-            #idx = distances.argmin()
-            return pets[distances <= collisionDistanceThreshold].min()
+        smallDistances = (distances <= collisionDistanceThreshold)
+        if smallDistances.any():
+            smallPets = pets[smallDistances]
+            petIdx = smallPets.argmin()
+            distanceIndices = argwhere(smallDistances)[petIdx]
+            return smallPets[petIdx], obj1.getFirstInstant()+distanceIndices[0], obj2.getFirstInstant()+distanceIndices[1]
         else:
-            return None
+            return None, None, None
 
     def predictPosition(self, instant, nTimeSteps, externalAcceleration = Point(0,0)):
         '''Predicts the position of object at instant+deltaT,