127 lines
3.6 KiB
C++
127 lines
3.6 KiB
C++
#include "Messages.h"
|
|
|
|
#include "LowLevelMessages.h"
|
|
// #include "Messages/CustomMsg.h"
|
|
#include "Participant.h"
|
|
#include "string.h"
|
|
|
|
#pragma region IMessage
|
|
|
|
IMessage::IMessage() {}
|
|
|
|
// IMessage::IMessage(unsigned char *buffer) { Deserialize(buffer); }
|
|
|
|
unsigned char IMessage::Serialize(char *buffer) { return 0; }
|
|
|
|
// void IMessage::Deserialize(unsigned char *buffer) {}
|
|
|
|
// bool IMessage::SendMsg(Participant *client, IMessage msg) {
|
|
// // return SendMsg(client, client.buffer, );nameLength
|
|
// return client->SendBuffer(msg.Serialize(client->buffer));
|
|
// }
|
|
|
|
unsigned char *IMessage::ReceiveMsg(unsigned char packetSize) {
|
|
return nullptr;
|
|
}
|
|
|
|
// bool IMessage::Publish(Participant *participant) {
|
|
// return participant->PublishBuffer(Serialize(participant->buffer));
|
|
// }
|
|
// bool IMessage::SendTo(Participant *participant) {
|
|
// return participant->SendBuffer(Serialize(participant->buffer));
|
|
// }
|
|
|
|
// IMessage
|
|
#pragma endregion
|
|
|
|
#pragma region Investigate
|
|
|
|
InvestigateMsg::InvestigateMsg(char *buffer) {
|
|
unsigned ix = 1; // first byte is msgId
|
|
this->networkId = buffer[ix++];
|
|
this->thingId = buffer[ix++];
|
|
}
|
|
InvestigateMsg::InvestigateMsg(unsigned char networkId, unsigned char thingId) {
|
|
this->networkId = networkId;
|
|
this->thingId = thingId;
|
|
}
|
|
|
|
unsigned char InvestigateMsg::Serialize(char *buffer) {
|
|
unsigned char ix = 0;
|
|
buffer[ix++] = this->id;
|
|
buffer[ix++] = this->networkId;
|
|
buffer[ix++] = this->thingId;
|
|
return ix;
|
|
}
|
|
|
|
// bool InvestigateMsg::Send(Participant *participant, unsigned char networkId,
|
|
// unsigned char thingId) {
|
|
// InvestigateMsg msg = InvestigateMsg(networkId, thingId);
|
|
// return msg.Send(participant);
|
|
// }
|
|
|
|
// Investigate
|
|
#pragma endregion
|
|
|
|
#pragma region PoseMsg
|
|
|
|
PoseMsg::PoseMsg(unsigned char networkId, unsigned char thingId,
|
|
unsigned char poseType, Spherical16 position,
|
|
SwingTwist16 orientation, Spherical16 linearVelocity,
|
|
Spherical16 angularVelocity) {
|
|
this->networkId = networkId;
|
|
this->thingId = thingId;
|
|
|
|
this->poseType = poseType;
|
|
this->position = position;
|
|
this->orientation = orientation;
|
|
this->linearVelocity = linearVelocity;
|
|
this->angularVelocity = angularVelocity;
|
|
}
|
|
PoseMsg::PoseMsg(const char *buffer) {
|
|
unsigned char ix = 1; // First byte is msg id
|
|
this->networkId = buffer[ix++];
|
|
this->thingId = buffer[ix++];
|
|
this->poseType = buffer[ix++];
|
|
this->position = LowLevelMessages::ReceiveSpherical16(buffer, &ix);
|
|
this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
|
|
}
|
|
|
|
unsigned char PoseMsg::Serialize(char *buffer) {
|
|
unsigned char ix = 0;
|
|
buffer[ix++] = PoseMsg::id;
|
|
buffer[ix++] = this->networkId;
|
|
buffer[ix++] = this->thingId;
|
|
buffer[ix++] = this->poseType;
|
|
if ((this->poseType & Pose_Position) != 0)
|
|
LowLevelMessages::SendSpherical16(buffer, &ix, this->position);
|
|
if ((this->poseType & Pose_Orientation) != 0)
|
|
LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
|
|
if ((this->poseType & Pose_LinearVelocity) != 0)
|
|
LowLevelMessages::SendSpherical16(buffer, &ix, this->linearVelocity);
|
|
if ((this->poseType & Pose_AngularVelocity) != 0)
|
|
LowLevelMessages::SendSpherical16(buffer, &ix, this->angularVelocity);
|
|
return ix;
|
|
}
|
|
|
|
// Pose
|
|
#pragma endregion
|
|
|
|
#pragma region DestroyMsg
|
|
|
|
DestroyMsg::DestroyMsg(unsigned char networkId, Thing *thing) {
|
|
this->networkId = networkId;
|
|
this->thingId = thing->id;
|
|
}
|
|
|
|
unsigned char DestroyMsg::Serialize(char *buffer) {
|
|
unsigned char ix = 0;
|
|
buffer[ix++] = this->id;
|
|
buffer[ix++] = this->networkId;
|
|
buffer[ix++] = this->thingId;
|
|
return ix;
|
|
}
|
|
|
|
// DestroyMsg
|
|
#pragma endregion
|