Mercurial > hg > nsaunier > traffic-intelligence
comparison trafficintelligence/prediction.py @ 1210:1bad5f9b60de
work in progress
| author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
|---|---|
| date | Fri, 28 Apr 2023 17:03:39 -0400 |
| parents | 2064e52019db |
| children | a095d4fbb2ea |
comparison
equal
deleted
inserted
replaced
| 1209:2064e52019db | 1210:1bad5f9b60de |
|---|---|
| 166 while t < timeHorizon and not collision: | 166 while t < timeHorizon and not collision: |
| 167 t += 1 | 167 t += 1 |
| 168 p1 = predictedTrajectory1.predictPosition(t) | 168 p1 = predictedTrajectory1.predictPosition(t) |
| 169 p2 = predictedTrajectory2.predictPosition(t) | 169 p2 = predictedTrajectory2.predictPosition(t) |
| 170 collision = (p1-p2).norm2() <= collisionDistanceThreshold | 170 collision = (p1-p2).norm2() <= collisionDistanceThreshold |
| 171 return collision, t, p1, p2 | |
| 172 | |
| 173 def computeCollisionTimePolygon(predictedTrajectories1, predictedTrajectories2, timeHorizon): | |
| 174 '''Computes the first instant | |
| 175 at which two objects represented by a series of points (eg box) | |
| 176 Computes all the times including timeHorizon | |
| 177 | |
| 178 User has to check the first variable collision to know about a collision''' | |
| 179 t = 1 | |
| 180 poly1 = moving.pointsToShapely([t1.predictPosition(t) for t1 in predictedTrajectories1]) | |
| 181 poly2 = moving.pointsToShapely([t2.predictPosition(t) for t2 in predictedTrajectories2]) | |
| 182 collision = poly1.overlaps(poly2) | |
| 183 while t < timeHorizon and not collision: | |
| 184 t += 1 | |
| 185 poly1 = moving.pointsToShapely([t1.predictPosition(t) for t1 in predictedTrajectories1]) | |
| 186 poly2 = moving.pointsToShapely([t2.predictPosition(t) for t2 in predictedTrajectories2]) | |
| 187 collision = poly1.overlaps(poly2) | |
| 171 return collision, t, p1, p2 | 188 return collision, t, p1, p2 |
| 172 | 189 |
| 173 def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon, printFigure = True): | 190 def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon, printFigure = True): |
| 174 from matplotlib.pyplot import figure, axis, title, clf, savefig | 191 from matplotlib.pyplot import figure, axis, title, clf, savefig |
| 175 if printFigure: | 192 if printFigure: |