00001
00002 #ifndef _UTILS_H_
00003 #define _UTILS_H_
00004
00005 #include <jmath/jblas.hpp>
00006 #include <image/Image.hpp>
00007
00008 inline double corrAngle( double a1, double a2)
00009 {
00010 return cos(a1 - a2);
00011 }
00012
00013 template< typename _T1_, typename _T2_>
00014 inline bool check(_T1_ v, _T2_ max)
00015 {
00016 return v >= 0 and v <= max;
00017 }
00018
00019 inline bool check2( const jblas::vec2& v, const jblas::vec2& max, double offset )
00020 {
00021 return check(v(0) - offset, max(0)) and check( v(1) - offset, max(1));
00022 }
00023
00024 inline double computeDirection( double angle )
00025 {
00026 if( angle > 0.5 * M_PI ) angle -= M_PI;
00027 else if( angle < -0.5 * M_PI ) angle += M_PI;
00028 return angle;
00029 }
00030
00031 inline double sign( double a )
00032 {
00033 if( a < 0.0 ) return -1.0;
00034 return 1.0;
00035 }
00036
00037 inline double pow2( double a )
00038 {
00039 return a * a;
00040 }
00041
00042 inline void projection( double &x_r, double &y_r, double x_A, double y_A, double a, double b )
00043 {
00044 double aa = a * a;
00045 double bb = b * b;
00046 double ab = a * b;
00047 double num = 1.0 / (aa + bb);
00048 x_r = (x_A * bb - y_A * ab - a) * num;
00049 y_r = -(x_A * ab - y_A * aa + b) * num;
00050 }
00051
00052 inline double max(double v1, double v2)
00053 {
00054 return v1 > v2 ? v1 : v2;
00055 }
00056
00057 inline double ratio(double v1, double v2)
00058 {
00059 if( v1 > v2 )
00060 {
00061 return v2 / v1;
00062 }
00063 return v1 / v2;
00064 }
00065
00066 inline double sampleAngle( const jafar::image::Image& _dx, const jafar::image::Image& _dy, double x_, double y_ )
00067 {
00068 double dx = _dx.getSubPixelValue<short>( x_ - 0.5, y_ - 0.5, 0, JfrImage_INTERP_CUBIC);
00069 double dy = _dy.getSubPixelValue<short>( x_ - 0.5, y_ - 0.5, 0, JfrImage_INTERP_CUBIC);
00070 return atan2(dy, dx);
00071 }
00072
00073 #endif
