#include "map.h"
#include <math.h>
#include <stdlib.h>
+//#include "robot.h"
/**
* @name A* Constants
void initGraphStructure(void);
int getPathLength(int xstart, int ystart, int xgoal, int ygoal);
+void init_bitmap_straight(bitmap_dim_straight bitmap_straight[]);
+bool check_bot_position_streight(int x_0, int y_0, bitmap_dim_straight bitmap_straight[], int angle);
+//int get_angle_between_cells(int x_0, int y_0, int x_1, int y_1);
+//int get_round_actual_angle();
+
+void init_bitmap_straight(bitmap_dim_straight bitmap_straight[]){
+
+ bitmap_straight[0].x_min=-BITMAP_AB;
+ bitmap_straight[0].x_max=BITMAP_AF;
+ bitmap_straight[0].y_min=-BITMAP_W2;
+ bitmap_straight[0].y_max=BITMAP_W2;
+
+ bitmap_straight[1].x_min=-BITMAP_W2;
+ bitmap_straight[1].x_max=BITMAP_W2;
+ bitmap_straight[1].y_min=-BITMAP_AB;
+ bitmap_straight[1].y_max=BITMAP_AF;
+
+ bitmap_straight[2].x_min=-BITMAP_AF;
+ bitmap_straight[2].x_max=BITMAP_AB;
+ bitmap_straight[2].y_min=-BITMAP_W2;
+ bitmap_straight[2].y_max=BITMAP_W2;
+
+ bitmap_straight[3].x_min=-BITMAP_W2;
+ bitmap_straight[3].x_max=BITMAP_W2;
+ bitmap_straight[3].y_min=-BITMAP_AF;
+ bitmap_straight[3].y_max=BITMAP_AB;
+
+}
+
+bool check_bot_position_streight(int x_0, int y_0, bitmap_dim_straight bitmap_straight[], int angle){
+
+ if (angle==0){ //angle must be in range 90-360
+ angle=360;
+ }
+
+ angle=360/angle;
+
+ int x,y;
+
+ for(x=x_0-bitmap_straight[angle].x_min; x<=x_0+bitmap_straight[angle].x_max;x++){
+ for(y=y_0-bitmap_straight[angle].y_min; y<=y_0+bitmap_straight[angle].y_max;y++){
+ if (!ShmapIsFreeCell(x,y)){ //1 no obstacle,-1 no in map, 0 otherwise
+ return false;
+ }
+ }
+ }
+
+ return true;
+}
+
+// int get_angle_between_cells(int x_0, int y_0, int x_1, int y_1){
+//
+// double angle=0;
+//
+// angle= atan2((double)(y_1-y_0),(double)(x_1-x_0)); //angle in range -pi, pi
+// angle=(angle*360)/(2*pi()); //angle in deg.
+//
+// return (int) angle;
+// }
+
+// int get_round_actual_angle(){
+//
+// double x, y, phi;
+// int phi_int;
+//
+// robot_get_est_pos(&x,&y,&phi);
+//
+// phi_int=(int)phi;
+// phi_int=phi_int-(phi_int % 45);
+//
+// return phi_int;
+// }
+
/**
* Returns X and Y index of a graph cell identified by a pointer.
*
}
+
+int aAlgorithm_shape(double xstart_real,double ystart_real, double xgoal_real, double ygoal_real, GraphMapCell **original_path)
+{
+ /**
+ * prioritySet : Set of nodes to process order by heuristic distance
+ */
+#ifndef CONFIG_PP_USES_RBTREE
+ graph_queue = NULL;
+#else
+ graph_queue = RB_ROOT;
+#endif
+ /**
+ * nbest : Best node to reach the goal
+ */
+ GraphMapCell *nbest, *start, *goal, *contig;
+ int i, x, y;
+ int xcontig, ycontig, xstart, ystart, xgoal, ygoal;
+ struct map *map = ShmapIsMapInit();
+ bool valid;
+
+ bitmap_dim_straight bitmap_straight[4];
+
+ init_bitmap_straight(bitmap_straight);
+
+ if (!map) return -1;
+ // Transform real data in cell data
+ ShmapPoint2Cell(xstart_real, ystart_real, &xstart, &ystart, &valid);
+ if (!valid)
+ return -1;
+ start = &graph[ystart][xstart];
+ ShmapPoint2Cell(xgoal_real, ygoal_real, &xgoal, &ygoal, &valid);
+ if (!valid)
+ return -1;
+ goal = &graph[ygoal][xgoal];
+
+ /**@{*/
+ /// <ol>
+ /// <li> Init graph structure with default values.
+ initGraphStructure();
+ /// <li> Calculate Heuristic distances.
+ calculateMapHeuristic(xgoal, ygoal);
+
+ /// <li> Initial values : put start node in queue.
+ start->g = 0;
+ start->f = start->g + start->h;
+ start->backpointer = start;
+ pushNodeInOrder(start);
+
+ /// <li> Expand nbest :for all x E Star(nbest) that are not in C
+
+ //printf("*cntr %d \n", cntr);
+
+ /// <li> REPEAT until the queue is empty.
+
+ do {
+ /// <ol>
+ /// <li> Pick nbest from prioritySet and remove
+#ifndef CONFIG_PP_USES_RBTREE
+ nbest = graph_queue;
+ graph_queue = graph_queue->next;
+#else
+ nbest = container_of(rb_first(&graph_queue), GraphMapCell, node);
+ rb_erase(&nbest->node, &graph_queue);
+#endif
+
+ GraphCell2XY(nbest, &x, &y);
+/* map->cells[y][x].detected_obstacle = 255; */
+/* usleep(100000); */
+
+ /// <li> add to processed
+ nbest->processed = 1;
+
+ /// <li> IF nbest == goal ; EXIT
+ if (nbest==goal) {
+ break;
+ }
+
+ /// <li> Expand nbest :for all x E Star(nbest) that are not in C
+ const struct { int x; int y; int angle;} neighbours[] = { { 0, 1, 90}, /*{ 1, 1},*/ { 1, 0, 0}, /*{-1, 1},*/
+ {-1, 0, 180}/*, {-1,-1}*/, { 0,-1, 270}/*, { 1,-1}*/};
+
+ for (i=0; i< sizeof(neighbours)/sizeof(neighbours[0]); i++) {
+ xcontig = x + neighbours[i].x;
+ ycontig = y + neighbours[i].y;
+
+ if(ShmapIsCellInMap(xcontig, ycontig) && (graph[ycontig][xcontig].processed == 0)){
+ double cost = cFunction(x, y, xcontig, ycontig);
+ contig = &graph[ycontig][xcontig];
+ /// <li> IF is not in Priorityset add to prioritySet.
+ if (!contig->in_queue) {
+// if(ShmapIsFreeCell(xcontig, ycontig)) {
+// // add to prioritySet.
+// contig->g=nbest->g + cost;
+// contig->f = contig->g + contig->h;
+// contig->backpointer=nbest;
+// pushNodeInOrder(contig);
+// }
+ //int angle=get_angle_between_cells(x,y,xcontig,ycontig);
+ if(check_bot_position_streight(xcontig,ycontig,bitmap_straight,neighbours[i].angle)){
+ // add to prioritySet.
+ contig->g=nbest->g + cost;
+ contig->f = contig->g + contig->h;
+ contig->backpointer=nbest;
+ pushNodeInOrder(contig);
+
+ }
+ } else if (nbest->g + cost < contig->g) {
+ /// <li> ELSE update backpointer to point to nbest.
+ contig->backpointer=nbest;
+ contig->g = nbest->g + cost;
+ }
+ }
+ }
+ /// </ol>
+#ifndef CONFIG_PP_USES_RBTREE
+ } while (graph_queue);
+#else
+ } while (!RB_EMPTY_ROOT(&graph_queue));
+#endif
+
+ if (nbest==goal) {
+ int nbpoints, x, y;
+ GraphMapCell *cell;
+
+ // Clear previous path
+ for (y=0;y<MAP_HEIGHT;y++){
+ for(x=0;x<MAP_WIDTH;x++){
+ map->cells[y][x].flags &= ~(MAP_FLAG_PATH | MAP_FLAG_START | MAP_FLAG_GOAL);
+ }
+ }
+
+
+ /* Construct forward list for the found path. */
+ cell = goal;
+ cell->next = NULL;
+ nbpoints = 1;
+ while (cell != start) {
+ cell->backpointer->next = cell;
+ cell = cell->backpointer;
+ nbpoints++;
+ GraphCell2XY(cell, &x, &y);
+ map->cells[y][x].flags |= MAP_FLAG_PATH;
+ }
+
+ map->cells[ystart][xstart].flags |= MAP_FLAG_START | MAP_FLAG_PATH;
+ map->cells[ygoal][xgoal].flags |= MAP_FLAG_GOAL | MAP_FLAG_PATH;
+
+ *original_path = start;
+ return nbpoints;
+ } else {
+ return -1;
+ }
+}
/**
* @name A* Main function
* @{
projects / eurobot / public.git / commitdiff