smp_func.cpp

2002年

/*********************************
Author:	Yaojinyi
Date:	2002-3-1 13:17:36
Description:

Last modifier:	Yaojinyi
Last modification date:	2002-3-1 13:17:36
Note:
*********************************/
#include "smp_func.h"

float NormalizeAngle(float ang, float min){
	while (ang >= 360 + min)  
		ang -=360;
	while (ang < min) 
		ang +=360;
	return ang;
}

/*compute resolutions out of quadratic formula */
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;
	}
}


/* ciwp's functions */
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;
}
/****************      Parse  String  (CMU)     **************************/
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;
  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;

  if (**str_ptr=='-'){
		m_flag=1;
		(*str_ptr)++;
  }
  /* Process the number bit by bit */
  while(isdigit(**str_ptr)){
     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)++;
}