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

@@ -80,30 +80,38 @@ protected:
   NetworkPerception* networkPerception;
 
   void PublishTrackedObject(Roboid* roboid, InterestingThing* object);
-  void PublishRelativeObject(Buffer sendBuffer, UInt8 parentId,
+  void PublishRelativeObject(Buffer sendBuffer,
+                             UInt8 parentId,
                              InterestingThing* object);
 
   void SendSingle100(unsigned char* data, unsigned int startIndex, float value);
   void SendFloat16(unsigned char* data, unsigned char* startIndex, float 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 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,13 +19,21 @@ 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) {
   if (parent == nullptr)
@@ -35,7 +42,9 @@ void Thing::SetParent(Thing *parent) {
   parent->AddChild(this);
 }
 
-Thing *Thing::GetParent() { return this->parent; }
+Thing* Thing::GetParent() {
+  return this->parent;
+}
 
 void Thing::AddChild(Thing* child) {
   Thing** newChildren = new Thing*[this->childCount + 1];

Thing.h

@@ -1,7 +1,8 @@
 #pragma once
 
-#include "LinearAlgebra/Polar.h"
+// #include "LinearAlgebra/Polar.h"
 #include "LinearAlgebra/Quaternion.h"
+#include "LinearAlgebra/Spherical.h"
 
 namespace Passer {
 namespace RoboidControl {
@@ -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