light.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 "light.h"

float Light::rlight[27][180][8];
float Light::r;
float Light::MinLightDistance;
float Light::step;
/**
 * Constructor for Light, creates a new light source
 * @param fp A file descriptor open for reading which contains the data for the new lightsource.
 */
Light::Light(FILE *fp){
     fscanf(fp,"light %f %f\n", 
	    &x,
	    &y
	    );
   if(verboseLevel>0)
     printf("Light x %f y %f \n",x,y);
}

/**
 * Constructor for Light, creates a new light source
 * @param str A string  which contains the data for the new lightsource.
 */
Light::Light(char *str){
     sscanf(str,"light %f %f\n", 
	    &x,
	    &y
	    );
   if(verboseLevel>0)
     printf("Light x %f y %f \n",x,y);
}


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

/**
 * Initialize all lightsources. (static class members)
 * @param fileName The name of the camerafile to read.
 */
void Light::init(char *fileName){
  FILE *fp;
  int    i,v,s,steps,angles,sensors;

  if(verboseLevel>0) printf("***[Init light objects]***\n");
  fp=fopen(fileName, "r");
  if(fp==NULL){
    printf("Couldn't open light 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",&r);
  if(verboseLevel>0) printf("- MAX DISTANS %f\n",r);
  fscanf(fp,"MIN DISTANS %f\n",&MinLightDistance);
  if(verboseLevel>0) printf("- MIN DISTANS %f\n",MinLightDistance);
  for (v=0; v < 27; v++){
    fscanf(fp,"TURN %d\n",&v);
    for (i = 0; i < 180; i++){
      for (s=0; s < 8; s++){
	fscanf(fp,"%f ",&rlight[v][i][s]);
      }
      fscanf(fp,"\n");
    }
  }
  step = r / steps;
  fflush(fp);   
  fclose(fp);
}

/**
 * Get the relative angle between the light source and the khepera robot.
 * @param kx Robot \a x coordinate.
 * @param ky Robot \a y coordinate.
 * @param kangle Robot angle (direction/orientation).
 * @return The relative angle.
 */
int Light::relangle(float kx, float ky, float kangle){
   
   double ox,oy;
   double oad;
   int oa;
   int ka;
   int ra;

   /* light is in direction */
   ox = (double)(x - kx);
   oy = (double)(y - ky);
   oad = atan2(ox,oy);
   oa =(int)g_RADtoDGR(oad);
   /* coordinate system is upside down */
   oa = oa * -1;

   /* rotate kangle to match oa */
   /* coordinate system is upside down */
   ka = g_adjustAngle360((int)kangle,270);
   ka = g_splitAngle180(ka);

   ra =(int)(oa - ka);
   ra = g_modulo(ra,360);   
   return ra;
}

/**
 * Get the sensor values that the light source 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 light is out of range.
 */
float * Light::sensorVal(float kx, float ky, float kangle, float robRadius){

  float dist;
  int relang,reldist;
  dist = g_distPoint(kx,ky,x,y); 
  if (dist < r){

    relang = relangle(kx,ky,kangle);
    relang = relang >> 1;
    if (dist < MinLightDistance)
      dist = 0;
    else 
      dist -= MinLightDistance;;
    reldist = (int)(dist / step);
    return rlight[reldist][relang];
  }
  else{
    return NULL;
  }
}