world.cc

神经网络和遗传算法组合应用

#include "callbacks.h"
#include "world.h"
#include <geom.h>
#include <param.h>
#include <gdk/gdk.h>
#include <param.h>
//static int sensorHeight=20;
//static int nrOfSensors=8;
//static int sensorHistory=500;

void guiCreateSensorArea(int nrOfSensors, int heightPerSensor, int historyLength){
//     sensorArea = tgiCreate(historyLength,
// 			   nrOfSensors*heightPerSensor);
//     sensorHeight=heightPerSensor;
//     tgiSetCurrent(sensorArea);
//     guiDefineColours();
//     tgiSetCurrent(simArea); /* DEFAULT */
//     sensorAreaVisible=1;
}

/* rewrite */
void sensorM_activate(){
//   if(sensorAreaVisible){
//     sensorAreaVisible=0;
//     tgiDelete(sensorArea);
//   }
//   else{
//     guiCreateSensorArea(nrOfSensors,sensorHeight,sensorHistory);
//     sensorAreaVisible=1;
//   }
}

void gui_draw_text(GdkPixmap *p, gint x, gint y, const gchar *text){
  gdk_draw_text(p,
		defaultFont,
		pen_simArea,
		x,y,
		text,
		strlen(text));
}


void gui_drawSensor(GdkPixmap *p, int xs, int ys, double value, int w){
  gdk_gc_set_foreground(pen_simArea,
			pal_simArea[(63 - (int)(value * 63.0)) + 6]
			);
  gdk_draw_rectangle(p,
                     pen_simArea,
                     1,
                     (gint)(xs - w),(gint)(ys - w),
		     w*2,w*2);
}

void gui_updateSensorWindow(int robotNr){
  int w;
  gchar text[5];
  gdk_threads_enter();
  gdk_gc_set_foreground(pen_simArea,
			pal_simArea[GUI_WHITE]
			);
  gdk_draw_rectangle(bb_simRobots[robotNr],
		     pen_simArea,
		     1,
		     0,0,
		     51,90);
  
  gdk_gc_set_foreground(pen_simArea,
			pal_simArea[(robotNr%GUI_COLORS) + 1]
			);
  gdk_draw_arc(bb_simRobots[robotNr],
	       pen_simArea,
	       0,
	       0,0,
	       50,50,
	       0,360*64);
  g_snprintf(text,5,"%d",robotNr);

  gui_draw_text(bb_simRobots[robotNr],
		21,28,
		text);
		

  w=3;
  gui_drawSensor(bb_simRobots[robotNr],5,20,simoutput[robotNr][0],w);
  gui_drawSensor(bb_simRobots[robotNr],13,10,simoutput[robotNr][1],w);
  gui_drawSensor(bb_simRobots[robotNr],20,5,simoutput[robotNr][2],w);
  gui_drawSensor(bb_simRobots[robotNr],30,5,simoutput[robotNr][3],w);
  gui_drawSensor(bb_simRobots[robotNr],37,10,simoutput[robotNr][4],w);
  gui_drawSensor(bb_simRobots[robotNr],45,20,simoutput[robotNr][5],w);
  gui_drawSensor(bb_simRobots[robotNr],20,40,simoutput[robotNr][7],w);
  gui_drawSensor(bb_simRobots[robotNr],30,40,simoutput[robotNr][6],w);

  if(useRodSensor){
    w=(int)(50/ROD_RESOLUTION);
    gdk_gc_set_foreground(pen_simArea,
			  pal_simArea[GUI_BLACK]
			  );
    for(int i=0; i < ROD_RESOLUTION; i++){
      gdk_draw_rectangle(bb_simRobots[robotNr],
			 pen_simArea,
			 1,
			 w*i,70-(int)(simoutput[robotNr][RodSensor+i]*10),
			 w*i+w,70);
    } 
  } 
  if(useEnergy){
    gdk_gc_set_foreground(pen_simArea,
			  pal_simArea[GUI_GREEN]
			  );
    gdk_draw_rectangle(bb_simRobots[robotNr],
		       pen_simArea,
		       1,
		       0,73,
		       (int)(50*simoutput[robotNr][EnergySensor]),83);
  }
  gdk_threads_leave();
}

void gui_drawRobot(int x, int y, double direction, int radius, int layer, int color){
  int x2,y2;
  float aL;

  gdk_threads_enter();
  gdk_gc_set_foreground(pen_simArea,
			pal_simArea[(color%GUI_COLORS) + 1]
			);
  if(layer==2){
    if(robotType==1){
      x2=(int)(x+radius*cos(g_DGRtoRAD(direction)));
      y2=(int)(y+radius*sin(g_DGRtoRAD(direction)));
      
      gdk_draw_line(bg_simArea,pen_simArea,x,y,
	    x2,y2);
      aL=radius*0.8;
      gdk_draw_line(bg_simArea,pen_simArea,x2,y2,
	      (int)(x+aL*cos(g_DGRtoRAD(direction-15))),
	      (int)(y+aL*sin(g_DGRtoRAD(direction-15))));
      gdk_draw_line(bg_simArea,pen_simArea,x2,y2,
	      (int)(x+aL*cos(g_DGRtoRAD(direction+15))),
	      (int)(y+aL*sin(g_DGRtoRAD(direction+15))));
      if(useRodSensor){
	gdk_draw_line(bg_simArea,pen_simArea,x,y,
		      (int)(x+xScale*ROD_MAX_DISTANCE*cos(g_DGRtoRAD(direction-(int)(ROD_ANGLE/2)))),
		      (int)(y+xScale*ROD_MAX_DISTANCE*sin(g_DGRtoRAD(direction-(int)(ROD_ANGLE/2)))));
	gdk_draw_line(bg_simArea,pen_simArea,x,y,
		      (int)(x+xScale*ROD_MAX_DISTANCE*cos(g_DGRtoRAD(direction+(int)(ROD_ANGLE/2)))),
		      (int)(y+xScale*ROD_MAX_DISTANCE*sin(g_DGRtoRAD(direction+(int)(ROD_ANGLE/2)))));
      }
    }
    else{
      gdk_draw_line(bg_simArea,pen_simArea,(int)(x+radius*cos(g_DGRtoRAD(direction-90))),
	      (int)(y+radius*sin(g_DGRtoRAD(direction-90))),
	      (int)(x+radius*cos(g_DGRtoRAD(direction+90))),
	      (int)(y+radius*sin(g_DGRtoRAD(direction+90))));
    }
    if(robotBody){
      gdk_draw_arc(bg_simArea,
		   pen_simArea,
		   0,
		   x-radius,y-radius,
		   2*radius,2*radius,
		   0,360*64);
    }
  }
  else{
    if(robotType==1){
      x2=(int)(x+radius*cos(g_DGRtoRAD(direction)));
      y2=(int)(y+radius*sin(g_DGRtoRAD(direction)));

      gdk_draw_line(bb_simArea,pen_simArea,x,y,
	    x2,y2);
      aL=radius*0.8;
      gdk_draw_line(bb_simArea,pen_simArea,x2,y2,
	      (int)(x+aL*cos(g_DGRtoRAD(direction-15))),
	      (int)(y+aL*sin(g_DGRtoRAD(direction-15))));
      gdk_draw_line(bb_simArea,pen_simArea,x2,y2,
	      (int)(x+aL*cos(g_DGRtoRAD(direction+15))),
	      (int)(y+aL*sin(g_DGRtoRAD(direction+15))));
      if(useRodSensor){
	gdk_draw_line(bb_simArea,pen_simArea,x,y,
		      (int)(x+xScale*ROD_MAX_DISTANCE*cos(g_DGRtoRAD(direction-(int)(ROD_ANGLE/2)))),
		      (int)(y+xScale*ROD_MAX_DISTANCE*sin(g_DGRtoRAD(direction-(int)(ROD_ANGLE/2)))));
	gdk_draw_line(bb_simArea,pen_simArea,x,y,
		      (int)(x+xScale*ROD_MAX_DISTANCE*cos(g_DGRtoRAD(direction+(int)(ROD_ANGLE/2)))),
		      (int)(y+xScale*ROD_MAX_DISTANCE*sin(g_DGRtoRAD(direction+(int)(ROD_ANGLE/2)))));
      }
    }
    else{
      gdk_draw_line(bb_simArea,pen_simArea,(int)(x+radius*cos(g_DGRtoRAD(direction-90))),
	      (int)(y+radius*sin(g_DGRtoRAD(direction-90))),
	      (int)(x+radius*cos(g_DGRtoRAD(direction+90))),
	      (int)(y+radius*sin(g_DGRtoRAD(direction+90))));
    }
    if(robotBody){
      gdk_draw_arc(bb_simArea,
		   pen_simArea,
		   0,
		   x-radius,y-radius,
		   2*radius,2*radius,
		   0,360*64);
    }
  }
  gdk_threads_leave();
}


void gui_drawGripper(int *a, int *b, int layer, int color,int c){
  gdk_threads_enter();
  gdk_gc_set_foreground(pen_simArea,
	 		pal_simArea[(color%GUI_COLORS) + 1]
			);
  if(layer==2){ 
    gdk_draw_line(bg_simArea,pen_simArea,a[0],a[1],a[2],a[3]);
    gdk_draw_line(bg_simArea,pen_simArea,b[0],b[1],b[2],b[3]);
  }
  else{
    gdk_draw_line(bb_simArea,pen_simArea,a[0],a[1],a[2],a[3]);
    gdk_draw_line(bb_simArea,pen_simArea,b[0],b[1],b[2],b[3]);
    //   gdk_draw_line(bb_simArea,pen_simArea,a[0],a[1],b[0],b[1]);
    if(c>2){
      gdk_draw_line(bb_simArea,pen_simArea,a[4],a[5],b[4],b[5]);
      gdk_draw_line(bb_simArea,pen_simArea,a[4],a[5],a[6],a[7]);
      gdk_draw_line(bb_simArea,pen_simArea,b[4],b[5],b[6],b[7]);
      gdk_draw_line(bb_simArea,pen_simArea,a[6],a[7],b[6],b[7]);
    }
  }
  gdk_threads_leave();
  
}

void gui_clear(){
  gdk_threads_enter();
  gdk_gc_set_foreground(pen_simArea,
			pal_simArea[GUI_WHITE]
			);
  gdk_draw_rectangle(bb_simArea,
                     pen_simArea,
                     1,
                     1,1,
                     sim_area_x_size,sim_area_y_size);
  gdk_draw_rectangle(bg_simArea,
                     pen_simArea,
                     1,
                     1,1,
                     sim_area_x_size,sim_area_y_size);
  gdk_threads_leave();
}


void gui_drawWall(int x1, int y1, int x2, int y2){
  gdk_threads_enter();
  gdk_gc_set_foreground(pen_simArea,
			pal_simArea[GUI_BLACK]
			);
  gdk_draw_line(bg_simArea,pen_simArea,x1,y1,x2,y2);
  gdk_threads_leave();
}

void gui_drawSObst(int x, int y, int radius){
  gdk_threads_enter();
  gdk_gc_set_foreground(pen_simArea,
			pal_simArea[GUI_RED]
			);
  gdk_draw_arc(bb_simArea,
               pen_simArea,
               0,
               x-radius,y-radius,
               2*radius,2*radius,
               0,360*64);
  gdk_threads_leave();
}

void gui_drawObst(int x, int y, int radius){
  gdk_threads_enter();
  gdk_gc_set_foreground(pen_simArea,
			pal_simArea[GUI_RED]
			);
  gdk_draw_arc(bg_simArea,
               pen_simArea,
               0,
               x-radius,y-radius,
               2*radius,2*radius,
               0,360*64);
  gdk_threads_leave();
}

void gui_drawZone(int x, int y, int radius){
  gdk_threads_enter();
  gdk_gc_set_foreground(pen_simArea,
			pal_simArea[GUI_GREEN]
			);
  gdk_draw_arc(bg_simArea,
               pen_simArea,
               0,
               x-radius,y-radius,