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

Using Discrete angles

Browse Source
This commit is contained in:
Pascal Serrarens 2024-03-05 09:55:29 +01:00
parent c90d11ae87
commit 730720d6e2
3 changed files with 29 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
LinearAlgebra

@ -1 +1 @@
Subproject commit c01640429c3c9829e81f38788a6c5973ee84eb60 Subproject commit 3714507059d558c586685e1af286adc20356eb60

25
NetworkSync.cpp
View File

@ -4,13 +4,31 @@
#include <Arduino.h> #include <Arduino.h>
#endif #endif
void NetworkSync::SendVector3(unsigned char *data, int startIndex, Vector3 v) { #include "LinearAlgebra/DiscreteAngle.h"
void NetworkSync::SendVector3(unsigned char *data, const int startIndex,
const Vector3 v) {
SendSingle100(data, startIndex, v.x); SendSingle100(data, startIndex, v.x);
SendSingle100(data, startIndex + 4, v.y); SendSingle100(data, startIndex + 4, v.y);
SendSingle100(data, startIndex + 8, v.z); SendSingle100(data, startIndex + 8, v.z);
} }
void NetworkSync::SendSingle100(unsigned char *data, int startIndex, void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex,
const Quaternion q) {
Vector3 angles = Quaternion::ToAngles(q);
int ix = startIndex;
SendAngle(data, ix++, angles.x);
SendAngle(data, ix++, angles.y);
SendAngle(data, ix, angles.z);
}
void NetworkSync::SendAngle(unsigned char *data, const int startIndex,
float angle) {
AngleUsing<unsigned char> packedAngle = AngleUsing<unsigned char>(angle);
data[startIndex] = packedAngle.GetValue();
}
void NetworkSync::SendSingle100(unsigned char *data, const 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;
@ -20,7 +38,8 @@ void NetworkSync::SendSingle100(unsigned char *data, int startIndex,
// } // }
} }
void NetworkSync::SendInt32(unsigned char *data, int startIndex, Int32 value) { void NetworkSync::SendInt32(unsigned char *data, const int startIndex,
Int32 value) {
for (unsigned char ix = 0; ix < 4; ix++) { for (unsigned char ix = 0; ix < 4; ix++) {
data[startIndex + ix] = ((unsigned char *)&value)[ix]; data[startIndex + ix] = ((unsigned char *)&value)[ix];
} }

9
NetworkSync.h
View File

@ -50,9 +50,12 @@ protected:
NetworkPerception *networkPerception; NetworkPerception *networkPerception;
void PublishTrackedObject(SendBuffer sendBuffer, InterestingThing *object); void PublishTrackedObject(SendBuffer sendBuffer, InterestingThing *object);
void SendVector3(unsigned char *data, int startIndex, Vector3 v); void SendSingle100(unsigned char *data, const int startIndex, float value);
void SendSingle100(unsigned char *data, int startIndex, float value); void SendInt32(unsigned char *data, const int startIndex, Int32 value);
void SendInt32(unsigned char *data, int startIndex, Int32 value); void SendAngle(unsigned char *data, const int startIndex, float value);
void SendVector3(unsigned char *data, const int startIndex, const Vector3 v);
void SendQuaternion(unsigned char *data, const int startIndex,
const Quaternion q);
}; };
} // namespace RoboidControl } // namespace RoboidControl