Mercurial > hg > nsaunier > traffic-intelligence
comparison python/moving.py @ 504:a40c75f04903
optimized direct time to collision computation and added tests
| author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
|---|---|
| date | Mon, 02 Jun 2014 17:33:22 -0400 |
| parents | c81cbd6953fb |
| children | 727e3c529519 |
comparison
equal
deleted
inserted
replaced
| 503:7978b286fcfa | 504:a40c75f04903 |
|---|---|
| 266 'Indicates whether the cosine of the vector and refDirection is smaller than cosineThreshold' | 266 'Indicates whether the cosine of the vector and refDirection is smaller than cosineThreshold' |
| 267 return Point.cosine(self, refDirection) >= cosineThreshold | 267 return Point.cosine(self, refDirection) >= cosineThreshold |
| 268 | 268 |
| 269 @staticmethod | 269 @staticmethod |
| 270 def timeToCollision(p1, p2, v1, v2, collisionThreshold): | 270 def timeToCollision(p1, p2, v1, v2, collisionThreshold): |
| 271 '''Computes exact time to collision with a distance threshold | |
| 272 The unknown of the equation is the time to reach the intersection | |
| 273 between the relative trajectory of one road user | |
| 274 and the circle of radius collisionThreshold around the other road user''' | |
| 271 from math import sqrt | 275 from math import sqrt |
| 272 a = (v1.x-v2.x)**2 + (v1.y-v2.y)**2 | 276 dv = v1-v2 |
| 273 b = 2 * ((p1.x-p2.x) * (v1.x-v2.x) + (p1.y-p2.y) * (v1.y-v2.y)) | 277 dp = p1-p2 |
| 274 c = (p1.x-p2.x)**2 + (p1.y-p2.y)**2 - collisionThreshold**2 | 278 a = dv.norm2Squared()#(v1.x-v2.x)**2 + (v1.y-v2.y)**2 |
| 275 | 279 b = 2*dot(dv, dp)#2 * ((p1.x-p2.x) * (v1.x-v2.x) + (p1.y-p2.y) * (v1.y-v2.y)) |
| 276 if b**2 >= 4*a*c: | 280 c = dp.norm2Squared() - collisionThreshold**2#(p1.x-p2.x)**2 + (p1.y-p2.y)**2 - collisionThreshold**2 |
| 277 ttc1 = (-b + sqrt(b**2 - 4*a*c)) / (2*a) | 281 |
| 278 ttc2 = (-b - sqrt(b**2 - 4*a*c)) / (2*a) | 282 delta = b**2 - 4*a*c |
| 283 if delta >= 0: | |
| 284 deltaRoot = sqrt(delta) | |
| 285 ttc1 = (-b + deltaRoot)/(2*a) | |
| 286 ttc2 = (-b - deltaRoot)/(2*a) | |
| 279 if ttc1 >= 0 and ttc2 >= 0: | 287 if ttc1 >= 0 and ttc2 >= 0: |
| 280 ttc = min(ttc1,ttc2) | 288 ttc = min(ttc1,ttc2) |
| 281 else: | 289 else: |
| 282 if ttc1 < 0: | 290 if ttc1 < 0: |
| 283 ttc = ttc2 | 291 ttc = ttc2 |