utils.c

RoboCup 2D 仿真组冠军源代码之1997年冠军队——CMUnited97源代码

/* utils.c
 * CMUnited-97 (soccer client for Robocup-97)
 * Peter Stone <[e-mail]>
 * Computer Science Department
 * Carnegie Mellon University
 * Copyright (C) 1997 Peter Stone
 *
 * CMUnited-97 was created by Peter Stone 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.
 */

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

#include "global.h"

my_error(char *msg){
  printf("PETER's ERROR (player %d, time %d): %s\n",
	 Mem->MyNumber,Mem->CurrentTime,msg);
  scanf("%d");
}

CleanAngle(int *ang){
  if (*ang > 180) *ang-=360;
  if (*ang < -180) *ang+=360;
}

CleanAngle(float *ang){
  if (*ang > 180) *ang-=360;
  if (*ang < -180) *ang+=360;
}

CleanAngleRad(float *ang){
  if (*ang > M_PI) *ang-=2*M_PI;
  if (*ang < -M_PI) *ang+=2*M_PI;
}

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

/****************************************************************************/
/* These routines are to save time instead of using sscanf or atof, etc.    */
/* *str must start with the number (no white space)                         */

double get_double(char *str){

  double d_frac, result;
  int  m_flag, d_flag;

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

  while((*str!=')') && (*str!=' ') && (*str!='\n')){
    if (*str=='.')
      d_flag=1;
    else if (*str=='-')
      m_flag=1;
    else if (d_flag){
      d_frac *= 10.0;
      result+=(double)(*str-'0')/d_frac;
    }
    else
      result=result*10.0+(double)(*str-'0');
    str++;
  }
  if (m_flag)
    result=-result;

  return result;
}

float get_float(char *str){

  float d_frac, result;
  int  m_flag, d_flag;

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

  while((*str!=')') && (*str!=' ') && (*str!='\n')){
    if (*str=='.')
      d_flag=1;
    else if (*str=='-')
      m_flag=1;
    else if (d_flag){
      d_frac *= 10.0;
      result+=(float)(*str-'0')/d_frac;
    }
    else
      result=result*10.0+(float)(*str-'0');
    str++;
  }
  if (m_flag)
    result=-result;

  return result;
}

int get_int(char *str){

  int result;
  int m_flag;

  result = 0;
  m_flag = 0;

  while((*str!=')') && (*str!=' ') && (*str!='\n')){
    if (*str=='-')
      m_flag=1;
    else
      result=result*10+(int)(*str-'0');
    str++;
  }
  if (m_flag)
    result=-result;

  return result;
}

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

void BubbleSort(int length, int *elements, float *keys){
  
/*  for (int a=0; a<length; a++){
    printf("%d--%.1f ",elements[a], keys[a]);
  }
  printf("\n");*/

  /* Sort the elements in increasing order according to the keys */
  float keytemp;
  int eltemp;
  for (int i=0; i<length; i++){
    for (int j=i+1; j<length; j++){
      if ( keys[j] < keys[i] ){
	keytemp = keys[i];
	keys[i] = keys[j];
	keys[j] = keytemp;
	eltemp = elements[i];
	elements[i] = elements[j];
	elements[j] = eltemp;
      }
    }
  }
/*  for (int a=0; a<length; a++){
    printf("%d--%.1f ",elements[a], keys[a]);
  }
  printf("\n");*/
}

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

int BinarySearch(int length, float *elements, float key){
  
  /* Assume the list is already sorted in increasing order */
  int lbound = 0, ubound = length;
  
  for ( int index = length/2; ubound-lbound > 0; index = lbound+(ubound-lbound)/2 ){
    /* printf("%d ",index); */
    if ( elements[index] == key ){
      lbound = ubound = index;
    }
    else if ( elements[index] < key ){
      lbound = index+1;
    }
    else {
      ubound = index-1;
    }
  }
 
  int toReturn = MAX(ubound,lbound);
  if (elements[toReturn] < key) toReturn++;  /* Guarantees >= key */

  return toReturn;
}

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

/* replace all occurrences in a string */
void StrReplace(char *str, char oldchar, char newchar){
  int i=0, numReplaced=0;
  int strLength = strlen(str);
  while ( i++ < strLength ){
    if ( str[i] == oldchar ){
      str[i] = newchar;
#if 0
      numReplaced++;
#endif
    }
    if ( i==1000 ) 
      my_error("String of length >1000?");
  }
#if 0
  printf("***Replaced %d %c's in string of length %d (%d): %s***\n",
	 numReplaced,oldchar,strlen(str),i,str);
#endif
}

/****************************************************************************/
/***************************   random stuff    ******************************/
/****************************************************************************/
/* From Andrew's C package                                                  */

int int_random(int n)
{
  static int FirstTime = TRUE;
  
  if ( FirstTime ){
    /* initialize the random number seed. */
    timeval tp;
    gettimeofday( &tp, NULL );
    srandom( (unsigned int) tp.tv_usec );
    FirstTime = FALSE;
  }

  if ( n > 2 )
    return( random() % n );
  else if ( n == 2 )
    return( ( (random() % 112) >= 56 ) ? 0 : 1 );
  else if ( n == 1 )
    return(0);
  else
  {
    printf("int_random(%d) ?\n",n);
    my_error("You called int_random(<=0)");
    return(0);
  }
}

float range_random(float lo, float hi)
{
  int x1 = int_random(10000);
  int x2 = int_random(10000);
  float r = (((float) x1) + 10000.0 * ((float) x2))/(10000.0 * 10000.0);
  return( lo + (hi - lo) * r );
}

int very_random_int(int n)
{
  int result = (int) range_random(0.0,(float)n);  /* rounds down */
  if ( result == n ) result = n-1;
  return(result);
}

/****************************************************************************/
/***************************   pointlist stuff   ****************************/
/****************************************************************************/

#define QIX(z) ((head + (z)) % maxsize)

PointQueue::PointQueue(int maxsize_)
{
  head = tail = 0;
  currsize = 0;
  maxsize = maxsize_;
  x =    new float[maxsize];
  y =    new float[maxsize];
  time = new int[maxsize];
}

PointQueue::~PointQueue(){
  delete x;
  delete y;
  delete time;
}

float PointQueue::GetIndexX(int ind)
{
  if (ind > currsize)
    my_error("PointQueue - INDEX NOT IN Queue");
  
  return x[QIX(ind)];
}

float PointQueue::GetIndexY(int ind)
{
  if (ind > currsize)
    my_error("PointQueue - INDEX NOT IN Queue");
  
  return y[QIX(ind)];
}

int PointQueue::GetIndexTime(int ind)
{
  if (ind > currsize)
    my_error("PointQueue - INDEX NOT IN Queue");
  
  return time[QIX(ind)];
}

void PointQueue::Append(float x_, float y_, int t_)
{
  if (currsize == maxsize) {
    my_error("PointQueue -- can't append");
  }

  x[tail] = x_;
  y[tail] = y_;
  time[tail] = t_;

  tail = (tail + 1) % maxsize;
  currsize++;
}

void PointQueue::Pop()
{
  if (currsize == 0) {
    my_error("PointQUEUE - CAN'T POP");
  }

  head = QIX(1);
  currsize--;
}

void PointQueue::Reset()
{
  head = tail = 0;
  currsize = 0;
}

void PointQueue::Add(float x_, float y_, int t_){
  if (currsize == maxsize)
    Pop(); /* make room for the new one */

  Append(x_, y_, t_);
}

int  PointQueue::OldestIndexWithin(int bound){
  if ( GetTailTime() != Mem->CurrentTime )
    my_error("Global ball info isn't current");

  for (int i=0; i<currsize-1; i++) /* Don't want index == tail */
    if ( Mem->CurrentTime - GetIndexTime(i) < bound )
      return i;
  
  return END_OF_QUEUE;
}

void PointQueue::Print()
{
  int i;
  printf("Qsize=%4d: |",currsize);
  for (i=0; i<currsize; i++) {
    printf("%d:(%.1f %.1f) |", time[QIX(i)],x[QIX(i)],y[QIX(i)]);
  }
  printf("\n");
}