111 lines
4.7 KiB
C#
111 lines
4.7 KiB
C#
using System.IO;
|
|
using System.Threading.Tasks;
|
|
using Passer.LinearAlgebra;
|
|
|
|
namespace Passer.RoboidControl {
|
|
|
|
public class PoseMsg : IMessage {
|
|
public const byte Id = 0x10;
|
|
public const byte length = 4 + 4 + 4;
|
|
public byte networkId;
|
|
public byte thingId;
|
|
|
|
public byte poseType;
|
|
public const byte Pose_Position = 0x01;
|
|
public const byte Pose_Orientation = 0x02;
|
|
public const byte Pose_LinearVelocity = 0x04;
|
|
public const byte Pose_AngularVelocity = 0x08;
|
|
|
|
public Spherical position = Spherical.zero;
|
|
public SwingTwist orientation = SwingTwist.zero;
|
|
public Spherical linearVelocity = Spherical.zero;
|
|
public Spherical angularVelocity = Spherical.zero;
|
|
|
|
public PoseMsg(byte networkId, byte thingId, Spherical position, SwingTwist orientation) {
|
|
this.networkId = networkId;
|
|
this.thingId = thingId;
|
|
|
|
this.position = position;
|
|
this.orientation = orientation;
|
|
|
|
this.poseType = 0;
|
|
if (this.position != null)
|
|
this.poseType |= Pose_Position;
|
|
if (this.orientation != null)
|
|
this.poseType |= Pose_Orientation;
|
|
}
|
|
public PoseMsg(byte[] buffer) : base(buffer) { }
|
|
|
|
public override byte Serialize(ref byte[] buffer) {
|
|
byte ix = 0;
|
|
buffer[ix++] = PoseMsg.Id;
|
|
buffer[ix++] = this.networkId;
|
|
buffer[ix++] = this.thingId;
|
|
buffer[ix++] = this.poseType;
|
|
|
|
if ((poseType & Pose_Position) != 0)
|
|
LowLevelMessages.SendSpherical(buffer, ref ix, this.position);
|
|
if ((poseType & Pose_Orientation) != 0)
|
|
LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation);
|
|
if ((poseType & Pose_LinearVelocity) != 0)
|
|
LowLevelMessages.SendSpherical(buffer, ref ix, this.linearVelocity);
|
|
if ((poseType & Pose_AngularVelocity) != 0)
|
|
LowLevelMessages.SendSpherical(buffer, ref ix, this.angularVelocity);
|
|
return ix;
|
|
}
|
|
public override void Deserialize(byte[] buffer) {
|
|
byte ix = 0;
|
|
this.networkId = buffer[ix++];
|
|
this.thingId = buffer[ix++];
|
|
this.poseType = buffer[ix++];
|
|
|
|
if ((poseType & Pose_Position) != 0)
|
|
this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
|
|
if ((poseType & Pose_Orientation) != 0)
|
|
this.orientation = LowLevelMessages.ReceiveSwingTwist(buffer, ref ix);
|
|
if ((poseType & Pose_LinearVelocity) != 0)
|
|
this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
|
|
if ((poseType & Pose_AngularVelocity) != 0)
|
|
this.angularVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
|
|
}
|
|
|
|
public static bool Send(Participant client, byte thingId, Spherical position, SwingTwist orientation) {
|
|
PoseMsg msg = new PoseMsg(client.networkId, thingId, position, orientation);
|
|
return SendMsg(client, msg);
|
|
}
|
|
public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
|
|
byte[] buffer = await Receive(dataStream, packetSize);
|
|
PoseMsg msg = new PoseMsg(buffer);
|
|
|
|
// Do no process poses with nwid == 0 (== local)
|
|
if (msg.networkId == 0)
|
|
return true;
|
|
|
|
client.messageQueue.Enqueue(msg);
|
|
return true;
|
|
}
|
|
|
|
public static bool SendTo(Participant participant, Thing thing) {
|
|
if (participant == null || thing == null)
|
|
return false;
|
|
|
|
byte ix = 0;
|
|
participant.buffer[ix++] = PoseMsg.Id;
|
|
participant.buffer[ix++] = participant.networkId;
|
|
participant.buffer[ix++] = thing.id;
|
|
participant.buffer[ix++] = thing.poseUpdated;
|
|
|
|
if ((thing.poseUpdated & Pose_Position) != 0 && thing.position != null)
|
|
LowLevelMessages.SendSpherical(participant.buffer, ref ix, thing.position);
|
|
if ((thing.poseUpdated & Pose_Orientation) != 0 && thing.orientation != null)
|
|
LowLevelMessages.SendQuat32(participant.buffer, ref ix, thing.orientation);
|
|
if ((thing.poseUpdated & Pose_LinearVelocity) != 0 && thing.linearVelocity != null)
|
|
LowLevelMessages.SendSpherical(participant.buffer, ref ix, thing.linearVelocity);
|
|
if ((thing.poseUpdated & Pose_AngularVelocity) != 0 && thing.angularVelocity != null)
|
|
LowLevelMessages.SendSpherical(participant.buffer, ref ix, thing.angularVelocity);
|
|
|
|
return participant.SendBuffer(ix);
|
|
}
|
|
}
|
|
|
|
} |