robomath.c

/*
 * @(#)robomath.c       07/08/13
 * 
 * Description  : useful matemathic functions.
 *
 * Copyright    : (c) 2007 DCE Eurobot Dragon Team
 *                CTU FEE - Department of Control Engineering
 * License      : GNU GPL v.2
 */

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

/* Random function with Gaussian Probability Distribution */
double gaussrand()
{
        static double V2, fac;
        static int phase = 0;
        double S, Z, U1, U2, V1;
        
        if (phase)
                Z = V2 * fac;
        else {
                do {
                        U1 = drand48();
                        U2 = drand48();
                        
                        V1 = 2 * U1 - 1;
                        V2 = 2 * U2 - 1;
                        S = V1 * V1 + V2 * V2;
                } while(S >= 1);
                
                fac = sqrt (-2 * log(S) / S);
                Z = V1 * fac;
        }
        phase = 1 - phase;
        
        return Z;
}

/* Uniform random */
float urand()
{
        return ((float)rand()/RAND_MAX);
}

/* Another random function with Gaussian Probability Distribution. 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*urand()-1.0;
                        v2 = 2.0*urand()-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)
{
        return (1.0/(sqrt(2.0*M_PI) * sigma) *
                exp(-0.5 * (val*val / (sigma*sigma))));
}

/* Random generator with exponential distribution */
float exprand(float lambda)
{
        float u,x;
        
        u = urand();
        x = (-1/lambda)*log(u);
        
        return(x);
}

/* Using to test random functions */
int testrand(int n)
{
        float lambda = 2.0;
        int i;
        
        printf("\n==============================\n");
        printf("gaussrand():\n");
        for (i=0; i<n; i++)
                printf("%f ", gaussrand());

        printf("\n==============================\n");
        printf("gaussian_rand():\n");
        for (i=0; i<n; i++)
                printf("%f ", gaussrand());

        printf("\n==============================\n");
        printf("exprand():\n");
        for (i=0; i<n; i++)
                printf(" %2.4f ", exprand(lambda));
        
        return 0;
}

---