geom.c

遗传算法和神经网络结合

/*
  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 "geom.h"
#include "sin.h"

/** 
 *  A sinus function using lookup tables
 *  @see g_cos()
 *  @param ang The angle
 *  @return \f$sin(ang)\f$
 */

double g_sin(double ang){
  return(sin_tbl[g_modulo(ang * 360/M_PI,720)]);
}
/**
 * A cosinus function using lookup tables
 * @see g_sin()
 * @param ang The angle
 * @return \f$cos(ang)\f$
 */
double g_cos(double ang){
  return(cos_tbl[g_modulo(ang * 360/M_PI,720)]);
}

/**
 * Geometric library init function.
 * Must be called before using g_rans() and g_mrand()
 * @see g_rans()
 * @see g_mrand()
 * @return none
 */
void geometricInit(){
  srand((unsigned int)time(0));
}

/**
 * Correct and adjust an angle to be in the range [0..360].
 * @param ang Angle i degrees
 * @param corr The rotation of the angle
 * @return \f$mod(ang - corr, 360)\f$
 */
int g_adjustAngle360(int ang,int corr){
  return(g_modulo(ang-corr,360));
}

/**
 * Adjust angle to be in the range [-180..180]
 * @param The angle
 * @return The adjustet angle
 */
int g_splitAngle180(int ang){
  ang -= 180;
  if(ang < -180)
    ang += 360;
  return ang;
}

/**
 * Calculates modulus of a, b
 * @param a The number to calculate modulus on
 * @param b The divider
 * @return \f$mod(a,b)\f$
 */
int g_modulo(int a,int b){
  if(a < 0 ){
    while(a < 0)
      a += b;
  }else{
    a = a % b;
  }
  return a;
}
/**
 * Calculates modulus of \f$a, b\f$ where \f$a\f$ is a double
 * @param a The number to calculate modulus on
 * @param b The divider
 * @return \f$mod(ab,b)\f$
 */

int g_modulo(double ab,int b){
  int a = (int)ab;
  if(a < 0 ){
    while(a < 0)
      a += b;
  }else{
    a = a % b;
  }
  return a;
}

/**
 * Produces a random number in the range [0..n]
 * @param n The upper limit
 * @return The random number
 */
float g_rans(double n){
  return (n * (rand()/(RAND_MAX + 1.0)));
}

/**
 * Produces a random number in the range [0..n]
 * @param n The upper limit
 * @return The random number
 */
int g_mrand(int n){
  return (int)fabs((rand()*(double)n)/(RAND_MAX+1));
}

/**
 * Converts an angle in radians to degrees
 * @param r as radian
 * @return r in degrees
 */
double g_RADtoDGR(double r){
  return(r/(M_PI/180));
}
/**
 * Converts an angle in degrees to radians
 * @param d in degrees
 * @return d in radians
 */
double g_DGRtoRAD(double d){ 
  return(d*(M_PI/180));
}

/**
 * \deprecated Calculates the distance between a point and a line
 * 
 */
double g_oldlineDist(float px,float py,float ax,float ay,float bx,float by){ 
  double d1,d2,d3; 
  d1 = ((px-ax)*(px-ax)) + ((py-ay)*(py-ay));      
  d2 = ((px-bx)*(px-bx)) + ((py-by)*(py-by));      
  d3 = ((ax-bx)*(ax-bx)) + ((ay-by)*(ay-by));           
  
  return(sqrt(d1 - (((d2 - d3 - d1) * (d2 - d3 - d1)) / (4 * d3)))); 
} 


/**
 * Calculates the distance between \f$ (x,y) (x_1,y_1) \f$
 * @param x Point 1 x
 * @param y Point 1 y
 * @param x1 Point 2 x
 * @param y1 Point 2 y
 * @return \f$sqrt((x-x1)^2+(y-y1)^2)\f$
 */

double g_distPoint (float x,float y, float  x1,float  y1){
   return(sqrt(((x-x1)*(x-x1)) + ((y-y1)*(y-y1))));
}

/**
 * Calculates the angle of a line
 * Line angle as seen as follow:
 *    - A horizential line has the angle zero
 *    - All angles are calculated from the fact that x1 and y1 are the origo
 *                    270
 *    -  quadrants   2 - 3
 *               180 | + | 0
 *                   1 - 0
 *                    90
 *  @param x1 Starting point of the line, x
 *  @param y1 Starting point of the line, y
 *  @param x2 End point of the line, x
 *  @param y2 End point of the line, y
 *  @return The angle in degrees [0..360]
 */

double g_lineAngle(float x1, float y1, float x2, float y2){
  int quadrant;
  double ang;
  ang=0.0;
  // Split the problems into the four quadrants
  if(x2 > x1){
    if(y2 > y1){
      quadrant = 0;
    }
    else{
      quadrant = 3;
    }
  }
  else{
    if(y2 > y1){
      quadrant = 1;
    }
    else{
      quadrant = 2;
    }
  }
  switch(quadrant){
  case 2:
    if(x2!=x1){
      ang = g_RADtoDGR(atan((y2-y1)/(x2-x1)));
      ang += 180;
    }
    else{
      ang = 270;
    }
    break;
  case 1:
    if(x2!=x1){
      ang = 180 + g_RADtoDGR(atan((y2-y1)/(x2-x1)));
    }
    else{
      ang = 90;
    }
    break;
  case 3:
    ang = 360 - fabs(g_RADtoDGR(atan((y2-y1)/(x2-x1))));
    break;
  case 0:
    ang = fabs(g_RADtoDGR(atan((y2-y1)/(x2-x1))));
  break;
  }
  return(ang);
}

/**
 * Calculate the angle of a line [-180..180]
 *  @param x  Point 1 of the line
 *  @param y  Point 1 of the line
 *  @param x2 Point 2 of the line
 *  @param y2 Point 2 of the line
 *  @return \f$ atan2(dy^2,dx^2) \f$
 */
double g_lineAngle180(int x, int y, int x1, int y1){

    double dx,dy,res;

    dx =(double)x1-x;
    dy =(double)y1-y;
    res=g_RADtoDGR(atan2((dy*dy),(dx*dx)));
    return(res);
}
/**
 * Calculate the angle of a line [-180..180]
 *  @param x  Point 1 of the line
 *  @param y  Point 1 of the line
 *  @param x2 Point 2 of the line
 *  @param y2 Point 2 of the line
 *  @return \f$ atan2(dy^2,dx^2) \f$
 */
double g_lineAngle180(float x, float y, float x1,float y1){

    double dx,dy,res;

    dx =(double)x1-x;
    dy =(double)y1-y;
    res=g_RADtoDGR(atan2((dy*dy),(dx*dx)));
    return(res);
 }


/**
 * \deprecated It takes in input the amoeba and the food coordinate
 * \deprecated and calculate the absolute angle
 */

double g_mang (int x, int  y, int  x1, int  y1, double dist){

   double angolo;
   int       dxang,dyang;
   double    buffer;
   double    alfa,pi2;
   double temp;
   if  (x==x1 && y==y1)
      angolo=0.0;
     else
      {
       dxang=(x1-x);
       dyang=(y1-y);
       buffer=abs(dyang);
       pi2 =asin(1.0);
       temp = buffer/dist;
       if (temp>1.0) temp=1.0;
       alfa=asin(temp);
       alfa=(alfa/pi2)*90;
       if (dxang>0)
	     {
              if (dyang>0);
	      if (dyang<0)
                 alfa=360-alfa; /*alfa+270;*/
	     }
        if (dxang<0)
	     {
              if (dyang>0) alfa=180-alfa; /*alfa+90;*/
	      if (dyang<0) alfa=alfa+180;
	      if (dyang==0)alfa=180;
	      }
	 if (dxang==0)
	     {
		 if (dyang>0) ;
		 if (dyang<0) alfa=270;
		 if (dyang==0)alfa=0;
	     }
	  angolo=alfa;
       }
     return(angolo);
}

/** 
 * Calculates the relative angle
 * @param absolute Absolute angle
 * @param khepera Relative angle
 * @return \f$ mod(abs(absolute - khepera),360) \f$
 */
int g_relAngle(float absolute, float khepera){

    return ( (int)fabs(absolute - khepera) % 360 ); 
}

/** 
 * Calculates the relative angle
 * @param absolute Absolute angle
 * @param khepera Relative angle
 * @return \f$ mod(abs(absolute - khepera),360) \f$
 */
double g_relAngle(double absolute, double kepera){

    double  relative;      

    relative = 360 - (kepera - absolute);    

    return (double)( (int)fabs(relative) % 360 ); 
}

/**
 * Adjust angle to be in range [0..360]
 * @param Angle in degrees
 * @return \f$ mod(and,360) \f$
 */

int  g_controlAngle(int ang){
  return(g_modulo(ang,360)); 
}

/**
 * Displace x with distance d and angle 
 * @see g_cos()
 * @see g_displaceY()
 * @see g_displaceYR()
 * @see g_displaceXR()
 * @param x Cord
 * @param d Distance
 * @param angle Angle in degrees
 * @return \f$ x + d*g\_cos(angle) \f$
 */
float g_displaceX(float x,float d,double angle){
    float   res;
    res=  x+d*g_cos(g_DGRtoRAD(angle));
    return(res);
}

/**
 * Displace y with distance d and angle 
 * @see g_sin()
 * @see g_displaceX()
 * @see g_displaceXR()
 * @see g_displaceYR()
 * @param y Cord
 * @param d Distance
 * @param angle Angle in degrees
 * @return \f$ y + d*g\_sin(angle) \f$
 */
float g_displaceY(float y,float d,double angle){
    float   res;
    res = y+d*g_sin(g_DGRtoRAD(angle));
    return(res);
}
/**
 * Displace y with distance d and angle 
 * @see g_sin()
 * @see g_displaceX()
 * @see g_displaceY()
 * @see g_displaceYR()
 * @param y Cord
 * @param d Distance
 * @param angle Angle in radians
 * @return \f$ y + d*g\_cos(angle) \f$
 */

float g_displaceXR(float x,float d,double angle){
    return( x+d*g_cos(angle));
}
/**
 * Displace y with distance d and angle 
 * @see g_cos()
 * @see g_displaceY()
 * @see g_displaceX()
 * @see g_displaceXR()
 * @param y Cord
 * @param d Distance
 * @param angle Angle in radians
 * @return \f$ y + d*g\_sin(angle) \f$
 */

float g_displaceYR(float y,float d,double angle){
  return(y+d*g_sin(angle));
}

/**
 * Check if two lines intersects.
 * @param a* Line A
 * @param b* Line B
 * @return 1 if they share a common point, otherwise 0.
 */
int g_linesCrosses(float ax1, float ay1, float ax2,float  ay2,
		   float bx1, float by1, float bx2,float  by2){
  float ta,tb;
  float ka,kb;
  /* 
     For once im going to share my thought process:

     Do line a and b share a common point?. 
     There will allways exist a solution unless a and b are 
     parallell in which case there angle will be the same.
     
     calculate angels
     
     ax2 = ax1 + ta*ka  \--\  where ta=1 (ax2-ax1) = ka = (ay2-ay1)
     ay2 = ay1 + ta*ka  /--/

     Simplified
     
     ka = (ax2-ax1)/(ay2-ay1)  => line equation is

     x = ax1 + at*(ax2-ax1)/(ay2-ay1)
     y = ay1 + at*(ax2-ax1)/(ay2-ay1)
     
     precalc 
     
     ka = (ax2-ax1)/(ay2-ay1)

     Same story different lines => 
     x = ax1 + ka*ta
     y = ay1 + ka*ta
     x = bx1 + kb*tb
     y = by1 + kb*tb

     unless a and b is parallell (ka!=kb) so:
     ax1 + ka*ta =  bx1 + kb*tb
     ax1 + ka*ta - bx1 = kb*tb
     (ax1 + ka*ta -bx1)/kb = tb (e1)

     ay1 + ka*ta = by1 + kb*tb
     ta = (by1 + kb*tb - ay1)/ka (e2)

     (e1 + e2) gives:
     ta = (by1 + kb * (ax1 + ka*ta - bx1)/kb )
     ta = by1 + ax1 + ka*ta
     ta - ka*ta = by1 + ax1
     ta (1 - ka) = by1 + ax1
     ta = (by1 + ax1)/(1-ka) (e3)
     if ta not is in the intervall 0.0 >= ta =< 1.0 then they dont
     share a common point (sure they do but not a point that we care
     about).
     tb = (ax1 + ka*ta - bx1)/kb 
     same check as tb.
     

     -* complete *-
  */
  ka = (ax2-ax1)/(ay2-ay1);
  kb = (bx2-bx1)/(by2-by1);
  if(ka == kb) // they are parallell baby
    return(0);
  ta = (by1 + ax1)/(1-ka);
  if(ta < 0.0 || ta > 1.0)
    return(0);
  tb = (ax1 + ka*ta - bx1)/kb;
  if(tb < 0.0 || tb > 1.0)
    return(0);
  return(1);
}

/**
 * Checks if a circle is between two parallell lines
 * @param a* Line A
 * @param b* Line B
 * @param c_* Center of the circle
 * @param rad Radius of the circle
 * @return The relation between A,B and the circle
 */

g_Relation g_circleBetweenLineAB(float ax1, float ay1, float ax2,float  ay2,
				 float bx1, float by1, float bx2,float  by2,
				 float c_x, float c_y, float rad){
  float ta,tb,ax,ay,bx,by,ad,bd;
  float tc,cx,cy,cd;
  ad=g_distLinePoint(ax1,ay1,ax2,ay2,c_x,c_y,&ax,&ay,&ta);
  bd=g_distLinePoint(bx1,by1,bx2,by2,c_x,c_y,&bx,&by,&tb);
  
  // Circle intersects with both line A and B
  if(rad > ad  && rad > bd)
    return(INT_LAB);
  // Circle intersects with line A
  if(rad > ad)
    return(INT_LA);
  // Circle intersects with line B
  if(rad > bd)
    return(INT_LB);

  // Circle centrum is outside ...
  // seen "vertically"
  if(ta < 0 || ta > 1.0)
    return(OUTSIDE);

  // Case where centrum is outside
  // seen "horizontally"
  cd=g_distLinePoint(ax1,ay1,bx1,by1,c_x,c_y,&cx,&cy,&tc);
  
  if(tc < 0 || tc > 1.0)
    return(OUTSIDE);
  // Only case left

  return(BETWEEN);
}
/**
 * Calculates the distance between a point and a line, and returns the nearest point.
 * @param ax1 Line x1
 * @param ay1 Line y1
 * @param ax2 Line x2
 * @param ay2 Line y2
 * @param bx Point x
 * @param by Point y
 * @param xt Returns nearest point on line x
 * @param yt Returns nearest point on line y
 * @param t  Returns at which t xt,yt is
 * @return The distance
 * @see g_distLinePoint(float,float,float,float,float,float)
 * @see g_distPoint()
 */

float g_distLinePoint(float ax1, float ay1, float ax2,float ay2,
		    float bx,float by, float *xt, float *yt, float *t){
  float ddxy;
  float dx;
  float dy;

  dx = ax2-ax1;
  dy = ay2-ay1;
  ddxy = dx*dx+dy*dy;

  *t=-(dx*(ax1-bx)+dy*(ay1-by))/ddxy;
  if(*t<0){
    *xt = ax1;
    *yt = ay1;
    return g_distPoint(bx,by,ax1,ay1);
  }
  else if(*t>1){
    *xt = ax2;
    *yt = ay2;
    return g_distPoint(bx,by,ax2,ay2);
  }
  else{
    *xt = (ax1+*t*dx-bx);
    *yt = (ay1+*t*dy-by);
    return sqrt(*xt**xt+*yt**yt);
  }
}

/**
 * Calculates the distance between a point and a line, and returns the nearest point.
 * @param ax1 Line x1
 * @param ay1 Line y1
 * @param ax2 Line x2
 * @param ay2 Line y2
 * @param bx Point x
 * @param by Point y
 * @return The distance
 * @see g_distLinePoint()
 * @see g_distPoint()
 */

double g_distLinePoint(float ax1, float ay1, float ax2, float ay2,float bx, float by){
  float dx;
  float dy;
  float ddxy;
  float xt,yt;
  float t;

  dx = ax2-ax1;
  dy = ay2-ay1;
  ddxy = dx*dx+dy*dy;
  
  t=-(dx*(ax1-bx)+dy*(ay1-by))/ddxy;
  if(t<0){
    return g_distPoint(bx,by,ax1,ay1);
  }
  else if(t>1){
    return g_distPoint(bx,by,ax2,ay2);
  }
  else{
    xt = (ax1+t*dx-bx)*(ax1+t*dx-bx);
    yt = (ay1+t*dy-by)*(ay1+t*dy-by);
    return sqrt(xt+yt);
  }
}