utility.cpp

机器人的行为控制模拟程序。用于机器人的环境识别。A robot action decision simulation used for robot enviroment recognition.

/* 
    Robot Simulator

    This program is free software; you can redistribute it and/or
    modify it under the terms of the GNU General Public License
    as published by the Free Software Foundation; either version 2
    of the License, or (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

    (C) 2006 Jason Hunt
    [e-mail] 
*/

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

#include "utility.h"

/* Return a random value in [s..e] */
double f_rand(double s, double e)
{
    double val = s + (rand() / (double) RAND_MAX) * (e - s);

   // if (val == 0) return(f_rand(s, e));

    return(val);
}
/* End of f_rand */


/* Wrap an angle around the unit circle */
double angle_wrap(double a)
{
    if (a > 2*M_PI ) return(a - 2*M_PI);

    if (a < 0 ) return(a + 2*M_PI);    

    return(a);
}
/* End of angle_wrap */




/* Return current time in seconds */
double get_time( void )
{

    static LARGE_INTEGER freq;
    bool init = 0;

    if (!init)
    {
        QueryPerformanceFrequency(&freq);
        init = 1;
    }

    LARGE_INTEGER c;

    QueryPerformanceCounter(&c);

    double value = c.QuadPart / (double)freq.QuadPart;
    
    
    return(value);
}
/* End of get_time */


/* Return size of vector */
double vect_mag(double x, double y)
{
    return(sqrt(x*x+y*y));
}
/* End of vect mag */