Updated LinearAlgebra

DifferentialDrive.cpp

@@ -65,7 +65,7 @@ void DifferentialDrive::SetTwistSpeed(Vector2 linear, float yaw) {
 
 void DifferentialDrive::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
                                       float roll) {
-  SetTwistSpeed(linear.z, yaw);
+  SetTwistSpeed(linear.Forward(), yaw);
 }
 
 void DifferentialDrive::SetVelocity(Polar velocity) {

LinearAlgebra

@@ -1 +1 @@
-Subproject commit 5489b3c45cb2eded59262983c2a5f23340465f26
+Subproject commit ee62cbae3ec63b7847df242e02b453a43defa5de

NetworkSync.cpp

@@ -29,11 +29,11 @@ void NetworkSync::ReceiveNetworkId() {
 
 void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex,
                               const Vector3 v) {
-  SendSingle100(data, *startIndex, v.x);
+  SendSingle100(data, *startIndex, v.Right());
   (*startIndex) += 4;
-  SendSingle100(data, *startIndex, v.y);
+  SendSingle100(data, *startIndex, v.Up());
   (*startIndex) += 4;
-  SendSingle100(data, *startIndex, v.z);
+  SendSingle100(data, *startIndex, v.Forward());
   (*startIndex) += 4;
 }
 
@@ -41,9 +41,9 @@ void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex,
                                  const Quaternion q) {
   Vector3 angles = Quaternion::ToAngles(q);
   int ix = startIndex;
-  SendAngle8(data, ix++, angles.x);
-  SendAngle8(data, ix++, angles.y);
-  SendAngle8(data, ix++, angles.z);
+  SendAngle8(data, ix++, angles.Right());
+  SendAngle8(data, ix++, angles.Up());
+  SendAngle8(data, ix++, angles.Forward());
 }
 
 void NetworkSync::SendPolar(unsigned char *data, unsigned char *startIndex,
@@ -175,11 +175,11 @@ void NetworkSync::PublishTrackedObject(Buffer sendBuffer,
   Serial.print("Send Pose ");
   Serial.print((int)object->id);
   Serial.print("  Position  ");
-  Serial.print(worldPosition3.x);
+  Serial.print(worldPosition3.Right());
   Serial.print(", ");
-  Serial.print(worldPosition3.y);
+  Serial.print(worldPosition3.Up());
   Serial.print(", ");
-  Serial.println(worldPosition3.z);
+  Serial.println(worldPosition3.Forward());
 #else
   const UInt16 bufferSize = 3 + 12;
   UInt8 buffer[bufferSize] = {
@@ -219,17 +219,17 @@ void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) {
 
 #ifdef RC_DEBUG
   Serial.print("Send Pose 0  LinearVelocity  ");
-  Serial.print(worldVelocity3.x);
+  Serial.print(worldVelocity3.Right());
   Serial.print(", ");
-  Serial.print(worldVelocity3.y);
+  Serial.print(worldVelocity3.Up());
   Serial.print(", ");
-  Serial.print(worldVelocity3.z);
+  Serial.print(worldVelocity3.Forward());
   Serial.print("  AngularVelocity ");
-  Serial.print(worldAngularVelocity.x);
+  Serial.print(worldAngularVelocity.Right());
   Serial.print(", ");
-  Serial.print(worldAngularVelocity.y);
+  Serial.print(worldAngularVelocity.Up());
   Serial.print(", ");
-  Serial.println(worldAngularVelocity.z);
+  Serial.println(worldAngularVelocity.Forward());
 #else
   const unsigned int bufferSize = 3 + 12 + 12;
   unsigned char buffer[bufferSize] = {

Perception.cpp

@@ -549,7 +549,7 @@ void Perception::UpdatePose(Quaternion rotation) {
   Vector3 rotationAxis;
   rotation.ToAngleAxis(&rotationAngle, &rotationAxis);
   // Make sure rotation axis is positive
-  if (rotationAxis.y < 0)
+  if (rotationAxis.Up() < 0)
     rotationAngle = -rotationAngle;
 
   for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {

Roboid.cpp

@@ -73,7 +73,7 @@ void Roboid::Update(float currentTimeMs) {
 
 Vector3 Roboid::GetPosition() { return this->worldPosition; }
 Vector2 Roboid::GetPosition2D() {
-  return Vector2(this->worldPosition.x, this->worldPosition.z);
+  return Vector2(this->worldPosition.Right(), this->worldPosition.Forward());
 }
 
 #include <Arduino.h>