rendered paste body//pathfindingwaypoint map[WAYPOINTS];int map_counter=0; //anzahl der mapeinträge (indizes: -1)void find_possible_ways(int point, char *points, int *num_points){ int i; for(i=point;i<map_counter;i++) { if((map[i].x_start == map[point].x_stop && map[i].y_start == map[point].y_stop)) { points[(*num_points)++]=i; } }}void arraycopy(char *from,char *to,int anzahl){ int i; for(i=0;i<anzahl;i++) { to[i]=from[i]; }}void checkwege(char wege[MAXWEGE][WAYPOINTS],int len_weg[MAXWEGE],int *num_wege, int aktueller_weg){ char possible_ways[3]; int num_points=0,i,len_tmp=0; char tmp[WAYPOINTS]; int aktueller_punkt; aktueller_punkt = wege[aktueller_weg][len_weg[aktueller_weg]-1]; find_possible_ways(aktueller_punkt,possible_ways,&num_points); if((num_points == 0) || (aktueller_punkt >= map_counter)) { //fertig } else { arraycopy(wege[aktueller_weg],tmp, len_weg[aktueller_weg]); len_tmp=len_weg[aktueller_weg]; wege[aktueller_weg][len_weg[aktueller_weg]++]=possible_ways[0]; checkwege(wege,len_weg,num_wege, aktueller_weg); for(i=1;i<num_points;i++) { arraycopy(tmp,wege[(*num_wege)++],len_tmp); wege[(*num_wege)-1][len_tmp]=possible_ways[i]; len_weg[(*num_wege)-1] = len_tmp+1; checkwege(wege,len_weg,num_wege, *num_wege-1); } }}long int length_of_weg(char dieser_weg[WAYPOINTS],int hops) { long int laenge=0; int i,h; for(i=0;i<hops;i++) { h=(map[(int)dieser_weg[i]].x_start-map[(int)dieser_weg[i]].x_stop); laenge+=(h*h); h=(map[(int)dieser_weg[i]].y_start-map[(int)dieser_weg[i]].y_stop); laenge+=(h*h); } return laenge;}void addwp(unsigned int start, unsigned int end, int direction){ if((map_counter) <= WAYPOINTS) { unsigned int difference=(end-start); signed char x_tmp=0,y_tmp=0; int i,found=0; map[map_counter].direction=direction; map[map_counter].x_start=map[map_counter-1].x_stop; map[map_counter].y_start=map[map_counter-1].y_stop; if(direction == MAP_LEFT) { x_tmp=map[map_counter-1].x_stop-difference; y_tmp=map[map_counter-1].y_stop; } else if(direction == MAP_RIGHT) { x_tmp=map[map_counter-1].x_stop+difference; y_tmp=map[map_counter-1].y_stop; } else if(direction == MAP_UP) { x_tmp=map[map_counter-1].x_stop; y_tmp=map[map_counter-1].y_stop+difference; } else if(direction == MAP_DOWN) { x_tmp=map[map_counter-1].x_stop; y_tmp=map[map_counter-1].y_stop-difference; } for(i=0;i<map_counter;i++) { if((abs(map[i].x_start-x_tmp) < (TURNLIMIT-2)) && abs(map[i].y_start-y_tmp) < (TURNLIMIT-2)) { found=1; map[map_counter].x_stop=map[i].x_start; map[map_counter].y_stop=map[i].y_start; break; } else if(0) { ledsOff(); motor(MOT_LEFT,0); motor(MOT_RIGHT,0); ledOn(2); waitForButton(2); showBin(i); waitForButton(3); showBin(map[i].x_start); waitForButton(3); showBin(x_tmp); waitForButton(3); showBin(map[i].y_start); waitForButton(3); showBin(y_tmp); waitForButton(3); } } if(!found) { map[map_counter].x_stop=x_tmp; map[map_counter].y_stop=y_tmp; } map_counter++; } else { error(1); }}bool getdirection(int index1, int index2,int direction){ if(direction == MAP_UP) { if((map[index1].direction == map[index2].direction) && map[index1].direction == MAP_UP) { return true; } } else if(direction == MAP_RIGHT) { if((map[index1].direction == MAP_UP && map[index2].direction==MAP_RIGHT) || (map[index1].direction == MAP_RIGHT && map[index2].direction==MAP_DOWN) || (map[index1].direction == MAP_DOWN && map[index2].direction==MAP_LEFT) || (map[index1].direction == MAP_LEFT && map[index2].direction==MAP_UP)) { return true; } } else if(direction == MAP_LEFT) { if((map[index1].direction == MAP_UP && map[index2].direction==MAP_LEFT) || (map[index1].direction == MAP_LEFT && map[index2].direction==MAP_DOWN) || (map[index1].direction == MAP_DOWN && map[index2].direction==MAP_RIGHT) || (map[index1].direction == MAP_RIGHT && map[index2].direction==MAP_UP)) { return true; } } return false;}int where_should_i_go(int index1,int index2){ if((map[index1].direction == MAP_UP && map[index2].direction==MAP_LEFT) || (map[index1].direction == MAP_LEFT && map[index2].direction==MAP_DOWN) || (map[index1].direction == MAP_DOWN && map[index2].direction==MAP_RIGHT) || (map[index1].direction == MAP_RIGHT && map[index2].direction==MAP_UP)) { return MAP_LEFT; } else if((map[index1].direction == MAP_UP && map[index2].direction==MAP_RIGHT) || (map[index1].direction == MAP_RIGHT && map[index2].direction==MAP_DOWN) || (map[index1].direction == MAP_DOWN && map[index2].direction==MAP_LEFT) || (map[index1].direction == MAP_LEFT && map[index2].direction==MAP_UP)) { return MAP_RIGHT; } else return MAP_UP;}