robot.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 <math.h>
#include <windows.h>
#include <stdio.h>


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

extern world_type world;
extern bool mode_manual;

double max_animal_speed = 0.25;  // The animals cal move no faster than this


robot_type robot;


/* Draw line in world cords */
void draw_line(HDC dc, line l)
{   
    MoveToEx(dc, get_screen_x(l.x1), get_screen_y(l.y1), NULL);
    LineTo(dc, get_screen_x(l.x2), get_screen_y(l.y2));    
}
/* End of draw line */



/* Draw a sensor for an animal */
void draw_sensor(HDC dc, double x, double y, double dir, double dist, double size, COLORREF c)
{
    double sx = x + dist * cos(dir);
    double sy = y + dist * sin(dir);
    
    RECT r;
    
    r.left = get_screen_x(sx - size / 2.0);
    r.top = get_screen_y(sy - size / 2.0);
    r.right = get_screen_x(sx + size / 2.0);
    r.bottom = get_screen_y(sy + size / 2.0);
    
    HBRUSH b = CreateSolidBrush(c);

    HBRUSH hold = (HBRUSH) SelectObject(dc, b);

    Ellipse(dc, r.left, r.top, r.right, r.bottom);    
    
    SelectObject(dc, hold);    
        
    DeleteObject(b);
}
/* End of draw sensor */


/* Decode the sensor into a color */
COLORREF get_sensor_color( bool val )
{
    if (val) return(RGB(255, 0, 0));     // Red for well

    return(RGB(255, 255, 255));                 // No hit, return white
}
/* End of get_sensor_color */


/* Draw an animal to a device context */
void robot_draw(HDC dc)
{
    
    SelectObject(dc, (HPEN)world.black_pen);    
    RECT r;
    get_bounding_rect(robot.x, robot.y, robot.size, robot.size, r);

    SelectObject(dc, (HBRUSH)world.green_brush);

    // Draw robot body
    Ellipse(dc, r.left, r.top, r.right, r.bottom);

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

    // Get distance from center and size of the sensors
    double size = robot.sensor_size;
    double dist = robot.sensor_dist;

    // Draw the direction arrow
    draw_arrow(dc, robot.x, robot.y, robot.dir, size);
 
 
 
    // Draw each sensor
    // The sensor index increases in a clockwise direction

    double angle = - M_PI / 3.0;
    double delta = 2 * M_PI / 6.0;

    for (int i = 0; i < 6; i++, angle += delta)
        draw_sensor(dc,  robot.x, robot.y, robot.dir + angle, dist, size, 
                       get_sensor_color(robot.touch[i]));
                       
                       
  /*  for (int i = 0; i < robot.num_vision_points * 2; i++)
    {
        if (robot.vision[i/ 2].food > 0)
        {

            MoveToEx(dc, get_screen_x(robot.x),get_screen_y(robot.y),NULL);
            LineTo(dc, get_screen_x(robot.end_x[i]),get_screen_y(robot.end_y[i]));
        }
    }*/
        
        
    if (robot.draw_vision)
    // Draw vision lines
    for (int i = 0; i < 6; i++)
        if (robot.wall_found[i])
        {
            line l;
            l.x1 = robot.x;
            l.y1 = robot.y;
            l.x2 = robot.wall_points[i].x;
            l.y2 =robot.wall_points[i].y;
        
            SelectObject(dc, (HPEN)world.green_pen);
    
            draw_line(dc, l);
        }        
                
                
    /// Draw nav vector                
 
    double mag = robot.nav_mag;
    
    if (mag > 0.5) mag = 0.5;
 
   SelectObject(dc, (HPEN)world.blue_pen); 
    draw_arrow(dc, robot.x, robot.y, robot.nav_dir, 0.1);
                
                                
}
/* End of draw sensors */



/* Check the spac that the sensor tip occupies, and set values to match */
bool check_sensor(double x, double y, double dir, double dist) 
{
    // Determine location of sensor tip
    double sx = x + dist * cos(dir);
    double sy = y + dist * sin(dir);
    
    if (world_check_wall_point(sx, sy)) 
    {
        //world_add_impact(sx, sy);
        return(true);        
    }

    if (sx < 0 || sx > world.x_size) 
    {
      //  world_add_impact(sx, sy);
        return(true);
    }

    if (sy < 0 || sy > world.y_size)
    { 
      // world_add_impact(sx, sy);
        return(true);
    }
    
    return(false);
}
/* End of check sensor */





/* compute intersection between two lines */
bool check_intersect(line line1, line line2, point &i)
{
    // Constants for line 1
    // a1*x + b1*y + c1 = 0 
    double a1 = line1.y2 - line1.y1;
    double b1 = line1.x1 - line1.x2;
    double c1 = line1.x2*line1.y1 - line1.x1*line1.y2;  

    // Constants for line 2
    // a2*x + b2*y + c2 = 0 
    double a2 = line2.y2 - line2.y1;
    double b2 = line2.x1 - line2.x2;
    double c2 = line2.x2*line2.y1 - line2.x1*line2.y2;  

    // Descriment
    double  d = a1*b2 - a2*b1;
    
    if (d == 0) return(false);

    i.x = (b1*c2 - b2*c1)/d;
    i.y = (a2*c1 - a1*c2)/d;

    if (line1.x1 != line1.x2 && i.x < line1.x1 && i.x < line1.x2) return(false);
    if (line1.x1 != line1.x2 && i.x > line1.x1 && i.x > line1.x2) return(false);

    if (line1.y1 != line1.y2 && i.y < line1.y1 && i.y < line1.y2) return(false);
    if (line1.y1 != line1.y2 && i.y > line1.y1 && i.y > line1.y2) return(false);

    if (line2.x1 != line2.x2 && i.x < line2.x1 && i.x < line2.x2) return(false);
    if (line2.x1 != line2.x2 && i.x > line2.x1 && i.x > line2.x2) return(false);

    if (line2.y1 != line2.y2 && i.y < line2.y1 && i.y < line2.y2) return(false);
    if (line2.y1 != line2.y2 && i.y > line2.y1 && i.y > line2.y2) return(false);

    return(true);    
}
/* End of check_intersect */


/* Check intersection between line and box */
double check_box_intersect(line main, line box, point &intersect)
{
    line test_line;
    
    bool hit[4] = {false};
    point points[4];
    double dist[4] = {MAX_IR_DIST, MAX_IR_DIST, MAX_IR_DIST, MAX_IR_DIST};
        
    // Check upper line
    test_line.x1 = box.x1;   test_line.y1 = box.y1;
    test_line.x2 = box.x2;   test_line.y2 = box.y1;

    if (check_intersect(main, test_line, points[0]))
    {
        dist[0] = sqrt((points[0].x - main.x1) * (points[0].x - main.x1)+   
                       (points[0].y - main.y1) * (points[0].y - main.y1)); 

    }

    // Check lower line
    test_line.x1 = box.x1;   test_line.y1 = box.y2;
    test_line.x2 = box.x2;   test_line.y2 = box.y2;

    if (check_intersect(main, test_line, points[1]))
        dist[1] = sqrt((points[1].x - main.x1) * (points[1].x - main.x1)+   
                       (points[1].y - main.y1) * (points[1].y - main.y1));   

    // Check left line
    test_line.x1 = box.x1;   test_line.y1 = box.y1;
    test_line.x2 = box.x1;   test_line.y2 = box.y2;

    if (check_intersect(main, test_line, points[2]))
        dist[2] = sqrt((points[2].x - main.x1) * (points[2].x - main.x1)+   
                       (points[2].y - main.y1) * (points[2].y - main.y1));   
    // Check right line
    test_line.x1 = box.x2;   test_line.y1 = box.y1;
    test_line.x2 = box.x2;   test_line.y2 = box.y2;

    if (check_intersect(main, test_line, points[3]))
        dist[3] = sqrt((points[3].x - main.x1) * (points[3].x - main.x1)+   
                       (points[3].y - main.y1) * (points[3].y - main.y1));   


    // Calculate and return distance to closest line in box
    double smallest_dist = world.x_size + world.y_size;
    int smallest_index = 0;
    
    char b[400];
    
    for (int i = 0; i < 4; i++)
    {
        if (dist[i] > 0 && dist[i] <= smallest_dist)
        {
            smallest_dist = dist[i];
            smallest_index = i;
        }
    }
   
    if (dist[smallest_index] < 1)
   {
        
            world_add_impact(points[smallest_index].x, points[smallest_index].y);
    }
   
    intersect = points[smallest_index];    
    


    return(dist[smallest_index]);
}



/* Check the space that the sensor tip occupies, and set values to match */
double check_vision(double x, double y, double dir, bool &found, point &p) 
{
    // Determine location of sensor tip
    double max_size = world.x_size * world.y_size;

    line main_line;
    
    main_line.x1 = x;
    main_line.y1 = y;
    
    main_line.x2 = x + max_size * cos(dir);
    main_line.y2 = y + max_size * sin(dir);
        
    wall_type *w = world.walls;    
   
    found = false;
   
    point points[world.num_walls];
    double dist[world.num_walls];

    
    point intersect;
    line box;