rc2007-soccer/source/Concept/Framework/tools.h

#ifndef _TOOLS_H
#define _TOOLS_H

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

#ifndef new
	void* operator new(size_t sz);
	void operator delete(void* p);
#endif

#define max(a, b) (((a) > (b)) ? (a) : (b))
#define min(a, b) (((a) < (b)) ? (a) : (b))
#define sign(a) (((a) < 0) ? (-1) : (1))

inline float easyAngle(float angle)
{
	while(angle < 0)
	{
		angle += 2.0f * PI;
	}

	while(angle >= 2.0f * PI)
	{
		angle -= 2.0f * PI;
	}

	return angle;
}

inline void sleep(uint16 sec)
{
  for (uint16 s = 0; s < sec; s++) {
    for (uint32 i = 0; i < 1405678; i++) {
      asm volatile("nop"); 
    }
  }
};

inline void msleep(uint16 msec)
{
  for (uint16 s = 0; s < msec; s++) {
    for (uint16 i = 0; i < 1405; i++) {
      asm volatile("nop"); 
    }
  }
};

inline void usleep(uint16 usec)
{
  for (uint16 s = 0; s < usec; s++) {
    for (uint8 i = 0; i < 3; i++) {
      asm volatile("nop"); 
    }
  }
};

inline float distance2d(float x1, float y1, float x2, float y2)
{
	return sqrt(((x1 - x2) * (x1 - x2)) + ((y1 - y2) * (y1 - y2)));
}

inline float direction2d(float x1, float y1, float x2, float y2)
{
	return atan2((y2 - y1), (x2 - x1));
}

#endif