Mercurial Hosting > traffic-intelligence
comparison python/moving.py @ 258:d90be3c02267
reasonably efficient computation of collision points and crossing zones
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Tue, 24 Jul 2012 12:37:47 -0400 |
parents | dc1faa7287bd |
children | 8ab76b95ee72 |
comparison
equal
deleted
inserted
replaced
257:9281878ff19e | 258:d90be3c02267 |
---|---|
1 #! /usr/bin/env python | 1 #! /usr/bin/env python |
2 '''Libraries for moving objects, trajectories...''' | 2 '''Libraries for moving objects, trajectories...''' |
3 | 3 |
4 import utils; | 4 import utils |
5 import cvutils; | 5 import cvutils |
6 | 6 |
7 from math import sqrt; | 7 from math import sqrt |
8 | 8 |
9 #from shapely.geometry import Polygon | 9 #from shapely.geometry import Polygon |
10 | 10 |
11 __metaclass__ = type | 11 __metaclass__ = type |
12 | 12 |
293 def segmentIntersection(p1, p2, p3, p4): | 293 def segmentIntersection(p1, p2, p3, p4): |
294 '''Returns the intersecting point of the segments [p1, p2] and [p3, p4], None otherwise''' | 294 '''Returns the intersecting point of the segments [p1, p2] and [p3, p4], None otherwise''' |
295 from numpy import matrix | 295 from numpy import matrix |
296 from numpy.linalg import linalg, det | 296 from numpy.linalg import linalg, det |
297 | 297 |
298 dp1 = p2-p1#[s1[0][1]-s1[0][0], s1[1][1]-s1[1][0]] | 298 if (Interval(p1.x,p2.x, True).intersection(Interval(p3.x,p4.x, True)).empty() |
299 dp2 = p4-p3#[s2[0][1]-s2[0][0], s2[1][1]-s2[1][0]] | 299 or Interval(p1.y,p2.y, True).intersection(Interval(p3.y,p4.y, True)).empty()): |
300 | |
301 A = matrix([[dp1.y, -dp1.x], | |
302 [dp2.y, -dp2.x]]) | |
303 B = matrix([[dp1.y*p1.x-dp1.x*p1.y], | |
304 [dp2.y*p3.x-dp2.x*p3.y]]) | |
305 | |
306 if linalg.det(A) == 0:#crossProduct(ds1, ds2) == 0: | |
307 return None | 300 return None |
308 else: | 301 else: |
309 intersection = linalg.solve(A,B) | 302 dp1 = p2-p1#[s1[0][1]-s1[0][0], s1[1][1]-s1[1][0]] |
310 if (utils.inBetween(p1.x, p2.x, intersection[0,0]) | 303 dp2 = p4-p3#[s2[0][1]-s2[0][0], s2[1][1]-s2[1][0]] |
311 and utils.inBetween(p3.x, p4.x, intersection[0,0]) | 304 |
312 and utils.inBetween(p1.y, p2.y, intersection[1,0]) | 305 A = matrix([[dp1.y, -dp1.x], |
313 and utils.inBetween(p3.y, p4.y, intersection[1,0])): | 306 [dp2.y, -dp2.x]]) |
314 return Point(intersection[0,0], intersection[1,0]) | 307 B = matrix([[dp1.y*p1.x-dp1.x*p1.y], |
315 else: | 308 [dp2.y*p3.x-dp2.x*p3.y]]) |
309 | |
310 if linalg.det(A) == 0:#crossProduct(ds1, ds2) == 0: | |
316 return None | 311 return None |
312 else: | |
313 intersection = linalg.solve(A,B) | |
314 if (utils.inBetween(p1.x, p2.x, intersection[0,0]) | |
315 and utils.inBetween(p3.x, p4.x, intersection[0,0]) | |
316 and utils.inBetween(p1.y, p2.y, intersection[1,0]) | |
317 and utils.inBetween(p3.y, p4.y, intersection[1,0])): | |
318 return Point(intersection[0,0], intersection[1,0]) | |
319 else: | |
320 return None | |
321 | |
322 # TODO: implement a better algorithm for intersections of sets of segments http://en.wikipedia.org/wiki/Line_segment_intersection | |
317 | 323 |
318 class Trajectory: | 324 class Trajectory: |
319 '''Class for trajectories | 325 '''Class for trajectories |
320 i.e. a temporal sequence of positions | 326 i.e. a temporal sequence of positions |
321 | 327 |
406 from numpy.core.multiarray import array | 412 from numpy.core.multiarray import array |
407 return array(self.positions) | 413 return array(self.positions) |
408 | 414 |
409 def xBounds(self): | 415 def xBounds(self): |
410 # look for function that does min and max in one pass | 416 # look for function that does min and max in one pass |
411 return [min(self.getXCoordinates()), max(self.getXCoordinates())] | 417 return Interval(min(self.getXCoordinates()), max(self.getXCoordinates())) |
412 | 418 |
413 def yBounds(self): | 419 def yBounds(self): |
414 # look for function that does min and max in one pass | 420 # look for function that does min and max in one pass |
415 return [min(self.getYCoordinates()), max(self.getYCoordinates())] | 421 return Interval(min(self.getYCoordinates()), max(self.getYCoordinates())) |
416 | 422 |
417 def add(self, traj2): | 423 def add(self, traj2): |
418 '''Returns a new trajectory of the same length''' | 424 '''Returns a new trajectory of the same length''' |
419 if self.length() != traj2.length(): | 425 if self.length() != traj2.length(): |
420 print 'Trajectories of different lengths' | 426 print 'Trajectories of different lengths' |