utils.cpp

robocup源代码2001年清华机器人源代码

/*
    Copyright (C) 2001  Tsinghuaeolus

    Authors : ChenJiang, YaoJinyi, CaiYunpeng, Lishi

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or (at your option) any later version.

    This library 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
    Lesser General Public License for more details.

	If you make any changes or have any comments we would appreciate a 
	message to [e-mail].
*/

#include "stdafx.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <iostream.h>
#include <math.h>

#include "Global.h"


/***************************** my_error  **********************************/

char error[CP_MaxLines][MAXMESG];
int current = CP_MaxLines;
int lines = -1;
int enable_my_error = 0;

char outstring[100];

void my_error(char *format,...)
{
	if (!enable_my_error) return;

	va_list argptr;
	va_start(argptr,format);
	vsprintf(outstring, format, argptr);
	va_end(argptr);
	
	current = (current + 1) % CP_MaxLines;
	for(int i = 0; i < CP_MaxRows; i++){
		error[current][i] = ' ';
	}
	sprintf(error[current], "%s (player %d, time %d): %s", CP_TeamName,
		MyNumber, situation.CurrentTime, outstring);
	error[current][strlen(error[current])] = ' ';

	lines = (lines +1) < CP_MaxLines ? (lines+1) : CP_MaxLines;

	SendMessage(hWND, TEXT_OUT, 0,0);
}


/****************      Parse  String          **************************/
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;

  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 !=0)
    (*str_ptr)++;
  if (**str_ptr == 0) return 0.0f;

  /* Process the number bit by bit */
  while((**str_ptr!=')') && (**str_ptr!=' ') && (**str_ptr!='\n') && (**str_ptr!=',')){
    if (**str_ptr=='e'){
      //my_error("There's an e in my float! %s",*str_ptr);
	  result =0;
	  while((**str_ptr!=')') && (**str_ptr!=' ') && (**str_ptr!='\n') && (**str_ptr!=','))
		  (*str_ptr)++;
	  break;
	}
    if (**str_ptr=='.')
      d_flag=1;
    else if (**str_ptr=='-')
      m_flag=1;
    else if (d_flag){
      d_frac *= 10.0;
      result+=(float)(**str_ptr-'0')/d_frac;
    }
    else
      result=result*10.0f+(float)(**str_ptr-'0');
    (*str_ptr)++;
  }
  if (m_flag)
    result=-result;

  return result;
}

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

float get_float(char *str){
  char **str_ptr = &str;
  return get_float(str_ptr);
}

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

int get_int(char **str_ptr){

  int result;
  int m_flag;

  result = 0;
  m_flag = 0;

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

  if (**str_ptr==0)
	  return 0;

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

  return result;
}

int get_int(char *str){
  char **str_ptr = &str;
  return get_int(str_ptr);
}

/****************************************************************************/
void get_to_left_blanket(char **str_ptr){
	while( ! (**str_ptr == '(') &&  **str_ptr != 0) (*str_ptr)++;
}

/****************************************************************************/
void get_to_right_blanket(char **str_ptr){
	while( ! (**str_ptr == ')') &&  **str_ptr != 0) (*str_ptr)++;
}

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

void get_word(char **str_ptr){
  while ( !isalpha(**str_ptr) && **str_ptr!=0) (*str_ptr)++;
}

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

void get_next_word(char **str_ptr){
  while ( isalpha(**str_ptr) ) (*str_ptr)++;
  get_word(str_ptr);
}

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

void advance_past_space(char **str_ptr)
{
  while ( (*str_ptr) && isspace(**str_ptr)) (*str_ptr)++;
}

void get_token (char **str_ptr)
{
  advance_past_space(str_ptr);
  while ( (*str_ptr) && !isspace(**str_ptr)) (*str_ptr)++;
}



void advance_to(char c, char **str_ptr){
  while ( **str_ptr != c ) (*str_ptr)++;
}

/***********************   mathematics    **************************************/
inline AngleDeg Rad2Deg(float x) { return float(x * 57.2958);}
inline float Deg2Rad(AngleDeg x) { return float(x * 1.74532e-2); }

inline float Cos(float deg){return float(cos(Deg2Rad(deg)));}
inline float Sin(float deg){return float(sin(Deg2Rad(deg)));}
inline float Tan(float deg){return float(tan(Deg2Rad(deg)));}
inline float ATan(float x){ return float(Rad2Deg(float(atan(x)))); }
inline float ATan2(float x, float y){return Rad2Deg(float(atan2(y,x)));}
inline float ASin(float x){return Rad2Deg(float(asin(x)));}
inline float ACos(float x){return Rad2Deg(float(acos(x)));}

inline float Exp(float x, float t){return ((float)pow(x, t));}

inline void  pose_limitation(float& x, float min, float max){
	x = x < max ? x : max;
	x = x > min ? x : min;
}

float NormalizeAngle(float ang, float min){
	if (fabs(ang) > 5000 || fabs(ang) < 0){
		my_error("The Angle is too huge");
		return 0;
	}
	while (ang >= 360 + min)  ang -=360;
	while (ang < min) ang +=360;
	return ang;
}

float LowPassFilter(float passfreq,float cutfreq,float currentfreq){
	if (currentfreq <= passfreq) return 1;
	if (currentfreq >= cutfreq) return 0;
	if (cutfreq - passfreq < 0.001f) return 0;
	else return 0.5f * (1.0f+Cos((currentfreq - passfreq)/(cutfreq - passfreq)*180));

}

float HighPassFilter(float cutfreq,float passfreq,float currentfreq){
	if (currentfreq <= cutfreq) return 0;
	if (currentfreq >= passfreq) return 1;
	if (passfreq- cutfreq <0.001f) return 0;
	else return (1.0f-Cos((currentfreq - cutfreq)/(passfreq - cutfreq)*180))/2;
}

AngleDeg MidAngle(AngleDeg angle,AngleDeg angle_inf){
	AngleDeg ang = NormalizeAngle(angle_inf - angle, 0.0f);
	return NormalizeAngle(angle + ang / 2.0f);
}

AngleDeg MidAngle2(AngleDeg angle,AngleDeg angle_inf){ 
	AngleDeg ang1 = (angle + angle_inf)/2;
	AngleDeg ang2 = ang1 + 180.0f;
	return fabs(NormalizeAngle(ang1 - angle)) < fabs(NormalizeAngle(ang2 - angle)) ?
		NormalizeAngle(ang1) : NormalizeAngle(ang2);
}

AngleDeg AngleDif(AngleDeg angle1, AngleDeg angle2){ 
	return (float)fabs(NormalizeAngle(angle1 - angle2));
}


inline float random0_1(){ return float(rand())/RAND_MAX; }
inline int   random(int max){ return rand() % abs(max);}
inline bool  chance(float luck){ return random0_1() < luck;}
inline bool  halfchance(){return random0_1() > 0.5;}

int QuadraticFormula(float a, float b, float c, float& psol1, float& psol2){
	float d = Sqr(b) - 4 * a * c;
	if (fabs(d) < FLOAT_EPS) {
		psol1 = -b / (2 * a);
		return 1;
	}
	else if (d < 0) {
	    return 0;
	}
	else{
		d = (float)sqrt(d);
		psol1 = (-b + d ) / (2 * a);
		psol2 = (-b - d ) / (2 * a);
		return 2;
	}
}


int Angle_between( float ang, float low, float up )
{
	return bool(NormalizeAngle(ang - low, 0) < NormalizeAngle(up - low, 0));
}

int IsNegativeInf(float x){ return (fabs(x) < 0);}

float priority_sum(float priority1, float priority2){ return (float)sqrt(Sqr(priority1) + Sqr(priority2));}
float priority_sub(float priority1, float priority2){ if (priority2 > priority1) return 0.0f; else return (float)sqrt(Sqr(priority1) - Sqr(priority2));}
/******** Get Options from command lines or file ******************************/
#define NULLCHAR	'\000'

void GetOptions(int argc,char** argv){
	option_t opt[] = {
	{"version",        		(void *)&SP_version,          V_FLOAT},
    {"team_size",      		(void *)&SP_team_size,          V_INT},
    {"half",        		(void *)&SP_half,          V_INT},
    {"host",           		(void *)&SP_host,               V_STRING},
    {"goal_width",   		(void *)&SP_goal_width,         V_FLOAT},
    {"player_size",   		(void *)&SP_player_size,	V_FLOAT},
    {"player_decay",        (void *)&SP_player_decay,    	V_FLOAT},
    {"player_rand",   		(void *)&SP_player_rand,	V_FLOAT},
    {"player_weight",       (void *)&SP_player_weight,	V_FLOAT},
    {"player_speed_max",    (void *)&SP_player_speed_max,	V_FLOAT},
    {"stamina_max",   		(void *)&SP_stamina_max,	V_FLOAT},
    {"stamina_inc_max",     (void *)&SP_stamina_inc,	V_FLOAT},
    {"recover_dec_thr",     (void *)&SP_recover_dec_thr,	V_FLOAT},
    {"recover_min",   		(void *)&SP_recover_min, 	V_FLOAT},
    {"recover_dec",   		(void *)&SP_recover_dec, 	V_FLOAT},
    {"effort_dec_thr",      (void *)&SP_effort_dec_thr,	V_FLOAT},
    {"effort_min",   		(void *)&SP_effort_min, 	V_FLOAT},
    {"effort_dec",   		(void *)&SP_effort_dec, 	V_FLOAT},
    {"effort_inc_thr",      (void *)&SP_effort_inc_thr,	V_FLOAT},