00001
00006
00007
00008
00009
00010
00011
00012
00013 #ifndef VL_MATHOP
00014 #define VL_MATHOP
00015
00016 #include "generic.h"
00017
00026 VL_INLINE
00027 float vl_mod_2pi_f (float x)
00028 {
00029 while (x < 0.0 ) x += (float) (2 * VL_PI);
00030 while (x > 2 * VL_PI) x -= (float) (2 * VL_PI);
00031 return x ;
00032 }
00033
00034 VL_INLINE
00035 double vl_mod_2pi_d (double x)
00036 {
00037 while (x < 0.0 ) x += 2 * VL_PI ;
00038 while (x > 2 * VL_PI) x -= 2 * VL_PI ;
00039 return x ;
00040 }
00049 VL_INLINE
00050 int
00051 vl_floor_f (float x)
00052 {
00053 int xi = (int) x ;
00054 if (x >= 0 || (float) xi == x) return xi ;
00055 else return xi - 1 ;
00056 }
00057
00058 VL_INLINE
00059 int
00060 vl_floor_d (double x)
00061 {
00062 int xi = (int) x ;
00063 if (x >= 0 || (double) xi == x) return xi ;
00064 else return xi - 1 ;
00065 }
00066
00067
00068
00075 VL_INLINE
00076 float
00077 vl_abs_f (float x)
00078 {
00079 return (x >= 0) ? x : -x ;
00080 }
00081
00082 VL_INLINE
00083 double
00084 vl_abs_d (double x)
00085 {
00086 return (x >= 0) ? x : -x ;
00087 }
00088
00089
00120 VL_INLINE
00121 float
00122 vl_fast_atan2_f (float y, float x)
00123 {
00124
00125
00126
00127
00128 float angle, r ;
00129 float const c3 = 0.1821F ;
00130 float const c1 = 0.9675F ;
00131 float abs_y = vl_abs_f (y) + 1e-10F ;
00132
00133 if (x >= 0) {
00134 r = (x - abs_y) / (x + abs_y) ;
00135 angle = (float) (VL_PI / 4) ;
00136 } else {
00137 r = (x + abs_y) / (abs_y - x) ;
00138 angle = (float) (3 * VL_PI / 4) ;
00139 }
00140 angle += (c3*r*r - c1) * r ;
00141 return (y < 0) ? - angle : angle ;
00142 }
00143
00144 VL_INLINE
00145 double
00146 vl_fast_atan2_d (double y, double x)
00147 {
00148 double angle, r ;
00149 double const c3 = 0.1821 ;
00150 double const c1 = 0.9675 ;
00151 double abs_y = vl_abs_d (y) + 1e-10 ;
00152
00153 if (x >= 0) {
00154 r = (x - abs_y) / (x + abs_y) ;
00155 angle = VL_PI / 4 ;
00156 } else {
00157 r = (x + abs_y) / (abs_y - x) ;
00158 angle = 3 * VL_PI / 4 ;
00159 }
00160 angle += (c3*r*r - c1) * r ;
00161 return (y < 0) ? - angle : angle ;
00162 }
00163
00164
00173 VL_INLINE
00174 float
00175 vl_fast_resqrt_f (float x)
00176 {
00177
00178 union {
00179 float x ;
00180 vl_int32 i ;
00181 } u ;
00182
00183 float xhalf = (float) 0.5 * x ;
00184
00185
00186 u.x = x ;
00187
00188
00189 u.i = 0x5f3759df - (u.i >> 1);
00190
00191
00192
00193 u.x = u.x * ( (float) 1.5 - xhalf*u.x*u.x) ;
00194 u.x = u.x * ( (float) 1.5 - xhalf*u.x*u.x) ;
00195 return u.x ;
00196 }
00197
00198 VL_INLINE
00199 double
00200 vl_fast_resqrt_d (double x)
00201 {
00202
00203 union {
00204 double x ;
00205 vl_int64 i ;
00206 } u ;
00207
00208 double xhalf = (double) 0.5 * x ;
00209
00210
00211 u.x = x ;
00212
00213
00214 #ifdef VL_COMPILER_MSC
00215 u.i = 0x5fe6ec85e7de30dai64 - (u.i >> 1) ;
00216 #else
00217 u.i = 0x5fe6ec85e7de30daLL - (u.i >> 1) ;
00218 #endif
00219
00220
00221 u.x = u.x * ( (double) 1.5 - xhalf*u.x*u.x) ;
00222 u.x = u.x * ( (double) 1.5 - xhalf*u.x*u.x) ;
00223 return u.x ;
00224 }
00225
00226
00227
00235 VL_INLINE
00236 float
00237 vl_fast_sqrt_f (float x)
00238 {
00239 return (x < 1e-8) ? 0 : x * vl_fast_resqrt_f (x) ;
00240 }
00241
00242 VL_INLINE
00243 double
00244 vl_fast_sqrt_d (float x)
00245 {
00246 return (x < 1e-8) ? 0 : x * vl_fast_resqrt_d (x) ;
00247 }
00248
00249 VL_INLINE
00250 vl_uint32
00251 vl_fast_sqrt_i(vl_uint32 x)
00252 {
00253 vl_uint32 temp, g=0, b = 0x8000, bshft = 15 ;
00254 do {
00255 if (x >= (temp = (((g << 1) + b)<<bshft--))) {
00256 g += b;
00257 x -= temp;
00258 }
00259 } while (b >>= 1);
00260 return g;
00261 }
00262
00263
00264
00265
00266 #endif