RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Releases Activity

Improve AngleOf usage

Browse Source
This commit is contained in:
Pascal Serrarens 2024-07-31 11:44:43 +02:00
parent 42d64a6afe
commit 00f40fea98
2 changed files with 63 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
LinearAlgebra

@ -1 +1 @@
Subproject commit c70c079efc4fcfe9e9b2c29cd82f67faa63c827f Subproject commit b81b77b1c95de8fda430a0bd428dd551c89db380

36
NetworkSync.cpp
View File

@ -7,12 +7,15 @@
#endif #endif
#include "LinearAlgebra/Angle8.h" #include "LinearAlgebra/Angle8.h"
#include "LinearAlgebra/AngleUsing.h"
#include "LinearAlgebra/Spherical.h" #include "LinearAlgebra/Spherical.h"
#include "float16/float16.h" #include "float16/float16.h"
#include <string.h> #include <string.h>
NetworkSync::NetworkSync(Roboid *roboid) { this->roboid = roboid; } NetworkSync::NetworkSync(Roboid* roboid) {
this->roboid = roboid;
}
void NetworkSync::ReceiveMessage(Roboid* roboid, unsigned char bytecount) { void NetworkSync::ReceiveMessage(Roboid* roboid, unsigned char bytecount) {
networkPerception->ProcessPacket(roboid, buffer, bytecount); networkPerception->ProcessPacket(roboid, buffer, bytecount);
@ -318,7 +321,8 @@ void NetworkSync::SendInt(const int x) {
// Low-level functions // Low-level functions
void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex, void NetworkSync::SendVector3(unsigned char* data,
unsigned char* startIndex,
const Vector3 v) { const Vector3 v) {
SendSingle100(data, *startIndex, v.Right()); SendSingle100(data, *startIndex, v.Right());
(*startIndex) += 4; (*startIndex) += 4;
@ -328,7 +332,8 @@ void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex,
(*startIndex) += 4; (*startIndex) += 4;
} }
void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex, void NetworkSync::SendQuaternion(unsigned char* data,
const int startIndex,
const Quaternion q) { const Quaternion q) {
Vector3 angles = Quaternion::ToAngles(q); Vector3 angles = Quaternion::ToAngles(q);
int ix = startIndex; int ix = startIndex;
@ -337,13 +342,15 @@ void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex,
SendAngle8(data, ix++, angles.Forward()); SendAngle8(data, ix++, angles.Forward());
} }
void NetworkSync::SendPolar(unsigned char *data, unsigned char *startIndex, void NetworkSync::SendPolar(unsigned char* data,
unsigned char* startIndex,
Polar p) { Polar p) {
SendAngle8(data, *startIndex, (const float)p.angle); SendAngle8(data, *startIndex, (const float)p.angle);
SendSingle100(data, (*startIndex) + 1, p.distance); SendSingle100(data, (*startIndex) + 1, p.distance);
} }
void NetworkSync::SendSpherical(unsigned char *data, unsigned char *startIndex, void NetworkSync::SendSpherical(unsigned char* data,
unsigned char* startIndex,
Spherical s) { Spherical s) {
SendAngle8(data, (*startIndex)++, s.horizontalAngle); SendAngle8(data, (*startIndex)++, s.horizontalAngle);
SendAngle8(data, (*startIndex)++, s.verticalAngle); SendAngle8(data, (*startIndex)++, s.verticalAngle);
@ -351,7 +358,8 @@ void NetworkSync::SendSpherical(unsigned char *data, unsigned char *startIndex,
SendFloat16(data, startIndex, s.distance); SendFloat16(data, startIndex, s.distance);
} }
void NetworkSync::SendQuat32(unsigned char *data, unsigned char *startIndex, void NetworkSync::SendQuat32(unsigned char* data,
unsigned char* startIndex,
const Quaternion q) { const Quaternion q) {
unsigned char qx = (char)(q.x * 127 + 128); unsigned char qx = (char)(q.x * 127 + 128);
unsigned char qy = (char)(q.y * 127 + 128); unsigned char qy = (char)(q.y * 127 + 128);
@ -370,10 +378,11 @@ void NetworkSync::SendQuat32(unsigned char *data, unsigned char *startIndex,
data[(*startIndex)++] = qw; data[(*startIndex)++] = qw;
} }
void NetworkSync::SendAngle8(unsigned char *data, unsigned int startIndex, void NetworkSync::SendAngle8(unsigned char* data,
unsigned int startIndex,
const float angle) { const float angle) {
AngleUsing<signed char> packedAngle = AngleUsing<signed char>(angle); AngleOf<signed char> packedAngle2 = AngleOf<signed char>(angle);
data[startIndex] = packedAngle.GetValue(); data[startIndex] = packedAngle2.GetBinary();
} }
// void NetworkSync::SendAngle16(unsigned char *data, unsigned int startIndex, // void NetworkSync::SendAngle16(unsigned char *data, unsigned int startIndex,
@ -397,7 +406,8 @@ void NetworkSync::SendAngle8(unsigned char *data, unsigned int startIndex,
// // data[startIndex + 3]); // // data[startIndex + 3]);
// } // }
void NetworkSync::SendSingle100(unsigned char *data, unsigned int startIndex, void NetworkSync::SendSingle100(unsigned char* data,
unsigned int startIndex,
float value) { float value) {
// Sends a float with truncated 2 decimal precision // Sends a float with truncated 2 decimal precision
Int32 intValue = value * 100; Int32 intValue = value * 100;
@ -406,7 +416,8 @@ void NetworkSync::SendSingle100(unsigned char *data, unsigned int startIndex,
// data[startIndex + ix] = ((unsigned char *)&intValue)[ix]; // data[startIndex + ix] = ((unsigned char *)&intValue)[ix];
// } // }
} }
void NetworkSync::SendFloat16(unsigned char *data, unsigned char *startIndex, void NetworkSync::SendFloat16(unsigned char* data,
unsigned char* startIndex,
float value) { float value) {
float16 value16 = float16(value); float16 value16 = float16(value);
short binary = value16.getBinary(); short binary = value16.getBinary();
@ -415,7 +426,8 @@ void NetworkSync::SendFloat16(unsigned char *data, unsigned char *startIndex,
data[(*startIndex)++] = binary & 0xFF; data[(*startIndex)++] = binary & 0xFF;
} }
void NetworkSync::SendInt32(unsigned char *data, unsigned int startIndex, void NetworkSync::SendInt32(unsigned char* data,
unsigned int startIndex,
Int32 value) { Int32 value) {
for (unsigned char ix = 0; ix < 4; ix++) { for (unsigned char ix = 0; ix < 4; ix++) {
data[startIndex++] = ((unsigned char*)&value)[ix]; data[startIndex++] = ((unsigned char*)&value)[ix];