Mercurial Hosting > traffic-intelligence
comparison trafficintelligence/prediction.py @ 1215:1b472cddf9b1
updated reading by interpolating missing frames
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Fri, 05 May 2023 22:52:25 -0400 |
parents | 01c24c1cdb70 |
children | ca70a79688ae |
comparison
equal
deleted
inserted
replaced
1214:01c24c1cdb70 | 1215:1b472cddf9b1 |
---|---|
556 timeInterval2 = moving.TimeInterval(max(0,dist2-halfCollisionDistanceThreshold)/s2, (dist2+halfCollisionDistanceThreshold)/s2) | 556 timeInterval2 = moving.TimeInterval(max(0,dist2-halfCollisionDistanceThreshold)/s2, (dist2+halfCollisionDistanceThreshold)/s2) |
557 collisionTimeInterval = moving.TimeInterval.intersection(timeInterval1, timeInterval2) | 557 collisionTimeInterval = moving.TimeInterval.intersection(timeInterval1, timeInterval2) |
558 | 558 |
559 if collisionTimeInterval.empty(): | 559 if collisionTimeInterval.empty(): |
560 if computeCZ: | 560 if computeCZ: |
561 crossingZones = [SafetyPoint(intersection, 1., timeInterval1.distance(timeInterval2))] | 561 crossingZones.append(SafetyPoint(intersection, 1., timeInterval1.distance(timeInterval2))) |
562 else: | 562 else: |
563 collisionPoints = [SafetyPoint(intersection, 1., collisionTimeInterval.center())] | 563 collisionPoints.append(SafetyPoint(intersection, 1., collisionTimeInterval.center())) |
564 | 564 |
565 if debug and intersection is not None: | 565 if debug and intersection is not None: |
566 from matplotlib.pyplot import plot, figure, axis, title | 566 from matplotlib.pyplot import plot, figure, axis, title |
567 figure() | 567 figure() |
568 plot([p1.x, intersection.x], [p1.y, intersection.y], 'r') | 568 plot([p1.x, intersection.x], [p1.y, intersection.y], 'r') |
584 PredictionParameters.__init__(self, 'constant velocity (direct exact computation)', None, useCurvilinear) | 584 PredictionParameters.__init__(self, 'constant velocity (direct exact computation)', None, useCurvilinear) |
585 | 585 |
586 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs): | 586 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs): |
587 'TODO compute pPET' | 587 'TODO compute pPET' |
588 collisionPoints = [] | 588 collisionPoints = [] |
589 crossingZones = [] | 589 if computeCZ: |
590 crossingZones = [] | |
591 else: | |
592 crossingZones = None | |
590 | 593 |
591 if self.useCurvilinear: | 594 if self.useCurvilinear: |
592 pass # Lionel | 595 pass # Lionel |
593 else: | 596 else: |
594 p1 = obj1.getPositionAtInstant(currentInstant) | 597 p1 = obj1.getPositionAtInstant(currentInstant) |
598 #intersection = moving.intersection(p1, p1+v1, p2, p2+v2) | 601 #intersection = moving.intersection(p1, p1+v1, p2, p2+v2) |
599 | 602 |
600 if not moving.Point.parallel(v1, v2): | 603 if not moving.Point.parallel(v1, v2): |
601 ttc = moving.Point.timeToCollision(p1, p2, v1, v2, collisionDistanceThreshold) | 604 ttc = moving.Point.timeToCollision(p1, p2, v1, v2, collisionDistanceThreshold) |
602 if ttc is not None: | 605 if ttc is not None: |
603 collisionPoints = [SafetyPoint((p1+(v1*ttc)+p2+(v2*ttc))*0.5, 1., ttc)] | 606 collisionPoints.append(SafetyPoint((p1+(v1*ttc)+p2+(v2*ttc))*0.5, 1., ttc)) |
604 else: | 607 else: |
605 pass # compute pPET | 608 pass # compute pPET |
606 return collisionPoints, crossingZones | 609 return collisionPoints, crossingZones |
607 | 610 |
608 class CVExactPolyPredictionParameters(PredictionParameters): | 611 class CVExactPolyPredictionParameters(PredictionParameters): |
614 PredictionParameters.__init__(self, 'constant velocity for polygon representation (direct exact)', None) | 617 PredictionParameters.__init__(self, 'constant velocity for polygon representation (direct exact)', None) |
615 | 618 |
616 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs): | 619 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs): |
617 'TODO compute pPET' | 620 'TODO compute pPET' |
618 collisionPoints = [] | 621 collisionPoints = [] |
619 crossingZones = [] | 622 if computeCZ: |
623 crossingZones = [] | |
624 else: | |
625 crossingZones = None | |
620 | 626 |
621 if self.useCurvilinear: | 627 if self.useCurvilinear: |
622 pass # Lionel | 628 pass # Lionel |
623 else: | 629 else: |
624 poly1 = obj1.getBoundingPolygon(currentInstant) | 630 poly1 = obj1.getBoundingPolygon(currentInstant) |
627 v2 = obj2.getVelocityAtInstant(currentInstant) | 633 v2 = obj2.getVelocityAtInstant(currentInstant) |
628 | 634 |
629 if not moving.Point.parallel(v1, v2): | 635 if not moving.Point.parallel(v1, v2): |
630 ttc = moving.Point.timeToCollisionPoly(poly1, v1, poly2, v2) | 636 ttc = moving.Point.timeToCollisionPoly(poly1, v1, poly2, v2) |
631 if ttc is not None: | 637 if ttc is not None: |
632 collisionPoints = [SafetyPoint((obj1.getPositionAtInstant(currentInstant)+(v1*ttc)+obj2.getPositionAtInstant(currentInstant)+(v2*ttc))*0.5, 1., ttc)] | 638 collisionPoints.append(SafetyPoint((obj1.getPositionAtInstant(currentInstant)+(v1*ttc)+obj2.getPositionAtInstant(currentInstant)+(v2*ttc))*0.5, 1., ttc)) |
633 else: | 639 else: |
634 pass # compute pPET | 640 pass # compute pPET |
635 return collisionPoints, crossingZones | 641 return collisionPoints, crossingZones |
636 | 642 |
637 | 643 |