Mercurial > hg > nsaunier > traffic-intelligence

comparison python/utils.py @ 27:44689029a86f

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
updated segmentIntersection and other
author Nicolas Saunier <nico@confins.net>
date Sat, 05 Dec 2009 15:40:28 -0500
parents 6fb59cfb201e
children ca8e716cc231
comparison
equal deleted inserted replaced
26:54d9cb0c902b 27:44689029a86f
2 ''' Generic utilities.''' 2 ''' Generic utilities.'''
3 3
4 #from numpy import * 4 #from numpy import *
5 #from pylab import * 5 #from pylab import *
6 6
7 from moving import Point,Interval
8
7 __metaclass__ = type 9 __metaclass__ = type
8 10
9 commentChar = '#'; 11 commentChar = '#';
10 12
11 # maths 13 #########################
14 # maths section
15 #########################
12 16
13 def segmentIntersection(s1, s2): 17 def segmentIntersection(p1, p2, p3, p4):
14 '''Returns the intersecting point, None otherwise 18 '''Returns the intersecting point of the segments [p1, p2] and [p3, p4], None otherwise
15 format for a segment is [[x1, x2], [y1,y2]]
16 19
17 >>> segmentIntersection([[0, 1], [0,1]], [[0, 1], [1,2]]) 20 >>> segmentIntersection(Point(0,0),Point(1,1), Point(0,1), Point(1,2))
18 >>> segmentIntersection([[0, 1], [1,0]], [[0, 1], [2,1]]) 21 >>> segmentIntersection(Point(0,1),Point(1,0), Point(0,2), Point(2,1))
19 >>> segmentIntersection([[0, 2], [0,0]], [[1, 1], [-1,1]]) 22 >>> segmentIntersection(Point(0,0),Point(2,0), Point(1,-1),Point(1,1))
20 [1.0, 0.0] 23 (1.000000,0.000000)
21 >>> segmentIntersection([[0, 2], [0,0]], [[1, 1], [1,2]]) 24 >>> segmentIntersection(Point(0,1),Point(2,0),Point(1,1),Point(1,2))
22 ''' 25 '''
23 from numpy import matrix 26 from numpy import matrix
24 from numpy.linalg import linalg, det 27 from numpy.linalg import linalg, det
25 28
26 ds1 = [s1[0][1]-s1[0][0], s1[1][1]-s1[1][0]] 29 dp1 = p2-p1#[s1[0][1]-s1[0][0], s1[1][1]-s1[1][0]]
27 ds2 = [s2[0][1]-s2[0][0], s2[1][1]-s2[1][0]] 30 dp2 = p4-p3#[s2[0][1]-s2[0][0], s2[1][1]-s2[1][0]]
28 31
29 A = matrix([[ds1[1], -ds1[0]], 32 A = matrix([[dp1.y, -dp1.x],
30 [ds2[1], -ds2[0]]]) 33 [dp2.y, -dp2.x]])
31 B = matrix([[ds1[1]*s1[0][0]-ds1[0]*s1[1][0]], 34 B = matrix([[dp1.y*p1.x-dp1.x*p1.y],
32 [ds2[1]*s2[0][0]-ds2[0]*s2[1][0]]]) 35 [dp2.y*p3.x-dp2.x*p3.y]])
33 36
34 if linalg.det(A) == 0:#crossProduct(ds1, ds2) == 0: 37 if linalg.det(A) == 0:#crossProduct(ds1, ds2) == 0:
35 return None 38 return None
36 else: 39 else:
37 intersection = linalg.solve(A,B) 40 intersection = linalg.solve(A,B)
38 if (intersection[0,0] >= s1[0][0] and intersection[0,0] <= s1[0][1] 41 if (Interval(p1.x, p2.x, True).contains(intersection[0,0])
39 and intersection[0,0] >= s2[0][0] and intersection[0,0] <= s2[0][1] 42 and Interval(p3.x, p4.x, True).contains(intersection[0,0])
40 and intersection[1,0] >= s1[1][0] and intersection[1,0] <= s1[1][1] 43 and Interval(p1.y, p2.y, True).contains(intersection[1,0])
41 and intersection[1,0] >= s2[1][0] and intersection[1,0] <= s2[1][1]): 44 and Interval(p3.y, p4.y, True).contains(intersection[1,0])):
42 return [intersection[0,0], intersection[1,0]] 45 return Point(intersection[0,0], intersection[1,0])
43 else: 46 else:
44 return None 47 return None
45 48
46 def crossProduct(l1, l2): 49 def crossProduct(l1, l2):
47 return l1[0]*l2[1]-l1[1]*l2[0] 50 return l1[0]*l2[1]-l1[1]*l2[0]
48 51
49 # file I/O 52 #########################
53 # file I/O section
54 #########################
50 55
51 def openCheck(filename, option = 'r', quit = False): 56 def openCheck(filename, option = 'r', quit = False):
52 '''Open file filename in read mode by default 57 '''Open file filename in read mode by default
53 and checks it is open 58 and checks it is open
54 59
101 if (os.path.exists(filename)): 106 if (os.path.exists(filename)):
102 os.remove(filename) 107 os.remove(filename)
103 108
104 def invertHomography(homography): 109 def invertHomography(homography):
105 'Returns an inverted homography' 110 'Returns an inverted homography'
111 from numpy.linalg.linalg import inv
106 invH = inv(homography) 112 invH = inv(homography)
107 invH /= invH[2,2] 113 invH /= invH[2,2]
108 return invH 114 return invH
109 115
110 def project(homography, p): 116 def project(homography, p):
120 projected[1] /= projected[2] 126 projected[1] /= projected[2]
121 else: 127 else:
122 projected = p 128 projected = p
123 return projected[:2] 129 return projected[:2]
124 130
125 def printPoint(x,y): 131 def projectTrajectory(homography, trajectory):
126 return '(%f,%f)'%(x,y) 132 '''Projects a series of points in the format
133 [[x1, x2, ...],
134 [y1, y2, ...]]
135
136 Warning: not optimized, calls project()'''
137 projected = [[],[]]
138 for x, y in zip(trajectory[0], trajectory[1]):
139 p = [x,y]
140 pp = project(homography, p)
141 projected[0].append(pp[0])
142 projected[1].append(pp[1])
143 return projected
127 144
128 def plotPolygon(poly, options = ''): 145 def plotPolygon(poly, options = ''):
129 from numpy.core.multiarray import array 146 from numpy.core.multiarray import array
130 from matplotlib.pyplot import plot 147 from matplotlib.pyplot import plot
131 from shapely.geometry import Polygon 148 from shapely.geometry import Polygon