00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #include <cmath>
00011 #include <cstdlib>
00012 #include <cstdio>
00013 #include <cstring>
00014 #include <pion/algorithm.hpp>
00015 #include <boost/assert.hpp>
00016
00017
00018 #define SHIFT_BITMASK(ptr, mask) if (mask & 0x01) { mask = 0x80; ++ptr; } else mask >>= 1;
00019
00020
00021 namespace pion {
00022
00023
00024 bool algorithm::base64_decode(const std::string &input, std::string &output)
00025 {
00026 static const char nop = -1;
00027 static const char decoding_data[] = {
00028 nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop,
00029 nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop,
00030 nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop, 62, nop,nop,nop, 63,
00031 52, 53, 54, 55, 56, 57, 58, 59, 60, 61,nop,nop, nop,nop,nop,nop,
00032 nop, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
00033 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25,nop, nop,nop,nop,nop,
00034 nop,26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40,
00035 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51,nop, nop,nop,nop,nop,
00036 nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop,
00037 nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop,
00038 nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop,
00039 nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop,
00040 nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop,
00041 nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop,
00042 nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop,
00043 nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop
00044 };
00045
00046 unsigned int input_length=input.size();
00047 const char * input_ptr = input.data();
00048
00049
00050 output.clear();
00051 output.reserve(((input_length+2)/3)*4);
00052
00053
00054
00055
00056 for (unsigned int i=0; i<input_length;i++) {
00057 char base64code0;
00058 char base64code1;
00059 char base64code2 = 0;
00060 char base64code3;
00061
00062 base64code0 = decoding_data[static_cast<int>(input_ptr[i])];
00063 if(base64code0==nop)
00064 return false;
00065 if(!(++i<input_length))
00066 return false;
00067 base64code1 = decoding_data[static_cast<int>(input_ptr[i])];
00068 if(base64code1==nop)
00069 return false;
00070
00071 output += ((base64code0 << 2) | ((base64code1 >> 4) & 0x3));
00072
00073 if(++i<input_length) {
00074 char c = input_ptr[i];
00075 if(c =='=') {
00076 BOOST_ASSERT( (base64code1 & 0x0f)==0);
00077 return true;
00078 }
00079 base64code2 = decoding_data[static_cast<int>(input_ptr[i])];
00080 if(base64code2==nop)
00081 return false;
00082
00083 output += ((base64code1 << 4) & 0xf0) | ((base64code2 >> 2) & 0x0f);
00084 }
00085
00086 if(++i<input_length) {
00087 char c = input_ptr[i];
00088 if(c =='=') {
00089 BOOST_ASSERT( (base64code2 & 0x03)==0);
00090 return true;
00091 }
00092 base64code3 = decoding_data[static_cast<int>(input_ptr[i])];
00093 if(base64code3==nop)
00094 return false;
00095
00096 output += (((base64code2 << 6) & 0xc0) | base64code3 );
00097 }
00098
00099 }
00100
00101 return true;
00102 }
00103
00104 bool algorithm::base64_encode(const std::string &input, std::string &output)
00105 {
00106 static const char encoding_data[] =
00107 "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
00108
00109 unsigned int input_length=input.size();
00110 const char * input_ptr = input.data();
00111
00112
00113 output.clear();
00114 output.reserve(((input_length+2)/3)*4);
00115
00116
00117
00118
00119 for (unsigned int i=0; i<input_length;i++) {
00120 int base64code0=0;
00121 int base64code1=0;
00122 int base64code2=0;
00123 int base64code3=0;
00124
00125 base64code0 = (input_ptr[i] >> 2) & 0x3f;
00126 output += encoding_data[base64code0];
00127 base64code1 = (input_ptr[i] << 4 ) & 0x3f;
00128
00129 if (++i < input_length) {
00130 base64code1 |= (input_ptr[i] >> 4) & 0x0f;
00131 output += encoding_data[base64code1];
00132 base64code2 = (input_ptr[i] << 2) & 0x3f;
00133
00134 if (++i < input_length) {
00135 base64code2 |= (input_ptr[i] >> 6) & 0x03;
00136 base64code3 = input_ptr[i] & 0x3f;
00137 output += encoding_data[base64code2];
00138 output += encoding_data[base64code3];
00139 } else {
00140 output += encoding_data[base64code2];
00141 output += '=';
00142 }
00143 } else {
00144 output += encoding_data[base64code1];
00145 output += '=';
00146 output += '=';
00147 }
00148 }
00149
00150 return true;
00151 }
00152
00153 std::string algorithm::url_decode(const std::string& str)
00154 {
00155 char decode_buf[3];
00156 std::string result;
00157 result.reserve(str.size());
00158
00159 for (std::string::size_type pos = 0; pos < str.size(); ++pos) {
00160 switch(str[pos]) {
00161 case '+':
00162
00163 result += ' ';
00164 break;
00165 case '%':
00166
00167 if (pos + 2 < str.size()) {
00168 decode_buf[0] = str[++pos];
00169 decode_buf[1] = str[++pos];
00170 decode_buf[2] = '\0';
00171 result += static_cast<char>( strtol(decode_buf, 0, 16) );
00172 } else {
00173
00174 result += '%';
00175 }
00176 break;
00177 default:
00178
00179 result += str[pos];
00180 }
00181 };
00182
00183 return result;
00184 }
00185
00186 std::string algorithm::url_encode(const std::string& str)
00187 {
00188 char encode_buf[4];
00189 std::string result;
00190 encode_buf[0] = '%';
00191 result.reserve(str.size());
00192
00193
00194
00195
00196 for (std::string::size_type pos = 0; pos < str.size(); ++pos) {
00197 switch(str[pos]) {
00198 default:
00199 if (str[pos] > 32 && str[pos] < 127) {
00200
00201 result += str[pos];
00202 break;
00203 }
00204
00205 case ' ':
00206 case '$': case '&': case '+': case ',': case '/': case ':':
00207 case ';': case '=': case '?': case '@': case '"': case '<':
00208 case '>': case '#': case '%': case '{': case '}': case '|':
00209 case '\\': case '^': case '~': case '[': case ']': case '`':
00210
00211 sprintf(encode_buf+1, "%.2X", (unsigned char)(str[pos]));
00212 result += encode_buf;
00213 break;
00214 }
00215 };
00216
00217 return result;
00218 }
00219
00220 void algorithm::float_from_bytes(long double& value, const unsigned char *ptr, size_t num_exp_bits, size_t num_fraction_bits)
00221 {
00222
00223 const int value_sign = (*ptr & 0x80) ? -1 : 1;
00224
00225
00226 unsigned char mask = 0x80;
00227 boost::int16_t exponent = 0;
00228 for (size_t n = 0; n < num_exp_bits; ++n) {
00229 SHIFT_BITMASK(ptr, mask);
00230 exponent *= 2;
00231 if (*ptr & mask)
00232 exponent += 1;
00233 }
00234
00235
00236 long double significand = exponent ? 1.0 : 0.0;
00237 long double significand_value = 1.0;
00238 while (num_fraction_bits) {
00239 SHIFT_BITMASK(ptr, mask);
00240 significand_value /= 2;
00241 if (*ptr & mask)
00242 significand += significand_value;
00243 --num_fraction_bits;
00244 }
00245
00246
00247 exponent -= (::pow((long double)2, (int)(num_exp_bits - 1)) - 1);
00248 value = value_sign * significand * ::pow((long double)2, exponent);
00249 }
00250
00251 void algorithm::float_to_bytes(long double value, unsigned char *buf, size_t num_exp_bits, size_t num_fraction_bits)
00252 {
00253
00254 unsigned char *ptr = buf;
00255 memset(ptr, 0x00, ::ceil(static_cast<float>(num_exp_bits + num_fraction_bits + 1) / 8));
00256
00257
00258 if (value < 0) {
00259 *ptr = 0x80;
00260 value *= -1;
00261 }
00262
00263
00264 boost::int16_t exponent = 0;
00265 while (value >= 1) {
00266 value /= 2;
00267 ++exponent;
00268 }
00269
00270
00271 unsigned char mask = 0x40;
00272 for (size_t n = num_exp_bits; n > 0; --n) {
00273 if (n >= 8) {
00274 ++ptr;
00275 n -= 7;
00276 } else {
00277 SHIFT_BITMASK(ptr, mask);
00278 }
00279 }
00280
00281
00282 bool got_exponent = false;
00283 boost::uint16_t num_bits = 0;
00284 while (value && num_bits < num_fraction_bits) {
00285 value *= 2;
00286 if (got_exponent) {
00287 if (value >= 1.0) {
00288 *ptr |= mask;
00289 value -= 1.0;
00290 }
00291 SHIFT_BITMASK(ptr, mask);
00292 ++num_bits;
00293 } else {
00294 --exponent;
00295 if (value >= 1.0) {
00296 value -= 1.0;
00297 got_exponent = true;
00298 }
00299 }
00300 }
00301
00302
00303
00304 boost::int32_t high_bit = ::pow((long double)2, (int)(num_exp_bits - 1));
00305 if (got_exponent)
00306 exponent += (high_bit - 1);
00307 else
00308 exponent = 0;
00309
00310
00311 ptr = buf;
00312 mask = 0x80;
00313 for (size_t n = 0; n < num_exp_bits; ++n) {
00314 SHIFT_BITMASK(ptr, mask);
00315 if (exponent >= high_bit) {
00316 *ptr |= mask;
00317 exponent -= high_bit;
00318 }
00319 high_bit /= 2;
00320 }
00321 }
00322
00323 }