// Implements Box-Muller transformation to create Gaussian; uses polar form.

// TO CALL THIS FUNCTION TO CREATE LASER SCANNER NOISE, YOUR 
// CALLING ROUTINE SHOULD INCLUDE THE FOLLOWING:
//
//  #define SIGMA_RATIO 0.005    // If you make this 0, then the variance = 0
//  extern double gaussian(double, double);
//  ...
//  gaussian(lp[j],SIGMA_RATIO*lp[j]*[lp[j]);     // Here, lp[j] is a specific laser 
//                                                // reading.  This call sets the variance
//                                                // to be a function of the square of the
//                                                // distance, attenuated by SIGMA_RATIO.
//                                                // This function returns the laser reading
//                                                // with Gaussian noise added.


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

double  gaussian(double m, double s)  /* mean m, standard deviation s */
{				        
  float x1, x2, w, y1;
  static float y2;
  static int use_last = 0;

  if (use_last)		        /* use value from previous call */
    {
      y1 = y2;
      use_last = 0;
    }
  else
    {
      do {
	x1 = 2.0 * drand48() - 1.0;
	x2 = 2.0 * drand48() - 1.0;
	w = x1 * x1 + x2 * x2;
      } while ( w >= 1.0 );

      w = sqrt( (-2.0 * log( w ) ) / w );
      y1 = x1 * w;
      y2 = x2 * w;
      use_last = 1;
    }
	
  return( m + y1 * s );
}