comparison python/prediction.py @ 692:9a258687af4c dev

corrected some errors for ttc
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Mon, 29 Jun 2015 23:09:58 -0400
parents 15e244d2a1b5
children ae137e3b1990
comparison
equal deleted inserted replaced
691:fa9aa5f08210 692:9a258687af4c
319 else: 319 else:
320 commonTimeInterval = obj1.commonTimeInterval(obj2) 320 commonTimeInterval = obj1.commonTimeInterval(obj2)
321 if nProcesses == 1: 321 if nProcesses == 1:
322 if usePrototypes: 322 if usePrototypes:
323 firstInstant= next( (x for x in xrange(commonTimeInterval.first,commonTimeInterval.last) if x-obj1.getFirstInstant() >= acceptPartialLength and x-obj2.getFirstInstant() >= acceptPartialLength), commonTimeInterval.last) 323 firstInstant= next( (x for x in xrange(commonTimeInterval.first,commonTimeInterval.last) if x-obj1.getFirstInstant() >= acceptPartialLength and x-obj2.getFirstInstant() >= acceptPartialLength), commonTimeInterval.last)
324 commonTimeIntervalList1= list(xrange(firstInstant,commonTimeInterval.last-1)) # do not look at the 1 last position/velocities, often with errors 324 commonTimeIntervalList1= range(firstInstant,commonTimeInterval.last-1) # do not look at the 1 last position/velocities, often with errors
325 commonTimeIntervalList2= list(xrange(firstInstant,commonTimeInterval.last-1,step)) # do not look at the 1 last position/velocities, often with errors 325 commonTimeIntervalList2= range(firstInstant,commonTimeInterval.last-1,step) # do not look at the 1 last position/velocities, often with errors
326 for i in commonTimeIntervalList2: 326 for i in commonTimeIntervalList2:
327 i, cp, cz = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) 327 i, cp, cz = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
328 if len(cp) != 0: 328 if len(cp) != 0:
329 collisionPoints[i] = cp 329 collisionPoints[i] = cp
330 if len(cz) != 0: 330 if len(cz) != 0:
531 if collisionTimeInterval.empty(): 531 if collisionTimeInterval.empty():
532 if computeCZ: 532 if computeCZ:
533 crossingZones = [SafetyPoint(intersection, 1., timeInterval1.distance(timeInterval2))] 533 crossingZones = [SafetyPoint(intersection, 1., timeInterval1.distance(timeInterval2))]
534 else: 534 else:
535 collisionPoints = [SafetyPoint(intersection, 1., collisionTimeInterval.center())] 535 collisionPoints = [SafetyPoint(intersection, 1., collisionTimeInterval.center())]
536 # elif computeCZ and (dot1 > 0 or dot2 > 0):
537 # if dot1 > 0:
538 # firstUser = obj2 # first through crossingzone
539 # secondUser = obj1 # second through crossingzone
540 # elif dot2 > 0:
541 # firstUser = obj1
542 # secondUser = obj2
543 # p2 = secondUser.getPositionAtInstant(currentInstant)
544 # v2 = secondUser.getVelocityAtInstant(currentInstant)
545 # indices, intersections = firstUser.getPositions().getLineIntersections(p2, p2+v2)
546 # if indices is not None:
547 # pass
548 # else: # one has to predict !!!
549 536
550 if debug and intersection is not None: 537 if debug and intersection is not None:
551 from matplotlib.pyplot import plot, figure, axis, title 538 from matplotlib.pyplot import plot, figure, axis, title
552 figure() 539 figure()
553 plot([p1.x, intersection.x], [p1.y, intersection.y], 'r') 540 plot([p1.x, intersection.x], [p1.y, intersection.y], 'r')