Lucas de Vries
1255 lines
24 KiB
C++
1255 lines
24 KiB
C++
#include <network/message.h>
|
|
#include <stdio.h>
|
|
#include <assert.h>
|
|
#include <math.h>
|
|
#include <algorithm>
|
|
#include <stdint.h>
|
|
|
|
#if defined(_MSC_VER)
|
|
#include <intrin.h>
|
|
#define GET_MSB(var, x) do{ unsigned long _index_; _BitScanReverse(&_index_, x); var = _index_ + 1; } while(false)
|
|
#elif defined(__GNUC__)
|
|
#define GET_MSB(var, x) do { var = (32 - __builtin_clz(x)); } while(false)
|
|
#else
|
|
unsigned _get_msb(unsigned v) {
|
|
unsigned index = 0;
|
|
while((1 << index) <= v)
|
|
++index;
|
|
return index;
|
|
}
|
|
#define GET_MSB(var, x) do { var = _get_msb(x); } while(false)
|
|
#endif
|
|
|
|
namespace net {
|
|
|
|
static uint8_t bitmasks[8] = {
|
|
0,
|
|
254,
|
|
252,
|
|
248,
|
|
240,
|
|
224,
|
|
192,
|
|
128,
|
|
};
|
|
|
|
#ifdef _DEBUG
|
|
#define check_read(read_size) \
|
|
if(size() < read_bytes + read_size) {\
|
|
readError = true;\
|
|
throw MessageReadError();\
|
|
}
|
|
#else
|
|
#define check_read(read_size) \
|
|
if(size() < read_bytes + read_size) {\
|
|
readError = true;\
|
|
}
|
|
#endif
|
|
|
|
uint8_t* Message::Buffer::data() {
|
|
return (uint8_t*)this;
|
|
}
|
|
|
|
Message::Buffer* Message::Buffer::create(msize_t bytes) {
|
|
Buffer* b = (Buffer*)malloc(bytes);
|
|
b->type = 0;
|
|
return b;
|
|
}
|
|
|
|
Message::Buffer* Message::Buffer::copyPacket(uint8_t* pBytes, msize_t bytes) {
|
|
Buffer* b = (Buffer*)malloc(bytes);
|
|
memcpy(b, pBytes, bytes);
|
|
return b;
|
|
}
|
|
|
|
void Message::Buffer::resize(Buffer*& buff, msize_t prevSize, msize_t bytes) {
|
|
Buffer* newBuff = (Buffer*)realloc(buff, bytes);
|
|
if(newBuff) {
|
|
buff = newBuff;
|
|
}
|
|
else {
|
|
newBuff = (Buffer*)malloc(bytes);
|
|
if(newBuff == 0)
|
|
throw MessageReadError();
|
|
memcpy(newBuff, buff, prevSize <= bytes ? prevSize : bytes);
|
|
free(buff);
|
|
buff = newBuff;
|
|
}
|
|
}
|
|
|
|
void Message::getAsPacket(char*& pBytes, msize_t& bytes) {
|
|
pBytes = (char*)buffer->data();
|
|
bytes = size();
|
|
}
|
|
|
|
void Message::setPacket(char* pBytes, msize_t bytes) {
|
|
if(!buffer) {
|
|
buffer = Buffer::copyPacket((uint8_t*)pBytes, bytes);
|
|
}
|
|
else {
|
|
Buffer::resize(buffer, allocated, bytes);
|
|
memcpy(buffer, pBytes, bytes);
|
|
}
|
|
|
|
used_bytes = bytes;
|
|
used_bits = 0;
|
|
allocated = bytes;
|
|
|
|
if(hasFlags()) {
|
|
if(buffer->flags & MF_Sequenced)
|
|
read_bytes = 6;
|
|
else if(buffer->flags & MF_Reliable)
|
|
read_bytes = 4;
|
|
else
|
|
read_bytes = 2;
|
|
}
|
|
else {
|
|
read_bytes = 1;
|
|
}
|
|
}
|
|
|
|
uint8_t& Message::operator[](msize_t byte) {
|
|
assert(byte < size());
|
|
return buffer->data()[byte];
|
|
}
|
|
|
|
const uint8_t& Message::operator[](msize_t byte) const {
|
|
assert(byte < size());
|
|
return buffer->data()[byte];
|
|
}
|
|
|
|
void Message::_prepwrite(msize_t bytes) {
|
|
msize_t newUsed = used_bytes + bytes;
|
|
assert(newUsed > used_bytes);
|
|
|
|
if(newUsed > allocated) {
|
|
if(allocated == 0) {
|
|
//Always make sure the message has a type
|
|
unsigned alloc = 16;
|
|
if(bytes+1 > alloc)
|
|
alloc = bytes + 1;
|
|
buffer = Buffer::create(alloc);
|
|
buffer->type = MT_Invalid;
|
|
used_bytes = 1;
|
|
read_bytes = 1;
|
|
allocated = alloc;
|
|
}
|
|
else {
|
|
//Make the message bigger
|
|
auto newSize = allocated;
|
|
|
|
do {
|
|
newSize = (newSize+1) * 2;
|
|
} while(newSize < newUsed);
|
|
|
|
allocate(newSize);
|
|
}
|
|
}
|
|
}
|
|
|
|
Message::Message()
|
|
: buffer(0), allocated(0), used_bytes(0), used_bits(0), read_bytes(0), read_bits(0), HasID(false), readError(false) {
|
|
}
|
|
|
|
Message::Message(uint8_t* data, unsigned bytes)
|
|
: buffer(0), allocated(0), used_bytes(0), used_bits(0), read_bytes(0), read_bits(0), HasID(false), readError(false)
|
|
{
|
|
setPacket((char*)data, bytes);
|
|
}
|
|
|
|
Message::Message(uint8_t type, uint8_t flags)
|
|
: buffer(Buffer::create(64)), allocated(64), used_bytes(0), used_bits(0), read_bytes(0), read_bits(0), HasID(false), readError(false)
|
|
{
|
|
buffer->type = type;
|
|
|
|
if(hasFlags()) {
|
|
buffer->flags = flags;
|
|
|
|
if(flags & MF_Sequenced)
|
|
used_bytes = 6;
|
|
else if(flags & MF_Reliable)
|
|
used_bytes = 4;
|
|
else
|
|
used_bytes = 2;
|
|
|
|
memset(((char*)buffer)+2, 0, used_bytes-2);
|
|
}
|
|
else {
|
|
used_bytes = 1;
|
|
}
|
|
|
|
read_bytes = used_bytes;
|
|
}
|
|
|
|
Message::Message(Message& other) {
|
|
//Copy over all message data
|
|
memcpy(this, &other, sizeof(Message));
|
|
buffer = Buffer::create(other.size());
|
|
|
|
//Copy over buffer
|
|
msize_t bytes;
|
|
char* pBytes;
|
|
other.getAsPacket(pBytes, bytes);
|
|
|
|
memcpy(buffer, pBytes, bytes);
|
|
}
|
|
|
|
Message::~Message() {
|
|
if(buffer)
|
|
free(buffer);
|
|
}
|
|
|
|
void Message::allocate(msize_t size) {
|
|
if(size <= allocated)
|
|
return;
|
|
|
|
if(buffer)
|
|
Buffer::resize(buffer, allocated, size);
|
|
else
|
|
buffer = Buffer::create(size);
|
|
allocated = size;
|
|
}
|
|
|
|
void Message::clear() {
|
|
if(buffer) {
|
|
free(buffer);
|
|
buffer = 0;
|
|
}
|
|
allocated = 0;
|
|
used_bytes = 0;
|
|
used_bits = 0;
|
|
read_bytes = 0;
|
|
read_bits = 0;
|
|
readError = false;
|
|
}
|
|
|
|
void Message::move(Message& other) {
|
|
other.buffer = buffer;
|
|
other.allocated = allocated;
|
|
other.used_bytes = used_bytes;
|
|
other.used_bits = used_bits;
|
|
other.read_bytes = read_bytes;
|
|
other.read_bits = read_bits;
|
|
other.readError = readError;
|
|
|
|
buffer = 0;
|
|
clear();
|
|
}
|
|
|
|
void Message::reset() {
|
|
if(buffer == 0)
|
|
return;
|
|
|
|
//Reset write to the beginning of the message
|
|
if(hasFlags()) {
|
|
if(buffer->flags & MF_Sequenced)
|
|
used_bytes = 6;
|
|
else if(buffer->flags & MF_Reliable)
|
|
used_bytes = 4;
|
|
else
|
|
used_bytes = 2;
|
|
|
|
memset(((char*)buffer)+2, 0, used_bytes-2);
|
|
}
|
|
else {
|
|
used_bytes = 1;
|
|
}
|
|
used_bits = 0;
|
|
}
|
|
|
|
void Message::operator=(const Message& other) {
|
|
used_bytes = other.used_bytes;
|
|
used_bits = other.used_bits;
|
|
read_bytes= other.read_bytes;
|
|
read_bits = other.read_bits;
|
|
readError = other.readError;
|
|
|
|
allocate(other.allocated);
|
|
memcpy(buffer, other.buffer, allocated);
|
|
}
|
|
|
|
bool Message::hasError() const {
|
|
return readError;
|
|
}
|
|
|
|
void Message::setType(uint8_t type) {
|
|
buffer->type = type;
|
|
}
|
|
|
|
uint8_t Message::getType() const {
|
|
if(!buffer)
|
|
return MT_Invalid;
|
|
else
|
|
return buffer->type;
|
|
}
|
|
|
|
uint8_t Message::getFlags() const {
|
|
if(!hasFlags())
|
|
return 0;
|
|
else
|
|
return buffer->flags;
|
|
}
|
|
|
|
bool Message::getFlag(uint8_t flag) const {
|
|
if(!hasFlags())
|
|
return false;
|
|
else
|
|
return (buffer->flags & flag) != 0;
|
|
}
|
|
|
|
bool Message::hasFlags() const {
|
|
if(!buffer)
|
|
return false;
|
|
if(buffer->type >= MessageFlagsAll)
|
|
return true;
|
|
return MessageHasFlags[buffer->type];
|
|
}
|
|
|
|
void Message::setFlags(uint8_t flags) {
|
|
if(!hasFlags())
|
|
return;
|
|
buffer->flags = flags;
|
|
}
|
|
|
|
bool Message::hasID() const {
|
|
return HasID;
|
|
}
|
|
|
|
void Message::setID(unsigned short id) {
|
|
if(!buffer || !getFlag(MF_Reliable | MF_Sequenced))
|
|
return;
|
|
buffer->id = id;
|
|
HasID = true;
|
|
}
|
|
|
|
unsigned short Message::getID() const {
|
|
if(!buffer || !getFlag(MF_Reliable | MF_Sequenced))
|
|
return 0;
|
|
return buffer->id;
|
|
}
|
|
|
|
void Message::setSeqID(unsigned short id) {
|
|
if(!buffer || !getFlag(MF_Sequenced))
|
|
return;
|
|
buffer->seqID = id;
|
|
}
|
|
|
|
unsigned short Message::getSeqID() const {
|
|
if(!buffer || !getFlag(MF_Sequenced))
|
|
return 0;
|
|
return buffer->seqID;
|
|
}
|
|
|
|
msize_t Message::size() const {
|
|
if(!buffer)
|
|
return 0;
|
|
else if(used_bits > 0)
|
|
return used_bytes + 1;
|
|
else
|
|
return used_bytes;
|
|
}
|
|
|
|
void Message::finalize() {
|
|
writeAlign();
|
|
}
|
|
|
|
void Message::writeAlign() {
|
|
if(used_bits != 0) {
|
|
_prepwrite(1);
|
|
used_bits = 0;
|
|
used_bytes += 1;
|
|
}
|
|
}
|
|
|
|
void Message::readAlign() {
|
|
if(read_bits != 0) {
|
|
check_read(1);
|
|
if(readError)
|
|
return;
|
|
read_bits = 0;
|
|
read_bytes += 1;
|
|
}
|
|
}
|
|
|
|
void Message::dump() {
|
|
unsigned i = 0, cnt = size();
|
|
uint8_t* data = buffer->data();
|
|
for(; i < cnt; ++i) {
|
|
printf("0x%x ", data[i]);
|
|
}
|
|
printf("\n");
|
|
}
|
|
|
|
void Message::rewind() {
|
|
//Rewind read message
|
|
if(hasFlags()) {
|
|
if(buffer->flags & MF_Sequenced)
|
|
read_bytes = 6;
|
|
else if(buffer->flags & MF_Reliable)
|
|
read_bytes = 4;
|
|
else
|
|
read_bytes = 2;
|
|
}
|
|
else {
|
|
read_bytes = 1;
|
|
}
|
|
|
|
read_bits = 0;
|
|
}
|
|
|
|
void Message::rewind(unsigned bytes, unsigned bits) {
|
|
read_bits -= bits;
|
|
while(read_bits < 0) {
|
|
read_bits += 8;
|
|
bytes += 1;
|
|
}
|
|
|
|
if(bytes < read_bytes)
|
|
read_bytes -= bytes;
|
|
else
|
|
read_bytes = 0;
|
|
}
|
|
|
|
void Message::advance(unsigned bits) {
|
|
unsigned bytes = bits / 8;
|
|
bits = bits % 8;
|
|
read_bits += bits;
|
|
if(read_bits >= 8) {
|
|
read_bits -= 8;
|
|
bytes += 1;
|
|
}
|
|
|
|
read_bytes += bytes;
|
|
}
|
|
|
|
Message::Position Message::getReadPosition() {
|
|
Position pos;
|
|
pos.bytes = read_bytes;
|
|
pos.bits = read_bits;
|
|
return pos;
|
|
}
|
|
|
|
Message::Position Message::getWritePosition() {
|
|
Position pos;
|
|
pos.bytes = used_bytes;
|
|
pos.bits = used_bits;
|
|
return pos;
|
|
}
|
|
|
|
void Message::setReadPosition(Position pos) {
|
|
readError = false;
|
|
if(pos.bytes >= read_bytes) {
|
|
if(pos.bits != 0) {
|
|
check_read(pos.bytes + 1 - read_bytes);
|
|
}
|
|
else if(pos.bytes != read_bytes) {
|
|
check_read(pos.bytes - read_bytes);
|
|
}
|
|
|
|
if(readError)
|
|
return;
|
|
}
|
|
read_bytes = pos.bytes;
|
|
read_bits = pos.bits;
|
|
}
|
|
|
|
void Message::setWritePosition(Position pos) {
|
|
if(pos.bytes >= used_bytes) {
|
|
if(pos.bits != 0)
|
|
_prepwrite(pos.bytes + 1 - used_bytes);
|
|
else if(pos.bytes != used_bytes)
|
|
_prepwrite(pos.bytes - used_bytes);
|
|
}
|
|
used_bytes = pos.bytes;
|
|
used_bits = pos.bits;
|
|
}
|
|
|
|
void Message::copyTo(Message& to, msize_t fromPos, msize_t toPos) const {
|
|
auto sz = size();
|
|
if(fromPos >= sz || toPos > sz)
|
|
throw MessageReadError();
|
|
if(toPos == 0) {
|
|
if(used_bits == 0)
|
|
to.writeBits(buffer->data() + fromPos, (used_bytes - fromPos) * 8);
|
|
else
|
|
to.writeBits(buffer->data() + fromPos, (used_bytes - fromPos) * 8 + used_bits);
|
|
}
|
|
else {
|
|
to.writeBits(buffer->data() + fromPos, (toPos - fromPos) * 8);
|
|
}
|
|
}
|
|
|
|
msize_t Message::reserve(size_t bytes) {
|
|
if(used_bits != 0)
|
|
throw "Message needs to be aligned before reserving space";
|
|
|
|
_prepwrite(bytes);
|
|
msize_t pos = used_bytes;
|
|
used_bytes += bytes;
|
|
return pos;
|
|
}
|
|
|
|
void Message::writeBits(uint8_t* ptr, unsigned bits) {
|
|
unsigned bytes = bits / 8;
|
|
bits = bits % 8;
|
|
|
|
if(used_bits == 0) {
|
|
if(bits == 0) {
|
|
if(bytes == 0)
|
|
return;
|
|
_prepwrite(bytes);
|
|
|
|
memcpy(buffer->data() + used_bytes, ptr, bytes);
|
|
used_bytes += bytes;
|
|
return;
|
|
}
|
|
else {
|
|
_prepwrite(bytes + 1);
|
|
|
|
memcpy(buffer->data() + used_bytes, ptr, bytes + 1);
|
|
used_bytes += bytes;
|
|
used_bits = bits;
|
|
}
|
|
}
|
|
else {
|
|
if(used_bits + bits <= 8) {
|
|
_prepwrite(bytes + 1);
|
|
}
|
|
else {
|
|
_prepwrite(bytes + 2);
|
|
}
|
|
|
|
uint8_t* data = buffer->data();
|
|
|
|
uint8_t* pBuffer = (uint8_t*)alloca(bytes+1);
|
|
//Set to upper portion of input data
|
|
for(unsigned i = 0; i < bytes; ++i)
|
|
pBuffer[i] = ptr[i] >> used_bits;
|
|
pBuffer[bytes] = 0;
|
|
|
|
//Overlay existing bits
|
|
pBuffer[0] |= data[used_bytes] & (0xff00 >> used_bits);
|
|
//Overlay lower portion of input data
|
|
for(unsigned i = 0; i < bytes; ++i)
|
|
pBuffer[i+1] |= ptr[i] << (8 - used_bits);
|
|
memcpy(data + used_bytes, pBuffer, bytes+1);
|
|
|
|
used_bytes += bytes;
|
|
|
|
if(bits != 0) {
|
|
uint8_t front_mask = bitmasks[used_bits];
|
|
uint8_t back_mask = ~front_mask;
|
|
uint8_t rev_bits = (8 - used_bits);
|
|
uint8_t ex_mask = bitmasks[rev_bits];
|
|
|
|
uint8_t source = ptr[bytes];
|
|
|
|
if(used_bits + bits <= 8) {
|
|
data[used_bytes] = (data[used_bytes] & ex_mask) | (source >> used_bits);
|
|
|
|
if(used_bits + bits == 8) {
|
|
used_bytes += 1;
|
|
used_bits = 0;
|
|
}
|
|
else
|
|
used_bits += bits;
|
|
}
|
|
else {
|
|
uint8_t front = source & front_mask;
|
|
uint8_t back = source & back_mask;
|
|
|
|
data[used_bytes] = (data[used_bytes] & ex_mask) | (front >> used_bits);
|
|
used_bytes += 1;
|
|
data[used_bytes] = back << rev_bits;
|
|
used_bits = (used_bits + bits) % 8;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void Message::readBits(uint8_t* ptr, unsigned bits) {
|
|
unsigned bytes = bits / 8;
|
|
bits = bits % 8;
|
|
|
|
if(read_bits == 0) {
|
|
if(bits == 0) {
|
|
check_read(bytes);
|
|
if(readError)
|
|
return;
|
|
memcpy(ptr, buffer->data() + read_bytes, bytes);
|
|
read_bytes += bytes;
|
|
}
|
|
else {
|
|
check_read(bytes + 1);
|
|
if(readError)
|
|
return;
|
|
memcpy(ptr, buffer->data() + read_bytes, bytes + 1);
|
|
read_bytes += bytes;
|
|
read_bits = bits;
|
|
}
|
|
}
|
|
else {
|
|
if(read_bits + bits <= 8) {
|
|
check_read(bytes + 1);
|
|
}
|
|
else {
|
|
check_read(bytes + 2);
|
|
}
|
|
if(readError)
|
|
return;
|
|
|
|
uint8_t* data = buffer->data();
|
|
|
|
for(unsigned i = 0; i < bytes; ++i) {
|
|
ptr[i] = (data[read_bytes] << read_bits) | (data[read_bytes+1] >> (8 - read_bits));
|
|
read_bytes += 1;
|
|
}
|
|
|
|
if(bits != 0) {
|
|
uint8_t rev_bits = (8 - read_bits);
|
|
uint8_t back_mask = bitmasks[rev_bits];
|
|
uint8_t front_mask = ~back_mask;
|
|
|
|
if(read_bits + bits <= 8) {
|
|
ptr[bytes] = (data[read_bytes] & front_mask) << read_bits;
|
|
|
|
if(read_bits + bits == 8) {
|
|
read_bytes += 1;
|
|
read_bits = 0;
|
|
}
|
|
else
|
|
read_bits += bits;
|
|
}
|
|
else {
|
|
uint8_t front = data[read_bytes] & front_mask;
|
|
read_bytes += 1;
|
|
uint8_t back = data[read_bytes] & back_mask;
|
|
|
|
ptr[bytes] = (front << read_bits) | (back >> rev_bits);
|
|
read_bits = (read_bits + bits) % 8;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void Message::writeBit(bool bit) {
|
|
if(bit)
|
|
write1();
|
|
else
|
|
write0();
|
|
}
|
|
|
|
void Message::write1() {
|
|
if(used_bits == 0) {
|
|
_prepwrite(1);
|
|
buffer->data()[used_bytes] = 128;
|
|
used_bits = 1;
|
|
}
|
|
else if (used_bits == 7) {
|
|
buffer->data()[used_bytes] |= 1;
|
|
used_bits = 0;
|
|
used_bytes += 1;
|
|
}
|
|
else {
|
|
buffer->data()[used_bytes] |= 128 >> used_bits;
|
|
used_bits += 1;
|
|
}
|
|
}
|
|
|
|
void Message::write0() {
|
|
if(used_bits == 0) {
|
|
_prepwrite(1);
|
|
buffer->data()[used_bytes] = 0;
|
|
used_bits = 1;
|
|
}
|
|
else if (used_bits == 7) {
|
|
buffer->data()[used_bytes] &= 254;
|
|
used_bits = 0;
|
|
used_bytes += 1;
|
|
}
|
|
else {
|
|
buffer->data()[used_bytes] &= ~(128 >> used_bits);
|
|
used_bits += 1;
|
|
}
|
|
}
|
|
|
|
bool Message::readBit() {
|
|
if(read_bits == 0) {
|
|
check_read(1);
|
|
if(readError)
|
|
return false;
|
|
read_bits = 1;
|
|
return (buffer->data()[read_bytes] & 128) != 0;
|
|
}
|
|
else if(read_bits == 7) {
|
|
bool res = (buffer->data()[read_bytes] & 1) != 0;
|
|
read_bytes += 1;
|
|
read_bits = 0;
|
|
return res;
|
|
}
|
|
else {
|
|
bool res = (buffer->data()[read_bytes] & (128 >> read_bits)) != 0;
|
|
read_bits += 1;
|
|
return res;
|
|
}
|
|
}
|
|
|
|
Message& Message::operator<<(bool bit) {
|
|
writeBit(bit);
|
|
return *this;
|
|
}
|
|
|
|
Message& Message::operator<<(const char* str) {
|
|
writeAlign();
|
|
|
|
int len = strlen(str) + 1;
|
|
_prepwrite(len);
|
|
|
|
memcpy(buffer->data() + used_bytes, str, len);
|
|
used_bytes += len;
|
|
return *this;
|
|
}
|
|
|
|
Message& Message::operator<<(const std::string& str) {
|
|
writeAlign();
|
|
|
|
_prepwrite(str.size() + 1);
|
|
memcpy(buffer->data() + used_bytes, str.c_str(), str.size() + 1);
|
|
used_bytes += str.size() + 1;
|
|
return *this;
|
|
}
|
|
|
|
Message& Message::operator>>(bool& bit) {
|
|
bit = readBit();
|
|
return *this;
|
|
}
|
|
|
|
Message& Message::operator>>(char*& str) {
|
|
readAlign();
|
|
|
|
int maxlen = used_bytes - read_bytes;
|
|
int len = strnlen((char*)(buffer->data() + read_bytes), maxlen) + 1;
|
|
if(len > maxlen) {
|
|
str = nullptr;
|
|
throw MessageReadError();
|
|
}
|
|
|
|
str = (char*)malloc(len);
|
|
memcpy(str, buffer->data() + read_bytes, len);
|
|
read_bytes += len;
|
|
return *this;
|
|
}
|
|
|
|
Message& Message::operator>>(std::string& str) {
|
|
readAlign();
|
|
|
|
int maxlen = used_bytes - read_bytes;
|
|
int len = strnlen((char*)(buffer->data() + read_bytes), maxlen);
|
|
if(len >= maxlen)
|
|
throw MessageReadError();
|
|
|
|
str.assign((char*)(buffer->data() + read_bytes), len);
|
|
read_bytes += len + 1;
|
|
return *this;
|
|
}
|
|
|
|
void Message::writeSmall(unsigned value) {
|
|
if(value <= 0x7f) {
|
|
unsigned char chr = ((unsigned char)value) | 0x80;
|
|
writeBits((uint8_t*)&chr, 8);
|
|
}
|
|
else if(value <= 0x3fff) {
|
|
unsigned short chr = ((unsigned short)value);
|
|
uint8_t* ptr = (uint8_t*)&chr;
|
|
//ptr[1] &= ~0x80;
|
|
ptr[1] |= 0x40;
|
|
writeBits(&ptr[1], 8);
|
|
writeBits(&ptr[0], 8);
|
|
}
|
|
else if(value <= 0x1fffffff) {
|
|
unsigned chr = value;
|
|
uint8_t* ptr = (uint8_t*)&chr;
|
|
//ptr[3] &= ~0x80;
|
|
//ptr[3] &= ~0x40;
|
|
ptr[3] |= 0x20;
|
|
writeBits(&ptr[3], 8);
|
|
writeBits(&ptr[2], 8);
|
|
writeBits(&ptr[1], 8);
|
|
writeBits(&ptr[0], 8);
|
|
}
|
|
else {
|
|
write0();
|
|
write0();
|
|
write0();
|
|
writeBits((uint8_t*)&value, 32);
|
|
}
|
|
}
|
|
|
|
unsigned Message::readSmall() {
|
|
if(readBit()) {
|
|
unsigned char chr;
|
|
readBits((uint8_t*)&chr, 7);
|
|
return chr >> 1;
|
|
}
|
|
else if(readBit()){
|
|
unsigned short chr;
|
|
uint8_t* ptr = (uint8_t*)&chr;
|
|
readBits(&ptr[1], 6);
|
|
readBits(&ptr[0], 8);
|
|
ptr[1] >>= 2;
|
|
return chr;
|
|
}
|
|
else if(readBit()){
|
|
unsigned chr;
|
|
uint8_t* ptr = (uint8_t*)&chr;
|
|
readBits(&ptr[3], 5);
|
|
readBits(&ptr[2], 8);
|
|
readBits(&ptr[1], 8);
|
|
readBits(&ptr[0], 8);
|
|
ptr[3] >>= 3;
|
|
return chr;
|
|
}
|
|
else {
|
|
unsigned chr;
|
|
readBits((uint8_t*)&chr, 32);
|
|
return chr;
|
|
}
|
|
}
|
|
|
|
void Message::writeSignedSmall(int value) {
|
|
unsigned absValue;
|
|
bool sign;
|
|
if(value < 0) {
|
|
absValue = (unsigned)(value * -1);
|
|
sign = true;
|
|
}
|
|
else {
|
|
absValue = (unsigned)value;
|
|
sign = false;
|
|
}
|
|
|
|
if(absValue <= 0x3f) {
|
|
unsigned char chr = ((unsigned char)absValue) | 0x80;
|
|
if(sign)
|
|
chr |= 0x40;
|
|
writeBits((uint8_t*)&chr, 8);
|
|
}
|
|
else if(absValue <= 0x1fff) {
|
|
unsigned short chr = ((unsigned short)absValue);
|
|
uint8_t* ptr = (uint8_t*)&chr;
|
|
//ptr[1] &= ~0x80;
|
|
ptr[1] |= 0x40;
|
|
if(sign)
|
|
ptr[1] |= 0x20;
|
|
// else
|
|
// ptr[1] &= ~0x20;
|
|
writeBits(&ptr[1], 8);
|
|
writeBits(&ptr[0], 8);
|
|
}
|
|
else if(absValue <= 0x0fffffff) {
|
|
unsigned chr = absValue;
|
|
uint8_t* ptr = (uint8_t*)&chr;
|
|
//ptr[3] &= ~0x80;
|
|
//ptr[3] &= ~0x40;
|
|
ptr[3] |= 0x20;
|
|
if(sign)
|
|
ptr[3] |= 0x10;
|
|
// else
|
|
// ptr[3] &= ~0x10;
|
|
writeBits(&ptr[3], 8);
|
|
writeBits(&ptr[2], 8);
|
|
writeBits(&ptr[1], 8);
|
|
writeBits(&ptr[0], 8);
|
|
}
|
|
else {
|
|
write0();
|
|
write0();
|
|
write0();
|
|
writeBits((uint8_t*)&value, 32);
|
|
}
|
|
}
|
|
|
|
int Message::readSignedSmall() {
|
|
if(readBit()) {
|
|
bool sign = readBit();
|
|
unsigned char chr = 0;
|
|
readBits((uint8_t*)&chr, 6);
|
|
chr >>= 2;
|
|
if(sign)
|
|
return -1 * (char)chr;
|
|
return (char)chr;
|
|
}
|
|
else if(readBit()){
|
|
bool sign = readBit();
|
|
short chr = 0;
|
|
uint8_t* ptr = (uint8_t*)&chr;
|
|
readBits(&ptr[1], 5);
|
|
readBits(&ptr[0], 8);
|
|
ptr[1] >>= 3;
|
|
if(sign)
|
|
chr *= -1;
|
|
return chr;
|
|
}
|
|
else if(readBit()){
|
|
bool sign = readBit();
|
|
int chr = 0;
|
|
uint8_t* ptr = (uint8_t*)&chr;
|
|
readBits(&ptr[3], 4);
|
|
readBits(&ptr[2], 8);
|
|
readBits(&ptr[1], 8);
|
|
readBits(&ptr[0], 8);
|
|
ptr[3] >>= 4;
|
|
if(sign)
|
|
chr *= -1;
|
|
return chr;
|
|
}
|
|
else {
|
|
int chr = 0;
|
|
readBits((uint8_t*)&chr, 32);
|
|
return chr;
|
|
}
|
|
}
|
|
|
|
void Message::writeBitValue(unsigned value, uint8_t bits) {
|
|
uint8_t* ptr = (uint8_t*)&value;
|
|
|
|
uint8_t index = 0;
|
|
while(bits >= 8 && index <= 3) {
|
|
writeBits(&ptr[index], 8);
|
|
bits -= 8;
|
|
++index;
|
|
}
|
|
|
|
if(bits != 0 && index <= 3) {
|
|
ptr[index] <<= (8 - bits);
|
|
writeBits(&ptr[index], bits);
|
|
}
|
|
}
|
|
|
|
unsigned Message::readBitValue(uint8_t bits) {
|
|
int value = 0;
|
|
uint8_t* ptr = (uint8_t*)&value;
|
|
|
|
uint8_t index = 0;
|
|
while(bits >= 8 && index <= 3) {
|
|
readBits(&ptr[index], 8);
|
|
bits -= 8;
|
|
++index;
|
|
}
|
|
|
|
if(bits != 0 && index <= 3) {
|
|
readBits(&ptr[index], bits);
|
|
ptr[index] >>= (8 - bits);
|
|
}
|
|
return value;
|
|
}
|
|
|
|
void Message::writeLimited(unsigned value, unsigned limit) {
|
|
if(limit == 0)
|
|
return;
|
|
int msb = 0;
|
|
GET_MSB(msb, limit);
|
|
writeBitValue(value, msb);
|
|
}
|
|
|
|
void Message::writeLimited(unsigned value, unsigned min, unsigned max) {
|
|
writeLimited(value, max-min);
|
|
}
|
|
|
|
unsigned Message::readLimited(unsigned limit) {
|
|
if(limit == 0)
|
|
return 0;
|
|
int msb = 0;
|
|
GET_MSB(msb, limit);
|
|
return readBitValue(msb);
|
|
}
|
|
|
|
unsigned Message::readLimited(unsigned min, unsigned max) {
|
|
return readLimited(max-min) + min;
|
|
}
|
|
|
|
void Message::writeSmallVec3(double x, double y, double z) {
|
|
unsigned mode = 0;
|
|
double primaryDim;
|
|
|
|
double xa = abs(x), ya = abs(y), za = abs(z);
|
|
|
|
if(xa > ya) {
|
|
if(xa > za) {
|
|
//mode = 0;
|
|
primaryDim = x;
|
|
}
|
|
else {
|
|
mode = 2;
|
|
primaryDim = z;
|
|
}
|
|
}
|
|
else if(ya > za) {
|
|
mode = 1;
|
|
primaryDim = y;
|
|
}
|
|
else if(za > 0) {
|
|
mode = 2;
|
|
primaryDim = z;
|
|
}
|
|
else {
|
|
mode = 3;
|
|
}
|
|
writeBitValue(mode, 2);
|
|
if(mode == 3)
|
|
return;
|
|
|
|
bool flip;
|
|
double secondaryDim, tertiaryDim;
|
|
|
|
if(mode == 0) {
|
|
flip = za > ya;
|
|
secondaryDim = y;
|
|
tertiaryDim = z;
|
|
}
|
|
else if(mode == 1) {
|
|
flip = xa > za;
|
|
secondaryDim = z;
|
|
tertiaryDim = x;
|
|
}
|
|
else if(mode == 2) {
|
|
flip = ya > xa;
|
|
secondaryDim = x;
|
|
tertiaryDim = y;
|
|
}
|
|
|
|
writeBit(flip);
|
|
if(flip)
|
|
std::swap(secondaryDim, tertiaryDim);
|
|
writeFixed(secondaryDim / primaryDim, -1.0, 1.0, 18);
|
|
if(secondaryDim != 0.0)
|
|
writeFixed(tertiaryDim / secondaryDim, -1.0, 1.0, 15);
|
|
else
|
|
writeFixed(0.0, -1.0, 1.0, 15);
|
|
|
|
float fPrimaryDim = primaryDim;
|
|
unsigned floatBits = *(unsigned*)&fPrimaryDim;
|
|
writeBitValue(floatBits >> 4, 28);
|
|
}
|
|
|
|
void Message::readSmallVec3(double& x, double& y, double& z) {
|
|
unsigned mode = readBitValue(2);
|
|
if(mode == 3) {
|
|
x = y = z = 0;
|
|
return;
|
|
}
|
|
|
|
bool flip = readBit();
|
|
|
|
double minorDims[2];
|
|
minorDims[0] = readFixed(-1.0, 1.0, 18);
|
|
minorDims[1] = readFixed(-1.0, 1.0, 15) * minorDims[0];
|
|
|
|
unsigned floatBits = readBitValue(28) << 4;
|
|
double primaryDim = *(float*)&floatBits;
|
|
|
|
if(flip)
|
|
std::swap(minorDims[0], minorDims[1]);
|
|
|
|
switch(mode) {
|
|
case 0:
|
|
x = primaryDim;
|
|
y = primaryDim * minorDims[0];
|
|
z = primaryDim * minorDims[1];
|
|
break;
|
|
case 1:
|
|
y = primaryDim;
|
|
z = primaryDim * minorDims[0];
|
|
x = primaryDim * minorDims[1];
|
|
break;
|
|
case 2:
|
|
z = primaryDim;
|
|
x = primaryDim * minorDims[0];
|
|
y = primaryDim * minorDims[1];
|
|
break;
|
|
}
|
|
}
|
|
|
|
void Message::writeMedVec3(double x, double y, double z) {
|
|
unsigned mode = 0;
|
|
double primaryDim;
|
|
|
|
double xa = abs(x), ya = abs(y), za = abs(z);
|
|
|
|
if(xa > ya) {
|
|
if(xa > za) {
|
|
//mode = 0;
|
|
primaryDim = x;
|
|
}
|
|
else {
|
|
mode = 2;
|
|
primaryDim = z;
|
|
}
|
|
}
|
|
else if(ya > za) {
|
|
mode = 1;
|
|
primaryDim = y;
|
|
}
|
|
else if(za > 0) {
|
|
mode = 2;
|
|
primaryDim = z;
|
|
}
|
|
else {
|
|
mode = 3;
|
|
}
|
|
writeBitValue(mode, 2);
|
|
if(mode == 3)
|
|
return;
|
|
|
|
bool flip;
|
|
double secondaryDim, tertiaryDim;
|
|
|
|
if(mode == 0) {
|
|
flip = za > ya;
|
|
secondaryDim = y;
|
|
tertiaryDim = z;
|
|
}
|
|
else if(mode == 1) {
|
|
flip = xa > za;
|
|
secondaryDim = z;
|
|
tertiaryDim = x;
|
|
}
|
|
else if(mode == 2) {
|
|
flip = ya > xa;
|
|
secondaryDim = x;
|
|
tertiaryDim = y;
|
|
}
|
|
|
|
writeBit(flip);
|
|
if(flip)
|
|
std::swap(secondaryDim, tertiaryDim);
|
|
writeFixed(secondaryDim / primaryDim, -1.0, 1.0, 32);
|
|
if(secondaryDim != 0.0)
|
|
writeFixed(tertiaryDim / secondaryDim, -1.0, 1.0, 29);
|
|
else
|
|
writeFixed(0.0, -1.0, 1.0, 29);
|
|
|
|
*this << (float)primaryDim;
|
|
}
|
|
|
|
void Message::readMedVec3(double& x, double& y, double& z) {
|
|
unsigned mode = readBitValue(2);
|
|
if(mode == 3) {
|
|
x = y = z = 0;
|
|
return;
|
|
}
|
|
|
|
bool flip = readBit();
|
|
|
|
double minorDims[2];
|
|
minorDims[0] = readFixed(-1.0, 1.0, 32);
|
|
minorDims[1] = readFixed(-1.0, 1.0, 29) * minorDims[0];
|
|
float fPrimaryDim;
|
|
*this >> fPrimaryDim;
|
|
double primaryDim = fPrimaryDim;
|
|
|
|
if(flip)
|
|
std::swap(minorDims[0], minorDims[1]);
|
|
|
|
switch(mode) {
|
|
case 0:
|
|
x = primaryDim;
|
|
y = primaryDim * minorDims[0];
|
|
z = primaryDim * minorDims[1];
|
|
break;
|
|
case 1:
|
|
y = primaryDim;
|
|
z = primaryDim * minorDims[0];
|
|
x = primaryDim * minorDims[1];
|
|
break;
|
|
case 2:
|
|
z = primaryDim;
|
|
x = primaryDim * minorDims[0];
|
|
y = primaryDim * minorDims[1];
|
|
break;
|
|
}
|
|
}
|
|
|
|
//Slightly larger than pi to avoid checking for bounds
|
|
const double _pi = 3.141593;
|
|
|
|
void Message::writeDirection(double x, double y, double z, unsigned acc) {
|
|
writeFixed(acos(z),0.0,_pi,acc-1);
|
|
writeFixed(atan2(y,x),-_pi,_pi,acc+1);
|
|
}
|
|
|
|
void Message::readDirection(double& x, double& y, double& z, unsigned acc) {
|
|
z = cos(readFixed(0.0,_pi,acc-1));
|
|
double ang = readFixed(-_pi,_pi,acc+1);
|
|
|
|
double l = sqrt(1.0 - z*z);
|
|
x = cos(ang) * l;
|
|
y = sin(ang) * l;
|
|
|
|
l = sqrt(x*x + y*y + z*z);
|
|
x /= l;
|
|
y /= l;
|
|
z /= l;
|
|
}
|
|
|
|
void Message::writeRotation(double x, double y, double z, double w) {
|
|
writeFixed(acos(x),0.0,_pi,11);
|
|
double l = sqrt(y*y + z*z + w*w);
|
|
writeFixed(acos(y/l),0.0,_pi,11);
|
|
writeFixed(atan2(w,z),-_pi,_pi,10);
|
|
}
|
|
|
|
void Message::readRotation(double& x, double& y, double& z, double& w) {
|
|
x = cos(readFixed(0.0,_pi,11));
|
|
|
|
double l = sqrt(1.0 - std::min(x*x,1.0));
|
|
y = cos(readFixed(0.0,_pi,11)) * l;
|
|
|
|
double ang = readFixed(-_pi,_pi,10);
|
|
l = sqrt(1.0 - std::min(x*x+y*y,1.0));
|
|
z = cos(ang) * l;
|
|
w = sin(ang) * l;
|
|
|
|
l = sqrt(x*x + y*y + z*z + w*w);
|
|
|
|
x /= l;
|
|
y /= l;
|
|
z /= l;
|
|
w /= l;
|
|
}
|
|
|
|
void Message::writeFixed(double value, double min, double max, uint8_t bits) {
|
|
if(bits > 32)
|
|
bits = 32;
|
|
else if(bits < 2)
|
|
bits = 2;
|
|
|
|
double step = (max - min) / (double)(((int64_t)1 << bits) - 1);
|
|
double halfStep = step * 0.5;
|
|
|
|
if(value <= min) {
|
|
writeBitValue(0, bits);
|
|
return;
|
|
}
|
|
else if(value >= max - halfStep) {
|
|
writeBitValue(0xffffffff, bits);
|
|
return;
|
|
}
|
|
|
|
unsigned x = (unsigned)((value - min + halfStep) / step);
|
|
writeBitValue(x, bits);
|
|
}
|
|
|
|
double Message::readFixed(double min, double max, uint8_t bits) {
|
|
if(bits > 32)
|
|
bits = 32;
|
|
else if(bits < 2)
|
|
bits = 2;
|
|
|
|
double step = (max - min) / (double)(((int64_t)1 << bits) - 1);
|
|
unsigned x = (unsigned)readBitValue(bits);
|
|
return min + (step * (double)x);
|
|
}
|
|
|
|
};
|