smp_func.h

来自「2002年」· C头文件 代码 · 共 144 行

#ifndef __SMP_FUNC_H__
#define __SMP_FUNC_H__

#include <math.h>
#include <stdlib.h>
#include <ctype.h>

const float PI = 3.14159f;
const float InvalidAngle = 360.0f;
const float FLOAT_EPS = 0.001f;
/***********************   mathematics    **************************************/
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;}

template<class T> inline const T Min(const T& x, const T& y){
	return x < y ? x : y;
}
template<class T> inline const T Max(const T& x, const T& y){
	return x > y ? x : y;
}
template<class T> inline void Swap(T& x, T& y){
	T tmp =x; x = y; y = tmp;
}

#define round(x) ((x)>0?int((x)+0.5f):int((x)-0.5f))
#define Quantize(v,q)	((round((v)/(q)))*(q))
#define Constrain(x,min,max) Min(max,Max(min,(x)))
#define  Sqr(x)     ((x)*(x))
#define  Idx(No)    ((No) - 1)
#define ffabs(x) ((float)fabs(x))
#define RecurrentNext(idx, size) ((idx+1)%size)
#define RecurrentPrev(idx, size) ((idx+size-1)%size)
#define MoveFormula(speed, time, accel) (speed*(1-pow(accel, time))/(1-accel))

inline float Rad2Deg(float x) { return float(x * 57.2958);}
inline float Deg2Rad(float 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;
}

extern float NormalizeAngle(float ang, float min = -180.0f);

//compute 劣弧's angle between ang0 and ang1;
inline float AngleDifference(float ang0, float ang1){
	return (float)fabs(NormalizeAngle(ang0-ang1));
}

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

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

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

/*compute resolutions out of quadratic formula */
extern int QuadraticFormula(float a, float b, float c, float& psol1, float& psol2);

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

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

/* special functions for priority operation*/
inline float priority_sum(float priority1, float priority2){
	return (float)sqrt(Sqr(priority1) + Sqr(priority2));
}

inline float priority_sub(float priority1, float priority2){ 
	return (priority2 > priority1) ? 0.0f : (float)sqrt(Sqr(priority1) - Sqr(priority2));
}

/* ciwp's functions */
extern float LowPassFilter(float passfreq,float cutfreq,float currentfreq);
extern float HighPassFilter(float cutfreq,float passfreq,float currentfreq);

/****************      Parse  String  (CMU)     **************************/

extern double get_double(char **str_ptr);

/* Get the number, but don't advance pointer */
extern double get_double(char *str);

extern float get_float(char **str_ptr);

/* Get the number, but don't advance pointer */
extern float get_float(char *str);

extern int get_int(char **str_ptr);

extern int get_int(char *str);

/****************************************************************************/
extern void get_to_left_blanket(char **str_ptr);

/****************************************************************************/
extern void get_to_right_blanket(char **str_ptr);

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

extern void get_word(char **str_ptr);

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

extern void get_next_word(char **str_ptr);

/****************************************************************************/
extern void advance_past_space(char **str_ptr);

extern void get_token (char **str_ptr);

extern void advance_to(char c, char **str_ptr);

#endif //__SMP_FUNC_H__