static and tourel beacon
[aversive.git] / projects / microb2010 / tests / oa / graph.py
1 import sys, re, math
2 import numpy as np
3 import matplotlib
4 import matplotlib.path as mpath
5 import matplotlib.patches as mpatches
6 import matplotlib.pyplot as plt
7 from matplotlib.patches import Arrow, Circle, Wedge, Polygon
8 from matplotlib.collections import PatchCollection
9 import popen2, random
10
11 OFFSET_CORN_X=150
12 OFFSET_CORN_Y=222
13 STEP_CORN_X=225
14 STEP_CORN_Y=250
15
16 WAYPOINTS_NBX = 13
17 WAYPOINTS_NBY = 8
18
19 TYPE_WAYPOINT=0
20 TYPE_DANGEROUS=1
21 TYPE_WHITE_CORN=2
22 TYPE_BLACK_CORN=3
23 TYPE_OBSTACLE=4
24 TYPE_NEIGH=5
25
26 col = [TYPE_WAYPOINT] * WAYPOINTS_NBY
27 waypoints = [col[:] for i in range(WAYPOINTS_NBX)]
28 corn_table = [TYPE_WHITE_CORN]*18
29
30 corn_side_confs = [
31     [ 1, 4 ],
32     [ 0, 4 ],
33     [ 2, 4 ],
34     [ 2, 3 ],
35     [ 0, 3 ],
36     [ 1, 3 ],
37     [ 1, 6 ],
38     [ 0, 6 ],
39     [ 2, 6 ],
40     ]
41 corn_center_confs = [
42     [ 5, 8 ],
43     [ 7, 8 ],
44     [ 5, 9 ],
45     [ 7, 8 ],
46     ]
47
48 Path = mpath.Path
49
50 def mark_one_neigh(i, j):
51     global waypoints
52     if i < 0 or j < 0 or i >= WAYPOINTS_NBX or j >= WAYPOINTS_NBY:
53         return
54     if waypoints[i][j] != TYPE_WAYPOINT:
55         return
56     waypoints[i][j] = TYPE_NEIGH
57
58 def mark_all_neigh(i, j):
59     global waypoints
60     if waypoints[i][j] != TYPE_WAYPOINT:
61         return
62     waypoints[i][j] = TYPE_NEIGH
63     mark_one_neigh(i,j+1)
64     mark_one_neigh(i,j-1)
65     mark_one_neigh(i+1,j)
66     mark_one_neigh(i-1,j)
67     if i & 1 == 0:
68         mark_one_neigh(i+1,j-1)
69         mark_one_neigh(i-1,j-1)
70     else:
71         mark_one_neigh(i+1,j+1)
72         mark_one_neigh(i-1,j+1)
73
74 def pt2corn(i,j):
75     if i == 0 and j == 2: return 0
76     if i == 0 and j == 4: return 1
77     if i == 0 and j == 6: return 2
78     if i == 2 and j == 3: return 3
79     if i == 2 and j == 5: return 4
80     if i == 2 and j == 7: return 5
81     if i == 4 and j == 4: return 6
82     if i == 4 and j == 6: return 7
83     if i == 6 and j == 5: return 8
84     if i == 6 and j == 7: return 9
85     if i == 8 and j == 4: return 10
86     if i == 8 and j == 6: return 11
87     if i == 10 and j == 3: return 12
88     if i == 10 and j == 5: return 13
89     if i == 10 and j == 7: return 14
90     if i == 12 and j == 2: return 15
91     if i == 12 and j == 4: return 16
92     if i == 12 and j == 6: return 17
93     return -1
94
95 def corn_get_sym(i):
96     sym = [15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2]
97     return sym[i]
98
99 def init_corn_table(conf_side, conf_center):
100     global corn_table, corn_side_confs, corn_center_confs
101     print "confs = %d, %d"%(conf_side, conf_center)
102     for i in range(18):
103         if i in corn_side_confs[conf_side]:
104             corn_table[i] = TYPE_BLACK_CORN
105             continue
106         if corn_get_sym(i) in corn_side_confs[conf_side]:
107             corn_table[i] = TYPE_BLACK_CORN
108             continue
109         if i in corn_center_confs[conf_center]:
110             corn_table[i] = TYPE_BLACK_CORN
111             continue
112         if corn_get_sym(i) in corn_center_confs[conf_center]:
113             corn_table[i] = TYPE_BLACK_CORN
114             continue
115         corn_table[i] = TYPE_WHITE_CORN
116             
117 def init_waypoints():
118     global waypoints, corn_table
119
120     for i in range(WAYPOINTS_NBX):
121         for j in range(WAYPOINTS_NBY):
122             # corn
123             c = pt2corn(i, j)
124             if c >= 0:
125                 waypoints[i][j] = corn_table[c]
126                 continue
127             # too close of border
128             if (i & 1) == 1 and j == WAYPOINTS_NBY -1:
129                 waypoints[i][j] = TYPE_OBSTACLE
130                 continue
131             # hill
132             if i >= 2 and i < WAYPOINTS_NBX - 2 and j < 2:
133                 waypoints[i][j] = TYPE_OBSTACLE
134                 continue
135             # dangerous points
136             if i == 0 or i == WAYPOINTS_NBX-1:
137                 waypoints[i][j] = TYPE_DANGEROUS
138                 continue
139             if (i&1) == 0 and j == WAYPOINTS_NBY-1:
140                 waypoints[i][j] = TYPE_DANGEROUS
141                 continue
142
143             waypoints[i][j] = TYPE_WAYPOINT
144             
145         print i, waypoints[i]
146     return waypoints
147
148 def build_poly(ptlist):
149     polydata = []
150     polydata.append((Path.MOVETO, (ptlist[0])))
151     for pt in ptlist[1:]:
152         polydata.append((Path.LINETO, (pt)))
153     polydata.append((Path.CLOSEPOLY, (ptlist[0])))
154     codes, verts = zip(*polydata)
155     poly = mpath.Path(verts, codes)
156     x, y = zip(*poly.vertices)
157     return x,y
158
159 def build_path(ptlist):
160     polydata = []
161     polydata.append((Path.MOVETO, (ptlist[0])))
162     for pt in ptlist[1:]:
163         polydata.append((Path.LINETO, (pt)))
164     codes, verts = zip(*polydata)
165     poly = mpath.Path(verts, codes)
166     x, y = zip(*poly.vertices)
167     return x,y
168
169 def nearest_corn(x, y):
170     OFFX=150
171     OFFY=222
172     STEPX=450
173     STEPY=500
174
175     x -= OFFX
176     x += STEPX/2
177     x /= STEPX
178
179     y -= OFFY
180     y += STEPY/2
181     if (x & 1) == 1:
182         y -= STEPY/2
183     y /= STEPY
184
185     i = (x * 2)
186     j = (y * 2) + (x & 1)
187     if i >= WAYPOINTS_NBX:
188         return None
189     if j >= WAYPOINTS_NBY:
190         return None
191
192     if (i & 1) == 0:
193         y = OFFSET_CORN_Y
194     else:
195         y = OFFSET_CORN_Y + STEP_CORN_Y/2
196     y += (j * STEP_CORN_Y)
197
198     x = OFFSET_CORN_X + (i * STEP_CORN_X)
199
200     return x, y
201
202
203 def build_area(ax):
204     # area
205     x,y = build_poly([(0,0), (3000,0), (3000,2100), (0,2100)])
206     ax.plot(x, y, 'g-')
207
208     x,y = build_poly([(0,0), (0,500), (500,500), (500,0)])
209     ax.plot(x, y, 'y-')
210
211     x,y = build_poly([(3000,0), (3000,500), (2500,500), (2500,0)])
212     ax.plot(x, y, 'b-')
213
214     x,y = build_poly([(740,0), (740,500), (2260,500), (2260,0)])
215     ax.plot(x, y, 'g--')
216
217     """
218     # courbes a 2 balles
219     x,y = build_path([(375,0), (375,2050)])
220     ax.plot(x, y, 'r-')
221
222     x,y = build_path([(150,1972), (2850,472)])
223     ax.plot(x, y, 'r-')
224
225     x,y = build_path([(0,1722), (3000,1722)])
226     ax.plot(x, y, 'r-')
227
228     x,y = build_path([(150,972), (1950,1972)])
229     ax.plot(x, y, 'r-')
230
231     acoef = (2850-150)/5.
232     bcoef = (472-1972)/5.
233     x,y = build_path([(600,1972), (600+bcoef, 1972-acoef)])
234     ax.plot(x, y, 'r-')
235
236     p = PatchCollection([Circle((2400,972), 225)],
237                         cmap=matplotlib.cm.jet,
238                         alpha=0.5, facecolor=(1.,0.,0.))
239     ax.add_collection(p)
240
241     """
242
243     """
244     #courbes a 2 balles 50
245     for xx in range(0, 3000, 100):
246         print xx
247         for yy in range(0, 2100, 100):
248             pouet = nearest_corn(xx, yy)
249             if pouet == None:
250                 continue
251             xxx, yyy = pouet
252             x,y = build_path([(xx,yy), (xxx,yyy)])
253             ax.plot(x, y, 'r-')
254     """
255
256    # limit
257     #x,y = build_poly([(250,250), (2750,250), (2750,1850), (250,1850)])
258     #ax.plot(x, y, 'g--')
259
260     xtick = [i * STEP_CORN_X + OFFSET_CORN_X for i in range(WAYPOINTS_NBX)]
261     ax.set_xticks(xtick)
262     ytick = [(i-1) * STEP_CORN_Y/2 + OFFSET_CORN_Y for i in range(WAYPOINTS_NBY*2)]
263     ax.set_yticks(ytick)
264
265     init_corn_table(random.randint(0,8), random.randint(0,3))
266
267     waypoints = init_waypoints()
268     mark_all_neigh(5,6)
269
270     wcorn = []
271     bcorn = []
272     points = []
273     dpoints = []
274     neighs = []
275
276     i = 0
277     j = 0
278     x = OFFSET_CORN_X
279     while x < 3000:
280         if (i & 1) == 0:
281             y = OFFSET_CORN_Y
282         else:
283             y = OFFSET_CORN_Y + STEP_CORN_Y/2
284         j = 0
285         while y < 2100:
286
287             if waypoints[i][j] == TYPE_WHITE_CORN:
288                 wcorn.append(Circle((x, y), 25))
289             if waypoints[i][j] == TYPE_BLACK_CORN:
290                 bcorn.append(Circle((x, y), 25))
291             elif waypoints[i][j] == TYPE_WAYPOINT:
292                 points.append(Circle((x, y), 10))
293             elif waypoints[i][j] == TYPE_DANGEROUS:
294                 dpoints.append(Circle((x, y), 10))
295             elif waypoints[i][j] == TYPE_NEIGH:
296                 neighs.append(Circle((x, y), 10))
297
298             j += 1
299             y += STEP_CORN_Y
300         i += 1
301         x += STEP_CORN_X
302
303     p = PatchCollection(wcorn, cmap=matplotlib.cm.jet,
304                         alpha=0.5, facecolor=(.5,.5,1))
305     ax.add_collection(p)
306
307     p = PatchCollection(bcorn, cmap=matplotlib.cm.jet,
308                         alpha=0.7, facecolor=(0,0,0))
309     ax.add_collection(p)
310
311     p = PatchCollection(points, cmap=matplotlib.cm.jet,
312                         alpha=0.3, facecolor=(1,1,0))
313     ax.add_collection(p)
314
315     p = PatchCollection(dpoints, cmap=matplotlib.cm.jet,
316                         alpha=0.5, facecolor=(1,0,1))
317     ax.add_collection(p)
318
319     p = PatchCollection(neighs, cmap=matplotlib.cm.jet,
320                         alpha=1., facecolor=(1,0,0),
321                         edgecolor=(1,0,0))
322     ax.add_collection(p)
323
324
325 def graph(filename):
326     fig = plt.figure()
327     ax = fig.add_subplot(111, aspect='equal')
328
329     build_area(ax)
330
331     ax.grid(color = (0.3, 0.3, 0.3))
332     ax.set_xlim(-100, 3100)
333     ax.set_ylim(-100, 2200)
334     #ax.set_xlim(0, 825)
335     #ax.set_ylim(1472, 1972)
336     #ax.set_title('spline paths')
337     #plt.show()
338     fig.savefig(filename)
339
340 graph("test.png")