Synchronize with upstream release 0.3 from https://monav.googlecode.com/svn/tags...
[monav:monav.git] / utils / bithelpers.h
1 /*
2 Copyright 2010  Christian Vetter veaac.fdirct@gmail.com
3
4 This file is part of MoNav.
5
6 MoNav is free software: you can redistribute it and/or modify
7 it under the terms of the GNU General Public License as published by
8 the Free Software Foundation, either version 3 of the License, or
9 (at your option) any later version.
10
11 MoNav is distributed in the hope that it will be useful,
12 but WITHOUT ANY WARRANTY; without even the implied warranty of
13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14 GNU General Public License for more details.
15
16 You should have received a copy of the GNU General Public License
17 along with MoNav.  If not, see <http://www.gnu.org/licenses/>.
18 */
19
20 #ifndef BITHELPERS_H
21 #define BITHELPERS_H
22
23 #include <cstring>
24 #include <algorithm>
25 #include <vector>
26
27 template< class T >
28 static inline T readUnaligned( const char* buffer ) {
29         T temp;
30         memcpy( &temp, buffer, sizeof( T ) );
31         return temp;
32 }
33
34 // reads first bits to a max of 31 bits ( 31 because 1u << 32 is undefined )
35 // offset has to be <8
36 // safe with unaligned memory access
37 static inline unsigned read_unaligned_unsigned( const unsigned char* buffer, int offset ){
38         assert ( offset <= 7 );
39
40         const int diff = ( ( size_t ) buffer ) & 3;
41         buffer -= diff;
42         offset += 8 * diff;
43
44         unsigned temp = * ( unsigned * ) buffer;
45         if ( offset == 0 )
46                 return temp;
47         unsigned temp2 = * ( ( ( unsigned * ) buffer ) + 1);
48         return ( temp >> offset ) | ( temp2 << ( 32 - offset ) );
49 }
50
51 static inline unsigned read_unaligned_unsigned( const unsigned char** buffer, int bits, int* offset ){
52         assert ( *offset <= 7 );
53
54         const int diff = ( ( size_t ) *buffer ) & 3;
55         const unsigned char* alignedBuffer = *buffer - diff;
56         int alignedOffset = *offset + 8 * diff;
57         unsigned temp = * ( unsigned * ) alignedBuffer;
58         unsigned temp2 = * ( ( ( unsigned * ) alignedBuffer ) + 1);
59         unsigned result;
60         if ( alignedOffset == 0 )
61                 result =  temp;
62         else
63                 result =  ( temp >> alignedOffset ) | ( temp2 << ( 32 - alignedOffset ) );
64
65         *offset += bits;
66         *buffer += ( *offset ) >> 3;
67         *offset &= 7;
68
69         if ( bits == 32 )
70                 return result;
71
72         return result & ( ( 1u << bits ) - 1 );
73 }
74
75 static inline unsigned read_unaligned_unsigned( const unsigned char* buffer, int bits, int offset ){
76         assert ( offset <= 7 );
77
78         const int diff = ( ( size_t ) buffer ) & 3;
79         const unsigned char* alignedBuffer = buffer - diff;
80         int alignedOffset = offset + 8 * diff;
81         unsigned temp = * ( unsigned * ) alignedBuffer;
82         unsigned temp2 = * ( ( ( unsigned * ) alignedBuffer ) + 1);
83         unsigned result;
84         if ( alignedOffset == 0 )
85                 result =  temp;
86         else
87                 result =  ( temp >> alignedOffset ) | ( temp2 << ( 32 - alignedOffset ) );
88
89         if ( bits == 32 )
90                 return result;
91
92         return result & ( ( 1u << bits ) - 1 );
93 }
94
95 // writes #bits bits of data into the buffer at the offset
96 // offset has to be <8, **buffer has to be zeroed
97 // modifies buffer and offset to point after the inserted data
98 static inline void write_unaligned_unsigned( unsigned char** buffer, unsigned data, int bits, int* offset ) {
99         ( ( unsigned* ) *buffer )[0] |= ( data << ( *offset ) );
100         ( ( unsigned* ) ( *buffer + 1 ) )[0] |= ( data >> ( 8 - *offset ) );
101
102 #ifndef NDEBUG
103         const unsigned char* tempBuffer = *buffer;
104         int tempOffset = *offset;
105         unsigned tempData = read_unaligned_unsigned( &tempBuffer, bits, &tempOffset );
106         assert( tempData == data );
107 #endif
108
109         *offset += bits;
110         *buffer += ( *offset ) >> 3;
111         *offset &= 7;
112 }
113
114 static inline unsigned read_bits ( unsigned data, char bits ) {
115         if ( bits == 32 )
116                 return data;
117
118         return data & ( ( 1u << bits ) - 1 );
119 }
120
121 static inline unsigned log2_rounded ( unsigned x ) {
122         static const unsigned bit_position[32] = {
123                 0, 1, 28, 2, 29, 14, 24, 3, 30, 22, 20, 15, 25, 17, 4, 8,
124                 31, 27, 13, 23, 21, 19, 16, 7, 26, 12, 18, 6, 11, 5, 10, 9
125         };
126
127         //round up
128         --x;
129         x |= x >> 1;
130         x |= x >> 2;
131         x |= x >> 4;
132         x |= x >> 8;
133         x |= x >> 16;
134         ++x;
135
136         return bit_position[ ( x * 0x077CB531u ) >> 27];
137 }
138
139 // computes log2_rounded( x + 1 ), works even up to x = 2^32 - 1
140 static inline unsigned bits_needed( unsigned x )
141 {
142         static const unsigned bit_position[32] = {
143                 32, 1, 28, 2, 29, 14, 24, 3, 30, 22, 20, 15, 25, 17, 4, 8,
144                 31, 27, 13, 23, 21, 19, 16, 7, 26, 12, 18, 6, 11, 5, 10, 9
145         };
146         //slower, maybe think of a better workaround
147         if ( x == 0 )
148                 return 0;
149
150         //+1 and round up
151         x |= x >> 1;
152         x |= x >> 2;
153         x |= x >> 4;
154         x |= x >> 8;
155         x |= x >> 16;
156         ++x;
157
158         return bit_position[ ( x * 0x077CB531u ) >> 27];
159 }
160
161 template< int exponentBits, int significantBits >
162 static unsigned decode_integer( unsigned x )
163 {
164         if ( x == ( ( 1u << ( exponentBits + significantBits ) ) - 1 ) )
165                 return 0;
166         unsigned exponent = x >> significantBits;
167         unsigned significant = x & ( ( 1u << significantBits ) - 1 );
168         significant = ( significant << 1 ) | 1; // implicit 1
169         return significant << exponent;
170 }
171
172 template< int exponentBits, int significantBits >
173 static unsigned encode_integer( unsigned x )
174 {
175         assert ( exponentBits > 0 );
176         assert( significantBits > 0 );
177
178         static bool initialized = false;
179         static const unsigned numEncoded = 1u << ( exponentBits + significantBits );
180         typedef std::pair< unsigned, unsigned > Lookup;
181         static Lookup lookup[numEncoded];
182
183         if ( !initialized ) {
184                 for ( unsigned value = 0; value < numEncoded; value++ )
185                         lookup[value] = Lookup( decode_integer< exponentBits, significantBits >( value ), value );
186                 std::sort( lookup, lookup + numEncoded );
187                 initialized = true;
188         }
189
190         Lookup* value = std::lower_bound( lookup, lookup + numEncoded, Lookup( x, 0 ) );
191
192         if ( value == lookup )
193                 return lookup[0].second;
194         if ( value == lookup + numEncoded )
195                 return lookup[numEncoded - 1].second;
196
197         int diffFirst = x - ( value - 1 )->first;
198         int diffSecond = value->first - x;
199
200         assert( diffFirst >= 0 );
201         assert( diffSecond >= 0 );
202
203         if ( diffFirst < diffSecond )
204                 return ( value - 1 )->second;
205         return value->second;
206 }
207
208 // computes a minimal encoder table that can encode [min,max] with at most a factor ( 1 + 'error' ) error.
209 // uses the last table entry as min; has to be >= 0
210 // return the amount of bits needed to address a table entry
211 static int compute_encoder_table( std::vector< int >* encoderTable, int max, double error )
212 {
213         assert( encoderTable != NULL );
214         assert( encoderTable->size() != 0 );
215
216         while ( true ) {
217                 double maxValue = encoderTable->back() * ( 1 + error );
218                 if ( maxValue > max ) // safe <= double comparison
219                         break;
220                 double nextValue = ( ( int ) maxValue + 1 ) * ( 1 + error );
221                 if ( nextValue >= max ) {
222                         encoderTable->push_back( max );
223                         break;
224                 } else {
225                         encoderTable->push_back( nextValue );
226                 }
227         }
228
229         return bits_needed( encoderTable->size() );
230 }
231
232 // encode a value using a sorted encoder table
233 // return the corresponding value with the least rounding error
234 // rounds up if possible
235 static unsigned table_encode( int x, const std::vector< int >& encoderTable )
236 {
237         assert( !encoderTable.empty() );
238
239         std::vector< int >::const_iterator value = std::lower_bound( encoderTable.begin(), encoderTable.end(), x );
240         if ( value == encoderTable.begin() )
241                 return 0;
242         if ( value == encoderTable.end() )
243                 return encoderTable.size() - 1;
244
245         int diffFirst = x - *( value - 1 );
246         int diffSecond = *value - x;
247
248         assert( diffFirst >= 0 );
249         assert( diffSecond >= 0 );
250
251         if ( diffFirst < diffSecond )
252                 return value - encoderTable.begin() - 1;
253         return value - encoderTable.begin();
254 }
255
256 #endif // BITHELPERS_H