utils.c

1999年卡耐基梅陇大学的2D仿真源代码

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

/* utils.C
 * CMUnited99 (soccer client for Robocup99)
 * Peter Stone <[e-mail]>
 * Computer Science Department
 * Carnegie Mellon University
 * Copyright (C) 1999 Peter Stone
 *
 * CMUnited-99 was created by Peter Stone, Patrick Riley, and Manuela Veloso
 *
 * 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.
 * For more information, please see http://www.cs.cmu.edu/~robosoccer/
 */


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

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

#define MY_ERROR_LOG_LEVEL 5

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);
  Mem->LogAction3(MY_ERROR_LOG_LEVEL, "MyError: %s", msg);
  //msg[1000000]=0; /* to segfault */
  if (Mem->CP_stop_on_error) {
    int tmp;
    scanf("%d",&tmp);
  }
}

void my_error(char *msg, int param)
{
  char outstring[100];
  sprintf(outstring,msg,param);
  my_error(outstring);
}

void my_error(char *msg, int param1, int param2)
{
  char outstring[100];
  sprintf(outstring,msg,param1,param2);
  my_error(outstring);
}

void my_error(char *msg, int param1, int param2, int param3)
{
  char outstring[100];
  sprintf(outstring,msg,param1,param2,param3);
  my_error(outstring);
}

void my_error(char *msg, int param1, int param2, int param3, int param4)
{
  char outstring[100];
  sprintf(outstring,msg,param1,param2,param3,param4);
  my_error(outstring);
}

void my_error(char *msg, int param1, int param2, int param3, int param4, int param5)
{
  char outstring[100];
  sprintf(outstring,msg,param1,param2,param3,param4,param5);
  my_error(outstring);
}

void my_error(char *msg, int param1, int param2, int param3, int param4, int param5, int param6)
{
  char outstring[100];
  sprintf(outstring,msg,param1,param2,param3,param4,param5,param6);
  my_error(outstring);
}

void my_error(char *msg, int param1, int param2, int param3, int param4, int param5, char c1, int param6)
{
  char outstring[100];
  sprintf(outstring,msg,param1,param2,param3,param4,param5,c1,param6);
  my_error(outstring);
}

void my_error(char *msg, float param)
{
  char outstring[100];
  sprintf(outstring,msg,param);
  my_error(outstring);
}

void my_error(char *msg, float param1, float param2)
{  char outstring[100];
  sprintf(outstring,msg,param1,param2);
  my_error(outstring);
}

void my_error(char *msg, float param1, float param2, float param3)
{
  char outstring[100];
  sprintf(outstring,msg,param1,param2,param3);
  my_error(outstring);
}

void my_error(char *msg, float param1, float param2, float param3, float param4)
{
  char outstring[100];
  sprintf(outstring,msg,param1,param2,param3,param4);
  my_error(outstring);
}

void my_error(char *msg, float param1, int param2)
{
  char outstring[100];
  sprintf(outstring,msg,param1,param2);
  my_error(outstring);
}

void my_error(char *msg, char* param)
{
  char outstring[100];
  sprintf(outstring,msg,param);
  my_error(outstring);
}

void my_error(char *msg, char param1, int param2, int param3)
{
  char outstring[100];
  sprintf(outstring,msg,param1,param2,param3);
  my_error(outstring);
}

void my_error(char *msg , char param1, int param2, float param3, float param4)
{
  char outstring[100];
  sprintf(outstring,msg,param1,param2,param3,param4);
  my_error(outstring);
}

float int_pow(float x, int p)
{
  if (p < 0) 
    return (1.0 / int_pow(x,-p));
  else {
    float ans = 1.0;
    for (int i=0; i<p; i++)
      ans *= x;
    return ans;
  }
}



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 SolveForLengthGeomSeries(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);
}

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



/* returns a pointer to a static buffer, so be careful! */
char* repeat_char(char c, int n)
{
  const int MAX_REP = 100;
  static char out[MAX_REP+1];
  if (n > MAX_REP)
    my_error("repeat_char: asking for too many characters");
  for (int i=0; i<n; i++)
    out[i] = c;
  out[n] = 0;
  return out;
}


const char char_for_num_array[16] =
{'0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F'};




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

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 not 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;
}

Bool Time::CanISubtract(const Time &a)
{
  if ( s==0 ){
    if ( a.t < Mem->LastStartClockTime.t ){
      if ( a.t <= Mem->SecondLastStartClockTime.t )
	return FALSE;
      return TRUE;
    }
    return TRUE;
  }
  else if ( s > 0 ){
    if ( a.t <= Mem->SecondLastStartClockTime.t )   /* If they're equal, it's not OK */
      return FALSE;
    return TRUE;
  }
  else /* s<0 */
    my_error("s shouldn't be <0");
  return FALSE;
}




/****************************************************************************/
/* 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!='.')