diff --git a/LinearAlgebra b/LinearAlgebra index c70c079..b81b77b 160000 --- a/LinearAlgebra +++ b/LinearAlgebra @@ -1 +1 @@ -Subproject commit c70c079efc4fcfe9e9b2c29cd82f67faa63c827f +Subproject commit b81b77b1c95de8fda430a0bd428dd551c89db380 diff --git a/NetworkSync.cpp b/NetworkSync.cpp index 4292514..dbe55ca 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -7,21 +7,24 @@ #endif #include "LinearAlgebra/Angle8.h" +#include "LinearAlgebra/AngleUsing.h" #include "LinearAlgebra/Spherical.h" #include "float16/float16.h" #include -NetworkSync::NetworkSync(Roboid *roboid) { this->roboid = roboid; } +NetworkSync::NetworkSync(Roboid* roboid) { + this->roboid = roboid; +} -void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) { +void NetworkSync::ReceiveMessage(Roboid* roboid, unsigned char bytecount) { networkPerception->ProcessPacket(roboid, buffer, bytecount); switch (buffer[0]) { - case NetworkIdMsg: - // this->networkId = NetworkIdMsg; - ReceiveNetworkId(); - break; + case NetworkIdMsg: + // this->networkId = NetworkIdMsg; + ReceiveNetworkId(); + break; } } @@ -37,7 +40,7 @@ void NetworkSync::ReceiveNetworkId() { PublishRelativeThing(roboid->GetChild(0), true); } -void NetworkSync::NewObject(InterestingThing *thing) { +void NetworkSync::NewObject(InterestingThing* thing) { if (thing == nullptr || thing->networkId != 0x00) return; @@ -54,8 +57,8 @@ void NetworkSync::NewObject(InterestingThing *thing) { // PublishTrackedObject(roboid, obj); } -void NetworkSync::PublishRelativeThing(Thing *thing, bool recurse) { - Thing *parentThing = thing->GetParent(); +void NetworkSync::PublishRelativeThing(Thing* thing, bool recurse) { + Thing* parentThing = thing->GetParent(); unsigned char ix = 0; buffer[ix++] = RelativePoseMsg; @@ -72,13 +75,13 @@ void NetworkSync::PublishRelativeThing(Thing *thing, bool recurse) { // delay(1000); if (recurse) { - Thing *child = thing->GetChild(0); + Thing* child = thing->GetChild(0); if (child != nullptr) PublishRelativeThing(child, true); } } -void NetworkSync::PublishModel(Roboid *roboid) { +void NetworkSync::PublishModel(Roboid* roboid) { if (roboid->modelUrl == nullptr) return; @@ -87,11 +90,11 @@ void NetworkSync::PublishModel(Roboid *roboid) { return; unsigned char ix = 0; - buffer[ix++] = 0x90; // modelMsg - buffer[ix++] = 0x00; // objectId - Spherical s = Spherical::zero; //(roboid->modelPosition); + buffer[ix++] = 0x90; // modelMsg + buffer[ix++] = 0x00; // objectId + Spherical s = Spherical::zero; //(roboid->modelPosition); SendSpherical(buffer, &ix, s); - SendFloat16(buffer, &ix, 1); // roboid->modelScale); + SendFloat16(buffer, &ix, 1); // roboid->modelScale); buffer[ix++] = len; for (int urlIx = 0; urlIx < len; urlIx++) @@ -100,7 +103,7 @@ void NetworkSync::PublishModel(Roboid *roboid) { SendBuffer(ix); } -void NetworkSync::PublishModel(Thing *thing) { +void NetworkSync::PublishModel(Thing* thing) { if (thing->modelUrl == nullptr) return; @@ -109,11 +112,11 @@ void NetworkSync::PublishModel(Thing *thing) { return; unsigned char ix = 0; - buffer[ix++] = 0x90; // modelMsg - buffer[ix++] = thing->id; // objectId - Spherical s = Spherical::zero; // Spherical(thing->modelPosition); + buffer[ix++] = 0x90; // modelMsg + buffer[ix++] = thing->id; // objectId + Spherical s = Spherical::zero; // Spherical(thing->modelPosition); SendSpherical(buffer, &ix, s); - SendFloat16(buffer, &ix, 1); // thing->modelScale); + SendFloat16(buffer, &ix, 1); // thing->modelScale); buffer[ix++] = len; for (int urlIx = 0; urlIx < len; urlIx++) @@ -122,7 +125,7 @@ void NetworkSync::PublishModel(Thing *thing) { SendBuffer(ix); } -void NetworkSync::DestroyObject(InterestingThing *thing) { +void NetworkSync::DestroyObject(InterestingThing* thing) { unsigned char ix = 0; buffer[ix++] = DestroyMsg; buffer[ix++] = thing->id; @@ -133,7 +136,7 @@ void NetworkSync::DestroyObject(InterestingThing *thing) { #endif } -void NetworkSync::SendPose(Roboid *roboid, bool recurse) { +void NetworkSync::SendPose(Roboid* roboid, bool recurse) { unsigned char ix = 0; buffer[ix++] = PoseMsg; buffer[ix++] = 0x00; @@ -143,13 +146,13 @@ void NetworkSync::SendPose(Roboid *roboid, bool recurse) { SendBuffer(ix); if (recurse) { - Thing *child = roboid->GetChild(0); + Thing* child = roboid->GetChild(0); if (child != nullptr) SendPose(child, true); } } -void NetworkSync::SendPose(Thing *thing, bool recurse) { +void NetworkSync::SendPose(Thing* thing, bool recurse) { unsigned char ix = 0; buffer[ix++] = PoseMsg; buffer[ix++] = thing->id; @@ -160,7 +163,7 @@ void NetworkSync::SendPose(Thing *thing, bool recurse) { if (recurse) { // delay(10); - Thing *child = thing->GetChild(0); + Thing* child = thing->GetChild(0); if (child != nullptr) SendPose(child, true); } @@ -169,7 +172,7 @@ void NetworkSync::SendPose(Thing *thing, bool recurse) { void NetworkSync::PublishClient() { unsigned char ix = 0; buffer[ix++] = ClientMsg; - buffer[ix++] = 0; // No network ID + buffer[ix++] = 0; // No network ID SendBuffer(ix); #ifdef RC_DEBUG @@ -177,11 +180,11 @@ void NetworkSync::PublishClient() { #endif } -void NetworkSync::PublishTrackedObjects(Roboid *roboid, - InterestingThing **objects) { +void NetworkSync::PublishTrackedObjects(Roboid* roboid, + InterestingThing** objects) { int n = 0; for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++) { - InterestingThing *obj = objects[objIx]; + InterestingThing* obj = objects[objIx]; if (obj == nullptr) continue; @@ -192,8 +195,8 @@ void NetworkSync::PublishTrackedObjects(Roboid *roboid, } } -void NetworkSync::PublishTrackedObject(Roboid *roboid, - InterestingThing *object) { +void NetworkSync::PublishTrackedObject(Roboid* roboid, + InterestingThing* object) { if (object == nullptr || object->updated == false || object->networkId != 0x00) { return; @@ -211,8 +214,8 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid, Quaternion worldOrientation = roboidOrientation * object->orientation; unsigned char ix = 0; - buffer[ix++] = PoseMsg; // Position2DMsg; - buffer[ix++] = object->id; // objectId; + buffer[ix++] = PoseMsg; // Position2DMsg; + buffer[ix++] = object->id; // objectId; buffer[ix++] = Pose_Position | Pose_Orientation; // SendPolar(buffer, &ix, polar); // 3 bytes SendVector3(buffer, &ix, worldPosition); @@ -227,7 +230,7 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid, #endif } -void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) { +void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid* roboid) { Polar velocity = roboid->propulsion->GetVelocity(); Vector2 worldVelocity2 = Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle); @@ -253,7 +256,7 @@ void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) { const unsigned int bufferSize = 3 + 12 + 12; unsigned char buffer[bufferSize] = { PoseMsg, - 0, // objectId; + 0, // objectId; Pose_LinearVelocity | Pose_AngularVelocity, }; unsigned char ix = 3; @@ -264,7 +267,7 @@ void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) { #endif } -void NetworkSync::SendDestroyObject(Buffer sendBuffer, InterestingThing *obj) { +void NetworkSync::SendDestroyObject(Buffer sendBuffer, InterestingThing* obj) { #ifdef RC_DEBUG Serial.print("Send Destroy "); Serial.println((int)obj->id); @@ -274,7 +277,7 @@ void NetworkSync::SendDestroyObject(Buffer sendBuffer, InterestingThing *obj) { #endif } -void NetworkSync::SendInvestigateThing(InterestingThing *thing) { +void NetworkSync::SendInvestigateThing(InterestingThing* thing) { #ifdef RC_DEBUG printf("Investigate [%d/%d]\n", thing->networkId, thing->id); #endif @@ -285,7 +288,7 @@ void NetworkSync::SendInvestigateThing(InterestingThing *thing) { SendBuffer(ix); } -void NetworkSync::SendText(const char *s) { +void NetworkSync::SendText(const char* s) { unsigned char length; for (length = 0; length < 253; length++) { if (s[length] == '\0') @@ -318,7 +321,8 @@ void NetworkSync::SendInt(const int x) { // Low-level functions -void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex, +void NetworkSync::SendVector3(unsigned char* data, + unsigned char* startIndex, const Vector3 v) { SendSingle100(data, *startIndex, v.Right()); (*startIndex) += 4; @@ -328,7 +332,8 @@ void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex, (*startIndex) += 4; } -void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex, +void NetworkSync::SendQuaternion(unsigned char* data, + const int startIndex, const Quaternion q) { Vector3 angles = Quaternion::ToAngles(q); int ix = startIndex; @@ -337,13 +342,15 @@ void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex, SendAngle8(data, ix++, angles.Forward()); } -void NetworkSync::SendPolar(unsigned char *data, unsigned char *startIndex, +void NetworkSync::SendPolar(unsigned char* data, + unsigned char* startIndex, Polar p) { SendAngle8(data, *startIndex, (const float)p.angle); SendSingle100(data, (*startIndex) + 1, p.distance); } -void NetworkSync::SendSpherical(unsigned char *data, unsigned char *startIndex, +void NetworkSync::SendSpherical(unsigned char* data, + unsigned char* startIndex, Spherical s) { SendAngle8(data, (*startIndex)++, s.horizontalAngle); SendAngle8(data, (*startIndex)++, s.verticalAngle); @@ -351,7 +358,8 @@ void NetworkSync::SendSpherical(unsigned char *data, unsigned char *startIndex, SendFloat16(data, startIndex, s.distance); } -void NetworkSync::SendQuat32(unsigned char *data, unsigned char *startIndex, +void NetworkSync::SendQuat32(unsigned char* data, + unsigned char* startIndex, const Quaternion q) { unsigned char qx = (char)(q.x * 127 + 128); unsigned char qy = (char)(q.y * 127 + 128); @@ -370,10 +378,11 @@ void NetworkSync::SendQuat32(unsigned char *data, unsigned char *startIndex, data[(*startIndex)++] = qw; } -void NetworkSync::SendAngle8(unsigned char *data, unsigned int startIndex, +void NetworkSync::SendAngle8(unsigned char* data, + unsigned int startIndex, const float angle) { - AngleUsing packedAngle = AngleUsing(angle); - data[startIndex] = packedAngle.GetValue(); + AngleOf packedAngle2 = AngleOf(angle); + data[startIndex] = packedAngle2.GetBinary(); } // void NetworkSync::SendAngle16(unsigned char *data, unsigned int startIndex, @@ -397,7 +406,8 @@ void NetworkSync::SendAngle8(unsigned char *data, unsigned int startIndex, // // data[startIndex + 3]); // } -void NetworkSync::SendSingle100(unsigned char *data, unsigned int startIndex, +void NetworkSync::SendSingle100(unsigned char* data, + unsigned int startIndex, float value) { // Sends a float with truncated 2 decimal precision Int32 intValue = value * 100; @@ -406,7 +416,8 @@ void NetworkSync::SendSingle100(unsigned char *data, unsigned int startIndex, // data[startIndex + ix] = ((unsigned char *)&intValue)[ix]; // } } -void NetworkSync::SendFloat16(unsigned char *data, unsigned char *startIndex, +void NetworkSync::SendFloat16(unsigned char* data, + unsigned char* startIndex, float value) { float16 value16 = float16(value); short binary = value16.getBinary(); @@ -415,10 +426,11 @@ void NetworkSync::SendFloat16(unsigned char *data, unsigned char *startIndex, data[(*startIndex)++] = binary & 0xFF; } -void NetworkSync::SendInt32(unsigned char *data, unsigned int startIndex, +void NetworkSync::SendInt32(unsigned char* data, + unsigned int startIndex, Int32 value) { for (unsigned char ix = 0; ix < 4; ix++) { - data[startIndex++] = ((unsigned char *)&value)[ix]; + data[startIndex++] = ((unsigned char*)&value)[ix]; } }