world.cpp

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

    double x_size = 0.1;
    double y_size = 0.1;
    

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

    int count = 500;

    while (m != NULL && count--) 
    {
        RECT r;
        
        get_bounding_rect(m->x,m->y,x_size, y_size, r);

        Ellipse(dc, r.left, r.top, r.right, r.bottom);
        

        m = m->next;

    }
}    
/* End of world_draw_markers */

    
/* Add impact to world */
void world_add_impact(double x, double y)
{
    impact_type *i = world.impacts;
 
    while (i != NULL) 
    {
        double dist = (x-i->x)*(x-i->x) + (y-i->y)*(y-i->y);
        
        if (dist < 0.01) return;
        
        i = i->next;

    }

    i = world.impacts;
    
    world.impacts = (impact_type *) malloc(sizeof(impact_type));

    world.impacts->x = x;
    world.impacts->y = y;
       
    world.impacts->next = i;
    
    world.num_impact++;
    
    world_compute_map_grid();
}
/* End of world_add_impact */


/* Free impact memory */
void world_free_impacts( void )
{
    impact_type *tmp;
        
    while (world.impacts != NULL) 
    {
        tmp = world.impacts->next;

        free(world.impacts);

        world.impacts = tmp; // Unlink
    }
    world.num_impact = 0;
}    
/* End of world_free_impacts */


/* Draw imapcts to DC */
void world_draw_impacts( HDC dc )
{
    impact_type *i = world.impacts;

return;
    double x_size = 0.2;
    double y_size = 0.2;
    

    SelectObject(dc, (HBRUSH)world.red_brush);
    SelectObject(dc, (HPEN)world.black_pen);
    

    while (i != NULL) 
    {
        RECT r;
        
        get_bounding_rect(i->x,i->y,x_size, y_size, r);

        Ellipse(dc, r.left, r.top, r.right, r.bottom);

        i = i->next;
    }
}    
/* End of world_draw_impacts */


/* Set target location */
void world_set_target(double x, double y)
{
    world.target_enabled = true;

    world.target_x = x;
    world.target_y = y;    
    
    world_compute_map_grid();
}
/* End of world_set_target */



/* Get direction to current waypoint */
void compute_waypoint_vector(double x, double y, point &v)
{
    v.x = 0;
    v.y = 0;

    if (world.num_waypoints == 0) return;
  
   double waypoint_x;
   double waypoint_y;
   
    // find current waypoint

    waypoint_type *w = world.waypoints;
    waypoint_type *waypoint = NULL;

    // Find current waypoint
    while (w != NULL)
    {
        if (w->num == world.cur_waypoint)
           waypoint = w;
        
         w = w->next;       
    }

    // Current waypoint not found
    if (waypoint == NULL)
    {
        write_console("waypoint not found\n");
        return;
    }

    v.x = x - waypoint->x;
    v.y = y - waypoint->y;    
}
/* End of compute_waypoint_vector */


/* Compute vector to closest impact */
void compute_impact_vector(double x, double y, point &v)
{
    v.x = 0;
    v.y = 0;
    
     if (world.num_impact == 0) return;
    
    impact_type *i = world.impacts;
   
    double closest_impact_x;
    double closest_impact_y;
   
    double closest_impact_dist = world.x_size * world.y_size;
   
   
    // Find closest impact 
    while (i != NULL) 
    {
        double x_diff = x - i->x;
        double y_diff = y - i->y;    
          
        double dist_squared = x_diff * x_diff + y_diff * y_diff;

        if (dist_squared < closest_impact_dist)
        {
            closest_impact_dist = dist_squared;
            closest_impact_x = i->x;
            closest_impact_y = i->y;            
        }
            
        i = i->next;        
     }


    v.x = x - closest_impact_x;
    v.y = y - closest_impact_y;        
}
/* End of compute_impact_vector */



/* Compute gradient at a point */
void compute_grad_point(double x, double y, double &dir, double &mag)
{
    double x_sum = 0, y_sum = 0;
    mag = 0;
    
    double dist;

    point v;

    // Compute vector to closest impact
    compute_impact_vector(x, y, v);
    
    dist = vect_mag(v.x, v.y);

    if (dist > 0)
    {
        double  force_x =  world.scale1 * v.x / pow(dist, 1.5);
        double  force_y =  world.scale1 * v.y / pow(dist, 1.5);

        x_sum += force_x;
        y_sum += force_y;

        mag += vect_mag(force_x, force_y);
    
    }
    
    
    compute_waypoint_vector(x, y, v);
    
    
    dist = vect_mag(v.x, v.y);
    
    if (dist > 0)
    {
        double force_x = world.scale2 * v.x;
        double force_y = world.scale2 * v.y;
        
       
        x_sum -= force_x;
        y_sum -= force_y;
    
        mag -= vect_mag(force_x, force_y);
    }
    

    
    compute_map_nav(v, x, y);

    dist = vect_mag(v.x, v.y);
    
    if (dist > 0)
    {
        double force_x = world.scale3 * v.x/dist;
        double force_y = world.scale3 * v.y/dist;
        
        x_sum += force_x;
        y_sum += force_y;
    
        mag += vect_mag(force_x, force_y);
    }
    








    dir = atan2(y_sum, x_sum);//+f_rand(-0.5, 0.5); 
}
/* End of compute_grad_point */



/* Draw gradient field */
void world_draw_grad(HDC dc)
{

 SelectObject(dc, world.green_pen);

    double x, y;
    
    int x_size = display_x /NUM_X_GRAD;
    int y_size = display_y /NUM_Y_GRAD;
    
    
    HBRUSH br;
    HPEN pen;
    
    for (int xg = 0; xg < NUM_X_GRAD; xg++)
        for (int yg = 0; yg < NUM_Y_GRAD; yg++)
        {
            x = xg / (double)NUM_X_GRAD * world.x_size;
            y = yg / (double)NUM_Y_GRAD * world.y_size;

            double dir, mag;

              compute_grad_point(x,y, dir, mag);


            double val = mag;
            
            if (val < -1) val = -1;
            if (val > 1) val = 1;
            
            int color = (int)(fabs(val) * 255);
            
            if (val < 0)
            {
                 br = CreateSolidBrush(RGB(color, 0, 0));
                pen = CreatePen(PS_SOLID, 1, RGB(color,0,0));                
            }
            else
            { 
                br = CreateSolidBrush(RGB(0, 0, color));
                pen = CreatePen(PS_SOLID, 1, RGB(0,0,color));
            }


             SelectObject(dc, br);
             SelectObject(dc, pen);

             Rectangle(dc, get_screen_x(x)-x_size/2-1,
                           get_screen_y(y)-y_size/2-1,
                           get_screen_x(x)+x_size/2+1,
                           get_screen_y(y)+y_size/2+1);

             DeleteObject(br);
             DeleteObject(pen);

             SelectObject(dc, world.green_pen);    

           // double mag =  fabs(world.grad_mag[yg][xg]) / 100.0;
            
            mag = fabs(mag / 5.0);

            if (mag > 0.1) mag = 0.1;

            
         //   mag = 0.05;

            draw_arrow(dc, x, y, dir, mag);
    }
}
/* End of void world_draw_grad(HDC dc) */


/* Load world file */
void world_load( void )
{
    OPENFILENAME ofname;

    char filename[300] = {0};
             
    char filter[] = "World files\0*.dat\0\0";
           
    memset(&ofname, 0, sizeof(ofname));
             
    ofname.lStructSize = sizeof(ofname);
    ofname.lpstrFilter = filter; 
    ofname.lpstrFile = filename; 
    ofname.nMaxFile = 300; 
    ofname.lpstrFileTitle = NULL; 
//    ofname.nMaxFileTitle = 300; 
    ofname.lpstrInitialDir = "."; 

    // Get filename from user
    if (!GetOpenFileName(&ofname))
        return;

    FILE *f = fopen(filename, "rt");

    unsigned int num_walls;


    world_setup();

    world_free_walls();

    fscanf(f, "%lf\n", &robot.x);
    fscanf(f, "%lf\n", &robot.y);
    fscanf(f, "%lf\n", &robot.dir);

    fscanf(f, "%d\n", &num_walls);
  
    for (int i = 0; i < num_walls && !feof(f); i++)
    {
        double x, y, x_size, y_size;
        
        fscanf(f, "%lf %lf %lf %lf\n", &x, &y, &x_size, &y_size);

        world_add_wall(x, y, x_size, y_size);
    }
}
/* End of world_load */


/* Save world data */
void world_save( void )
{
    OPENFILENAME ofname;
    char filename[300] = {0};
    char filter[] = "Map files\0*.dat\0\0";             
             
    memset(&ofname, 0, sizeof(ofname));
             
    ofname.lStructSize = sizeof(ofname);
    ofname.lpstrFilter = filter; 
    ofname.lpstrFile = filename; 
    ofname.nMaxFile = 300; 
    ofname.lpstrFileTitle = NULL; 
    ofname.lpstrInitialDir = "."; 

    // Get filename from user
    if (!GetSaveFileName(&ofname))
    {
        write_console("erer");
        return;
    }


    FILE *f = fopen(filename, "wt");
    

    fprintf(f, "%f\n", robot.x);
    fprintf(f, "%f\n", robot.y);
    fprintf(f, "%f\n", robot.dir);

    fprintf(f, "%d\n", world.num_walls);
    
    
    wall_type *w = world.walls;
    
    while(w != NULL)
    {
        fprintf(f, "%f %f %f %f\n", w->x, w->y, w->x_size, w->y_size);   
        
        w = w -> next;
    }
    
    fclose(f);
}
/* End of save world */