Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

afsUtility.cc

Go to the documentation of this file.
00001 /*
00002  * Utility routines for AIBO FastSLAM.
00003  */
00004 
00005 #ifdef __cplusplus
00006 extern "C" {
00007 #endif
00008 
00009 #include <stdlib.h>
00010 #include <math.h>
00011 
00012 /* This routine takes the difference between two angles. It assumes that
00013  * any difference greater than 180 degrees is due to rollover. Input
00014  * angles should be in the range [0, 2pi). Output angles come in the range
00015  * [-pi, pi]. */
00016 double find_dtheta(double th1, double th2)
00017 {
00018   double naive_delta = th2 - th1;
00019 
00020   if(naive_delta <= -M_PI) return naive_delta + 2*M_PI;
00021   if(naive_delta > M_PI) return naive_delta - 2*M_PI;
00022   return naive_delta;
00023 }
00024 
00025 /* Generates normally-distributed random numbers with the Box-Muller
00026  * transform. See http://www.taygeta.com/random/gaussian.html */
00027 double normRand()
00028 {
00029   static int got_leftover_rand=0;
00030   static double leftover_rand;
00031   double urand1, urand2, w;
00032 
00033   if(got_leftover_rand) {
00034     got_leftover_rand = 0;
00035     return leftover_rand;
00036   }
00037 
00038   do {
00039     urand1 = 2.0*rand()/RAND_MAX - 1.0;
00040     urand2 = 2.0*rand()/RAND_MAX - 1.0;
00041     w = urand1 * urand1 + urand2 * urand2;
00042   } while(w >= 1.0);
00043 
00044   w = sqrt((-2.0*log(w)) / w);
00045   leftover_rand = urand1*w;
00046   return urand2*w;
00047 }
00048 
00049 #ifdef __cplusplus
00050 }
00051 #endif

Tekkotsu v1.4
Generated Sat Jul 19 00:06:29 2003 by Doxygen 1.3.2