Mercurial > hg > nsaunier > traffic-intelligence

comparison python/moving.py @ 258:d90be3c02267

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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'