Mercurial > hg > nsaunier > traffic-intelligence

comparison python/moving.py @ 359:619ae9a9a788

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
implemented prediction method at constant velocity with direct intersection computation
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Thu, 11 Jul 2013 02:17:12 -0400
parents e5fe0e6d48a1
children 2db4e76599a1
comparison
equal deleted inserted replaced
358:c41ff9f3c263 359:619ae9a9a788
27 def __repr__(self): 27 def __repr__(self):
28 return self.__str__() 28 return self.__str__()
29 29
30 def empty(self): 30 def empty(self):
31 return self.first > self.last 31 return self.first > self.last
32
33 def center(self):
34 return (self.first+self.last)/2.
32 35
33 def length(self): 36 def length(self):
34 '''Returns the length of the interval''' 37 '''Returns the length of the interval'''
35 return float(max(0,self.last-self.first)) 38 return float(max(0,self.last-self.first))
36 39
304 307
305 @staticmethod 308 @staticmethod
306 def similar(f1, f2, maxDistance2, maxDeltavelocity2): 309 def similar(f1, f2, maxDistance2, maxDeltavelocity2):
307 return (f1.position-f2.position).norm2Squared()<maxDistance2 and (f1.velocity-f2.velocity).norm2Squared()<maxDeltavelocity2 310 return (f1.position-f2.position).norm2Squared()<maxDistance2 and (f1.velocity-f2.velocity).norm2Squared()<maxDeltavelocity2
308 311
312 def intersection(p1, p2, dp1, dp2):
313 '''Returns the intersection point between the two lines
314 defined by the respective vectors (dp) and origin points (p)'''
315 from numpy import matrix
316 from numpy.linalg import linalg
317 A = matrix([[dp1.y, -dp1.x],
318 [dp2.y, -dp2.x]])
319 B = matrix([[dp1.y*p1.x-dp1.x*p1.y],
320 [dp2.y*p2.x-dp2.x*p2.y]])
321
322 if linalg.det(A) == 0:
323 return None
324 else:
325 intersection = linalg.solve(A,B)
326 return Point(intersection[0,0], intersection[1,0])
327
309 def segmentIntersection(p1, p2, p3, p4): 328 def segmentIntersection(p1, p2, p3, p4):
310 '''Returns the intersecting point of the segments [p1, p2] and [p3, p4], None otherwise''' 329 '''Returns the intersecting point of the segments [p1, p2] and [p3, p4], None otherwise'''
311 from numpy import matrix
312 from numpy.linalg import linalg, det
313 330
314 if (Interval.intersection(Interval(p1.x,p2.x,True), Interval(p3.x,p4.x,True)).empty()) or (Interval.intersection(Interval(p1.y,p2.y,True), Interval(p3.y,p4.y,True)).empty()): 331 if (Interval.intersection(Interval(p1.x,p2.x,True), Interval(p3.x,p4.x,True)).empty()) or (Interval.intersection(Interval(p1.y,p2.y,True), Interval(p3.y,p4.y,True)).empty()):
315 return None 332 return None
316 else: 333 else:
317 dp1 = p2-p1#[s1[0][1]-s1[0][0], s1[1][1]-s1[1][0]] 334 dp1 = p2-p1
318 dp2 = p4-p3#[s2[0][1]-s2[0][0], s2[1][1]-s2[1][0]] 335 dp3 = p4-p3
319 336 inter = intersection(p1, p3, dp1, dp3)
320 A = matrix([[dp1.y, -dp1.x], 337 if (inter != None
321 [dp2.y, -dp2.x]]) 338 and utils.inBetween(p1.x, p2.x, inter.x)
322 B = matrix([[dp1.y*p1.x-dp1.x*p1.y], 339 and utils.inBetween(p3.x, p4.x, inter.x)
323 [dp2.y*p3.x-dp2.x*p3.y]]) 340 and utils.inBetween(p1.y, p2.y, inter.y)
324 341 and utils.inBetween(p3.y, p4.y, inter.y)):
325 if linalg.det(A) == 0:#crossProduct(ds1, ds2) == 0: 342 return inter
343 else:
326 return None 344 return None
327 else:
328 intersection = linalg.solve(A,B)
329 if (utils.inBetween(p1.x, p2.x, intersection[0,0])
330 and utils.inBetween(p3.x, p4.x, intersection[0,0])
331 and utils.inBetween(p1.y, p2.y, intersection[1,0])
332 and utils.inBetween(p3.y, p4.y, intersection[1,0])):
333 return Point(intersection[0,0], intersection[1,0])
334 else:
335 return None
336 345
337 # TODO: implement a better algorithm for intersections of sets of segments http://en.wikipedia.org/wiki/Line_segment_intersection 346 # TODO: implement a better algorithm for intersections of sets of segments http://en.wikipedia.org/wiki/Line_segment_intersection
338 347
339 class Trajectory(object): 348 class Trajectory(object):
340 '''Class for trajectories: temporal sequence of positions 349 '''Class for trajectories: temporal sequence of positions