Mercurial Hosting > traffic-intelligence
comparison python/moving.py @ 359:619ae9a9a788
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 |