Usaging SphericalOf

DifferentialDrive.cpp

@@ -14,10 +14,10 @@ DifferentialDrive::DifferentialDrive(Motor* leftMotor, Motor* rightMotor) {
 
   float distance = this->wheelSeparation / 2;
   leftMotor->direction = Motor::Direction::CounterClockwise;
-  leftMotor->position.horizontalAngle = -90;
+  leftMotor->position.horizontal = -90;
   leftMotor->position.distance = distance;
   rightMotor->direction = Motor::Direction::Clockwise;
-  rightMotor->position.horizontalAngle = 90;
+  rightMotor->position.horizontal = 90;
   rightMotor->position.distance = distance;
 }
 
@@ -39,7 +39,7 @@ void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed,
     if (motor == nullptr)
       continue;
 
-    float xPosition = motors[motorIx]->position.horizontalAngle.ToFloat();
+    float xPosition = motors[motorIx]->position.horizontal.ToFloat();
     if (xPosition < 0)
       motor->SetTargetSpeed(leftSpeed);
     else if (xPosition > 0)

LinearAlgebra

@@ -1 +1 @@
-Subproject commit 353cb1bc7f3c12f703c0f963d89d08d567fc446a
+Subproject commit 9c3503f3cb678622fcdc7935b791a28c6c48d052

NetworkSync.cpp

@@ -70,7 +70,7 @@ void NetworkSync::PublishRelativeThing(Thing* thing, bool recurse) {
     buffer[ix++] = parentThing->id;
   else
     buffer[ix++] = 0x00;
-  SendSpherical(buffer, &ix, thing->position);
+  SendSpherical16(buffer, &ix, thing->position);
   SendBuffer(ix);
 
   PublishModel(thing);
@@ -162,7 +162,7 @@ void NetworkSync::SendPose(Thing* thing, bool recurse) {
   buffer[ix++] = PoseMsg;
   buffer[ix++] = thing->id;
   buffer[ix++] = Pose_Position | Pose_Orientation;
-  SendSpherical(buffer, &ix, thing->position);
+  SendSpherical16(buffer, &ix, thing->position);
   SendQuat32(buffer, &ix, thing->orientation);
   SendBuffer(ix);
 
@@ -361,7 +361,14 @@ void NetworkSync::SendSpherical(unsigned char* data,
                                 Spherical s) {
   SendAngle8(data, (*startIndex)++, s.horizontalAngle.ToFloat());
   SendAngle8(data, (*startIndex)++, s.verticalAngle.ToFloat());
-  // SendAngle8(data, startIndex++, s.distance);
+  SendFloat16(data, startIndex, s.distance);
+}
+
+void NetworkSync::SendSpherical16(unsigned char* data,
+                                  unsigned char* startIndex,
+                                  Spherical16 s) {
+  SendAngle8(data, (*startIndex)++, s.horizontal.ToFloat());
+  SendAngle8(data, (*startIndex)++, s.vertical.ToFloat());
   SendFloat16(data, startIndex, s.distance);
 }
 

NetworkSync.h

@@ -10,20 +10,20 @@ namespace RoboidControl {
 
 /// @brief Interface for synchronizaing state between clients across a network
 class NetworkSync {
-public:
+ public:
-  NetworkSync(Roboid *roboid);
+  NetworkSync(Roboid* roboid);
 
   unsigned char networkId;
 
   /// @brief Retreive and send the roboid state
   /// @param roboid The roboid for which the state is updated
-  virtual void NetworkUpdate(Roboid *roboid) = 0;
+  virtual void NetworkUpdate(Roboid* roboid) = 0;
   /// @brief Inform that the given object is no longer being tracked
   /// @param obj
-  virtual void DestroyObject(InterestingThing *obj);
+  virtual void DestroyObject(InterestingThing* obj);
-  virtual void NewObject(InterestingThing *obj);
+  virtual void NewObject(InterestingThing* obj);
-  virtual void PublishModel(Roboid *obj);
+  virtual void PublishModel(Roboid* obj);
-  void PublishModel(Thing *thing);
+  void PublishModel(Thing* thing);
 
   /// @brief The id of a Pose message
   static const unsigned char PoseMsg = 0x10;
@@ -52,58 +52,66 @@ public:
   static const unsigned char ClientMsg = 0xA0;
   static const unsigned char NetworkIdMsg = 0xA1;
 
-  typedef void (*Buffer)(UInt8 *buffer, UInt16 bufferSize);
+  typedef void (*Buffer)(UInt8* buffer, UInt16 bufferSize);
 
-  void ReceiveMessage(Roboid *roboid, unsigned char bytecount);
+  void ReceiveMessage(Roboid* roboid, unsigned char bytecount);
 
   void ReceiveNetworkId();
 
-  void SendInvestigateThing(InterestingThing *thing);
+  void SendInvestigateThing(InterestingThing* thing);
 
-  void SendPoseMsg(Buffer sendBuffer, Roboid *roboid);
+  void SendPoseMsg(Buffer sendBuffer, Roboid* roboid);
-  void SendDestroyObject(Buffer sendBuffer, InterestingThing *obj);
+  void SendDestroyObject(Buffer sendBuffer, InterestingThing* obj);
   void PublishNewObject();
-  void PublishRelativeThing(Thing *thing, bool recurse = false);
+  void PublishRelativeThing(Thing* thing, bool recurse = false);
 
-  void PublishTrackedObjects(Roboid *roboid, InterestingThing **objects);
+  void PublishTrackedObjects(Roboid* roboid, InterestingThing** objects);
 
   virtual void SendPosition(Vector3 worldPosition) {};
   virtual void SendPose(Vector3 worldPosition, Quaternion worldOrientation) {};
-  void SendPose(Roboid *roboid, bool recurse = true);
+  void SendPose(Roboid* roboid, bool recurse = true);
-  void SendPose(Thing *thing, bool recurse = true);
+  void SendPose(Thing* thing, bool recurse = true);
 
-  virtual void SendText(const char *s);
+  virtual void SendText(const char* s);
   void SendInt(const int x);
 
-protected:
+ protected:
-  Roboid *roboid;
+  Roboid* roboid;
-  NetworkPerception *networkPerception;
+  NetworkPerception* networkPerception;
 
-  void PublishTrackedObject(Roboid *roboid, InterestingThing *object);
+  void PublishTrackedObject(Roboid* roboid, InterestingThing* object);
-  void PublishRelativeObject(Buffer sendBuffer, UInt8 parentId,
+  void PublishRelativeObject(Buffer sendBuffer,
-                             InterestingThing *object);
+                             UInt8 parentId,
+                             InterestingThing* object);
 
-  void SendSingle100(unsigned char *data, unsigned int startIndex, float value);
+  void SendSingle100(unsigned char* data, unsigned int startIndex, float value);
-  void SendFloat16(unsigned char *data, unsigned char *startIndex, float value);
+  void SendFloat16(unsigned char* data, unsigned char* startIndex, float value);
 
-  void SendInt32(unsigned char *data, unsigned int startIndex, Int32 value);
+  void SendInt32(unsigned char* data, unsigned int startIndex, Int32 value);
-  void SendAngle8(unsigned char *data, unsigned int startIndex,
+  void SendAngle8(unsigned char* data,
+                  unsigned int startIndex,
                   const float value);
   // void SendAngle16(unsigned char *data, unsigned int startIndex,
   //                  const float value);
   // void SendAngle32(unsigned char *data, unsigned int startIndex,
   //                  const float value);
-  void SendVector3(unsigned char *data, unsigned char *startIndex,
+  void SendVector3(unsigned char* data,
+                   unsigned char* startIndex,
                    const Vector3 v);
-  void SendQuaternion(unsigned char *data, const int startIndex,
+  void SendQuaternion(unsigned char* data,
+                      const int startIndex,
                       const Quaternion q);
 
-  void SendPolar(unsigned char *data, unsigned char *startIndex, Polar p);
+  void SendPolar(unsigned char* data, unsigned char* startIndex, Polar p);
-  void SendSpherical(unsigned char *data, unsigned char *startIndex,
+  void SendSpherical(unsigned char* data,
+                     unsigned char* startIndex,
                      Spherical s);
-  // void SendSpherical16(unsigned char *data, int startIndex, Spherical s);
+  void SendSpherical16(unsigned char* data,
+                       unsigned char* startIndex,
+                       Spherical16 s);
   // void SendSpherical32(unsigned char *data, int startIndex, Spherical s);
-  void SendQuat32(unsigned char *data, unsigned char *startIndex,
+  void SendQuat32(unsigned char* data,
+                  unsigned char* startIndex,
                   const Quaternion q);
 
   unsigned char buffer[256];

Perception.cpp

@@ -483,9 +483,9 @@ void Perception::Update(unsigned long currentTimeMs) {
 
       float distance = distanceSensor->GetDistance();
       if (distance >= 0) {
-        Angle angle = sensor->position.horizontalAngle;
+        Angle16 angle = sensor->position.horizontal;
         // Polar position = Polar(angle, distance);
-        Polar position = Polar(distance, angle);
+        Polar position = Polar(distance, angle.ToFloat());
         AddTrackedObject(distanceSensor, position);
       }
 
@@ -494,7 +494,7 @@ void Perception::Update(unsigned long currentTimeMs) {
       if (switchSensor->IsOn()) {
         // Polar position = Polar(sensor->position.angle, nearbyDistance);
         Polar position =
-            Polar(nearbyDistance, sensor->position.horizontalAngle);
+            Polar(nearbyDistance, sensor->position.horizontal.ToFloat());
         // AddTrackedObject(switchSensor, position);
       }
     } else {

Thing.cpp

@@ -1,10 +1,9 @@
 #include "Thing.h"
 
-// #include "Roboid.h"
-
 using namespace Passer::RoboidControl;
 
-Thing::Thing(unsigned char id) : position(Polar::zero), id(id) {
+Thing::Thing(unsigned char id) : id(id) {
+  // this->position = SphericalOf<signed short>::zero;
   this->type = (unsigned int)Type::Undetermined;
   this->childCount = 0;
   this->parent = nullptr;
@@ -20,25 +19,35 @@ const unsigned int Thing::UncontrolledMotorType =
     MotorType | (unsigned int)Type::UncontrolledMotor;
 const unsigned int Thing::ServoType = (unsigned int)Type::Servo;
 
-bool Thing::IsMotor() { return (type & Thing::MotorType) != 0; }
+bool Thing::IsMotor() {
+  return (type & Thing::MotorType) != 0;
+}
 
-bool Thing::IsSensor() { return (type & Thing::SensorType) != 0; }
+bool Thing::IsSensor() {
+  return (type & Thing::SensorType) != 0;
+}
 
-bool Thing::IsRoboid() { return (type & Thing::RoboidType) != 0; }
+bool Thing::IsRoboid() {
+  return (type & Thing::RoboidType) != 0;
+}
 
-void Thing::SetModel(const char *url) { this->modelUrl = url; }
+void Thing::SetModel(const char* url) {
+  this->modelUrl = url;
+}
 
-void Thing::SetParent(Thing *parent) {
+void Thing::SetParent(Thing* parent) {
   if (parent == nullptr)
     return;
 
   parent->AddChild(this);
 }
 
-Thing *Thing::GetParent() { return this->parent; }
+Thing* Thing::GetParent() {
+  return this->parent;
+}
 
-void Thing::AddChild(Thing *child) {
+void Thing::AddChild(Thing* child) {
-  Thing **newChildren = new Thing *[this->childCount + 1];
+  Thing** newChildren = new Thing*[this->childCount + 1];
   for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
     newChildren[childIx] = this->children[childIx];
     if (this->children[childIx] == child) {
@@ -58,7 +67,7 @@ void Thing::AddChild(Thing *child) {
   this->childCount++;
 }
 
-Thing *Thing::GetChild(unsigned char childIx) {
+Thing* Thing::GetChild(unsigned char childIx) {
   if (childIx >= 0 && childIx < this->childCount) {
     return this->children[childIx];
   } else

Thing.h

@@ -1,14 +1,15 @@
 #pragma once
 
-#include "LinearAlgebra/Polar.h"
+// #include "LinearAlgebra/Polar.h"
 #include "LinearAlgebra/Quaternion.h"
+#include "LinearAlgebra/Spherical.h"
 
 namespace Passer {
 namespace RoboidControl {
 
 /// @brief A thing is a functional component on a robot
 class Thing {
-public:
+ public:
   /// @brief Default constructor for a Thing
   Thing(unsigned char id);
 
@@ -41,7 +42,7 @@ public:
   /// @brief The position of this Thing
   /// @remark When this Thing has a parent, the position is relative to the
   /// parent's position and orientation
-  Spherical position;
+  Spherical16 position;
   /// @brief The orientation of this Thing
   /// @remark When this Thing has a parent, the orientation is relative to the
   /// parent's orientation
@@ -50,27 +51,27 @@ public:
   /// @brief Sets the parent Thing
   /// @param parent The Thing which should become the parnet
   /// @remark This is equivalent to calling parent->AddChild(this);
-  virtual void SetParent(Thing *parent);
+  virtual void SetParent(Thing* parent);
   /// @brief Gets the parent Thing
   /// @return The parent Thing
-  Thing *GetParent();
+  Thing* GetParent();
 
   /// @brief Add a child Thing to this Thing
   /// @param child The Thing which should become a child
   /// @remark When the Thing is already a child, it will not be added again
-  virtual void AddChild(Thing *child);
+  virtual void AddChild(Thing* child);
   /// @brief Get the child at the given index
   /// @param childIx The index of the child
   /// @return The child at the given index or nullptr when the index is invalid
   /// or the child could not be found
-  Thing *GetChild(unsigned char childIx);
+  Thing* GetChild(unsigned char childIx);
 
   /// @brief Sets the location from where the 3D model of this Thing can be
   /// loaded from
   /// @param url The url of the model
   /// @remark Although the roboid implementation is not dependent on the model,
   /// the only official supported model format is .obj
-  void SetModel(const char *url);
+  void SetModel(const char* url);
 
   /// @brief Updates the state of the thing
   /// @param currentTimeMs The current clock time in milliseconds
@@ -78,9 +79,9 @@ public:
 
   unsigned char childCount = 0;
 
-  const char *modelUrl = nullptr;
+  const char* modelUrl = nullptr;
 
-protected:
+ protected:
   /// @brief Bitmask for Motor type
   static const unsigned int MotorType = 0x8000;
   /// @brief Bitmap for Sensor type
@@ -102,8 +103,8 @@ protected:
     ExternalSensor,
   };
 
-  Thing *parent = nullptr;
+  Thing* parent = nullptr;
-  Thing **children = nullptr;
+  Thing** children = nullptr;
 };
 
 }  // namespace RoboidControl