environ.cc

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

 * Check if robot \a robotNr is in a zone.
 * @param robotNr Which robot.
 * @return Returns the zone index else -1.
 */
int Environ::inZone(int robotNr) const{
  float rx,ry;

  rx=robot[robotNr]->getX();
  ry=robot[robotNr]->getY();

  for(int i=0; i < nrOfZones; i++){
    if(zone[i]->inZone(rx,ry,robotNr)){
      return i;
    }
  }
  return -1;
}

/**
 * Mark all zones not visited.
 * @see Zone::clearVisit()
 */
void Environ::resetZones(){
  for(int i=0; i < nrOfZones; i++){
    zone[i]->clearVisit();
  }
}
/**
 * Get how many different zones the robot have visited.
 * @param robotNr Which robot.
 * @return The number of zones visited.
 */
int Environ::zonesVisited(int robotNr) const{
  int visits = 0;

  for(int i=0; i < nrOfZones; i++){
    if(zone[i]->visits(robotNr)){
      visits++;
    }
  }  
  return visits;
}

/**
 * Resets small round obstacle with index \a whichObstacle.
 * @param whichObstacle Index of obstacle.
 * @see sobst::resetMove
 * @see sobst::taken
 */
void Environ::resetObstacle(int whichObstacle){
  float x,y;
  // if been carried
  sobst[whichObstacle]->resetMove();
  sobst[whichObstacle]->taken=0; 
  if (moveObstacles){
    x = g_rans(dx);
    y = g_rans(dy);
    while (notPlacedOk(whichObstacle,x,y,sobst[whichObstacle]->r)){
      x = g_rans(dx);
      y = g_rans(dy);
    }	   
    sobst[whichObstacle]->x = x;
    sobst[whichObstacle]->y = y;
  }
}

/**
 * Check if a _small_ round obstacle is placed ok. (Not placed on an another object)
 * @param obstNr Index of the obstacle.
 * @param x X coordinate of obstacle.
 * @param y Y coordinate of obstacle.
 * @param radius There for historical reasons.
 * @return 1 if collision with an object is detected otherwise 0.
 */
int Environ::notPlacedOk(int obstNr, float x, float y, float radius) const{
  float tmp=0;
  float r;

  r = sobst[obstNr]->r;
  for(int i=0; i < nrOfSObst; i++){
    if(i!=obstNr){
      tmp = g_distPoint(x, y, sobst[i]->x, sobst[i]->y) - sobst[i]->r;
      if(tmp < r)
	return 1;
    }
  }
  for(int i=0; i < nrOfObst; i++){
    tmp = g_distPoint(x, y, obst[i]->x, obst[i]->y) - obst[i]->r;
    if(tmp < r)
      return 1;
  }
  for(int i=0; i < nrOfWalls; i++){
     tmp = g_distLinePoint(wall[i]->x1, wall[i]->y1,
		      wall[i]->x2, wall[i]->y2,
		      x,y);
    if(tmp < r)
      return 1;
  }
  return 0;
}
/**
 * Load world for the environment, and initates all obstacles.
 * @param fileName The world to load.
 * @todo Fix so that we can have comments in world files and 
 * @todo ignore the number line with the obstacle counts.
 */
void Environ::loadWorld(const char *fileName){

   FILE *fp;
   float  radius=0;
   float  sradius=0;
   char   tmpStr[300];
   char   buf[200];
   if(fileName==NULL){
     printf("Environ::loadWorld No world file specified in option file\n");
     exit(-1);
   }
   if (verboseLevel > 0)
     printf("Loading world %s\n",fileName);
   fp=fopen(fileName, "r");
   if(fp==NULL){
     printf("Couldn't open worldfile %s for reading",fileName);
     exit(-1);
   }
   nrOfWalls = nrOfZones = nrOfLights = nrOfObst = nrOfSObst = nrOfStartPositions = 0;

   while(!feof(fp)){
     fgets(buf,200,fp);
     switch(buf[0]){
     case 'w':
       wall = (class Wall**) realloc(wall,sizeof(class Wall*) * (nrOfWalls + 1));
       wall[nrOfWalls] = new Wall(buf);
       if (wall[nrOfWalls]->x1 > dx)
	 dx =(int) wall[nrOfWalls]->x1;
       if (wall[nrOfWalls]->x2 > dx)
	 dx = (int) wall[nrOfWalls]->x2;
       if (wall[nrOfWalls]->y1 > dy)
	 dy = (int) wall[nrOfWalls]->y1;
       if (wall[nrOfWalls]->y2> dy)
	 dy = (int)wall[nrOfWalls]->y2; 
       nrOfWalls++;
       break;
     case 'z':
       zone = (class Zone**) realloc(zone,sizeof(class Zone*) * (nrOfZones + 1));
       zone[nrOfZones] = new Zone(buf);
       nrOfZones++;
       break;
     case 'l':
       light = (class Light**) realloc(light,sizeof(class Light*) * (nrOfLights + 1));
       light[nrOfLights] = new Light(buf);
       nrOfLights++;
       break;
     case 'r':
       switch(buf[1]){
       case 'a':
	 sscanf(buf,"radius %f\n", &radius);
	 break;
       case 'o':
	 obst = (class Obst**) realloc(obst, sizeof(class Obst*) * (nrOfObst + 1));
	 obst[nrOfObst] = new Obst(buf);
	 nrOfObst++;
	 break;
       default:
	 printf("ignoring %s in worldfile %s\n",buf,fileName);
       }
       break;
     case 's':
       switch(buf[1]){
       case 'r':
	 switch(buf[2]){
	 case 'a':
	   sscanf(buf,"sradius %f\n", &sradius);
	   break;
	 case 'o':
	   sobst = (class Sobst**) realloc(sobst, sizeof(class Sobst*) * (nrOfSObst + 1));
	   sobst[nrOfSObst] = new Sobst(buf);
	   nrOfSObst++;
	   break;
	 default:
	   printf("ignoring %s in worldfile %s\n",buf,fileName);
	 }
	 break;
       case 't':
	 start = (float**) realloc(start,(nrOfStartPositions+1)*sizeof(float*));
	 start[nrOfStartPositions] = (float *) malloc(3 * sizeof(float));
	 sscanf(buf,"start %f %f %f",
		&start[nrOfStartPositions][0],
		&start[nrOfStartPositions][1],
		&start[nrOfStartPositions][2]);
	 nrOfStartPositions++;
	 break;
       default:
	 printf("ignoring %s in worldfile %s\n",buf,fileName);
       }
       break;
     default:
       printf("ignoring %s in worldfile %s\n",buf,fileName);
     }
   }
   fclose(fp);

   /* init all */
   if(nrOfWalls>0){
     sprintf(tmpStr,"%s/%s",cameraPath,wallCamera);
     wall[0]->init(tmpStr);
   }
   if(nrOfObst > 0){
     obst[0]->setRadius(radius);   /* Common for all round obstacles (declared static in class Obst) */
     sprintf(tmpStr,"%s/%s",cameraPath,bigCamera);
     obst[0]->init(tmpStr);
   }
   if(nrOfSObst > 0){
     sprintf(tmpStr,"%s/%s",cameraPath,smallCamera);
     sobst[0]->init(tmpStr);
     sobst[0]->setRadius(sradius);
   }
   if(nrOfLights>0){
     sprintf(tmpStr,"%s/%s",cameraPath,lightCamera);     
     light[0]->init(tmpStr);
   }

#ifdef GUI
   gui_setWorldSize(dx, dy);
#endif /* GUI */
}
/**
 * Save the current world to file (here for the future - world editor).
 * @param worldFileName The name of file to save to.
 *
 */
void Environ::saveWorld(const char *worldFileName){
  int i,ii;
  FILE *fp;
  if(nrOfStartPositions > 0){
    for (i=0; i < nrOfStartPositions; i++){
      start[i][0] = (float) g_mrand(dx-40) + 20;	 
      start[i][1] = (float) g_mrand(dy-40) + 20;	 	   
      while (checkCrash(start[i][0],start[i][1]) != 0){
	start[i][0] = (float) g_mrand(dx-40) + 20;	
	start[i][1] = (float) g_mrand(dy-40) + 20;	 	 
      }	   
      start[i][2] = (float) g_mrand(360);	 	   	   
    } 	  
    fp=fopen(worldFileName, "w");
    if(fp==NULL){
      printf("Couldn't open world file %s for writing\n",worldFileName);
      exit(-1);
    }
    fprintf(fp,"%d %d %d %d %d %d\n",nrOfWalls ,nrOfZones,nrOfLights,nrOfObst,nrOfSObst,nrOfStartPositions);	 

    for (i=0; i < nrOfWalls; i++)
      fprintf(fp,"wall %f %f %f %f\n",
	      wall[i]->x1,wall[i]->y1,
	      wall[i]->x2,wall[i]->y2);

    for (i=0; i < nrOfZones; i++)
      fprintf(fp,"zone %f %f %f\n",
	      zone[i]->x,zone[i]->y,
	      zone[i]->r);

    for (i=0; i < nrOfLights; i++)
      fprintf(fp,"bulblight %f %f\n", 
	      light[i]->x,light[i]->y);

    if (nrOfObst > 0){
      fprintf(fp,"radius %f\n", obst[0]->r);                  

      for (i=0; i < nrOfObst; i++)
	fprintf(fp,"roundobst %f %f\n", 
		obst[i]->x,obst[i]->y);
    }

    if (nrOfSObst > 0){
      fprintf(fp,"sradius %f\n", sobst[0]->r);                         

      for (i=0; i < nrOfSObst; i++)
	fprintf(fp,"sroundobst %f %f\n", 
		sobst[i]->x,sobst[i]->y);       
    }

    for (i=0; i < nrOfStartPositions; i++){
      for (ii=0; ii < 3; ii++)        
	fprintf(fp,"%f ", start[i][ii]);	 
      fprintf(fp,"\n");	     
    }
    fclose(fp);       
  }
}
/**
 * Print the current world to stdout.
 */
void Environ::printWorld() const{
  int i,ii;
  printf("#world size = %dx%d \n",dx,dy);   
  printf("#%d walls  :",nrOfWalls);        
  for (i=0; i < nrOfWalls; i++)
    printf("%.0f %.0f %.0f %.0f  ",
	   wall[i]->x1,
	   wall[i]->y1,
	   wall[i]->x2,
	   wall[i]->y2
	   );
  printf("\n");
  if (nrOfZones > 0){
    printf("#%d zones  :",nrOfZones);        
    for (i=0; i < nrOfZones; i++)
      printf("%.0f %.0f %.0f  ",
	     zone[i]->x,
	     zone[i]->y,
	     zone[i]->r
	     );
    printf("\n");
  }
  if (nrOfLights > 0){
    printf("#%d light  :",nrOfLights);        	  
    for (i=0; i < nrOfLights; i++)
      printf("%.0f %.0f  ",
	     light[i]->x,
	     light[i]->y
	     );
    printf("\n");            
  }
  if (nrOfObst > 0){
    printf("#%d roundo :",nrOfObst);        	  	  
    printf("radius %.0f  ",obst[0]->r);          
    for (i=0; i < nrOfObst; i++)
      printf("%.0f %.0f  ", 
	     obst[i]->x,
	     obst[i]->y
	     );
    printf("\n");                 
  }
  if (nrOfSObst > 0){
    printf("#%d sroundo :",nrOfSObst);        	  	  
    printf("sradius %.0f  ", sobst[0]->r);          
    for (i=0; i < nrOfSObst; i++)
      printf("%.0f %.0f  ", 
	     sobst[i]->x,
	     sobst[i]->y
	     );
    printf("\n");                 
  }      
  if (nrOfStartPositions > 0){
    printf("starts\n");	     
    for (i=0; i < nrOfStartPositions; i++){
      for (ii=0; ii < 5; ii++)        
	printf("%.1f\t", start[i][ii]);	 
      printf("\n");	     
    } 
  }
}

/**
 * Method for placing all small round obstacle in a cluster.
 * @param cx Center X coordinate of the cluster.
 * @param cy Center Y coordinate of the cluster.
 * @param rd The distance to between the objects in the cluster.
 */
void Environ::placeSObstStack(float cx, float cy, float rd){
  int dimx = 0, dimy = 0;
  float r,sx,sy;
  int p,dx,dy;

  if(nrOfSObst>0){
    dimx = dimy = (int)sqrt(nrOfSObst);
    if(dimx*dimy != nrOfSObst)
      dimy++;

    r=sobst[0]->r;
    for(sx = cx-(r * dimx) - (rd*dimx)/2, p=0, dx=0; dx < dimx && p < nrOfSObst; dx++,sx+= (2 * r) + rd){
      for(sy = cy-(r * dimy) - (rd*dimy)/2 , dy=0; dy < dimy && p < nrOfSObst; dy++, sy+= (2 * r) + rd){
	sobst[p]->x = sx;
	sobst[p]->y = sy;
	p++;
      }
    }
  }
}

/**
 * Check if a robot has chrashed with something. 
 * @param whichRobot Which robot to check.
 * @param which Returns the index of the object which the robot crashed with or -1 if no crash.
 * @return The type of crash.
 * @see enum::crash_t
 */
crash_t Environ::checkCrash(int whichRobot, int *which) const{
  int i;
  double  dist;
  double  mindist;
  float   p1,p2;
  double direction;

  p1 = robot[whichRobot]->getX();
  p2 = robot[whichRobot]->getY();
  direction = robot[whichRobot]->direction;
  if(robot[whichRobot]->isaGripperRobot()){
    GripperRobot *rob;
    rob = (GripperRobot*)robot[whichRobot];
    if(rob->getCrashed()){
      *which = -1;
      return(GRIPPER_CRASH);
    }
  }
  mindist = robot[whichRobot]->getRadius();
  for(i=0;i<nrOfWalls;i++){
    dist = wall[i]->distR(p1,p2);
    if (dist < mindist){
      *which = i;
      return WALL_CRASH;
    }
  }
  for(i=0;i<nrOfObst;i++){
    dist = g_distPoint(p1,p2,obst[i]->x,obst[i]->y)-obst[0]->r;
    if (dist < mindist){
      *which = i;
      return OBST_CRASH;
    }
  }
  for(i=0; i < nrOfRobots; i++){
    dist = g_distPoint(p1, p2, robot[i]->getX(), robot[i]->getY()) - robot[i]->getRadius();
    if(dist < mindist && i!=whichRobot){
      *which = i;
      return ROBOT_CRASH;
    }
  }
  for(i=0;i<nrOfSObst;i++){
    if (sobst[i]->taken == 0.0){
      dist = g_distPoint(p1,p2,sobst[i]->x,sobst[i]->y)-sobst[0]->r;
      if (dist < mindist){
	*which = i;
	return SOBST_CRASH;
      }
    }
  }
 
  *which = -1;
  return(NO_CRASH);
}

/**