ARGoS 3
A parallel, multi-engine simulator for swarm robotics
byte_array.cpp
Go to the documentation of this file.
1
7#include "byte_array.h"
8
9#include <argos3/core/utility/math/general.h>
10
11#include <arpa/inet.h>
12#include <cstdlib>
13#include <cstring>
14#include <cmath>
15
16namespace argos {
17
18 /****************************************/
19 /****************************************/
20
21 static SInt64 MAX_MANTISSA = 9223372036854775806LL; // 2 << 63 - 2;
22
23 /****************************************/
24 /****************************************/
25
26#ifndef htonll
27 static UInt64 htonll(UInt64 un_value) {
28 /* Define a test variable for endianness - 42 is 'the' answer */
29 static const SInt32 nTest = 42;
30 /* Test for host endianness */
31 if(*reinterpret_cast<const UInt8*>(&nTest) == nTest) {
32 /* Host is little endian, network is big -> convert to big */
33 const UInt32 unHighWord = htonl(static_cast<UInt32>(un_value >> 32));
34 const UInt32 unLowWord = htonl(static_cast<UInt32>(un_value & 0xFFFFFFFFLL));
35 return static_cast<UInt64>(unLowWord) << 32 | unHighWord;
36 }
37 else {
38 /* Host is big endian - leave as is */
39 return un_value;
40 }
41 }
42#endif
43
44#ifndef ntohll
45 static UInt64 ntohll(UInt64 un_value) {
46 /* Define a test variable for endianness - 42 is 'the' answer */
47 static const SInt32 nTest = 42;
48 /* Test for host endianness */
49 if(*reinterpret_cast<const UInt8*>(&nTest) == nTest) {
50 /* Host is little endian, network is big -> convert to big */
51 const UInt32 unHighWord = ntohl(static_cast<UInt32>(un_value >> 32));
52 const UInt32 unLowWord = ntohl(static_cast<UInt32>(un_value & 0xFFFFFFFFLL));
53 return static_cast<UInt64>(unLowWord) << 32 | unHighWord;
54 }
55 else {
56 /* Host is big endian - leave as is */
57 return un_value;
58 }
59 }
60#endif
61
62 /****************************************/
63 /****************************************/
64
65 CByteArray::CByteArray(const UInt8* pun_buffer,
66 size_t un_size) {
67 AddBuffer(pun_buffer, un_size);
68 }
69
70 /****************************************/
71 /****************************************/
72
73 CByteArray::CByteArray(size_t un_size,
74 UInt8 un_value) {
75 m_vecBuffer.assign(un_size, un_value);
76 }
77
78 /****************************************/
79 /****************************************/
80
81 void CByteArray::Zero() {
82 ::memset(&m_vecBuffer[0], 0, sizeof(UInt8) * Size());
83 }
84
85 /****************************************/
86 /****************************************/
87
88 CByteArray& CByteArray::operator=(const CByteArray& c_byte_array) {
89 if(this != &c_byte_array) {
90 m_vecBuffer = c_byte_array.m_vecBuffer;
91 }
92 return *this;
93 }
94
95 /****************************************/
96 /****************************************/
97
98 bool CByteArray::operator==(const CByteArray& c_byte_array) const {
99 return m_vecBuffer == c_byte_array.m_vecBuffer;
100 }
101
102 /****************************************/
103 /****************************************/
104
105 CByteArray& CByteArray::AddBuffer(const UInt8* pun_buffer,
106 size_t un_size) {
107 for(size_t i = 0; i < un_size; ++i) {
108 m_vecBuffer.push_back(pun_buffer[i]);
109 }
110 return *this;
111 }
112
113 /****************************************/
114 /****************************************/
115
116 CByteArray& CByteArray::FetchBuffer(UInt8* pun_buffer,
117 size_t un_size) {
118 if(Size() < un_size) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (" << un_size << " requested, " << Size() << " available)");
119 for(size_t i = 0; i < un_size; ++i) {
120 *(pun_buffer+i) = m_vecBuffer[i];
121 }
122 m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + un_size);
123 return *this;
124 }
125
126 /****************************************/
127 /****************************************/
128
129 CByteArray* CByteArray::operator()(size_t un_start,
130 ssize_t un_end) {
131 if(un_start >= Size()) THROW_ARGOSEXCEPTION("Attempting to extract from byte array beyond the limits (" << un_start << " requested, " << Size() << " size)");
132 un_end = un_end < 0 ? Size() : Min<ssize_t>(un_end, Size());
133 return new CByteArray(ToCArray() + un_start, un_end - un_start);
134 }
135
136 /****************************************/
137 /****************************************/
138
139 CByteArray& CByteArray::operator<<(UInt8 un_value) {
140 m_vecBuffer.push_back(un_value);
141 return *this;
142 }
143
144 /****************************************/
145 /****************************************/
146
147 CByteArray& CByteArray::operator>>(UInt8& un_value) {
148 if(Size() < 1) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (1 requested, " << Size() << " available)");
149 un_value = m_vecBuffer.front();
150 m_vecBuffer.erase(m_vecBuffer.begin());
151 return *this;
152 }
153
154 /****************************************/
155 /****************************************/
156
157 CByteArray& CByteArray::operator<<(SInt8 n_value) {
158 m_vecBuffer.push_back(n_value);
159 return *this;
160 }
161
162 /****************************************/
163 /****************************************/
164
165 CByteArray& CByteArray::operator>>(SInt8& n_value) {
166 if(Size() < 1) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (1 requested, " << Size() << " available)");
167 n_value = m_vecBuffer.front();
168 m_vecBuffer.erase(m_vecBuffer.begin());
169 return *this;
170 }
171
172 /****************************************/
173 /****************************************/
174
175 CByteArray& CByteArray::operator<<(UInt16 un_value) {
176 un_value = htons(un_value);
177 auto* punByte = reinterpret_cast<UInt8*>(&un_value);
178 m_vecBuffer.push_back(punByte[0]);
179 m_vecBuffer.push_back(punByte[1]);
180 return *this;
181 }
182
183 /****************************************/
184 /****************************************/
185
186 CByteArray& CByteArray::operator>>(UInt16& un_value) {
187 if(Size() < 2) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (2 requested, " << Size() << " available)");
188 auto* punByte = reinterpret_cast<UInt8*>(&un_value);
189 punByte[0] = m_vecBuffer[0];
190 punByte[1] = m_vecBuffer[1];
191 m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + 2);
192 un_value = ntohs(un_value);
193 return *this;
194 }
195
196 /****************************************/
197 /****************************************/
198
199 CByteArray& CByteArray::operator<<(SInt16 n_value) {
200 n_value = htons(n_value);
201 auto* punByte = reinterpret_cast<UInt8*>(&n_value);
202 m_vecBuffer.push_back(punByte[0]);
203 m_vecBuffer.push_back(punByte[1]);
204 return *this;
205 }
206
207 /****************************************/
208 /****************************************/
209
210 CByteArray& CByteArray::operator>>(SInt16& n_value) {
211 if(Size() < 2) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (2 requested, " << Size() << " available)");
212 auto* punByte = reinterpret_cast<UInt8*>(&n_value);
213 punByte[0] = m_vecBuffer[0];
214 punByte[1] = m_vecBuffer[1];
215 m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + 2);
216 n_value = ntohs(n_value);
217 return *this;
218 }
219
220 /****************************************/
221 /****************************************/
222
223 CByteArray& CByteArray::operator<<(UInt32 un_value) {
224 un_value = htonl(un_value);
225 auto* punByte = reinterpret_cast<UInt8*>(&un_value);
226 m_vecBuffer.push_back(punByte[0]);
227 m_vecBuffer.push_back(punByte[1]);
228 m_vecBuffer.push_back(punByte[2]);
229 m_vecBuffer.push_back(punByte[3]);
230 return *this;
231 }
232
233 /****************************************/
234 /****************************************/
235
236 CByteArray& CByteArray::operator>>(UInt32& un_value) {
237 if(Size() < 4) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (4 requested, " << Size() << " available)");
238 auto* punByte = reinterpret_cast<UInt8*>(&un_value);
239 punByte[0] = m_vecBuffer[0];
240 punByte[1] = m_vecBuffer[1];
241 punByte[2] = m_vecBuffer[2];
242 punByte[3] = m_vecBuffer[3];
243 m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + 4);
244 un_value = ntohl(un_value);
245 return *this;
246 }
247
248 /****************************************/
249 /****************************************/
250
251 CByteArray& CByteArray::operator<<(SInt32 n_value) {
252 n_value = htonl(n_value);
253 auto* punByte = reinterpret_cast<UInt8*>(&n_value);
254 m_vecBuffer.push_back(punByte[0]);
255 m_vecBuffer.push_back(punByte[1]);
256 m_vecBuffer.push_back(punByte[2]);
257 m_vecBuffer.push_back(punByte[3]);
258 return *this;
259 }
260
261 /****************************************/
262 /****************************************/
263
264 CByteArray& CByteArray::operator>>(SInt32& n_value) {
265 if(Size() < 4) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (4 requested, " << Size() << " available)");
266 auto* punByte = reinterpret_cast<UInt8*>(&n_value);
267 punByte[0] = m_vecBuffer[0];
268 punByte[1] = m_vecBuffer[1];
269 punByte[2] = m_vecBuffer[2];
270 punByte[3] = m_vecBuffer[3];
271 m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + 4);
272 n_value = ntohl(n_value);
273 return *this;
274 }
275
276 /****************************************/
277 /****************************************/
278
279 CByteArray& CByteArray::operator<<(UInt64 un_value) {
280 un_value = htonll(un_value);
281 auto* punByte = reinterpret_cast<UInt8*>(&un_value);
282 m_vecBuffer.push_back(punByte[0]);
283 m_vecBuffer.push_back(punByte[1]);
284 m_vecBuffer.push_back(punByte[2]);
285 m_vecBuffer.push_back(punByte[3]);
286 m_vecBuffer.push_back(punByte[4]);
287 m_vecBuffer.push_back(punByte[5]);
288 m_vecBuffer.push_back(punByte[6]);
289 m_vecBuffer.push_back(punByte[7]);
290 return *this;
291 }
292
293 /****************************************/
294 /****************************************/
295
296 CByteArray& CByteArray::operator>>(UInt64& un_value) {
297 if(Size() < 8) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (8 requested, " << Size() << " available)");
298 auto* punByte = reinterpret_cast<UInt8*>(&un_value);
299 punByte[0] = m_vecBuffer[0];
300 punByte[1] = m_vecBuffer[1];
301 punByte[2] = m_vecBuffer[2];
302 punByte[3] = m_vecBuffer[3];
303 punByte[4] = m_vecBuffer[4];
304 punByte[5] = m_vecBuffer[5];
305 punByte[6] = m_vecBuffer[6];
306 punByte[7] = m_vecBuffer[7];
307 m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + 8);
308 un_value = ntohll(un_value);
309 return *this;
310 }
311
312 /****************************************/
313 /****************************************/
314
315 CByteArray& CByteArray::operator<<(SInt64 n_value) {
316 n_value = htonll(n_value);
317 auto* punByte = reinterpret_cast<UInt8*>(&n_value);
318 m_vecBuffer.push_back(punByte[0]);
319 m_vecBuffer.push_back(punByte[1]);
320 m_vecBuffer.push_back(punByte[2]);
321 m_vecBuffer.push_back(punByte[3]);
322 m_vecBuffer.push_back(punByte[4]);
323 m_vecBuffer.push_back(punByte[5]);
324 m_vecBuffer.push_back(punByte[6]);
325 m_vecBuffer.push_back(punByte[7]);
326 return *this;
327 }
328
329 /****************************************/
330 /****************************************/
331
332 CByteArray& CByteArray::operator>>(SInt64& n_value) {
333 if(Size() < 8) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (8 requested, " << Size() << " available)");
334 auto* punByte = reinterpret_cast<UInt8*>(&n_value);
335 punByte[0] = m_vecBuffer[0];
336 punByte[1] = m_vecBuffer[1];
337 punByte[2] = m_vecBuffer[2];
338 punByte[3] = m_vecBuffer[3];
339 punByte[4] = m_vecBuffer[4];
340 punByte[5] = m_vecBuffer[5];
341 punByte[6] = m_vecBuffer[6];
342 punByte[7] = m_vecBuffer[7];
343 m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + 8);
344 n_value = ntohll(n_value);
345 return *this;
346 }
347
348 /****************************************/
349 /****************************************/
350
351 CByteArray& CByteArray::operator<<(unsigned long int un_value) {
352 if(sizeof(un_value) == sizeof(UInt32)) {
353 *this << static_cast<UInt32>(un_value);
354 }
355 else if(sizeof(un_value) == sizeof(UInt64)) {
356 *this << static_cast<UInt64>(un_value);
357 }
358 return *this;
359 }
360
361 /****************************************/
362 /****************************************/
363
364 CByteArray& CByteArray::operator>>(unsigned long int& un_value) {
365 if(sizeof(un_value) == sizeof(UInt32)) {
366 *this >> *reinterpret_cast<UInt32*>(&un_value);
367 }
368 else if(sizeof(un_value) == sizeof(UInt64)) {
369 *this >> *reinterpret_cast<UInt64*>(&un_value);
370 }
371 return *this;
372 }
373
374 /****************************************/
375 /****************************************/
376
377 CByteArray& CByteArray::operator<<(signed long int n_value) {
378 if(sizeof(n_value) == sizeof(SInt32)) {
379 *this << static_cast<SInt32>(n_value);
380 }
381 else if(sizeof(n_value) == sizeof(SInt64)) {
382 *this << static_cast<SInt64>(n_value);
383 }
384 return *this;
385 }
386
387 /****************************************/
388 /****************************************/
389
390 CByteArray& CByteArray::operator>>(signed long int& n_value) {
391 if(sizeof(n_value) == sizeof(SInt32)) {
392 *this >> *reinterpret_cast<SInt32*>(&n_value);
393 }
394 else if(sizeof(n_value) == sizeof(SInt64)) {
395 *this >> *reinterpret_cast<SInt64*>(&n_value);
396 }
397 return *this;
398 }
399
400 /****************************************/
401 /****************************************/
402
403 CByteArray& CByteArray::operator<<(double f_value) {
404 /* Buffer for the mantissa */
405 SInt64 nMantissa;
406 /* Buffer for the exponent */
407 SInt32 nExponent;
408 /* Calculate exponent and shifted significand */
409 /* The absolute value of the significand is either 0 (when f_value is 0) or
410 * in the range [0.5,1). The sign of the significand is the same as f_value.
411 * The shifted significand is then -0.5 when the significand is 0, and in the
412 * range [0,0.5) otherwise.
413 */
414 double fShiftedSignificand = Abs(frexp(f_value, &nExponent)) - 0.5;
415 /* Calculate mantissa */
416 if(fShiftedSignificand < 0.0) {
417 /* This means f_value was either +0 or -0 */
418 nMantissa = 0;
419 }
420 else {
421 /* Calculate the mantissa
422 * Idea:
423 * - take the shifted significand, which is in [0,0.5)
424 * - multiply it by 2, so it's in [0,1)
425 * - multiply this by the maximum value the mantissa can have
426 * - add 1 to the result to avoid having a zero mantissa
427 * (because it maps to a zero double when trasforming back)
428 */
429 nMantissa = static_cast<SInt64>(fShiftedSignificand * 2.0 * (MAX_MANTISSA)) + 1;
430 /* Take care of the sign */
431 if(f_value < 0.0) {
432 nMantissa = -nMantissa;
433 }
434 }
435 /* Store exponent and mantissa in the buffer */
436 *this << nMantissa;
437 *this << nExponent;
438 return *this;
439 }
440
441 /****************************************/
442 /****************************************/
443
444 CByteArray& CByteArray::operator>>(double& f_value) {
445 if(Size() < sizeof(SInt64) + sizeof(SInt32)) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (" << sizeof(SInt64) + sizeof(SInt32) << " requested, " << Size() << " available)");
446 /* Buffer for the mantissa */
447 SInt64 nMantissa;
448 /* Buffer for the exponent */
449 SInt32 nExponent;
450 /* Extract the values from the buffer */
451 *this >> nMantissa;
452 *this >> nExponent;
453 /* Calculate double value */
454 if(nMantissa == 0) {
455 /* Special case: zero mantissa */
456 f_value = 0.0;
457 }
458 else {
459 /* Calculate the significand, whose absolute value must be in the range [0.5,1).
460 * Idea:
461 * - take the absolute value of the mantissa, which is in the range [1,MANTISSA_MAX]
462 * - subtract 1, so it's in [0,MANTISSA_MAX-1]
463 * - divide by MANTISSA_MAX, so you get a value in [0,1)
464 * - divide by 2, so it's in [0,0.5)
465 * - add 0.5, so you end up in [0.5,1)
466 */
467 double fSignificand = (static_cast<double>(llabs(nMantissa) - 1) / MAX_MANTISSA) / 2.0 + 0.5;
468 /* Calculate f_value */
469 f_value = ldexp(fSignificand, nExponent);
470 /* Take care of the sign */
471 if(nMantissa < 0.0) {
472 f_value = -f_value;
473 }
474 }
475 return *this;
476 }
477
478 /****************************************/
479 /****************************************/
480
481 CByteArray& CByteArray::operator<<(float f_value) {
482 *this << static_cast<double>(f_value);
483 return *this;
484 }
485
486 /****************************************/
487 /****************************************/
488
489 CByteArray& CByteArray::operator>>(float& f_value) {
490 double fDoubleValue;
491 *this >> fDoubleValue;
492 f_value = static_cast<float>(fDoubleValue);
493 return *this;
494 }
495
496 /****************************************/
497 /****************************************/
498
499 CByteArray& CByteArray::operator<<(const std::string& str_value) {
500 /* Insert string contents */
501 for(size_t i = 0; i < str_value.size(); ++i) {
502 *this << static_cast<UInt8>(str_value[i]);
503 }
504 /* Terminate string with a \0 */
505 *this << static_cast<UInt8>(0);
506 return *this;
507 }
508
509 /****************************************/
510 /****************************************/
511
512 CByteArray& CByteArray::operator>>(std::string& str_value) {
513 if(Empty()) THROW_ARGOSEXCEPTION("Attempting to extract values from empty byte array");
514 str_value.clear();
515 size_t i = 0;
516 while(i < Size() && m_vecBuffer[i] != '\0') {
517 str_value += m_vecBuffer[i];
518 ++i;
519 }
520 if(m_vecBuffer[i] == '\0') {
521 ++i;
522 }
523 m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + i);
524 return *this;
525 }
526
527 /****************************************/
528 /****************************************/
529
530 std::ostream& operator<<(std::ostream& c_os, const CByteArray& c_byte_array) {
531 c_os << "CByteArray [";
532 for(size_t i = 0; i < c_byte_array.Size(); ++i) {
533 c_os << " " << c_byte_array.m_vecBuffer[i];
534 }
535 c_os << " ]" << std::endl;
536 return c_os;
537 }
538
539 /****************************************/
540 /****************************************/
541
542}
SInt32
signed int SInt32
32-bit signed integer.
Definition datatypes.h:93
UInt32
unsigned int UInt32
32-bit unsigned integer.
Definition datatypes.h:97
SInt16
signed short SInt16
16-bit signed integer.
Definition datatypes.h:74
UInt8
unsigned char UInt8
8-bit unsigned integer.
Definition datatypes.h:60
SInt64
signed long long SInt64
64-bit signed integer.
Definition datatypes.h:103
UInt64
unsigned long long UInt64
64-bit unsigned integer.
Definition datatypes.h:107
UInt16
unsigned short UInt16
16-bit unsigned integer.
Definition datatypes.h:78
SInt8
signed char SInt8
8-bit signed integer.
Definition datatypes.h:45
THROW_ARGOSEXCEPTION
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
Definition argos_exception.h:111
argos
The namespace containing all the ARGoS related code.
Definition ci_actuator.h:12
argos::operator<<
std::ostream & operator<<(std::ostream &c_os, const CByteArray &c_byte_array)
Definition byte_array.cpp:530
argos::Abs
T Abs(const T &t_v)
Returns the absolute value of the passed argument.
Definition general.h:25
argos::CByteArray
Byte array utility class.
Definition byte_array.h:28
argos::CByteArray::Zero
void Zero()
Sets the contents of the byte array to all zeros.
Definition byte_array.cpp:81
argos::CByteArray::operator=
CByteArray & operator=(const CByteArray &c_byte_array)
Assignment operator.
Definition byte_array.cpp:88
argos::CByteArray::Size
size_t Size() const
Returns the current size of the byte array.
Definition byte_array.h:66
argos::CByteArray::AddBuffer
CByteArray & AddBuffer(const UInt8 *pun_buffer, size_t un_size)
Appends bytes to the byte array.
Definition byte_array.cpp:105
argos::CByteArray::FetchBuffer
CByteArray & FetchBuffer(UInt8 *pun_buffer, size_t un_size)
Moves elements from the byte array into the passed buffer.
Definition byte_array.cpp:116
argos::CByteArray::Empty
bool Empty() const
Returns true if the byte array is empty.
Definition byte_array.h:100
argos::CByteArray::CByteArray
CByteArray()
Class constructor.
Definition byte_array.h:35
argos::CByteArray::operator==
bool operator==(const CByteArray &c_byte_array) const
Equality comparison operator.
Definition byte_array.cpp:98
argos::CByteArray::operator>>
CByteArray & operator>>(UInt8 &un_value)
Moves an 8-bit unsigned integer from the beginning of the byte array to the target variable.
Definition byte_array.cpp:147
argos::CByteArray::ToCArray
const UInt8 * ToCArray() const
Returns the contents of the byte array as a const c-style array.
Definition byte_array.h:112
argos::CByteArray::operator<<
friend std::ostream & operator<<(std::ostream &c_os, const CByteArray &c_byte_array)
Stream operator.
Definition byte_array.cpp:530
argos::CByteArray::operator()
CByteArray * operator()(size_t un_start, ssize_t un_end=-1)
Returns a new byte array that corresponds to a part of this byte array.
Definition byte_array.cpp:129