/**************************************************************************
    misc.cc
    programmer: Paul Wiebe

    purpose:  Miscellaneous functions used by behaviors.
**************************************************************************/

#include <math.h>
#include <stdio.h>
#include "misc.h"
#include "pi.h"
#include "vector.h"

#define ROUND(x) ((int)((x) + .5))

#define TRUE 1
#define FALSE 0

/* ***************************************************************************
   FixAngle - returns an angle between pi and -pi
   Written by: Paul Wiebe
   takes:   the angle to fix
   returns: the angle fixed

   This function decreases angles greater then pi to fit in the range
   and increases angles less than -pi to fit in the range.
*************************************************************************** */
float FixAngle(float angle)
{
    while (angle > R_PI)  // while above pi subtract 2 pi
	angle -= R_2PI;
    while (angle <= R_NPI)  // while below pi add 2 pi
	angle += R_2PI;
    return angle;
}


/* ***************************************************************************
   FixAngle2 - returns an angle between 0 and 2pi
   Written by: Paul Wiebe
   takes:   the angle to fix
   returns: the angle fixed

   This function decreases angles greater then 2pi to fit in the range
   and increases angles less than 0 to fit in the range.
*************************************************************************** */
float FixAngle2(float angle)
{
    while (angle >= R_2PI)  // while above 2pi subtract 2 pi
	angle -= R_2PI;
    while (angle < 0.0)  // while below 0 add 2 pi
	angle += R_2PI;
    return angle;
}