Caddy
A 2005 Roborodentia entry with vision and path planning capability
 All Data Structures Files Functions Variables Typedefs Macros Pages
node_list.c
Go to the documentation of this file.
1 /*
2  * This file is part of Caddy.
3  *
4  * Caddy is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License as published by
6  * the Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8  *
9  * Caddy is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with Caddy. If not, see <http://www.gnu.org/licenses/>.
16  */
18 #include "node_list.h"
19 
20 // avr-libc
21 #include <string.h>
22 
23 inline bool isJunction(uint8_t nodeNum)
24 {
25  return (nodeNum >= JUNCTION_MIN && nodeNum <= JUNCTION_MAX);
26 }
27 
28 inline bool isBallNode(uint8_t nodeNum)
29 {
30  return (nodeNum >= BALL_NODE_MIN && nodeNum <= BALL_NODE_MAX);
31 }
32 
33 uint8_t getCostToNode(GraphNodeType *node, uint8_t nodeNum)
34 {
35  uint8_t i;
36  for (i = 0; i < node->numAdjNodes; i++)
37  {
38  if (node->adjNodes[i] == nodeNum)
39  {
40  return node->adjCosts[i];
41  }
42  }
43  return 0;
44 }
45 
46 uint8_t getNodeAtHeading(GraphNodeType *node, int8_t heading)
47 {
48  uint8_t i;
49  for (i = 0; i < node->numAdjNodes; i++)
50  {
51  if (node->adjHeadings[i] == heading)
52  {
53  return node->adjNodes[i];
54  }
55  }
56  return 0;
57 }
58 
59 void getNode(uint8_t nodeNum, GraphNodeType *node)
60 {
61  // Bounds check on input node number
62  if (nodeNum >= NUM_NODES)
63  {
64  node->numAdjNodes = 0;
65  return;
66  }
67 
68  switch (nodeNum)
69  {
70  case 0: // START_NODE
71  node->numAdjNodes = 1;
72  node-> adjNodes[0] = 21;
73  node-> adjCosts[0] = 9;
74  node->adjHeadings[0] = -64;
75  break;
76  case 1: // First ball node
77  node->numAdjNodes = 2;
78  node-> adjNodes[0] = 21;
79  node-> adjNodes[1] = 22;
80  node-> adjCosts[0] = 4;
81  node-> adjCosts[1] = 4;
82  node->adjHeadings[0] = -128;
83  node->adjHeadings[1] = 0;
84  break;
85  case 2:
86  node->numAdjNodes = 2;
87  node-> adjNodes[0] = 22;
88  node-> adjNodes[1] = 23;
89  node-> adjCosts[0] = 2;
90  node-> adjCosts[1] = 2;
91  node->adjHeadings[0] = -64;
92  node->adjHeadings[1] = 64;
93  break;
94  case 3:
95  node->numAdjNodes = 2;
96  node-> adjNodes[0] = 23;
97  node-> adjNodes[1] = 24;
98  node-> adjCosts[0] = 2;
99  node-> adjCosts[1] = 2;
100  node->adjHeadings[0] = 0;
101  node->adjHeadings[1] = -128;
102  break;
103  case 4:
104  node->numAdjNodes = 2;
105  node-> adjNodes[0] = 24;
106  node-> adjNodes[1] = 25;
107  node-> adjCosts[0] = 3;
108  node-> adjCosts[1] = 3;
109  node->adjHeadings[0] = -64;
110  node->adjHeadings[1] = 64;
111  break;
112  case 5:
113  node->numAdjNodes = 2;
114  node-> adjNodes[0] = 26;
115  node-> adjNodes[1] = 41;
116  node-> adjCosts[0] = 2;
117  node-> adjCosts[1] = 2;
118  node->adjHeadings[0] = -64;
119  node->adjHeadings[1] = 64;
120  break;
121  case 6:
122  node->numAdjNodes = 2;
123  node-> adjNodes[0] = 27;
124  node-> adjNodes[1] = 28;
125  node-> adjCosts[0] = 2;
126  node-> adjCosts[1] = 2;
127  node->adjHeadings[0] = 64;
128  node->adjHeadings[1] = -64;
129  break;
130  case 7:
131  node->numAdjNodes = 2;
132  node-> adjNodes[0] = 27;
133  node-> adjNodes[1] = 32;
134  node-> adjCosts[0] = 3;
135  node-> adjCosts[1] = 3;
136  node->adjHeadings[0] = -128;
137  node->adjHeadings[1] = 0;
138  break;
139  case 8:
140  node->numAdjNodes = 2;
141  node-> adjNodes[0] = 28;
142  node-> adjNodes[1] = 30;
143  node-> adjCosts[0] = 3;
144  node-> adjCosts[1] = 3;
145  node->adjHeadings[0] = -128;
146  node->adjHeadings[1] = 0;
147  break;
148  case 9:
149  node->numAdjNodes = 2;
150  node-> adjNodes[0] = 22;
151  node-> adjNodes[1] = 29;
152  node-> adjCosts[0] = 3;
153  node-> adjCosts[1] = 3;
154  node->adjHeadings[0] = -128;
155  node->adjHeadings[1] = 0;
156  break;
157  case 10:
158  node->numAdjNodes = 2;
159  node-> adjNodes[0] = 29;
160  node-> adjNodes[1] = 30;
161  node-> adjCosts[0] = 3;
162  node-> adjCosts[1] = 3;
163  node->adjHeadings[0] = -64;
164  node->adjHeadings[1] = 64;
165  break;
166  case 11:
167  node->numAdjNodes = 2;
168  node-> adjNodes[0] = 12;
169  node-> adjNodes[1] = 29;
170  node-> adjCosts[0] = 4;
171  node-> adjCosts[1] = 2;
172  node->adjHeadings[0] = 0;
173  node->adjHeadings[1] = -128;
174  break;
175  case 12:
176  node->numAdjNodes = 2;
177  node-> adjNodes[0] = 11;
178  node-> adjNodes[1] = 33;
179  node-> adjCosts[0] = 4;
180  node-> adjCosts[1] = 2;
181  node->adjHeadings[0] = -128;
182  node->adjHeadings[1] = 0;
183  break;
184  case 13:
185  node->numAdjNodes = 2;
186  node-> adjNodes[0] = 33;
187  node-> adjNodes[1] = 34;
188  node-> adjCosts[0] = 2;
189  node-> adjCosts[1] = 1;
190  node->adjHeadings[0] = -64;
191  node->adjHeadings[1] = 64;
192  break;
193  case 14:
194  node->numAdjNodes = 2;
195  node-> adjNodes[0] = 15;
196  node-> adjNodes[1] = 34;
197  node-> adjCosts[0] = 3;
198  node-> adjCosts[1] = 4;
199  node->adjHeadings[0] = S_EAST;
200  node->adjHeadings[1] = N_WEST;
201  break;
202  case 15:
203  node->numAdjNodes = 2;
204  node-> adjNodes[0] = 14;
205  node-> adjNodes[1] = 31;
206  node-> adjCosts[0] = 3;
207  node-> adjCosts[1] = 4;
208  node->adjHeadings[0] = N_WEST;
209  node->adjHeadings[1] = S_EAST;
210  break;
211  case 16:
212  node->numAdjNodes = 2;
213  node-> adjNodes[0] = 32;
214  node-> adjNodes[1] = 40;
215  node-> adjCosts[0] = 2;
216  node-> adjCosts[1] = 2;
217  node->adjHeadings[0] = -64;
218  node->adjHeadings[1] = 64;
219  break;
220  case 17:
221  node->numAdjNodes = 2;
222  node-> adjNodes[0] = 34;
223  node-> adjNodes[1] = 35;
224  node-> adjCosts[0] = 3;
225  node-> adjCosts[1] = 3;
226  node->adjHeadings[0] = -64;
227  node->adjHeadings[1] = 64;
228  break;
229  case 18:
230  node->numAdjNodes = 2;
231  node-> adjNodes[0] = 39;
232  node-> adjNodes[1] = 40;
233  node-> adjCosts[0] = 2;
234  node-> adjCosts[1] = 2;
235  node->adjHeadings[0] = 0;
236  node->adjHeadings[1] = -128;
237  break;
238  case 19:
239  node->numAdjNodes = 2;
240  node-> adjNodes[0] = 20;
241  node-> adjNodes[1] = 40;
242  node-> adjCosts[0] = 4;
243  node-> adjCosts[1] = 2;
244  node->adjHeadings[0] = -128;
245  node->adjHeadings[1] = 0;
246  break;
247  case 20:
248  node->numAdjNodes = 2;
249  node-> adjNodes[0] = 19;
250  node-> adjNodes[1] = 41;
251  node-> adjCosts[0] = 4;
252  node-> adjCosts[1] = 2;
253  node->adjHeadings[0] = 0;
254  node->adjHeadings[1] = -128;
255  break;
256  case 21: // First Junction Node
257  node->numAdjNodes = 2;
258  node-> adjNodes[0] = 0;
259  node-> adjNodes[1] = 1;
260  node-> adjCosts[0] = 9;
261  node-> adjCosts[1] = 4;
262  node->adjHeadings[0] = 64;
263  node->adjHeadings[1] = 0;
264  break;
265  case 22:
266  node->numAdjNodes = 3;
267  node-> adjNodes[0] = 1;
268  node-> adjNodes[1] = 2;
269  node-> adjNodes[2] = 9;
270  node-> adjCosts[0] = 4;
271  node-> adjCosts[1] = 2;
272  node-> adjCosts[2] = 3;
273  node->adjHeadings[0] = -128;
274  node->adjHeadings[1] = 64;
275  node->adjHeadings[2] = 0;
276  break;
277  case 23:
278  node->numAdjNodes = 3;
279  node-> adjNodes[0] = 2;
280  node-> adjNodes[1] = 3;
281  node-> adjNodes[2] = 28;
282  node-> adjCosts[0] = 2;
283  node-> adjCosts[1] = 2;
284  node-> adjCosts[2] = 2;
285  node->adjHeadings[0] = -64;
286  node->adjHeadings[1] = -128;
287  node->adjHeadings[2] = 64;
288  break;
289  case 24: // BONUS_BALL_1
290  node->numAdjNodes = 2;
291  node-> adjNodes[0] = 3;
292  node-> adjNodes[1] = 4;
293  node-> adjCosts[0] = 2;
294  node-> adjCosts[1] = 3;
295  node->adjHeadings[0] = 0;
296  node->adjHeadings[1] = 64;
297  break;
298  case 25:
299  node->numAdjNodes = 2;
300  node-> adjNodes[0] = 4;
301  node-> adjNodes[1] = 26;
302  node-> adjCosts[0] = 3;
303  node-> adjCosts[1] = 2;
304  node->adjHeadings[0] = -64;
305  node->adjHeadings[1] = 0;
306  break;
307  case 26:
308  node->numAdjNodes = 3;
309  node-> adjNodes[0] = 5;
310  node-> adjNodes[1] = 25;
311  node-> adjNodes[2] = 27;
312  node-> adjCosts[0] = 2;
313  node-> adjCosts[1] = 2;
314  node-> adjCosts[2] = 2;
315  node->adjHeadings[0] = 64;
316  node->adjHeadings[1] = -128;
317  node->adjHeadings[2] = 0;
318  break;
319  case 27:
320  node->numAdjNodes = 3;
321  node-> adjNodes[0] = 6;
322  node-> adjNodes[1] = 7;
323  node-> adjNodes[2] = 26;
324  node-> adjCosts[0] = 2;
325  node-> adjCosts[1] = 3;
326  node-> adjCosts[2] = 2;
327  node->adjHeadings[0] = -64;
328  node->adjHeadings[1] = 0;
329  node->adjHeadings[2] = -128;
330  break;
331  case 28:
332  node->numAdjNodes = 3;
333  node-> adjNodes[0] = 6;
334  node-> adjNodes[1] = 8;
335  node-> adjNodes[2] = 23;
336  node-> adjCosts[0] = 2;
337  node-> adjCosts[1] = 3;
338  node-> adjCosts[2] = 2;
339  node->adjHeadings[0] = 64;
340  node->adjHeadings[1] = 0;
341  node->adjHeadings[2] = -64;
342  break;
343  case 29:
344  node->numAdjNodes = 3;
345  node-> adjNodes[0] = 9;
346  node-> adjNodes[1] = 10;
347  node-> adjNodes[2] = 11;
348  node-> adjCosts[0] = 3;
349  node-> adjCosts[1] = 3;
350  node-> adjCosts[2] = 2;
351  node->adjHeadings[0] = -128;
352  node->adjHeadings[1] = 64;
353  node->adjHeadings[2] = 0;
354  break;
355  case 30: // BONUS_BALL_2
356  node->numAdjNodes = 3;
357  node-> adjNodes[0] = 8;
358  node-> adjNodes[1] = 10;
359  node-> adjNodes[2] = 31;
360  node-> adjCosts[0] = 3;
361  node-> adjCosts[1] = 3;
362  node-> adjCosts[2] = 3;
363  node->adjHeadings[0] = -128;
364  node->adjHeadings[1] = -64;
365  node->adjHeadings[2] = 64;
366  break;
367  case 31:
368  node->numAdjNodes = 3;
369  node-> adjNodes[0] = 15;
370  node-> adjNodes[1] = 30;
371  node-> adjNodes[2] = 32;
372  node-> adjCosts[0] = 4;
373  node-> adjCosts[1] = 3;
374  node-> adjCosts[2] = 1;
375  node->adjHeadings[0] = N_WEST;
376  node->adjHeadings[1] = -64;
377  node->adjHeadings[2] = 64;
378  break;
379  case 32:
380  node->numAdjNodes = 3;
381  node-> adjNodes[0] = 7;
382  node-> adjNodes[1] = 16;
383  node-> adjNodes[2] = 31;
384  node-> adjCosts[0] = 3;
385  node-> adjCosts[1] = 2;
386  node-> adjCosts[2] = 1;
387  node->adjHeadings[0] = -128;
388  node->adjHeadings[1] = 64;
389  node->adjHeadings[2] = -64;
390  break;
391  case 33:
392  node->numAdjNodes = 2;
393  node-> adjNodes[0] = 12;
394  node-> adjNodes[1] = 13;
395  node-> adjCosts[0] = 2;
396  node-> adjCosts[1] = 2;
397  node->adjHeadings[0] = -128;
398  node->adjHeadings[1] = 64;
399  break;
400  case 34:
401  node->numAdjNodes = 3;
402  node-> adjNodes[0] = 13;
403  node-> adjNodes[1] = 14;
404  node-> adjNodes[2] = 17;
405  node-> adjCosts[0] = 1;
406  node-> adjCosts[1] = 4;
407  node-> adjCosts[2] = 3;
408  node->adjHeadings[0] = -64;
409  node->adjHeadings[1] = S_EAST;
410  node->adjHeadings[2] = 64;
411  break;
412  case 35:
413  node->numAdjNodes = 2;
414  node-> adjNodes[0] = 17;
415  node-> adjNodes[1] = 36;
416  node-> adjCosts[0] = 3;
417  node-> adjCosts[1] = 2;
418  node->adjHeadings[0] = -64;
419  node->adjHeadings[1] = -128;
420  break;
421  case 36:
422  node->numAdjNodes = 2;
423  node-> adjNodes[0] = 35;
424  node-> adjNodes[1] = 37;
425  node-> adjCosts[0] = 2;
426  node-> adjCosts[1] = 2;
427  node->adjHeadings[0] = 0;
428  node->adjHeadings[1] = 64;
429  break;
430  case 37:
431  node->numAdjNodes = 2;
432  node-> adjNodes[0] = 36;
433  node-> adjNodes[1] = 38;
434  node-> adjCosts[0] = 2;
435  node-> adjCosts[1] = 2;
436  node->adjHeadings[0] = -64;
437  node->adjHeadings[1] = -128;
438  break;
439  case 38:
440  node->numAdjNodes = 2;
441  node-> adjNodes[0] = 37;
442  node-> adjNodes[1] = 39;
443  node-> adjCosts[0] = 2;
444  node-> adjCosts[1] = 2;
445  node->adjHeadings[0] = 0;
446  node->adjHeadings[1] = 64;
447  break;
448  case 39:
449  node->numAdjNodes = 2;
450  node-> adjNodes[0] = 18;
451  node-> adjNodes[1] = 38;
452  node-> adjCosts[0] = 2;
453  node-> adjCosts[1] = 2;
454  node->adjHeadings[0] = -128;
455  node->adjHeadings[1] = -64;
456  break;
457  case 40:
458  node->numAdjNodes = 3;
459  node-> adjNodes[0] = 16;
460  node-> adjNodes[1] = 18;
461  node-> adjNodes[2] = 19;
462  node-> adjCosts[0] = 2;
463  node-> adjCosts[1] = 2;
464  node-> adjCosts[2] = 2;
465  node->adjHeadings[0] = -64;
466  node->adjHeadings[1] = 0;
467  node->adjHeadings[2] = -128;
468  break;
469  case 41:
470  node->numAdjNodes = 3;
471  node-> adjNodes[0] = 5;
472  node-> adjNodes[1] = 20;
473  node-> adjNodes[2] = 42;
474  node-> adjCosts[0] = 2;
475  node-> adjCosts[1] = 2;
476  node-> adjCosts[2] = 5;
477  node->adjHeadings[0] = -64;
478  node->adjHeadings[1] = 0;
479  node->adjHeadings[2] = -128;
480  break;
481  case 42: // STOP_NODE
482  node->numAdjNodes = 1;
483  node-> adjNodes[0] = 41;
484  node-> adjCosts[0] = 5;
485  node->adjHeadings[0] = 0;
486  break;
487  }
488 }
Generated on Wed Apr 10 2013 22:02:29 for Caddy by   doxygen 1.8.1.2