IceUtils.h
Go to the documentation of this file.
00001 
00002 
00008 
00009 
00011 // Include Guard
00012 #ifndef __ICEUTILS_H__
00013 #define __ICEUTILS_H__
00014 
00015         #define START_RUNONCE   { static bool __RunOnce__ = false;      if(!__RunOnce__){
00016         #define END_RUNONCE             __RunOnce__ = true;}}
00017 
00020         inline_ void    ReverseBits(udword& n)
00021         {
00022                 n = ((n >>  1) & 0x55555555) | ((n <<  1) & 0xaaaaaaaa);
00023                 n = ((n >>  2) & 0x33333333) | ((n <<  2) & 0xcccccccc);
00024                 n = ((n >>  4) & 0x0f0f0f0f) | ((n <<  4) & 0xf0f0f0f0);
00025                 n = ((n >>  8) & 0x00ff00ff) | ((n <<  8) & 0xff00ff00);
00026                 n = ((n >> 16) & 0x0000ffff) | ((n << 16) & 0xffff0000);
00027                 // Etc for larger intergers (64 bits in Java)
00028                 // NOTE: the >> operation must be unsigned! (>>> in java)
00029         }
00030 
00032         inline_ udword  CountBits(udword n)
00033         {
00034                 // This relies of the fact that the count of n bits can NOT overflow 
00035                 // an n bit interger. EG: 1 bit count takes a 1 bit interger, 2 bit counts
00036                 // 2 bit interger, 3 bit count requires only a 2 bit interger.
00037                 // So we add all bit pairs, then each nible, then each byte etc...
00038                 n = (n & 0x55555555) + ((n & 0xaaaaaaaa) >> 1);
00039                 n = (n & 0x33333333) + ((n & 0xcccccccc) >> 2);
00040                 n = (n & 0x0f0f0f0f) + ((n & 0xf0f0f0f0) >> 4);
00041                 n = (n & 0x00ff00ff) + ((n & 0xff00ff00) >> 8);
00042                 n = (n & 0x0000ffff) + ((n & 0xffff0000) >> 16);
00043                 // Etc for larger intergers (64 bits in Java)
00044                 // NOTE: the >> operation must be unsigned! (>>> in java)
00045                 return n;
00046         }
00047 
00049         inline_ udword  CountBits2(udword bits)
00050         {
00051                 bits = bits - ((bits >> 1) & 0x55555555);
00052                 bits = ((bits >> 2) & 0x33333333) + (bits & 0x33333333);
00053                 bits = ((bits >> 4) + bits) & 0x0F0F0F0F;
00054                 return (bits * 0x01010101) >> 24;
00055         }
00056 
00062         inline_ void    SpreadBits(udword& n)
00063         {
00064                 n = ( n & 0x0000ffff) | (( n & 0xffff0000) << 16);
00065                 n = ( n & 0x000000ff) | (( n & 0x0000ff00) <<  8);
00066                 n = ( n & 0x000f000f) | (( n & 0x00f000f0) <<  4);
00067                 n = ( n & 0x03030303) | (( n & 0x0c0c0c0c) <<  2);
00068                 n = ( n & 0x11111111) | (( n & 0x22222222) <<  1);
00069         }
00070 
00071         // Next Largest Power of 2
00072         // Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm
00073         // that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with
00074         // the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next
00075         // largest power of 2. For a 32-bit value: 
00076         inline_ udword  nlpo2(udword x)
00077         {
00078                 x |= (x >> 1);
00079                 x |= (x >> 2);
00080                 x |= (x >> 4);
00081                 x |= (x >> 8);
00082                 x |= (x >> 16);
00083                 return x+1;
00084         }
00085 
00087         inline_ bool    IsPowerOfTwo(udword n)                          { return ((n&(n-1))==0);                                        }
00088 
00090         inline_ void    ZeroLeastSetBit(udword& n)                      { n&=(n-1);                                                                     }
00091 
00093         inline_ void    SetLeastNBits(udword& x, udword n)      { x|=~(~0<<n);                                                          }
00094 
00099         inline_ void    Swap(udword& x, udword& y)                      { x ^= y; y ^= x; x ^= y;                                       }
00100 
00115         inline_ char    LittleEndian()                                          { int i = 1; return *((char*)&i);                       }
00116 
00118         inline_ udword  abs_(sdword x)                                  { sdword y= x >> 31;    return (x^y)-y;         }
00119 
00121         inline_ sdword  min_(sdword a, sdword b)                        { sdword delta = b-a;   return a + (delta&(delta>>31)); }
00122 
00123         // Determine if one of the bytes in a 4 byte word is zero
00124         inline_ BOOL    HasNullByte(udword x)                   { return ((x + 0xfefefeff) & (~x) & 0x80808080);                }
00125 
00126         // To find the smallest 1 bit in a word  EG: ~~~~~~10---0    =>    0----010---0
00127         inline_ udword  LowestOneBit(udword w)                  { return ((w) & (~(w)+1));                                      }
00128 //      inline_ udword  LowestOneBit_(udword w)                 { return ((w) & (-(w)));                                        }
00129 
00130         // Most Significant 1 Bit
00131         // Given a binary integer value x, the most significant 1 bit (highest numbered element of a bit set)
00132         // can be computed using a SWAR algorithm that recursively "folds" the upper bits into the lower bits.
00133         // This process yields a bit vector with the same most significant 1 as x, but all 1's below it.
00134          // Bitwise AND of the original value with the complement of the "folded" value shifted down by one
00135         // yields the most significant bit. For a 32-bit value: 
00136         inline_ udword  msb32(udword x)
00137         {
00138                 x |= (x >> 1);
00139                 x |= (x >> 2);
00140                 x |= (x >> 4);
00141                 x |= (x >> 8);
00142                 x |= (x >> 16);
00143                 return (x & ~(x >> 1));
00144         }
00145 
00146         /*
00147         "Just call it repeatedly with various input values and always with the same variable as "memory".
00148         The sharpness determines the degree of filtering, where 0 completely filters out the input, and 1
00149         does no filtering at all.
00150 
00151         I seem to recall from college that this is called an IIR (Infinite Impulse Response) filter. As opposed
00152         to the more typical FIR (Finite Impulse Response).
00153 
00154         Also, I'd say that you can make more intelligent and interesting filters than this, for example filters
00155         that remove wrong responses from the mouse because it's being moved too fast. You'd want such a filter
00156         to be applied before this one, of course."
00157 
00158         (JCAB on Flipcode)
00159         */
00160         inline_ float   FeedbackFilter(float val, float& memory, float sharpness)
00161         {
00162                 ASSERT(sharpness>=0.0f && sharpness<=1.0f && "Invalid sharpness value in feedback filter");
00163                                 if(sharpness<0.0f)      sharpness = 0.0f;
00164                 else    if(sharpness>1.0f)      sharpness = 1.0f;
00165                 return memory = val * sharpness + memory * (1.0f - sharpness);
00166         }
00167 
00171         inline_ int     ClampToInt16(int x)
00172         {
00173 //              ASSERT(abs(x) < (int)((1<<31u)-32767));
00174 
00175                 int delta = 32767 - x;
00176                 x += (delta>>31) & delta;
00177                 delta = x + 32768;
00178                 x -= (delta>>31) & delta;
00179                 return x;
00180         }
00181 
00182         // Generic functions
00183         template<class Type> inline_ void TSwap(Type& a, Type& b)                                                               { const Type c = a; a = b; b = c;                       }
00184         template<class Type> inline_ Type TClamp(const Type& x, const Type& lo, const Type& hi) { return ((x<lo) ? lo : (x>hi) ? hi : x);       }
00185 
00186         template<class Type> inline_ void TSort(Type& a, Type& b)
00187         {
00188                 if(a>b) TSwap(a, b);
00189         }
00190 
00191         template<class Type> inline_ void TSort(Type& a, Type& b, Type& c)
00192         {
00193                 if(a>b) TSwap(a, b);
00194                 if(b>c) TSwap(b, c);
00195                 if(a>b) TSwap(a, b);
00196                 if(b>c) TSwap(b, c);
00197         }
00198 
00199         // Prevent nasty user-manipulations (strategy borrowed from Charles Bloom)
00200 //      #define PREVENT_COPY(curclass)  void operator = (const curclass& object)        {       ASSERT(!"Bad use of operator =");       }
00201         // ... actually this is better !
00202         #define PREVENT_COPY(cur_class) private: cur_class(const cur_class& object);    cur_class& operator=(const cur_class& object);
00203 
00205         #define OFFSET_OF(Class, Member)        (size_t)&(((Class*)0)->Member)
00206 
00207         #define ARRAYSIZE(p)                            (sizeof(p)/sizeof(p[0]))
00208 
00210 
00216 
00217         FUNCTION ICECORE_API udword Alignment(udword address);
00218 
00219         #define IS_ALIGNED_2(x)         ((x&1)==0)
00220         #define IS_ALIGNED_4(x)         ((x&3)==0)
00221         #define IS_ALIGNED_8(x)         ((x&7)==0)
00222 
00223         inline_ void _prefetch(void const* ptr)         { (void)*(char const volatile *)ptr;    }
00224 
00225         // Compute implicit coords from an index:
00226         // The idea is to get back 2D coords from a 1D index.
00227         // For example:
00228         //
00229         // 0            1               2       ...     nbu-1
00230         // nbu          nbu+1   i       ...
00231         //
00232         // We have i, we're looking for the equivalent (u=2, v=1) location.
00233         //              i = u + v*nbu
00234         // <=>  i/nbu = u/nbu + v
00235         // Since 0 <= u < nbu, u/nbu = 0 (integer)
00236         // Hence: v = i/nbu
00237         // Then we simply put it back in the original equation to compute u = i - v*nbu
00238         inline_ void Compute2DCoords(udword& u, udword& v, udword i, udword nbu)
00239         {
00240                 v = i / nbu;
00241                 u = i - (v * nbu);
00242         }
00243 
00244         // In 3D:       i = u + v*nbu + w*nbu*nbv
00245         // <=>          i/(nbu*nbv) = u/(nbu*nbv) + v/nbv + w
00246         // u/(nbu*nbv) is null since u/nbu was null already.
00247         // v/nbv is null as well for the same reason.
00248         // Hence w = i/(nbu*nbv)
00249         // Then we're left with a 2D problem: i' = i - w*nbu*nbv = u + v*nbu
00250         inline_ void Compute3DCoords(udword& u, udword& v, udword& w, udword i, udword nbu, udword nbu_nbv)
00251         {
00252                 w = i / (nbu_nbv);
00253                 Compute2DCoords(u, v, i - (w * nbu_nbv), nbu);
00254         }
00255 
00256 #endif // __ICEUTILS_H__


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:17