static char Math_id[] = "$Header: /usr/tiburon/xPlatform/utils/RCS/Math.cc,v 1.3 1997/04/29 10:15:49 oreilly Exp pean $";

/*
$Log: Math.cc,v $
Revision 1.3  1997/04/29 10:15:49  oreilly
Use more efficient algorithms for angle normalization

Revision 1.2  96/10/28  08:53:24  08:53:24  oreilly (Thomas C. O'Reilly)
Math utilities

Revision 1.1  96/08/14  13:30:38  13:30:38  oreilly (Thomas C. O'Reilly)
Initial revision

*/
#include "Math.h"

double Math::TwoPi = M_PI * 2.;
double Math::RadsPerDeg = M_PI / 180.;

void Math::zeroToTwoPi(Radians *angle)
{
  float m = 1.;
  if (*angle < 0.)
    m = -1.;
  
  double ratio = fabs(*angle) / TwoPi;

  *angle = m * TwoPi * (ratio - floor(ratio));

  while (*angle < 0.)
    (*angle) += TwoPi;
  
  while (*angle > TwoPi)
    (*angle) -= TwoPi; 
}


void Math::minusPiToPi(Radians *angle)
{
  Math::zeroToTwoPi(angle);
  
  if (*angle < 0.)
  {
    while (fabs(*angle) > M_PI)
      (*angle) += (TwoPi);
  }
  else
  {
    while (fabs(*angle) > M_PI)
      (*angle) -= (TwoPi);
  }
}


Radians Math::angularSeparation(Radians start, Radians stop)
{
  Radians delta = stop - start;
  while (delta > TwoPi)
    delta -= TwoPi;
  
  while (delta < 0.)
    delta += TwoPi;

  return delta;
}