00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef __UTIL_H__
00026 #define __UTIL_H__
00027
00028 #include <math.h>
00029 #include <string.h>
00030
00031 const double TODEG = (180.0 / M_PI);
00032
00033 #if (!defined(__SGI_STL_ALGOBASE_H) && !defined(ALGOBASE_H))
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058 #endif
00059
00060 template <class num1,class num2>
00061 inline num1 bound(num1 x,num2 low,num2 high)
00062 {
00063 if(x < low ) x = low;
00064 if(x > high) x = high;
00065 return(x);
00066 }
00067
00068 template <class num>
00069 inline num max3(num a,num b,num c)
00070 {
00071 if(a > b){
00072 return((a > c)? a : c);
00073 }else{
00074 return((b > c)? b : c);
00075 }
00076 }
00077
00078 template <class num>
00079 inline num min3(num a,num b,num c)
00080 {
00081 if(a < b){
00082 return((a < c)? a : c);
00083 }else{
00084 return((b < c)? b : c);
00085 }
00086 }
00087
00088 template <class num>
00089 inline void sort(num &a,num &b,num &c)
00090 {
00091 if(a > b) swap(a,b);
00092 if(b > c) swap(b,c);
00093 if(a > b) swap(a,b);
00094 }
00095
00096 template <class real>
00097 real fmodt(real x,real m)
00098
00099
00100 {
00101 return(x - floor(x / m)*m);
00102 }
00103
00104 template <class num1,class num2>
00105 inline num1 setbits(num1 val,num2 bits)
00106 {
00107 return(val |= bits);
00108 }
00109
00110 template <class num1,class num2>
00111 inline num1 clearbits(num1 val,num2 bits)
00112 {
00113 return(val &= ~bits);
00114 }
00115
00116 template <class real>
00117 real saw(real t)
00118 {
00119 t -= floor(t);
00120 return(2 * ((t < 0.5)? t : (1.0-t)));
00121 }
00122
00123 template <class data>
00124 inline int mcopy(data *dest,data *src,int num)
00125 {
00126 int i;
00127
00128 for(i=0; i<num; i++) dest[i] = src[i];
00129
00130 return(num);
00131 }
00132
00133 template <class data>
00134 inline data mset(data *dest,data val,int num)
00135 {
00136 int i;
00137
00138 for(i=0; i<num; i++) dest[i] = val;
00139
00140 return(val);
00141 }
00142
00143 template <class data>
00144 inline void mzero(data &d)
00145 {
00146 memset(&d,0,sizeof(d));
00147 }
00148
00149
00150 template <class node>
00151 int list_length(node *list)
00152 {
00153 node *p = list;
00154 int num = 0;
00155
00156 while(p){
00157 num++;
00158 p = p->next;
00159 }
00160
00161 return(num);
00162 }
00163
00164
00165
00166 inline double norm_angle(double angle)
00167 {
00168 angle=fmod(angle,2*M_PI);
00169
00170 if(angle < 0.0) angle += 2*M_PI;
00171 if(angle >= M_PI) angle -= 2*M_PI;
00172
00173 return(angle);
00174 }
00175
00176
00177 inline void angle_wrap(double &angle) {
00178 angle=fmod(angle,2*M_PI);
00179 if (angle < 0.0) angle+=2*M_PI;
00180 if (angle >= M_PI) angle-=2*M_PI;
00181 }
00182
00183
00184
00185 inline double avg_angle(double left, double right) {
00186 if (left < right) {
00187 left += 2*M_PI;
00188 }
00189 double result = (left+right)/2;
00190 if (result > M_PI) {
00191 result -= 2*M_PI;
00192 }
00193
00194 return result;
00195 }
00196
00197
00198
00199 inline double atan2a(double y, double x) {
00200 if (x == 0.0) {
00201 if (y == 0.0) {
00202 return 0.0;
00203 } else if (y > 0) return M_PI/2.0;
00204 else return -M_PI/2.0;
00205 }
00206 else return atan2(y, x);
00207 }
00208
00209 inline double atan2b(double y, double x)
00210 {
00211 if(fabs(x) < 1.0E-4){
00212 if(y == 0.0) return(0.0);
00213 return((y > 0)? M_PI/2.0 : -M_PI/2.0);
00214 }
00215 else return atan2(y, x);
00216 }
00217
00218 template <class num>
00219 inline int sign(num n)
00220 {
00221 if(n > 0) return( 1);
00222 if(n < 0) return(-1);
00223 return(0);
00224 }
00225
00226
00227 inline double gaussian_with_min(double x,double min_value)
00228 {
00229 double val;
00230
00231 val = exp(-(x*x)/2);
00232
00233 if(val < min_value)
00234 val = 0.0;
00235
00236 return(val);
00237 }
00238
00239 const double gaussian_constant = sqrt(2*M_PI);
00240
00241 inline double gaussian_prob(double x, double mean, double sigma) {
00242 double offset = x-mean;
00243 double top = exp(-offset*offset/(2*sigma*sigma));
00244 double bottom = gaussian_constant*sigma;
00245
00246 if (bottom < 1e-10) {
00247 if (offset < 1e-10) {
00248 return 1/1e-10;
00249 } else {
00250 return 0;
00251 }
00252 }
00253
00254 return top/bottom;
00255 }
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293
00294
00295
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309 #endif