utils.c

RoboCup 2D 仿真组冠军源代码之1998年冠军队——CMUnited98源代码

/* -*- Mode: C -*- */

/* utils.C
 * CMUnited98 (soccer client for Robocup98)
 * Peter Stone <[e-mail]>
 * Computer Science Department
 * Carnegie Mellon University
 * Copyright (C) 1998 Peter Stone
 *
 * CMUnited-98 was created by Peter Stone, Manuela Veloso, and Patrick Riley
 *
 * You may copy and distribute this program freely as long as you retain this notice.
 * If you make any changes or have any comments we would appreciate a message.
 */

/* utils.C implements generic utility functions that are used throughout the program
 * The Time class allows clients to reason about time progressing even when the 
 * soccerserver clock is stopped.
 */

#include "utils.h"
#include "client.h"

void dump_core(char *msg){
  my_stamp;  fprintf(stderr,"dumping core");
  msg[1000000]=0;
  my_error("Core didn't dump");
}

void my_error(char *msg){
  fprintf(stderr,"PETER's ERROR (player %d, time %d.%d): %s\n",
	 Mem->MyNumber,Mem->CurrentTime.t,Mem->CurrentTime.s,msg);
  //msg[1000000]=0; /* to segfault */
  int tmp;
  scanf("%d",&tmp);
}

void my_error(char *msg, int param){
  char outstring[100];
  sprintf(outstring,msg,param);
  fprintf(stderr,"PETER's ERROR (player %d, time %d.%d): %s\n",
	 Mem->MyNumber,Mem->CurrentTime.t,Mem->CurrentTime.s,outstring);
  //msg[1000000]=0; /* to segfault */
  int tmp;
  scanf("%d",&tmp);
}

void my_error(char *msg, int param1, int param2){
  char outstring[100];
  sprintf(outstring,msg,param1,param2);
  fprintf(stderr,"PETER's ERROR (player %d, time %d.%d): %s\n",
	 Mem->MyNumber,Mem->CurrentTime.t,Mem->CurrentTime.s,outstring);
  int tmp;
  scanf("%d",&tmp);
}

void my_error(char *msg, int param1, int param2, int param3){
  char outstring[100];
  sprintf(outstring,msg,param1,param2,param3);
  fprintf(stderr,"PETER's ERROR (player %d, time %d.%d): %s\n",
	 Mem->MyNumber,Mem->CurrentTime.t,Mem->CurrentTime.s,outstring);
  int tmp;
  scanf("%d",&tmp);
}

void my_error(char *msg, int param1, int param2, int param3, int param4){
  char outstring[100];
  sprintf(outstring,msg,param1,param2,param3,param4);
  fprintf(stderr,"PETER's ERROR (player %d, time %d.%d): %s\n",
	 Mem->MyNumber,Mem->CurrentTime.t,Mem->CurrentTime.s,outstring);
  int tmp;
  scanf("%d",&tmp);
}

void my_error(char *msg, float param){
  char outstring[100];
  sprintf(outstring,msg,param);
  fprintf(stderr,"PETER's ERROR (player %d, time %d.%d): %s\n",
	 Mem->MyNumber,Mem->CurrentTime.t,Mem->CurrentTime.s,outstring);
  int tmp;
  scanf("%d",&tmp);
}

void my_error(char *msg, float param1, float param2){
  char outstring[100];
  sprintf(outstring,msg,param1,param2);
  fprintf(stderr,"PETER's ERROR (player %d, time %d.%d): %s\n",
	 Mem->MyNumber,Mem->CurrentTime.t,Mem->CurrentTime.s,outstring);
  int tmp;
  scanf("%d",&tmp);
}

void my_error(char *msg, float param1, float param2, float param3){
  char outstring[100];
  sprintf(outstring,msg,param1,param2,param3);
  fprintf(stderr,"PETER's ERROR (player %d, time %d.%d): %s\n",
	 Mem->MyNumber,Mem->CurrentTime.t,Mem->CurrentTime.s,outstring);
  int tmp;
  scanf("%d",&tmp);
}

void my_error(char *msg, float param1, float param2, float param3, float param4){
  char outstring[100];
  sprintf(outstring,msg,param1,param2,param3,param4);
  fprintf(stderr,"PETER's ERROR (player %d, time %d.%d): %s\n",
	 Mem->MyNumber,Mem->CurrentTime.t,Mem->CurrentTime.s,outstring);
  int tmp;
  scanf("%d",&tmp);
}

int closest_int( float x ){
  if ( x < 0 ) x -= 0.5;
  else         x += 0.5;
  return (int) x;
}

void NormalizeAngleDeg(int *ang){
  if (fabs(*ang) > 5000)
    my_error("Huge angle passed to NormalizeAngleDeg");
  while (*ang > 180) *ang-=360;
  while (*ang < -180) *ang+=360;
}

void NormalizeAngleDeg(AngleDeg *ang){
  if (fabs(*ang) > 5000)
    my_error("Huge angle passed to NormalizeAngleDeg");
  while (*ang > 180) *ang-=360;
  while (*ang < -180) *ang+=360;
}

void NormalizeAngleRad(AngleRad *ang){
  while (*ang > M_PI) *ang-=2*M_PI;
  while (*ang < -M_PI) *ang+=2*M_PI;
}

AngleDeg GetNormalizeAngleDeg(AngleDeg ang){
  if (fabs(ang) > 5000)
    my_error("Huge angle passed to GetNormalizeAngleDeg");
  while (ang > 180) ang-=360;
  while (ang < -180) ang+=360;
  return ang;
}

float GetDistance(float *x, float *y, float *a, float *b){
  return sqrt((*x-*a)*(*x-*a) + (*y-*b)*(*y-*b));
}

float weighted_avg(float val1, float val2, float w1, float w2){
  return (val1*w1 + val2*w2)/(w1+w2);
}

float SumGeomSeries(float first_term, float r, int n)
{
  return first_term * (Exp(r, n) - 1) / (r - 1);
}

float SolveForSumGeomSeries(float first_term, float r, float sum)
{
  if (r < 0)
    my_error("SolveForSumGeomSeries: can't take r < 0");
  float temp = sum * (r-1) / first_term + 1;
  if (temp <= 0)
    return -1.0;
  return log(temp) / log(r);
}





/****************************************************************************/
/****************************************************************************/
/****************************************************************************/

Time Time::operator -(const int &a)
{ 
  if ( s==0 && t-a > Mem->LastStartClockTime.t ) /* default case */
    return Time(t-a,0);

  if ( s>0 ){ 
    if ( a <= s ) 
      return Time(t,s-a); /* Just take off from stopped time */
    else{ /* a > s */
      Time new_time = Time( t-(a-s),0 );  /* take off from stopped time, then server time */
      if ( new_time < Mem->LastStartClockTime ) 
	my_error ("can't be sure of this subtraction (1)");
      return new_time;
    }
  }
  else{ /* t-a <= Mem->LastStartClockTime.t */
    int stopped_time =  a - (t - Mem->LastStartClockTime.t);  
    if ( stopped_time > Mem->LastStartClockTime.s ) {
      if ( !Mem->LastStartClockTime.t ) /* if ==0, OK---account for players starting at different times */
	return Time(0,0);
      int tmp = Mem->LastStartClockTime.t - (stopped_time - Mem->LastStartClockTime.s);
      if ( tmp <= Mem->SecondLastStartClockTime.t ) 
	my_error("can't be sure of this subtraction (2) %d %d %d  %d",t,s,a,Mem->LastStartClockTime.s);
      return Time( tmp, 0 );
    }
    return Time( Mem->LastStartClockTime.t, Mem->LastStartClockTime.s - stopped_time );
  }
}

int  Time::operator -(const Time &a)
{
  if ( s==0 ){
    if ( a.t < Mem->LastStartClockTime.t ){
      if ( a.t <= Mem->SecondLastStartClockTime.t )
	my_error("Can't be sure of this subtraction (3) %d %d  -  %d %d",t,s,a.t,a.s);
      return (t - a.t) + Mem->LastStartClockTime.s;
    }
    if ( a.t > Mem->LastStartClockTime.t )
      return (t - a.t);
    if ( a.t == Mem->LastStartClockTime.t )
      return (t - a.t) + (Mem->LastStartClockTime.s - a.s);
  }
  else if ( s > 0 ){
    if ( a.t <= Mem->SecondLastStartClockTime.t )   /* If they're equal, it's OK */
      my_error("Can't be sure of this subtraction (4) %d %d  %d %d",t,s,a.t,a.s);
    if ( a.t < Mem->LastStartClockTime.t )
      return Mem->LastStartClockTime.s + (t - a.t);
    else if ( a.t == Mem->LastStartClockTime.t && a.t != t ) /* a is during the last stopped interval */
      return ( s + ( Mem->LastStartClockTime.s - a.s ) ) + (t - a.t);
    return (s - a.s) + (t - a.t);
  }
  else /* s<0 */
    my_error("s shouldn't be <0");
  return 0;
}

Time Time::operator +(const int &a)
{
  if ( s==0 && t > Mem->LastStartClockTime.t && t+a < Mem->CurrentTime.t ) /* default case */
    return Time(t+a,0);

  Time new_time;
  
  if ( s==0 ){
    if ( Mem->LastStartClockTime.t > t ){ /* Could've missed one already */
      my_error("Can't be sure of this addition (1) %d %d",Mem->LastStartClockTime.t,t);
      new_time = Time(t+a,0);
    }
    if ( t+a < Mem->CurrentTime.t )
      new_time = Time( t+a,0 );
    else  /* t+a >= Mem->CurrentTime.t */
      new_time = Time( Mem->CurrentTime.t, a-(Mem->CurrentTime.t-t) );
  }
  else if ( s>0 ){
    if ( t == Mem->CurrentTime.t ) /* clock still stopped */
      new_time = Time( t,s+a );
    else{
      if ( Mem->LastStartClockTime.t != t ) /* Could've missed one already */
	my_error("Can't be sure of this addition (2)");
      new_time = Time ( t+(a-(Mem->LastStartClockTime.s - s)),0 );
    }
  }
  else /* s<0 */
    my_error("s shouldn't be <0");

  if ( new_time > Mem->CurrentTime ) /* clock might stop */
    my_error("Can't be sure of this addition (3)");

  return new_time;
}




/****************************************************************************/
/* These routines are to save time instead of using sscanf or atof, etc.    */
/* When passing **str_ptr, the pointer is advanced past the number          */
/* When passing  *str    , the pointer remains where it was before          */
/****************************************************************************/

double get_double(char **str_ptr){

  double d_frac, result;
  int  m_flag, d_flag;

  d_frac = 1.0;
  result = 0.0;
  m_flag = d_flag = 0;

  /* Advance to the beginning of the number */
  while( !isdigit(**str_ptr) && **str_ptr!='-' && **str_ptr!='.')
    (*str_ptr)++;

  /* Process the number bit by bit */
  while((**str_ptr!=')') && (**str_ptr!=' ') && (**str_ptr!='\n') && (**str_ptr!=',')){
    if (**str_ptr=='.')
      d_flag=1;
    else if (**str_ptr=='-')
      m_flag=1;
    else if (d_flag){
      d_frac *= 10.0;
      result+=(double)(**str_ptr-'0')/d_frac;
    }
    else
      result=result*10.0+(double)(**str_ptr-'0');
    (*str_ptr)++;
  }
  if (m_flag)
    result=-result;

  //printf("%.1f\n",result);

  return result;
}

/* Get the number, but don't advance pointer */

double get_double(char *str){
  char **str_ptr = &str;
  return get_double(str_ptr);
}

/****************************************************************************/

float get_float(char **str_ptr){

  float d_frac, result;
  int  m_flag, d_flag;