utility.cpp

关于时下流行的粒子滤波程序的源码

#include <math.h>
#include <stdlib.h>
#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 */