map.cpp

机器人的行为控制模拟程序。用于机器人的环境识别。A robot action decision simulation used for robot enviroment recognition.

/* 
    Robot Simulator

    This program is free software; you can redistribute it and/or
    modify it under the terms of the GNU General Public License
    as published by the Free Software Foundation; either version 2
    of the License, or (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

    (C) 2006 Jason Hunt
    [e-mail] 
    
    File: map.cpp
*/

#include <windows.h>
#include <math.h>

#include "map.h"
#include "main.h"
#include "world.h"
#include "robot.h"


// x, y Offsets for map directions
int offsets [][2] = {{1,0},  {1,1},   {0,1},  {-1,1}, 
                    {-1,0}, {-1,-1}, {0,-1}, {1,-1}}; 
       

/* Convert world x to map x */
int get_map_x(double x)
{
    unsigned int mx = (int) ((x/world.x_size) * MAP_GRID_X);
    
    if (mx < 0) mx = 0;
    if (mx > MAP_GRID_X-1) mx = MAP_GRID_X-1;
    
    return(mx); 
}
/* End of get_map_x */


/* Convert world y to map y */
int get_map_y(double y)
{
    unsigned int my = (int) ((y/world.y_size) * MAP_GRID_Y);
    
    if (my < 0) my = 0;
    if (my > MAP_GRID_Y-1) my= MAP_GRID_Y-1;
    
    return(my); 
}
/* End of get_map_y */


/* Mark wal location on map */
void world_set_map(int x, int y)
{
    if (x < 0) x = 0;
    if (x > MAP_GRID_X-1) x = MAP_GRID_X-1;

    if (y < 0) y = 0;
    if (y > MAP_GRID_Y-1) y = MAP_GRID_Y-1;
    
    world.map_grid[x][y] = -1;
}
/* End of world_set_map */


/* Get map value */
int world_test_map( int map[MAP_GRID_X][MAP_GRID_Y], int x, int y)
{
    if (x < 0) return(255);
    if (y < 0) return(255);
   
    if (x > MAP_GRID_X - 1) return(255);
    if (y > MAP_GRID_Y - 1) return(255);
    
    return(world.map_grid[x][y] & 0x0fff);    
}
/* End of world_test_map */


/* COpute index of direction pointing to smallest neighbor */
int world_get_lowest_index(int map[MAP_GRID_X][MAP_GRID_Y], int x, int y)
{
    int lowest_index = 8;
    int lowest_num = 255;
    int v;

    for (int i  = 0; i < 8; i++)
    {    
        v = world_test_map( world.map_grid, x+offsets[i][0], y+offsets[i][1]);    

        if (v <lowest_num && v != -1) 
        { 
            lowest_num = v; 
            lowest_index = i;
        }
    }
    
    return(lowest_index);
}        
/* End of world_get_lowest_index */


/* Get vector for nagivation */
void compute_map_nav(point &nav, double x, double y)
{
    double angle = 0;

    double sum_x = 0;
    double sum_y = 0;

    for (int i  = 0; i < 8; i++)
    {    
        int v = world_test_map( world.map_grid, get_map_x(x)+offsets[i][0], get_map_y(y)+offsets[i][1]);    
    
        // Not a wall?
        if (v != -1)
        {
            sum_x -= v * cos(angle);
            sum_y -= v * sin(angle);
        }

        angle += M_PI/4.0;
    }

    nav.x = sum_x;
    nav.y = sum_y;    
}
/* End of compute_map_nav */


/* Perform map itteration */
void world_compute_map_pass( bool &change)
{
    int temp_map[MAP_GRID_X][MAP_GRID_Y];

    bool new_change = false;

    for (int y = 0; y < MAP_GRID_Y; y++)
    {
        for (int x = 0; x < MAP_GRID_X; x++)
        {
            int v, min = 255;

            v = world_test_map(world.map_grid, x-1, y);
            if (v < min && v != -1) min = v;
            
            v = world_test_map(world.map_grid,x+1, y);
            if (v < min && v != -1) min = v;

            v = world_test_map(world.map_grid,x, y-1);
            if (v < min && v != -1) min = v;

            v = world_test_map(world.map_grid,x, y+1);
            if (v < min && v != -1) min = v;

            if (min != 255 && world.map_grid[x][y] != 0 && world.map_grid[x][y] != -1)
            {
                temp_map[x][y] = min + 1;
            }
            else
                temp_map[x][y] = world.map_grid[x][y];
        }       
    }

    for (int y = 0; y < MAP_GRID_Y; y++)
        for (int x = 0; x < MAP_GRID_X; x++)
        {

            if (world.map_grid[x][y] != temp_map[x][y])
                new_change = true;

            world.map_grid[x][y] = temp_map[x][y];
            
        }
            
            
    if (!new_change) change = false;
}
/* End of world_compute_map_pass */


/* Compute navigation map */
void world_compute_map_grid( void )
{
    for (int map_x = 0; map_x < MAP_GRID_X; map_x++)
        for (int map_y = 0; map_y < MAP_GRID_Y; map_y++)
            world.map_grid[map_x][map_y] = 255;   
 
    impact_type *i = world.impacts;
 
     while (i != NULL)
     {
        world_set_map(get_map_x(i->x), get_map_y(i->y));

        world_set_map(get_map_x(i->x)+1, get_map_y(i->y));
        world_set_map(get_map_x(i->x)-1, get_map_y(i->y));
        world_set_map(get_map_x(i->x), get_map_y(i->y)+1);
        world_set_map(get_map_x(i->x), get_map_y(i->y)-1);
 
         i = i->next;
    }
 
    // Mark goal
    world.map_grid[get_map_x(world.target_x)][get_map_y(world.target_y)] = 0;
    world.map_grid[get_map_x(world.target_x)][get_map_y(world.target_y)] = 0;
 
    bool  change = true;

    // Itterage map
    while (change)
       world_compute_map_pass(change); 
 
 
    // mark path
    
    int p_x = get_map_x(robot.x);
    int p_y = get_map_y(robot.y);
    
    int val = 255;
    
    
   int old_map[MAP_GRID_X][MAP_GRID_Y];
    

    for (int y = 0; y < MAP_GRID_Y; y++)
        for (int x = 0; x < MAP_GRID_X; x++)
       old_map[x][y] = world.map_grid[x][y];
            
    int count = 0;

    int  max_path = 255;      
    
                              
    world_free_waypoints();                
                
    while(val > 0 && count++<max_path-1) 
    {
        unsigned int index = world_get_lowest_index(world.map_grid, p_x, p_y);

        // Add direction
        p_x += offsets[index][0];
        p_y += offsets[index][1];
        
        // Range check the result
        if (p_x < 0) p_x = 0;
        if (p_x > MAP_GRID_X -1) p_x = MAP_GRID_X -1;
        
        if (p_y < 0) p_y = 0;
        if (p_y > MAP_GRID_Y -1) p_y = MAP_GRID_Y -1;
        
        
        // Not a wall?, add waypoint
        if ( (val = world_test_map(world.map_grid, p_x, p_y)) != -1)
        {
            double world_x = p_x * world.x_size / (double)MAP_GRID_X;
            double world_y = p_y * world.y_size / (double)MAP_GRID_Y;
            
            point nav;

            compute_map_nav(nav, world_x, world_y);
            
            double dir = atan2(nav.y, nav.x);

            world.map_grid[p_x][p_y] |= 0x4000;

            world_add_waypoint( world_x, world_y, dir);
        }                        
    }
    
    // No path found
    if (count == max_path)
    {
         world_set_target(f_rand(1, world.x_size-1), f_rand(1, world.y_size-1));
         return;
    }

    // Number the way points
    
    waypoint_type *w = world.waypoints;
    
    int cur_waypoint = 0;
    
    while(w != NULL)
    {
        w->num = cur_waypoint++;
        w = w->next;
    }

    // Set starting waypoint
    world.cur_waypoint = world.num_waypoints - 1;
   
}
/* End of  world_compute_map_grid */





/* Draw navigation map grid */
void  world_draw_map_grid( HDC dc )
{
    double x_size = display_x /(double)(MAP_GRID_X);
    double y_size = display_y /(double)(MAP_GRID_Y);
    
    HBRUSH br;
    HPEN pen;
    return;
        
    for (int map_x = 0; map_x < MAP_GRID_X; map_x++)
        for (int map_y = 0; map_y < MAP_GRID_Y; map_y++)
        {
            int  val = world.map_grid[map_x][map_y];

            bool draw_cell = true;
   
           if (val == -1)
            {
                 draw_cell = false;
               //  br = CreateSolidBrush(RGB(255, 0, 0));
               //  pen = CreatePen(PS_SOLID, 1, RGB(255,0,0));                
            } else
            if (val & 0x4000)
            {
                 br = CreateSolidBrush(RGB(0, 0, 0));
                 pen = CreatePen(PS_SOLID, 1, RGB(0,0,0));                
            }
            else        
            { 

                draw_cell = false;
    
                val *= 10;
                if (val > 255) val = 255;

//val = 0;

              //  br = CreateSolidBrush(RGB(0, 0, val));
               // pen = CreatePen(PS_SOLID, 1, RGB(0,0,val));

               // br = CreateSolidBrush(RGB(255, 255, 255));
               //en = CreatePen(PS_SOLID, 1, RGB(255,255,255));


            }

        if (draw_cell)
        {
            SelectObject(dc, br);
            SelectObject(dc, pen);

            int screen_x = (int)(map_x * x_size);
            int screen_y = (int)(map_y * y_size);            

            Rectangle(dc, screen_x, screen_y, (int)(screen_x+x_size)+1, (int)(screen_y+y_size)+1);


         //  if (world.map_grid[map_x][map_y] < 255)
          /*  {
                    char b[400];
                 sprintf(b, "%4d", world.map_grid[map_x][map_y]);
                 TextOut(dc, screen_x, screen_y, b, strlen(b));
            }*/
            DeleteObject(br);
            DeleteObject(pen);
        }
    }
    
}
/* End of world_draw_map_grid */