89 lines
2.5 KiB
C#
89 lines
2.5 KiB
C#
using Passer.LinearAlgebra;
|
|
|
|
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.horizontal);
|
|
SendAngle8(buffer, ref ix, v.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(distance, horizontal, vertical);
|
|
return v;
|
|
}
|
|
|
|
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(
|
|
(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 void SendAngle8(byte[] buffer, ref byte ix, float angle)
|
|
{
|
|
// Normalize angle
|
|
while (angle >= 180)
|
|
angle -= 360;
|
|
while (angle < -180)
|
|
angle += 360;
|
|
buffer[ix++] = (byte)((angle / 360.0f) * 256.0f);
|
|
}
|
|
|
|
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(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);
|
|
data[ix++] = (byte)(binary & 255);
|
|
}
|
|
|
|
public static float ReceiveFloat16(byte[] data, ref byte ix)
|
|
{
|
|
ushort value = (ushort)(data[ix++] << 8 | data[ix++]);
|
|
float16 f16 = new();
|
|
f16.SetBinary(value);
|
|
float f = f16.toFloat();
|
|
return f;
|
|
}
|
|
}
|