diff --git a/LinearAlgebra b/LinearAlgebra index 51772a1..353cb1b 160000 --- a/LinearAlgebra +++ b/LinearAlgebra @@ -1 +1 @@ -Subproject commit 51772a18433c702244af814e0e7c24b28e904ada +Subproject commit 353cb1bc7f3c12f703c0f963d89d08d567fc446a diff --git a/NetworkSync.cpp b/NetworkSync.cpp index d22b6d4..54648f2 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -211,7 +211,8 @@ void NetworkSync::PublishTrackedObject(Roboid* roboid, // Vector3 localPosition = object->position.ToVector3(); Vector3 localPosition = Vector3(object->position); Vector3 worldPosition = roboidPosition + roboidOrientation * localPosition; - Quaternion worldOrientation = roboidOrientation * object->orientation; + Quaternion worldOrientation = + roboidOrientation * object->orientation.ToQuaternion(); unsigned char ix = 0; buffer[ix++] = PoseMsg; // Position2DMsg; diff --git a/Perception.cpp b/Perception.cpp index 0866944..8a7fea8 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -306,8 +306,8 @@ InterestingThing* Perception::AddTrackedObject(Sensor* sensor, // this->roboid->networkSync->SendText("Update\0"); // else // this->roboid->networkSync->SendText("Refresh\0"); - this->trackedObjects[thingIx]->Refresh(thing->position, - thing->orientation); + this->trackedObjects[thingIx]->Refresh( + thing->position, thing->orientation.ToQuaternion()); delete thing; return this->trackedObjects[thingIx]; } diff --git a/Roboid.cpp b/Roboid.cpp index 5348e59..8c70293 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -23,7 +23,7 @@ Roboid::Roboid() : Thing(0) { // this->actuation = nullptr; this->worldPosition = Vector3::zero; // this->worldOrientation = Quaternion::identity; - this->worldAngleAxis = AngleAxis(); + this->worldAngleAxis = AngleAxis(); } Roboid::Roboid(Propulsion* propulsion) : Roboid() { @@ -92,9 +92,9 @@ void Roboid::SetPosition(Vector3 newWorldPosition) { Quaternion roboidOrientation = this->GetOrientation(); Vector3 translation = newWorldPosition - this->worldPosition; float distance = translation.magnitude(); - float angle = Vector3::SignedAngle(roboidOrientation * Vector3::forward, - translation, Vector3::up); - Polar polarTranslation = Polar(angle, distance); + AngleOf angle = Vector3::SignedAngle( + roboidOrientation * Vector3::forward, translation, Vector3::up); + Polar polarTranslation = Polar(angle.ToFloat(), distance); if (perception != nullptr) perception->UpdatePose(polarTranslation); this->worldPosition = newWorldPosition; @@ -114,12 +114,12 @@ void Roboid::SetOrientation(Quaternion worldOrientation) { if (perception != nullptr) perception->UpdatePose(delta); - AngleAxis angleAxis = AngleAxis(angle, Axis(axis)); + AngleAxis angleAxis = AngleAxis(angle, Direction(axis)); this->worldAngleAxis = angleAxis; } void Roboid::SetOrientation2D(float angle) { - this->worldAngleAxis = AngleAxis(angle, Axis::up); + this->worldAngleAxis = AngleAxis(angle, Direction::up); } void Roboid::AddChild(Thing* child) { diff --git a/Roboid.h b/Roboid.h index 7578bf9..f858e4b 100644 --- a/Roboid.h +++ b/Roboid.h @@ -12,13 +12,13 @@ class NetworkSync; /// @brief A Roboid is used to control autonomous robots class Roboid : public Thing { -public: + public: /// @brief Default constructor for a Roboid Roboid(); /// @brief Creates a Roboid with Perception and Propulsion abilities /// @param perception The Perception implementation to use for this Roboid /// @param propulsion The Propulsion implementation to use for this Roboid - Roboid(Propulsion *propulsion); + Roboid(Propulsion* propulsion); /// @brief Update the state of the Roboid /// @param currentTimeMs The time in milliseconds when calling this @@ -26,13 +26,13 @@ public: void Update(unsigned long currentTimeMs); /// @brief The Perception module of this Roboid - Perception *perception = nullptr; + Perception* perception = nullptr; /// @brief The Propulsion module of this Roboid - Propulsion *propulsion = nullptr; + Propulsion* propulsion = nullptr; // ServoMotor *actuation = nullptr; /// @brief The reference to the module to synchronize states across a network - NetworkSync *networkSync = nullptr; + NetworkSync* networkSync = nullptr; /// @brief Retrieve the current position of the roboid /// @return The position in carthesian coordinates in world space @@ -65,9 +65,9 @@ public: virtual void SetOrientation(Quaternion worldOrientation); void SetOrientation2D(float angle); - virtual void AddChild(Thing *child) override; + virtual void AddChild(Thing* child) override; -private: + private: /// @brief The position of the roboid in carthesian coordinates in world space /// @details This position may be set when NetworkSync is used to receive /// positions from an external tracking system. These values should not be set @@ -79,12 +79,12 @@ private: /// set directly, but SetOrientation should be used instead. // Quaternion worldOrientation = Quaternion::identity; - AngleAxis worldAngleAxis = AngleAxis(); + AngleAxis worldAngleAxis = AngleAxis(); unsigned long lastUpdateTimeMs = 0; }; -} // namespace RoboidControl -} // namespace Passer +} // namespace RoboidControl +} // namespace Passer using namespace Passer::RoboidControl; \ No newline at end of file diff --git a/TrackedObject.cpp b/TrackedObject.cpp index 3a60bba..a9a0ee8 100644 --- a/TrackedObject.cpp +++ b/TrackedObject.cpp @@ -17,7 +17,10 @@ InterestingThing::InterestingThing(Sensor* sensor, this->confidence = maxConfidence; this->sensor = sensor; this->position = position; - this->orientation = orientation; + float angle; + Vector3 axis; + orientation.ToAngleAxis(&angle, &axis); + this->orientation = AngleAxis(angle, axis); this->updated = true; } @@ -76,7 +79,11 @@ void InterestingThing::Refresh(Polar position) { void InterestingThing::Refresh(Spherical position, Quaternion orientation) { this->position = position; - this->orientation = orientation; + + float angle; + Vector3 axis; + orientation.ToAngleAxis(&angle, &axis); + this->orientation = AngleAxis(angle, axis); this->confidence = maxConfidence; this->updated = true; } diff --git a/TrackedObject.h b/TrackedObject.h index 707d003..45d4e50 100644 --- a/TrackedObject.h +++ b/TrackedObject.h @@ -1,5 +1,6 @@ #pragma once +#include "LinearAlgebra/AngleAxis.h" #include "LinearAlgebra/Polar.h" #include "LinearAlgebra/Quaternion.h" #include "LinearAlgebra/Spherical.h" @@ -10,12 +11,13 @@ namespace RoboidControl { /// @brief An object tracked by the roboid class InterestingThing { -public: + public: /// @brief An object tracked by the roboid /// @param sensor The Sensor which detected this object /// @param position The position in polar coordinates local to the roboid - InterestingThing(Sensor *sensor, Polar position); - InterestingThing(Sensor *sensor, Spherical position, + InterestingThing(Sensor* sensor, Polar position); + InterestingThing(Sensor* sensor, + Spherical position, Quaternion orientation = Quaternion::identity); /// @brief Update the position of the object @@ -39,7 +41,7 @@ public: /// @return Returns true when both objects are considered the same /// The result of this check depends on the equalityDistance and equalityAngle /// value. - bool IsTheSameAs(InterestingThing *otherObj); + bool IsTheSameAs(InterestingThing* otherObj); /// @brief The maximum difference in distance from the roboid in which two /// objects may be considered the same /// @details When the difference in distance is exactly this @@ -66,18 +68,19 @@ public: /// @brief The current position of the object Spherical position = Spherical::zero; /// @brief The current orientation of the object - Quaternion orientation = Quaternion::identity; + AngleAxis orientation = AngleAxis(); + // Quaternion orientation = Quaternion::identity; /// @brief The sensor which provided that lastet pose this object - Sensor *sensor = nullptr; + Sensor* sensor = nullptr; unsigned char type = 0x00; unsigned char confidence; bool updated = false; -protected: + protected: static constexpr unsigned char maxConfidence = 255; - static constexpr unsigned char confidenceDropSpeed = 10; // 150; // 2; + static constexpr unsigned char confidenceDropSpeed = 10; // 150; // 2; }; -} // namespace RoboidControl -} // namespace Passer \ No newline at end of file +} // namespace RoboidControl +} // namespace Passer \ No newline at end of file