|
@@ -1,5 +1,7 @@
|
|
|
#include <cassert>
|
|
#include <cassert>
|
|
|
|
|
+#include <cstring>
|
|
|
|
|
|
|
|
|
|
+#include "MathUtils.h"
|
|
|
#include "Message.h"
|
|
#include "Message.h"
|
|
|
|
|
|
|
|
namespace crown
|
|
namespace crown
|
|
@@ -320,49 +322,88 @@ void Message::write_real(real f)
|
|
|
write_bits(*reinterpret_cast<int32_t *>(&f), 32);
|
|
write_bits(*reinterpret_cast<int32_t *>(&f), 32);
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
-void Message::write_real(real f, int32_t exp_bits, int32_t mant_bits)
|
|
|
|
|
-{
|
|
|
|
|
- //TODO:need to implement floatToBits function
|
|
|
|
|
-}
|
|
|
|
|
-
|
|
|
|
|
-void Message::write_angle(real f)
|
|
|
|
|
-{
|
|
|
|
|
- // needs to be implemented
|
|
|
|
|
-}
|
|
|
|
|
-
|
|
|
|
|
void Message::write_vec3(const Vec3& v, int32_t num_bits)
|
|
void Message::write_vec3(const Vec3& v, int32_t num_bits)
|
|
|
{
|
|
{
|
|
|
-
|
|
|
|
|
|
|
+ write_bits(vec3_to_bits(v, num_bits), num_bits);
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
-void Message::write_string(const char* s, int32_t max_len, bool make7Bit)
|
|
|
|
|
|
|
+void Message::write_string(const char* s, int32_t max_len, bool make_7_bit)
|
|
|
{
|
|
{
|
|
|
-
|
|
|
|
|
|
|
+ if (!s)
|
|
|
|
|
+ {
|
|
|
|
|
+ write_data("", 1);
|
|
|
|
|
+ }
|
|
|
|
|
+ else
|
|
|
|
|
+ {
|
|
|
|
|
+ int32_t i;
|
|
|
|
|
+ int32_t l;
|
|
|
|
|
+ uint8_t* data_ptr;
|
|
|
|
|
+ const uint8_t* byte_ptr;
|
|
|
|
|
+
|
|
|
|
|
+ // calculate s length
|
|
|
|
|
+ for (l = 0; s[l]; l++) {}
|
|
|
|
|
+
|
|
|
|
|
+ if (max_len >= 0 && l >= max_len)
|
|
|
|
|
+ {
|
|
|
|
|
+ l = max_len - 1;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ data_ptr = get_byte_space(l + 1);
|
|
|
|
|
+ byte_ptr = reinterpret_cast<const uint8_t*>(s);
|
|
|
|
|
+ if (make_7_bit)
|
|
|
|
|
+ {
|
|
|
|
|
+ for (i = 0; i < l; i++)
|
|
|
|
|
+ {
|
|
|
|
|
+ if ( byte_ptr[i] > 127 )
|
|
|
|
|
+ {
|
|
|
|
|
+ data_ptr[i] = '.';
|
|
|
|
|
+ }
|
|
|
|
|
+ else
|
|
|
|
|
+ {
|
|
|
|
|
+ data_ptr[i] = byte_ptr[i];
|
|
|
|
|
+ }
|
|
|
|
|
+ }
|
|
|
|
|
+ }
|
|
|
|
|
+ else
|
|
|
|
|
+ {
|
|
|
|
|
+ for (i = 0; i < l; i++)
|
|
|
|
|
+ {
|
|
|
|
|
+ data_ptr[i] = byte_ptr[i];
|
|
|
|
|
+ }
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ data_ptr[i] = '\0';
|
|
|
|
|
+ }
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void Message::write_data(const void* data, int32_t length)
|
|
void Message::write_data(const void* data, int32_t length)
|
|
|
{
|
|
{
|
|
|
-
|
|
|
|
|
|
|
+ memcpy(get_byte_space(length), data, length);
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void Message::write_ipv4addr(const os::IPv4Address addr)
|
|
void Message::write_ipv4addr(const os::IPv4Address addr)
|
|
|
{
|
|
{
|
|
|
-
|
|
|
|
|
|
|
+ uint8_t* ptr;
|
|
|
|
|
+
|
|
|
|
|
+ ptr = get_byte_space(4);
|
|
|
|
|
+ memcpy(ptr, addr.address, 4);
|
|
|
|
|
+ write_uint16(addr.port);
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void Message::begin_reading() const
|
|
void Message::begin_reading() const
|
|
|
{
|
|
{
|
|
|
-
|
|
|
|
|
|
|
+ read_count = 0;
|
|
|
|
|
+ read_bit = 0;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
int32_t Message::get_remaing_data() const
|
|
int32_t Message::get_remaing_data() const
|
|
|
{
|
|
{
|
|
|
-
|
|
|
|
|
|
|
+ cur_size - read_count;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void Message::read_byte_align() const
|
|
void Message::read_byte_align() const
|
|
|
{
|
|
{
|
|
|
-
|
|
|
|
|
|
|
+ read_bit = 0;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
int32_t Message::read_bits(int32_t num_bits) const
|
|
int32_t Message::read_bits(int32_t num_bits) const
|
|
@@ -373,12 +414,14 @@ int32_t Message::read_bits(int32_t num_bits) const
|
|
|
int32_t fraction;
|
|
int32_t fraction;
|
|
|
bool sgn;
|
|
bool sgn;
|
|
|
|
|
|
|
|
- if ( !r_data ) {
|
|
|
|
|
|
|
+ if (!r_data)
|
|
|
|
|
+ {
|
|
|
printf("cannot read from message");
|
|
printf("cannot read from message");
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// check if the number of bits is valid
|
|
// check if the number of bits is valid
|
|
|
- if ( num_bits == 0 || num_bits < -31 || num_bits > 32 ) {
|
|
|
|
|
|
|
+ if ( num_bits == 0 || num_bits < -31 || num_bits > 32 )
|
|
|
|
|
+ {
|
|
|
printf("bad number of bits %i", num_bits );
|
|
printf("bad number of bits %i", num_bits );
|
|
|
}
|
|
}
|
|
|
|
|
|
|
@@ -402,7 +445,7 @@ int32_t Message::read_bits(int32_t num_bits) const
|
|
|
return -1;
|
|
return -1;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
- while (value_bits < num_bits )
|
|
|
|
|
|
|
+ while (value_bits < num_bits)
|
|
|
{
|
|
{
|
|
|
if (read_bit == 0)
|
|
if (read_bit == 0)
|
|
|
{
|
|
{
|
|
@@ -438,63 +481,158 @@ int32_t Message::read_bits(int32_t num_bits) const
|
|
|
|
|
|
|
|
int32_t Message::read_int8() const
|
|
int32_t Message::read_int8() const
|
|
|
{
|
|
{
|
|
|
|
|
+ return (int32_t)read_bits(-8);
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
int32_t Message::read_uint8() const
|
|
int32_t Message::read_uint8() const
|
|
|
{
|
|
{
|
|
|
|
|
+ return (int32_t)read_bits(8);
|
|
|
|
|
+
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
int32_t Message::read_int16() const
|
|
int32_t Message::read_int16() const
|
|
|
{
|
|
{
|
|
|
-
|
|
|
|
|
|
|
+ return (int32_t)read_bits(-16);
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
int32_t Message::read_uint16() const
|
|
int32_t Message::read_uint16() const
|
|
|
{
|
|
{
|
|
|
-
|
|
|
|
|
|
|
+ return (int32_t)read_bits(16);
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
int32_t Message::read_int64() const
|
|
int32_t Message::read_int64() const
|
|
|
{
|
|
{
|
|
|
-
|
|
|
|
|
|
|
+ return (int32_t)read_bits(32);
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
real Message::read_real() const
|
|
real Message::read_real() const
|
|
|
{
|
|
{
|
|
|
-
|
|
|
|
|
|
|
+ float value;
|
|
|
|
|
+ *reinterpret_cast<int*>(&value) = read_bits(32);
|
|
|
|
|
+ return value;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
-real Message::read_real(int32_t exp_bits, int32_t mant_bits) const
|
|
|
|
|
|
|
+Vec3 Message::read_vec3(int32_t num_bits) const
|
|
|
{
|
|
{
|
|
|
-
|
|
|
|
|
|
|
+ return bits_to_vec3(read_bits(num_bits), num_bits);
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
-real Message::read_angle() const
|
|
|
|
|
|
|
+int32_t Message::read_string(char* buffer, int32_t buffer_size) const
|
|
|
{
|
|
{
|
|
|
-
|
|
|
|
|
|
|
+ int l = 0;
|
|
|
|
|
+ int c;
|
|
|
|
|
+
|
|
|
|
|
+ read_byte_align();
|
|
|
|
|
+
|
|
|
|
|
+ while(1)
|
|
|
|
|
+ {
|
|
|
|
|
+ c = read_uint8();
|
|
|
|
|
+ if (c <= 0 || c >= 255)
|
|
|
|
|
+ {
|
|
|
|
|
+ break;
|
|
|
|
|
+ }
|
|
|
|
|
+ // translate all fmt spec to avoid crash bugs in string routines
|
|
|
|
|
+ if ( c == '%' )
|
|
|
|
|
+ {
|
|
|
|
|
+ c = '.';
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ // we will read past any excessively long string, so
|
|
|
|
|
+ // the following data can be read, but the string will
|
|
|
|
|
+ // be truncated
|
|
|
|
|
+ if (l < buffer_size - 1)
|
|
|
|
|
+ {
|
|
|
|
|
+ buffer[l] = c;
|
|
|
|
|
+ l++;
|
|
|
|
|
+ }
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ buffer[l] = 0;
|
|
|
|
|
+ return l;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
-Vec3 Message::read_vec3(int32_t num_bits) const
|
|
|
|
|
|
|
+int32_t Message::read_data(void* data, int32_t length) const
|
|
|
{
|
|
{
|
|
|
-
|
|
|
|
|
|
|
+ int count;
|
|
|
|
|
+
|
|
|
|
|
+ read_byte_align();
|
|
|
|
|
+
|
|
|
|
|
+ count = read_count;
|
|
|
|
|
+
|
|
|
|
|
+ if (read_count + length > cur_size)
|
|
|
|
|
+ {
|
|
|
|
|
+ if (data)
|
|
|
|
|
+ {
|
|
|
|
|
+ memcpy(data, r_data + read_count, get_remaing_data());
|
|
|
|
|
+ }
|
|
|
|
|
+ read_count = cur_size;
|
|
|
|
|
+ }
|
|
|
|
|
+ else
|
|
|
|
|
+ {
|
|
|
|
|
+ if (data)
|
|
|
|
|
+ {
|
|
|
|
|
+ memcpy(data, r_data + read_count, length);
|
|
|
|
|
+ }
|
|
|
|
|
+ read_count += length;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ return (read_count - count);
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
-int32_t Message::read_string(char* buffer, int32_t buffer_size) const
|
|
|
|
|
|
|
+void Message::read_ipv4addr(os::IPv4Address* addr) const
|
|
|
{
|
|
{
|
|
|
-
|
|
|
|
|
|
|
+
|
|
|
|
|
+ for (int i = 0; i < 4; i++)
|
|
|
|
|
+ {
|
|
|
|
|
+ addr->address[i] = read_uint8();
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ addr->port = read_uint16();
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
-int32_t Message::read_data(void* data, int32_t length) const
|
|
|
|
|
|
|
+int32_t Message::vec3_to_bits(const Vec3& v, int32_t num_bits)
|
|
|
{
|
|
{
|
|
|
|
|
+ assert(num_bits >= 6 && num_bits <= 32);
|
|
|
|
|
+ assert(v.squared_length() - 1.0f < 0.01f);
|
|
|
|
|
|
|
|
|
|
+ int32_t max;
|
|
|
|
|
+ int32_t bits;
|
|
|
|
|
+ float bias;
|
|
|
|
|
+
|
|
|
|
|
+ num_bits /= 3;
|
|
|
|
|
+ max = (1 << (num_bits - 1)) - 1;
|
|
|
|
|
+ bias = 0.5f / max;
|
|
|
|
|
+
|
|
|
|
|
+ bits = FLOATSIGNBITSET(v.x) << (num_bits * 3 - 1);
|
|
|
|
|
+ bits |= ((int32_t)((math::abs(v.x) + bias) * max)) << (num_bits * 2);
|
|
|
|
|
+ bits |= FLOATSIGNBITSET(v.y) << (num_bits * 2 - 1);
|
|
|
|
|
+ bits |= ((int32_t)((math::abs(v.y) + bias) * max)) << (num_bits * 1);
|
|
|
|
|
+ bits |= FLOATSIGNBITSET(v.z) << (num_bits * 1 - 1);
|
|
|
|
|
+ bits |= ((int32_t)((math::abs(v.z) + bias) * max)) << (num_bits * 0);
|
|
|
|
|
+
|
|
|
|
|
+ return bits;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
-void Message::read_ipv4addr(os::IPv4Address* addr) const
|
|
|
|
|
|
|
+Vec3 Message::bits_to_vec3(int32_t bits, int32_t num_bits)
|
|
|
{
|
|
{
|
|
|
|
|
+ assert(num_bits >= 6 && num_bits <= 32);
|
|
|
|
|
|
|
|
|
|
+ static float sign[2] = {1.0f, -1.0f};
|
|
|
|
|
+ int max;
|
|
|
|
|
+ float inv_max;
|
|
|
|
|
+ Vec3 v;
|
|
|
|
|
+
|
|
|
|
|
+ num_bits /= 3;
|
|
|
|
|
+ max = (1 << (num_bits - 1)) - 1;
|
|
|
|
|
+ inv_max = 1.0f / max;
|
|
|
|
|
+
|
|
|
|
|
+ v.x = sign[(bits >> (num_bits * 3 - 1)) & 1] * ((bits >> (num_bits * 2)) & max) * inv_max;
|
|
|
|
|
+ v.y = sign[(bits >> (num_bits * 2 - 1)) & 1] * ((bits >> (num_bits * 1)) & max) * inv_max;
|
|
|
|
|
+ v.z = sign[(bits >> (num_bits * 1 - 1)) & 1] * ((bits >> (num_bits * 0)) & max) * inv_max;
|
|
|
|
|
+ v.normalize();
|
|
|
|
|
+
|
|
|
|
|
+ return v;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
-// static int32_t vec3_to_bits(const Vec3& v, int32_t num_bits);
|
|
|
|
|
-// static Vec3 bits_to_vec3(int32_t bits, int32_t num_bits);
|
|
|
|
|
}
|
|
}
|
|
|
}
|
|
}
|