112 lines
3.6 KiB
C#
112 lines
3.6 KiB
C#
using Passer.LinearAlgebra;
|
|
|
|
namespace RoboidControl
|
|
{
|
|
public class LowLevelMessages
|
|
{
|
|
|
|
public static void SendSpherical(byte[] buffer, ref byte ix, Spherical v)
|
|
{
|
|
SendFloat16(buffer, ref ix, new float16(v.distance));
|
|
SendAngle8(buffer, ref ix, v.direction.horizontal);
|
|
SendAngle8(buffer, ref ix, v.direction.vertical);
|
|
}
|
|
|
|
public static Spherical ReceiveSpherical(byte[] data, ref byte ix)
|
|
{
|
|
float distance = ReceiveFloat16(data, ref ix);
|
|
float horizontal = ReceiveAngle8(data, ref ix);
|
|
float vertical = ReceiveAngle8(data, ref ix);
|
|
Spherical v = new Spherical(distance, horizontal, vertical);
|
|
return v;
|
|
}
|
|
|
|
public static void SendQuat32(byte[] buffer, ref byte ix, SwingTwist s)
|
|
{
|
|
Quat32 q32 = Quat32.FromSwingTwist(s);
|
|
SendQuat32(buffer, ref ix, q32);
|
|
|
|
}
|
|
public static void SendQuat32(byte[] buffer, ref byte ix, Quat32 q)
|
|
{
|
|
int qx = (int)(q.x * 127 + 128);
|
|
int qy = (int)(q.y * 127 + 128);
|
|
int qz = (int)(q.z * 127 + 128);
|
|
int qw = (int)(q.w * 255);
|
|
if (q.w < 0)
|
|
{
|
|
qx = -qx;
|
|
qy = -qy;
|
|
qz = -qz;
|
|
qw = -qw;
|
|
}
|
|
|
|
buffer[ix++] = (byte)qx;
|
|
buffer[ix++] = (byte)qy;
|
|
buffer[ix++] = (byte)qz;
|
|
buffer[ix++] = (byte)qw;
|
|
}
|
|
public static Quat32 ReceiveQuat32(byte[] data, ref byte ix)
|
|
{
|
|
Quat32 q = new Quat32(
|
|
(data[ix++] - 128.0F) / 127.0F,
|
|
(data[ix++] - 128.0F) / 127.0F,
|
|
(data[ix++] - 128.0F) / 127.0F,
|
|
data[ix++] / 255.0F);
|
|
return q;
|
|
}
|
|
|
|
public static SwingTwist ReceiveSwingTwist(byte[] data, ref byte ix)
|
|
{
|
|
Quat32 q32 = ReceiveQuat32(data, ref ix);
|
|
// UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w);
|
|
// SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z);
|
|
|
|
SwingTwist r = SwingTwist.FromQuat32(q32);
|
|
return r;
|
|
}
|
|
|
|
public static void SendAngle8(byte[] buffer, ref byte ix, float angle)
|
|
{
|
|
// Normalize angle
|
|
while (angle >= 180)
|
|
angle -= 360;
|
|
while (angle < -180)
|
|
angle += 360;
|
|
sbyte value = (sbyte)(angle / 360.0f * 256.0f);
|
|
buffer[ix++] = (byte)value;
|
|
}
|
|
|
|
public static float ReceiveAngle8(byte[] data, ref byte ix)
|
|
{
|
|
float value = (data[ix++] * 180) / 128.0F;
|
|
return value;
|
|
}
|
|
|
|
public static void SendFloat16(byte[] data, ref byte ix, float f)
|
|
{
|
|
float16 f16 = new float16(f);
|
|
ushort binary = f16.GetBinary();
|
|
data[ix++] = (byte)(binary >> 8);
|
|
data[ix++] = (byte)(binary & 255);
|
|
}
|
|
public static void SendFloat16(byte[] data, ref byte ix, float16 f)
|
|
{
|
|
ushort binary = f.GetBinary();
|
|
data[ix++] = (byte)((binary >> 8) & 0xFF);
|
|
data[ix++] = (byte)(binary & 0xFF);
|
|
}
|
|
|
|
public static float ReceiveFloat16(byte[] data, ref byte ix)
|
|
{
|
|
byte msb = data[ix++];
|
|
byte lsb = data[ix++];
|
|
ushort value = (ushort)(msb << 8 | lsb);
|
|
float16 f16 = new float16();
|
|
f16.SetBinary(value);
|
|
float f = f16.toFloat();
|
|
return f;
|
|
}
|
|
}
|
|
|
|
} |