From 75c6688c7c35c0ce9904c25b3f7c6d644881adc7 Mon Sep 17 00:00:00 2001
From: pokormat <matous.pokorny@me.com>
Date: Tue, 19 Apr 2011 23:41:42 +0200
Subject: [PATCH] pathplan:add map cell flag path shape. Flag shows cells,
 which was checked by astar alg. and they make a robot floor shape. Add
 function for setting flag in one map cell.

---
 src/pathplan/aalgorithm.c     |  6 ++++++
 src/pathplan/map.c            | 20 ++++++++++++++++++++
 src/pathplan/map.h            |  3 +++
 src/pathplan/test/testastar.c |  4 ++++
 4 files changed, 33 insertions(+)

diff --git a/src/pathplan/aalgorithm.c b/src/pathplan/aalgorithm.c
index 2ba831f7..e763ee62 100644
--- a/src/pathplan/aalgorithm.c
+++ b/src/pathplan/aalgorithm.c
@@ -114,6 +114,9 @@ bool check_bot_position_streight(int x_0, int y_0, bitmap_dim_straight bitmap_st
   
   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++){
+      
+      ShmapSetCellFlag(x,y,MAP_FLAG_PATH_SHAPE);
+      
       if (!ShmapIsFreeCell(x,y)){	//1 no obstacle,-1 no in map, 0 otherwise
 	return false;
       }
@@ -123,6 +126,9 @@ bool check_bot_position_streight(int x_0, int y_0, bitmap_dim_straight bitmap_st
   return true;
 }
 
+      
+	ShmapSetCellFlag(x,x+q,MAP_FLAG_PATH_SHAPE);
+      
 // int get_angle_between_cells(int x_0, int y_0, int x_1, int y_1){
 //   
 //   double angle=0; 
diff --git a/src/pathplan/map.c b/src/pathplan/map.c
index e7134413..75f3bfda 100644
--- a/src/pathplan/map.c
+++ b/src/pathplan/map.c
@@ -208,6 +208,26 @@ int ShmapSetCircleFlag(double xs, double ys, double r, map_cell_flag_t set_flags
 	return 1;
 }
 
+/**
+ * @brief Set flag in the map cell
+ * @param x	Coordonate X (int) of the cell
+ * @param y	Coordonate Y (int) of the cell
+ * @param set_flags	Map cell falgs
+ * 
+ */
+int ShmapSetCellFlag(int x, int y, map_cell_flag_t set_flags)
+{
+  if (!ShmapIsCellInMap(x, y)){
+    return 0;
+  }
+          
+  map->cells[y][x].flags |= set_flags;
+  
+  return 1;
+}
+
+/** @} */
+
 /**
  * @brief Creates an obstacle in the map
  * @param x1	Coordonate X (in m) of the first point
diff --git a/src/pathplan/map.h b/src/pathplan/map.h
index 87b910dd..aa37f568 100644
--- a/src/pathplan/map.h
+++ b/src/pathplan/map.h
@@ -188,6 +188,7 @@
  * not during runtime obstacle avoidance. */
 #define MAP_FLAG_PLAN_MARGIN    	128
 #define MAP_FLAG_INVALIDATE_WALL	256 /**< Area, where the wall should be forgotten */
+#define MAP_FLAG_PATH_SHAPE		512
 
 /** @}*/
 /** @name Shared Memory macros */
@@ -265,6 +266,8 @@ int ShmapIsFreePoint(double x_m, double y_m);
 int ShmapSetRectangleFlag(double x1, double y1, double x2, double y2, map_cell_flag_t set_flags, map_cell_flag_t clear_flags);
 int ShmapSetCircleFlag(double xs, double ys, double r, map_cell_flag_t set_flags, map_cell_flag_t clear_flags);
 
+int ShmapSetCellFlag(int x, int y, map_cell_flag_t set_flags);
+
 #ifdef __cplusplus
 }
 #endif 
diff --git a/src/pathplan/test/testastar.c b/src/pathplan/test/testastar.c
index 8ca7a4ba..c2cdf089 100644
--- a/src/pathplan/test/testastar.c
+++ b/src/pathplan/test/testastar.c
@@ -24,9 +24,13 @@ void print_map(char *label)
 			char c;
 			if (ShmapIsFreeCell(x, y)) {
 				c = '.';
+				if (map->cells[y][x].flags & MAP_FLAG_PATH_SHAPE) {
+					c = 's';
+				}
 				if (map->cells[y][x].flags & MAP_FLAG_PATH) {
 					c = 'p';
 				}
+				
 			} else
 				c = '#';
 			putchar(c);
-- 
2.39.2