From c872a63eb204df63fb122404e43a260fcf482cc0 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Fri, 29 Dec 2023 10:41:28 +0100
Subject: [PATCH] Setters for verlocity and angular velocity

---
 Propulsion.cpp | 16 +++++++++++++++-
 Propulsion.h   | 11 +++++++++++
 2 files changed, 26 insertions(+), 1 deletion(-)

diff --git a/Propulsion.cpp b/Propulsion.cpp
index a62fb8f..eb55e4a 100644
--- a/Propulsion.cpp
+++ b/Propulsion.cpp
@@ -40,4 +40,18 @@ void Propulsion::SetTwistSpeed(Vector2 linear, float yaw) {}
 void Propulsion::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
                                float roll) {}
 
-Polar Propulsion::GetVelocity() { return Polar(0, 0); }
\ No newline at end of file
+Polar Propulsion::GetVelocity() { return Polar(0, 0); }
+
+float Propulsion::GetAngularVelocity() { return 0; }
+
+Vector3 Propulsion::GetPosition() { return this->worldPosition; }
+
+Quaternion Propulsion::GetOrientation() { return Quaternion::identity; }
+
+void Propulsion::SetPosition(Vector3 worldPosition) {
+  this->worldPosition = worldPosition;
+}
+
+void Propulsion::SetOrientation(Quaternion worldOrientation) {
+  this->worldOrientation = worldOrientation;
+}
\ No newline at end of file
diff --git a/Propulsion.h b/Propulsion.h
index a4086a2..7a7243b 100644
--- a/Propulsion.h
+++ b/Propulsion.h
@@ -3,6 +3,7 @@
 #include "Motor.h"
 #include "Placement.h"
 #include "Polar.h"
+#include "Quaternion.h"
 #include "Vector2.h"
 
 namespace Passer {
@@ -61,12 +62,22 @@ public:
                              float pitch = 0.0F, float roll = 0.0F);
 
   virtual Polar GetVelocity();
+  virtual float GetAngularVelocity();
+
+  virtual Vector3 GetPosition();
+  virtual Quaternion GetOrientation();
+
+  virtual void SetPosition(Vector3 worldPosition);
+  virtual void SetOrientation(Quaternion worldOrientation);
 
 protected:
   /// @brief The number of motors used for Propulsion
   unsigned int motorCount = 0;
   /// @brief The Placement of the motors used for Propulsion
   Placement *placement = nullptr;
+
+  Vector3 worldPosition = Vector3::zero;
+  Quaternion worldOrientation = Quaternion::identity;
 };
 
 } // namespace RoboidControl