root / trunk / code / projects / traffic_navigation / intersectData.c @ 1966
History | View | Annotate | Download (3.02 KB)
| 1 | 1887 | pdeo | #include "intersectData.h" |
|---|---|---|---|
| 2 | 1966 | alevkoy | #include "../linefollowing/lineDrive.h" |
| 3 | 1887 | pdeo | |
| 4 | 1966 | alevkoy | int NodeData[NUM_FEATURES];
|
| 5 | |||
| 6 | 1887 | pdeo | /*
|
| 7 | * This function serves to make the code for the intersection database
|
||
| 8 | * as reuseable as possible. This part creates the implementation of the
|
||
| 9 | * database that can be copied over for all projects.
|
||
| 10 | *
|
||
| 11 | */
|
||
| 12 | |||
| 13 | 1912 | pdeo | // Does nothing if your indmax is out of bounds.
|
| 14 | 1958 | pdeo | void insertNode(int key, int featureid, int intersect_num, int node_type, int node_info){ |
| 15 | 1966 | alevkoy | if(key < NUM_FEATURES)
|
| 16 | NodeData[key] = (featureid<<12) + (intersect_num<<5) + (node_type<<2) + node_info; |
||
| 17 | return;
|
||
| 18 | 1887 | pdeo | } |
| 19 | |||
| 20 | 1958 | pdeo | void insertIntersection(int key, int intersect_num, int node_type, int intersection_position){ |
| 21 | 1966 | alevkoy | if(key < NUM_FEATURES)
|
| 22 | NodeData[key] = (INTERSECTION_NODE<<12) + (intersect_num<<5) + (node_type<<2) |
||
| 23 | + intersection_position; |
||
| 24 | return;
|
||
| 25 | 1912 | pdeo | } |
| 26 | |||
| 27 | 1958 | pdeo | void insertRoad(int key, int intersect_num, int node_type, int road_type){ |
| 28 | 1966 | alevkoy | if(key < NUM_FEATURES)
|
| 29 | NodeData[key] = (ROAD_NODE<<12) + (intersect_num<<5) + (node_type<<2) + road_type; |
||
| 30 | return;
|
||
| 31 | 1912 | pdeo | } |
| 32 | |||
| 33 | 1958 | pdeo | /*
|
| 34 | * This function serves to make the code for the intersection database
|
||
| 35 | * as reuseable as possible. This part initializes the data and is the
|
||
| 36 | * only one that needs to be rewritten for ech map.
|
||
| 37 | *
|
||
| 38 | */
|
||
| 39 | void initializeData(){
|
||
| 40 | 1966 | alevkoy | //insert all intersections that are in the map.
|
| 41 | //THIS DATA FOR THE DEMO MAP
|
||
| 42 | insertIntersection(0, 1, SINGLE, SACROSS); |
||
| 43 | insertIntersection(1, 1, SINGLE, SUP); |
||
| 44 | insertIntersection(2, 2, SINGLE, SUP); |
||
| 45 | insertIntersection(3, 2, SINGLE, SACROSS); |
||
| 46 | insertIntersection(4, 3, DOUBLE_C, CLEFT); |
||
| 47 | insertIntersection(5, 3, DOUBLE_C, CTOP); |
||
| 48 | insertIntersection(6, 3, DOUBLE_C, CRIGHT); |
||
| 49 | insertIntersection(7, 3, DOUBLE_C, CBOT); |
||
| 50 | insertIntersection(8, 5, DOUBLE_T, TLEFT); |
||
| 51 | insertIntersection(9, 5, DOUBLE_T, TMIDDLE); |
||
| 52 | insertIntersection(10, 5, DOUBLE_T, TRIGHT); |
||
| 53 | insertIntersection(11, 6, ON_RAMP, R_LEFT); |
||
| 54 | insertIntersection(12, 6, ON_RAMP, R_RAMP); |
||
| 55 | insertIntersection(13, 6, ON_RAMP, R_RIGHT); |
||
| 56 | insertIntersection(14, 4, OFF_RAMP, R_RIGHT); |
||
| 57 | insertIntersection(15, 4, OFF_RAMP, R_RAMP); |
||
| 58 | insertIntersection(16, 4, OFF_RAMP, R_LEFT); |
||
| 59 | return;
|
||
| 60 | 1958 | pdeo | } |
| 61 | |||
| 62 | 1887 | pdeo | // Functions to get data from the Database:
|
| 63 | |||
| 64 | 1912 | pdeo | int getFeatureId(int key) |
| 65 | {
|
||
| 66 | 1966 | alevkoy | if(key < NUM_FEATURES)
|
| 67 | return (NodeData[key]>>12)&15; |
||
| 68 | else return -1; |
||
| 69 | 1887 | pdeo | } |
| 70 | |||
| 71 | 1912 | pdeo | int getIntersectType(int key){ |
| 72 | 1966 | alevkoy | if(key < NUM_FEATURES && getFeatureId(key) == INTERSECTION_NODE)
|
| 73 | return (NodeData[key]>>2)&7; |
||
| 74 | else return -1; |
||
| 75 | 1908 | pdeo | } |
| 76 | 1906 | jdcooper | |
| 77 | 1908 | pdeo | int getIntersectPos(int key){ |
| 78 | 1966 | alevkoy | if(key < NUM_FEATURES && getFeatureId(key) == INTERSECTION_NODE)
|
| 79 | return (NodeData[key])&3; |
||
| 80 | else return -1; |
||
| 81 | 1906 | jdcooper | } |
| 82 | 1912 | pdeo | |
| 83 | 1908 | pdeo | int getRoadType(int key){ |
| 84 | 1966 | alevkoy | if(key < NUM_FEATURES && getFeatureId(key) == ROAD_NODE)
|
| 85 | return (NodeData[key]>>2)&7; |
||
| 86 | else return -1; |
||
| 87 | 1908 | pdeo | } |
| 88 | 1912 | pdeo | |
| 89 | 1908 | pdeo | int getRoadInfo(int key){ |
| 90 | 1966 | alevkoy | if(key < NUM_FEATURES && getFeatureId(key) == ROAD_NODE)
|
| 91 | return (NodeData[key])&3; |
||
| 92 | else return -1; |
||
| 93 | 1908 | pdeo | } |
| 94 | |||
| 95 | 1958 | pdeo | int getIntersectNum(int key){ |
| 96 | 1966 | alevkoy | if(key < NUM_FEATURES && getFeatureId(key) == INTERSECTION_NODE)
|
| 97 | return (NodeData[key]>>5)&127; |
||
| 98 | else return -1; |
||
| 99 | 1958 | pdeo | } |