diff --git a/NetworkSync.cpp b/NetworkSync.cpp index 1cf50b9..353c336 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -22,6 +22,24 @@ void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex, SendAngle(data, ix, angles.z); } +void NetworkSync::SendQuat32(unsigned char *data, int startIndex, + const Quaternion q) { + unsigned char qx = (char)(q.x * 127 + 128); + unsigned char qy = (char)(q.y * 127 + 128); + unsigned char qz = (char)(q.z * 127 + 128); + unsigned char qw = (char)(q.w * 255); + if (q.w < 0) { + qx = -qx; + qy = -qy; + qz = -qz; + qw = -qw; + } + data[startIndex++] = qx; + data[startIndex++] = qy; + data[startIndex++] = qz; + data[startIndex++] = qw; +} + void NetworkSync::SendAngle(unsigned char *data, const int startIndex, float angle) { AngleUsing packedAngle = AngleUsing(angle); diff --git a/NetworkSync.h b/NetworkSync.h index 71805f5..7495116 100644 --- a/NetworkSync.h +++ b/NetworkSync.h @@ -56,6 +56,7 @@ protected: void SendVector3(unsigned char *data, const int startIndex, const Vector3 v); void SendQuaternion(unsigned char *data, const int startIndex, const Quaternion q); + void SendQuat32(unsigned char *data, int startIndex, const Quaternion q); }; } // namespace RoboidControl