vcs.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/Soft/Lib/Maths/maths.h

   1 /**
   2  *  \file     maths.h
   3  *  \brief    Maths library used by ARDrone
   4  *  \author   Sylvain Gaeremynck <sylvain.gaeremynck@parrot.com>
   5  *  \version  1.0
   6  */
   7
   8 #ifndef _MATHS_H_
   9 #define _MATHS_H_
  10
  11 #include <VP_Os/vp_os_types.h>
  12
  13 #ifndef __USE_GNU
  14 #define __USE_GNU
  15 #endif // __USE_GNU
  16
  17 #include <math.h>
  18 #include <float.h>
  19
  20 ///////////////////////////////////////////////
  21 // DEFINES
  22
  23 #define RAD_TO_MDEG (57295.779513f)
  24 #define RAD_TO_DEG  (57.295779513f)
  25 #define MDEG_TO_RAD (1.745329252e-05f)
  26 #define DEG_TO_RAD  (1.745329252e-02f)
  27 #define PI          (3.1415927f)
  28 #define GRAVITY     (9.81f)
  29
  30 #ifndef min
  31 #define min(a, b) (a) < (b) ? (a) : (b)
  32 #endif
  33
  34 #ifndef max
  35 #define max(a, b) (a) < (b) ? (b) : (a)
  36 #endif
  37
  38 typedef struct _screen_point_t {
  39   int32_t x;
  40   int32_t y;
  41 } screen_point_t;
  42
  43 typedef union _float_or_int_t {
  44   float32_t f;
  45   int32_t   i;
  46 } float_or_int_t;
  47
  48 extern float32_t f_epsilon;
  49
  50 bool_t f_is_zero( float32_t f );
  51 float32_t f_zero( float32_t f );
  52
  53 static inline float32_t f_round(float32_t f, int32_t nb_decimal)
  54 {
  55         float32_t puissance = (float32_t) (10 ^ nb_decimal);
  56         return ((int)(f * puissance) / puissance);
  57 }
  58
  59 // Returns f's absolute value
  60 static inline float32_t f_abs( float_or_int_t fi )
  61 {
  62   fi.i &= 0x7FFFFFFF;
  63
  64   return fi.f;
  65 }
  66
  67 // Inverse f's sign
  68 static inline void f_inv_sign( float_or_int_t* fi )
  69 {
  70   fi->i ^= 0x80000000;
  71 }
  72
  73 // Polar saturation
  74 void f_polar_sat( float32_t max, float32_t* phi, float32_t* theta);
  75
  76 // Returns clamp value with value's sign
  77 static inline float32_t f_set_clamp( float_or_int_t value, float_or_int_t clamp_value )
  78 {
  79   clamp_value.i |= value.i & 0x80000000;
  80
  81   return clamp_value.f;
  82 }
  83
  84 float32_t asin_taylor( float32_t x );
  85 float32_t atan2_taylor( float32_t num, float32_t den );
  86 float32_t exp_taylor( float32_t x );
  87 float32_t secant_taylor( float32_t x );
  88 float32_t cos_taylor( float32_t x );
  89 float32_t sin_taylor( float32_t x );
  90 float32_t time_navdata_in_ms( uint32_t current_time, int32_t dec );
  91
  92 static inline uint32_t iabs(int32_t v) { return ( v < 0 ) ? -v : v; }
  93
  94 #if defined( USE_MINGW32 )
  95 static inline void sincosf(float32_t a, float32_t* out_s, float32_t* out_c)
  96 {
  97   __asm(
  98     "flds %2\n"
  99     "fsincos\n"
 100     "fstps %1\n"
 101     "fstps %0"
 102     :"=m"(*out_s), "=m"(*out_c)
 103     :"m"(a)
 104   );
 105 }
 106 #elif defined( _MSC_VER ) || defined( TARGET_OS_IPHONE) || defined(TARGET_IPHONE_SIMULATOR)
 107 static inline void sincosf(float32_t a, float32_t* out_s, float32_t* out_c)
 108 {
 109         *out_s = sinf(a);
 110         *out_c = cosf(a);
 111 }
 112 #endif // _MSC_VER || USE_MINGW32
 113
 114 // 8 bits version
 115 uint32_t nb_bits_differents_8(uint32_t p, uint32_t q);
 116
 117 uint32_t nb_bits_differents(uint32_t p, uint32_t q);
 118
 119
 120 #endif // _MATHS_H_