wall.cc

遗传算法和神经网络结合

/*
  YAKS, a Khepera simulator including a separate GA and ANN 
  (Genetic Algoritm, Artificial Neural Net).
  Copyright (C) 2000  Johan Carlsson ([e-mail])
  
  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 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.

 */

#include "wall.h"

float Wall::wall[20][180][8];
float Wall::MaxObstacleDistans;
float Wall::MinObstacleDistans;
float Wall::Step;

/**
 * Constructor for Wall, creates a new wall and precalces some values for the simulation.
 * @param fp A file descriptor open for reading which contains the data for the new wall.
 */

Wall::Wall(FILE *fp){
  float x;
  float y;
  fscanf(fp,"wall %f %f %f %f\n",
	 &x1,
	 &y1,
	 &x2,
	 &y2
	 );

  if(x1 > x2){
    x = x1;
    y = y1;
    x1 = x2;
    y1 = y2;
    x2 = x;
    y2 = y;
  }

  /* Precalc */
  dx = x2-x1;
  dy = y2-y1;
  ddxy = dx*dx+dy*dy;

  angle = atan2(dx,dy)*180/M_PI;

}

/**
 * Constructor for Wall, creates a new wall and precalces some values for the simulation.
 * @param str A string  which contains the data for the new wall.
 */

Wall::Wall(char *str){
  float x;
  float y;
  sscanf(str,"wall %f %f %f %f\n",
	 &x1,
	 &y1,
	 &x2,
	 &y2
	 );

  if(x1 > x2){
    x = x1;
    y = y1;
    x1 = x2;
    y1 = y2;
    x2 = x;
    y2 = y;
  }

  /* Precalc */
  dx = x2-x1;
  dy = y2-y1;
  ddxy = dx*dx+dy*dy;

  angle = atan2(dx,dy)*180/M_PI;

}


/**
 * Display the lookup table of the wall to stdout.
 *
 */
void Wall::print(){
  int d,a,s;
  for (d = 0; d < 20; d++){
    printf("TURN %d\n",d);
    for (a = 0; a < 180; a++){
      for (s=0; s < 8; s++){
	printf("%f ",wall[d][a][s]);
      }
      printf("\n");
    }
  }
}

/**
 * Deconstructor for Wall, does nothing.
 */
Wall::~Wall(){
}

/**
 * Initialize all walls. (static class members)
 * @param fileName The name of the camerafile to read.
 */
void Wall::init(char *fileName){
  FILE *fp;
  int d,a,s,tmp;
  int angles,sensors,steps;
  if(verboseLevel>0){
    printf("***[Init walls]***\n");
  }
  fp=fopen(fileName, "r");
  if(fp==NULL){
    printf("Couldn't open wall objects camera file %s for reading\n",fileName);
    exit(-1);
  }

  if(verboseLevel>0) printf("- %s\n",fileName);
  fscanf(fp,"ANGLES %d\n",&angles);
  if(verboseLevel>0) printf("- ANGLES %d\n",angles);
  fscanf(fp,"SENSORS %d\n",&sensors);
  if(verboseLevel>0) printf("- SENSORS %d\n",sensors);
  fscanf(fp,"STEPS %d\n",&steps);
  if(verboseLevel>0) printf("- STEPS %d\n",steps);
  fscanf(fp,"MAX DISTANS %f\n",&MaxObstacleDistans);
  if(verboseLevel>0) printf("- MAX DISTANS %f\n",MaxObstacleDistans);
  fscanf(fp,"MIN DISTANS %f\n",&MinObstacleDistans);
  if(verboseLevel>0) printf("- MIN DISTANS %f\n",MinObstacleDistans); 

  Step = (MaxObstacleDistans - MinObstacleDistans) / steps;   

  for (d = 0; d < 20; d++){
    fscanf(fp,"TURN %d\n",&tmp);
    if(tmp!=d){
      printf("Something is wrong in camerafile %s\n",fileName);
      exit(-1);
    }
    for (a = 0; a < 180; a++){
      for (s = 0; s < 8; s++){
	fscanf(fp,"%d ",&tmp);
	wall[d][a][s]=(float)(tmp/1024.0);
      }
      fscanf(fp,"\n");
    }
  }
  fflush(fp);    
  fclose(fp);
}

/**
 * Get the distance between the wall and a point.
 * @param rx Point \a x coordinate.
 * @param ry Point \a y coordinate.
 * @param The closest distance.
 */
double Wall::distR(float rx, float ry){
   float xt;
  float yt; 
  t=-(dx*(x1-rx)+dy*(y1-ry))/ddxy;
  if(t<0){
    return g_distPoint(rx,ry,x1,y1);
  }
  else if(t>1){
    return g_distPoint(rx,ry,x2,y2);
  }
  else{
    xt = (x1+t*dx-rx)*(x1+t*dx-rx);
    yt = (y1+t*dy-ry)*(y1+t*dy-ry);
    return sqrt(xt+yt);
  }
}

/**
 * Get the sensor values that the wall will produce on the robot.
 * @param kx Robot \a x coordinate.
 * @param ky Robot \a y coordinate.
 * @param kangle Robot angle (direction/orientation).
 * @param robRadius Robot radius.
 * @return An array of eight floats or NULL if the wall is out of range
 */
float *Wall::sensorVal(float x, float y, float kangle,float radius){
  float dist;
  float dtx;  /* point in wall, where wall emitts sensor values*/
  float dty;
  float rdty,rdtx;
  float rangle;
  int relang;
  int ka;
  
  dist = distR(x,y) - radius;
  if(dist < MaxObstacleDistans &&
     t >= 0 && t <= 1){

    
    dtx = x1+dx*t;
    dty = y1+dy*t;
    rdtx = dtx-x;
    rdty = dty-y;

    /* robot position seen from the wall */
    rangle=atan2(rdtx,rdty)*180/M_PI; 

    ka = g_adjustAngle360((int)kangle,270);
    ka = g_splitAngle180(ka);

    relang = (int)(ka + rangle)/2;
    relang = 180 - relang;

    if (dist < MinObstacleDistans){    
      return(wall[0][relang]);
    }
    else{  /*  Min < dist < Max */
      return(wall[(int)((dist-MinObstacleDistans)/Step)][relang]);
    }

  }
  else{/* the obstacle is to far away to be seen */ 
    return NULL;
  }
}