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