art_map: art_map: MQMath.h Source File

00001 /* -*- mode: C++ -*- */
00002 
00003 // $Id: MQMath.h 74 2010-03-25 00:49:41Z jack.oquin $
00004 
00005 #include <math.h>
00006 #define PI 3.141592
00007 #define PI_L 3141592L
00008 
00009 #define DEG_TO_RAD(x)           ((double)((x)*PI)/180.0)
00010 #define RAD_TO_MICRO(x)         ((long)((x)*1000000))
00011 #define DEG_TO_MICRO(x)         (RAD_TO_MICRO((DEG_TO_RAD((x)))))
00012 #define RAD_TO_DEG(x)           ((double)((x)*180.0)/PI)
00013 #define MICRO_TO_RAD(x)         ((double)(x)/1000000.0)
00014 #define MICRO_TO_DEG(x)         (RAD_TO_DEG((MICRO_TO_RAD((x)))))
00015 
00016 #define DEG_T_RAD PI/180.0
00017 #define RAD_T_DEG 180.0/PI
00018 
00019 #define SQUARE(x)         ((x) * (x))
00020 #define ABS(x)            (((x)>0)?(x):-1*(x))
00021 
00022 //#define MAX(x,y) ((x) > (y) ? (x) : (y))
00023 //#define MIN(x,y) ((x) > (y) ? (y) : (x))
00024 
00025 inline double Normalise_PI(double angle)
00026 {
00027   while (angle>PI) {
00028     angle -= 2.0*PI;
00029   }
00030   while (angle<=-PI) {
00031     angle += 2.0*PI;
00032   }
00033   return angle;
00034 }
00035 
00036 inline double DistFromXY(double x, double y, double x2,double y2)
00037 {
00038   return sqrt(SQUARE(x2-x)+SQUARE(y2-y));
00039 }
00040 
00041 inline double AngleFromXY(double x,double y, double ori, double x2, double y2)
00042 {
00043   return Normalise_PI(atan2(y2-y,x2-x) - ori);
00044 }
00045 
00046 inline double AngleFromXY2(double x,double y, double ori, double x2, double y2)
00047 {
00048   return Normalise_PI(atan2(x2-x,y2-y) - ori);
00049 }