- Main Page
- Data Structures
- Files
- Related Pages

# mathop.h

Go to the documentation of this file.

    00001
    00006 /* AUTORIGHTS */
    00007
    00008 #ifndef VL_MATHOP
    00009 #define VL_MATHOP
    00010
    00011 #include "generic.h"
    00012
    00014 #define VL_PI 3.141592653589793
    00015
    00022 #define VL_SINGLE_EPSILON 1.19209290E-07F
    00023
    00029 #define VL_DOUBLE_EPSILON 2.220446049250313e-16
    00030
    00039 VL_INLINE
    00040 vl_single vl_mod_2pi_f (vl_single x)
    00041 {
    00042   while (x < 0.0      ) x += (vl_single) (2 * VL_PI);
    00043   while (x > 2 * VL_PI) x -= (vl_single) (2 * VL_PI);
    00044   return x ;
    00045 }
    00046
    00047 VL_INLINE
    00048 vl_double vl_mod_2pi_d (vl_double x)
    00049 {
    00050   while (x < 0.0      ) x += 2 * VL_PI ;
    00051   while (x > 2 * VL_PI) x -= 2 * VL_PI ;
    00052   return x ;
    00053 }
    00062 VL_INLINE
    00063 int
    00064 vl_floor_f (vl_single x)
    00065 {
    00066   int xi = (int) x ;
    00067   if (x >= 0 || (vl_single) xi == x) return xi ;
    00068   else return xi - 1 ;
    00069 }
    00070
    00071 VL_INLINE
    00072 int
    00073 vl_floor_d (vl_double x)
    00074 {
    00075   int xi = (int) x ;
    00076   if (x >= 0 || (vl_double) xi == x) return xi ;
    00077   else return xi - 1 ;
    00078 }
    00079 /* @} */
    00080
    00081
    00088 VL_INLINE
    00089 vl_single
    00090 vl_abs_f (vl_single x)
    00091 {
    00092   return (x >= 0) ? x : -x ;
    00093 }
    00094
    00095 VL_INLINE
    00096 vl_double
    00097 vl_abs_d (vl_double x)
    00098 {
    00099   return (x >= 0) ? x : -x ;
    00100 }
    00101 /* @} */
    00102
    00111 VL_INLINE
    00112 vl_single
    00113 vl_fast_atan2_f (vl_single y, vl_single x)
    00114 {
    00115   /*
    00116     The function f(r)=atan((1-r)/(1+r)) for r in [-1,1] is easier to
    00117     approximate than atan(z) for z in [0,inf]. To approximate f(r) to
    00118     the third degree we may solve the system
    00119
    00120     f(+1) = c0 + c1 + c2 + c3 = atan(0) = 0
    00121     f(-1) = c0 - c1 + c2 - c3 = atan(inf) = pi/2
    00122     f(0)  = c0                = atan(1) = pi/4
    00123
    00124     which constrains the polynomial to go through the end points and
    00125     the middle point.
    00126
    00127     We still miss a constrain, which might be simply a constarint on
    00128     the derivative in 0. Instead we minimize the Linf error in the
    00129     range [0,1] by searching for an optimal value of the free
    00130     parameter. This turns out to correspond to the solution
    00131
    00132     c0=pi/4, c1=-0.9675, c2=0, c3=0.1821
    00133
    00134     which has maxerr = 0.0061 rad = 0.35 grad.
    00135   */
    00136
    00137   vl_single angle, r ;
    00138   vl_single const c3 = 0.1821F ;
    00139   vl_single const c1 = 0.9675F ;
    00140   vl_single abs_y    = vl_abs_f (y) + 1e-10F ;
    00141
    00142   if (x >= 0) {
    00143     r = (x - abs_y) / (x + abs_y) ;
    00144     angle = (vl_single) (VL_PI / 4) ;
    00145   } else {
    00146     r = (x + abs_y) / (abs_y - x) ;
    00147     angle = (vl_single) (3 * VL_PI / 4) ;
    00148   }
    00149   angle += (c3*r*r - c1) * r ;
    00150   return (y < 0) ? - angle : angle ;
    00151 }
    00152
    00153 VL_INLINE
    00154 vl_double
    00155 vl_fast_atan2_d (vl_double y, vl_double x)
    00156 {
    00157   vl_double angle, r ;
    00158   vl_double const c3 = 0.1821 ;
    00159   vl_double const c1 = 0.9675 ;
    00160   vl_double abs_y    = vl_abs_d (y) + 1e-10 ;
    00161
    00162   if (x >= 0) {
    00163     r = (x - abs_y) / (x + abs_y) ;
    00164     angle = VL_PI / 4 ;
    00165   } else {
    00166     r = (x + abs_y) / (abs_y - x) ;
    00167     angle = 3 * VL_PI / 4 ;
    00168   }
    00169   angle += (c3*r*r - c1) * r ;
    00170   return (y < 0) ? - angle : angle ;
    00171 }
    00172 /* @} */
    00173
    00182 VL_INLINE
    00183 vl_single
    00184 vl_fast_resqrt_f (vl_single x)
    00185 {
    00186   /* 32-bit version */
    00187   union {
    00188     vl_single x ;
    00189     vl_int32  i ;
    00190   } u ;
    00191
    00192   vl_single xhalf = (vl_single) 0.5 * x ;
    00193
    00194   /* convert floating point value in RAW integer */
    00195   u.x = x ;
    00196
    00197   /* gives initial guess y0 */
    00198   u.i = 0x5f3759df - (u.i >> 1);
    00199   /*u.i = 0xdf59375f - (u.i>>1);*/
    00200
    00201   /* two Newton steps */
    00202   u.x = u.x * ( (vl_single) 1.5  - xhalf*u.x*u.x) ;
    00203   u.x = u.x * ( (vl_single) 1.5  - xhalf*u.x*u.x) ;
    00204   return u.x ;
    00205 }
    00206
    00207 VL_INLINE
    00208 vl_double
    00209 vl_fast_resqrt_d (vl_double x)
    00210 {
    00211   /* 64-bit version */
    00212   union {
    00213     vl_double x ;
    00214     vl_int64  i ;
    00215   } u ;
    00216
    00217   vl_double xhalf = (vl_double) 0.5 * x ;
    00218
    00219   /* convert floating point value in RAW integer */
    00220   u.x = x ;
    00221
    00222   /* gives initial guess y0 */
    00223   u.i = 0x5fe6ec85e7de30daLL - (u.i >> 1) ;
    00224
    00225   /* two Newton steps */
    00226   u.x = u.x * ( (vl_double) 1.5  - xhalf*u.x*u.x) ;
    00227   u.x = u.x * ( (vl_double) 1.5  - xhalf*u.x*u.x) ;
    00228   return u.x ;
    00229 }
    00230 /* @} */
    00231
    00232
    00239 VL_INLINE
    00240 vl_single
    00241 vl_fast_sqrt_f (vl_single x)
    00242 {
    00243   return (x < 1e-8) ? 0 : x * vl_fast_resqrt_f (x) ;
    00244 }
    00245
    00246 VL_INLINE
    00247 vl_double
    00248 vl_fast_sqrt_d (vl_double x)
    00249 {
    00250   return (x < 1e-8) ? 0 : x * vl_fast_resqrt_d (x) ;
    00251 }
    00252 /* @} */
    00253
    00254 /* VL_MATHOP */
    00255 #endif