world.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] 
*/

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

#include "utility.h"
#include "graphics.h"
#include "main.h"
#include "console.h"
#include "robot.h"
#include "world.h"
#include "map.h"

world_type world;       // All world data 


/* Inital world setup */
void world_setup(void )
{   
    // Size in world space
    world.x_size = 10;
    world.y_size = 10;

    world_destroy(); // free any existing world

    // Create brushs
    world.white_brush = CreateSolidBrush(RGB(255, 255, 255));
    world.black_brush = CreateSolidBrush(RGB(0, 0, 0));
    world.green_brush = CreateSolidBrush(RGB(0, 255, 0));
    world.blue_brush = CreateSolidBrush(RGB(0, 0, 255));
    world.red_brush = CreateSolidBrush(RGB(255, 0, 0));
    world.color1_brush = CreateSolidBrush(RGB(0, 128, 200));
    

    // Create pens
    world.white_pen = CreatePen(PS_SOLID, 1, RGB(255, 255, 255));
    world.black_pen = CreatePen(PS_SOLID, 1, RGB(0, 0, 0));
    world.green_pen = CreatePen(PS_SOLID, 1, RGB(0, 255, 0));
    world.blue_pen = CreatePen(PS_SOLID, 1, RGB(0, 0, 255));
    world.color1_pen = CreatePen(PS_SOLID, 1, RGB(0, 128, 200));

    // Run time tweakable values
    world.scale1 = 0.11;
    world.scale2 = 0.86;
    world.scale3 = 0.193;
        
 
    // Start showing walls
    world.show_walls = true;
 

    // No walls
    world.walls = NULL;
    world.num_walls = 0;

    // No markers
    world.markers = NULL;

    // No Impacts
    world.impacts = NULL;

    // No waypoints
    world.waypoints = NULL;
    world.num_waypoints = 0;
    world.cur_waypoint = 0;

    // Do not draw grad grid
    world.draw_grad = false;
    
    
    world.target_enabled = true;
    world.target_x = world.x_size /2.0;
    world.target_y = world.y_size /2.0;
    
    
    // Setup surronding walls
    
    double wall_width = 0.05;
    
    // top
    world_add_wall(0, 0, world.x_size, wall_width);

    // Bottom
    world_add_wall(0, world.y_size-wall_width, world.x_size, wall_width);
    
    // Left
    world_add_wall(0, 0, wall_width, world.y_size);    
    
    // Right
    world_add_wall(world.x_size-wall_width, 0, wall_width, world.y_size);
    

    // Setup cross walls in center
   // world_add_wall(world.x_size/2 - world.x_size/4 , world.y_size/2-wall_width/2, world.x_size/2, wall_width);
   // world_add_wall(world.x_size/2-wall_width/2, world.y_size/2 - world.y_size/4 , wall_width, world.y_size/2);

    // Setup robot
    robot_init();


    // Compute first map interation
    world_compute_map_grid();
  
}
/* End of void world_setup(void ) */


/* Free all world data */
void world_destroy( void )
{
    world.num_walls = 0;
    
    world_free_walls();    
    world_free_markers();
    world_free_impacts();
    world_free_waypoints();
    
    DeleteObject(world.black_brush);
    DeleteObject(world.white_brush);
    DeleteObject(world.green_brush);
    DeleteObject(world.blue_brush);
    DeleteObject(world.red_brush);
    DeleteObject(world.color1_brush);
    
    DeleteObject(world.white_pen);
    DeleteObject(world.black_pen);
    DeleteObject(world.green_pen);
    DeleteObject(world.blue_pen);
    DeleteObject(world.color1_pen);
    
}
/* End of world_destroy */



/* Draw entire world */
void world_draw( HDC dc )
{

    SelectObject(dc, world.white_pen);
    SelectObject(dc, world.white_brush);
      
    // Clear entire area
    Rectangle(dc, 0, 0, display_x, display_y);


    if (world.draw_grad)
        world_draw_grad(dc);



//    if (world.draw_grad)
//         world_draw_map_grid(dc);


    SelectObject(dc, world.black_pen);
    SelectObject(dc, world.white_brush);

    world_draw_markers(dc);




    if (world.show_walls)
        world_draw_walls(dc);

    if (world.target_enabled)
    {
        double target_size = 0.15;

        SelectObject(dc, world.green_brush);        

        Ellipse(dc, get_screen_x(world.target_x - target_size/2), 
                    get_screen_y(world.target_y - target_size/2), 
                    get_screen_x(world.target_x + target_size/2), 
                    get_screen_y(world.target_y + target_size/2));
                
    }    


    world_draw_impacts(dc);    

    world_draw_waypoints(dc);

    robot_draw(dc);
    
}
/* End of world_draw */


/* Update all world data */
void world_update( void )
{
 //   world_compute_map_grid();

    robot_update();
}
/* End of world_update */


    
/* Transform from world space to screen space */
int get_screen_x( double x )
{
    return((int)(x / (double) world.x_size * display_x));
}
/* End of get_screen_x */


/* Transform from world space to screen space */
int get_screen_y( double y )
{
    return((int)(y / (double) world.y_size * display_y));
}
/* End of get_screen_y */


/* Return world cords of a screen pos */
double get_world_x( unsigned int x)
{
    return((x / (double)display_x) * world.x_size);        
}
/* End of get_world_x */

/* Return world cords of a screen pos */
double get_world_y( unsigned int y)
{
    return((y / (double)display_y) * world.y_size);        
}
/* End of get_world_x */


/* Return a bounting box in screen space for an object */
void get_bounding_rect(double x, double y, double x_size, double y_size, RECT &r)
{
    r.left = get_screen_x(x - x_size / 2);
    r.top = get_screen_y(y - y_size / 2);
    r.right = get_screen_x(x + x_size / 2);
    r.bottom = get_screen_y(y + y_size / 2);
}
/* End of get_bounding_rect */
    


/* Return true if points is inside of wall */    
bool world_check_wall_point( double x, double y )
{
    wall_type *w = world.walls;

    while (w != NULL) 
    {
        if (x >= w->x && x <= w->x + w->x_size && 
            y >= w->y && y <= w->y + w->y_size)
            return(1);

        w = w->next;

    }
    return(false);
}    
/* End of world_check_wall_point */


/* Add a wall to the world */
void world_add_wall(double x, double y, double x_size, double y_size)
{
    wall_type *w = world.walls;
    
    world.walls = (wall_type *) malloc(sizeof(wall_type));

    world.walls->x = x;
    world.walls->y = y;

    world.walls->x_size = x_size;
    world.walls->y_size = y_size;
       
    world.walls->next = w;
    
    world.num_walls++;
}
/* End of world_add_wall */



/* Free wall memory */
void world_free_walls( void )
{
    wall_type *tmp;
        
    while (world.walls != NULL) 
    {
        tmp = world.walls->next;

        free(world.walls);

        world.walls = tmp; // Unlink
    }

    world.num_walls = 0;
}    
/* End of world_free walls */


    
/* Draw walls to DC */
void world_draw_walls( HDC dc )
{
    wall_type *tmp = world.walls;

    SelectObject(dc, (HBRUSH)world.color1_brush);
    SelectObject(dc, (HPEN)world.color1_pen);

    while (tmp != NULL) 
    {
        RECT r;
        
        Rectangle(dc, get_screen_x(tmp->x),
                      get_screen_y(tmp->y),
                      get_screen_x(tmp->x + tmp->x_size),
                      get_screen_y(tmp->y + tmp->y_size));

        tmp = tmp->next;
    }
}    
/* End of world_draw_walls */



/* Add waypoint to nav system */
void world_add_waypoint(double x, double y, double dir)
{
    waypoint_type *w = world.waypoints;
    
    world.waypoints = (waypoint_type *) malloc(sizeof(waypoint_type));

    world.waypoints->x = x;
    world.waypoints->y = y;
    world.waypoints->dir = dir;
      
    world.waypoints->next = w;
    
    world.num_waypoints++;
}
/* End of world_add_waypoint */


/* Free waypoint memory */
void world_free_waypoints( void )
{
    waypoint_type *tmp;
        
    while (world.waypoints != NULL) 
    {
        tmp = world.waypoints->next;

        free(world.waypoints);

        world.waypoints = tmp; // Unlink
    }
    world.num_waypoints = 0;
}    
/* End of world_free_waypoints */


/* Draw nav waypoints to DC */
void world_draw_waypoints( HDC dc )
{
    waypoint_type *tmp = world.waypoints;

    SelectObject(dc, (HBRUSH)world.color1_brush);
    SelectObject(dc, (HPEN)world.color1_pen);

    while (tmp != NULL) 
    {
        RECT r;

        if (tmp->num == world.cur_waypoint)
        {
           SelectObject(dc, (HBRUSH)world.red_brush);
           SelectObject(dc, (HPEN)world.black_pen);
            
            
        }else
        {
           SelectObject(dc, (HBRUSH)world.color1_brush);
           SelectObject(dc, (HPEN)world.color1_pen);
        }
        
        Ellipse(dc, get_screen_x(tmp->x - 0.05),
                      get_screen_y(tmp->y - 0.05),
                      get_screen_x(tmp->x  + 0.05),
                      get_screen_y(tmp->y  + 0.05));

        tmp = tmp->next;

    }
}
/* End of draw waypoints */

    

/* Add marker to the world */
void world_add_marker(double x, double y)
{
    return;
    marker_type *m = world.markers;
    
    world.markers = (marker_type *) malloc(sizeof(marker_type));

    world.markers->x = x;
    world.markers->y = y;
       
    world.markers->next = m;
}
/* End of world_add_marker */



/* Free marker memory */
void world_free_markers( void )
{
    marker_type *tmp;
        
    while (world.markers != NULL) 
    {
        tmp = world.markers->next;


        free(world.markers);

        world.markers = tmp; // Unlink
    }
}    
/* End of world free markers */


/* draw markers to DC */
void world_draw_markers( HDC dc )
{
    marker_type *m = world.markers;

return;