www.pudn.com > Particlefilter_code.zip > utility.cpp

#include 
#include 
#include "Utility.h" 
 
#ifndef PI 
#define PI 3.1415926 
#endif 

/* The following are some utility routines. */

/* This is the worst random number routine in the known universe, but
   I use it for portability. Feel free to replace it. */
double uniform_random(void)
{
	return (double) rand() / (double) RAND_MAX;
}

/* This Gaussian routine is stolen from Numerical Recipes and is their
   copyright. */

double gaussian_random(void)
{
  static int next_gaussian = 0;
  static double saved_gaussian_value;

  double fac, rsq, v1, v2;

  if (next_gaussian == 0) {
    do {
      v1 = 2.0*uniform_random()-1.0;
      v2 = 2.0*uniform_random()-1.0;
      rsq = v1*v1+v2*v2;
    } while (rsq >= 1.0 || rsq == 0.0);
    fac = sqrt(-2.0*log(rsq)/rsq);
    saved_gaussian_value=v1*fac;
    next_gaussian=1;
    return v2*fac;
  } else {
    next_gaussian=0;
    return saved_gaussian_value;
  }
}

double evaluate_gaussian(double val, double sigma)
{
  /* This private definition is used for portability */
  static const double Condense_PI = 3.14159265358979323846;

  return 1.0/(sqrt(2.0*Condense_PI) * sigma) *
    exp(-0.5 * (val*val / (sigma*sigma)));
}
void gaussian_vector(CDVector & Vec, double sigma, double mean) 
{ 
	int nSize = Vec.Length(); 
	for (int i = 0; i < nSize; i++) 
	{ 
		Vec[i] = mean + sigma * gaussian_random(); 
	} 
} 
/*Returns a Bernoulli variable with values 0 and 1 and prob(1) = p*/ 
bool bi_random(double prob) 
{ 
	return (uniform_random() <= prob); 
} 
/*reuturn atan, with special handling  on x = 0  */ 
double atan_u(double x, double y) 
{ 
	if (fabs(x) < 1.0e-3) 
	{ 
		if (y < -1.0e-3) 
			return (-PI /2 ); 
		else if ( y > 1.0e-3) 
			return (PI / 2); 
		else 
			return ((double)0.0); 
	} 
	else 
		return atan( y / x); 
} 
 
 
void SWAP(int &a,int &b) 
{ 
	int tmpswap; 
	tmpswap = a; a = b; b = tmpswap; 
} 
/* Bresenham algorithm: Find out the point at the line segment from (x0,y0) to (x1,y1) */ 
void line_pt(int x0, int y0,  
			 int x1, int y1,  
			 int* &x, int* &y, //ouput points 
			 int &nNum)//number of the points 
{ 
        int i; 
        int steep = 1; 
        int sx, sy;  /* step positive or negative (1 or -1) */ 
        int dx, dy;  /* delta (difference in X and Y between points) */ 
        int e; 
		int alloc_num = (int)(sqrt((x1-x0)*(x1-x0) + (y1-y0)*(y1-y0)) + 3); 
		x = new int[alloc_num]; 
		y = new int[alloc_num]; 
		nNum = 0; 
        /* 
         * optimize for vertical and horizontal lines here 
         */        
        dx = abs(x1 - x0); 
        sx = ((x1 - x0) > 0) ? 1 : -1; 
        dy = abs(y1 - y0); 
        sy = ((y1 - y0) > 0) ? 1 : -1; 
        if (dy > dx) { 
                steep = 0; 
                SWAP(x0, y0); 
                SWAP(dx, dy); 
                SWAP(sx, sy); 
        } 
        e = (dy << 1) - dx; 
        for (i = 0; i < dx; i++) { 
                if (steep) { 
                        x[nNum] = x0; y[nNum] = y0; 
						nNum++; 
                } else { 
						x[nNum] = y0; y[nNum] = x0; 
						nNum++; 
                } 
                while (e >= 0) { 
                        y0 += sy; 
                        e -= (dx << 1); 
                } 
                x0 += sx; 
                e += (dy << 1); 
        } 
 
} 
/* Set Vector/Matrix by another Vector/Matrix */ 
void SetVec(float *dest, const float *src, int nNum) 
{ 
	for (int i = 0; i < nNum; i++) 
	{ 
		*dest++ = *src++; 
	} 
} 
/* Set Vector/Matrix by a scarlar value */ 
void SetVec(float *dest, float value, int nNum) 
{ 
	for (int i = 0; i < nNum; i++) 
	{ 
		*dest++ = value; 
	} 
} 

/* End of utility routines */