Mercurial Hosting > traffic-intelligence
diff 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 |
line wrap: on
line diff
--- a/python/moving.py Thu Jul 11 00:17:25 2013 -0400 +++ b/python/moving.py Thu Jul 11 02:17:12 2013 -0400 @@ -30,6 +30,9 @@ def empty(self): return self.first > self.last + def center(self): + return (self.first+self.last)/2. + def length(self): '''Returns the length of the interval''' return float(max(0,self.last-self.first)) @@ -306,33 +309,39 @@ def similar(f1, f2, maxDistance2, maxDeltavelocity2): return (f1.position-f2.position).norm2Squared()<maxDistance2 and (f1.velocity-f2.velocity).norm2Squared()<maxDeltavelocity2 +def intersection(p1, p2, dp1, dp2): + '''Returns the intersection point between the two lines + defined by the respective vectors (dp) and origin points (p)''' + from numpy import matrix + from numpy.linalg import linalg + A = matrix([[dp1.y, -dp1.x], + [dp2.y, -dp2.x]]) + B = matrix([[dp1.y*p1.x-dp1.x*p1.y], + [dp2.y*p2.x-dp2.x*p2.y]]) + + if linalg.det(A) == 0: + return None + else: + intersection = linalg.solve(A,B) + return Point(intersection[0,0], intersection[1,0]) + def segmentIntersection(p1, p2, p3, p4): '''Returns the intersecting point of the segments [p1, p2] and [p3, p4], None otherwise''' - from numpy import matrix - from numpy.linalg import linalg, det 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()): return None else: - dp1 = p2-p1#[s1[0][1]-s1[0][0], s1[1][1]-s1[1][0]] - dp2 = p4-p3#[s2[0][1]-s2[0][0], s2[1][1]-s2[1][0]] - - A = matrix([[dp1.y, -dp1.x], - [dp2.y, -dp2.x]]) - B = matrix([[dp1.y*p1.x-dp1.x*p1.y], - [dp2.y*p3.x-dp2.x*p3.y]]) - - if linalg.det(A) == 0:#crossProduct(ds1, ds2) == 0: + dp1 = p2-p1 + dp3 = p4-p3 + inter = intersection(p1, p3, dp1, dp3) + if (inter != None + and utils.inBetween(p1.x, p2.x, inter.x) + and utils.inBetween(p3.x, p4.x, inter.x) + and utils.inBetween(p1.y, p2.y, inter.y) + and utils.inBetween(p3.y, p4.y, inter.y)): + return inter + else: return None - else: - intersection = linalg.solve(A,B) - if (utils.inBetween(p1.x, p2.x, intersection[0,0]) - and utils.inBetween(p3.x, p4.x, intersection[0,0]) - and utils.inBetween(p1.y, p2.y, intersection[1,0]) - and utils.inBetween(p3.y, p4.y, intersection[1,0])): - return Point(intersection[0,0], intersection[1,0]) - else: - return None # TODO: implement a better algorithm for intersections of sets of segments http://en.wikipedia.org/wiki/Line_segment_intersection