diff --git a/LinearAlgebra b/LinearAlgebra index 3363388..11259a9 160000 --- a/LinearAlgebra +++ b/LinearAlgebra @@ -1 +1 @@ -Subproject commit 3363388a95d8fe9708b615fc2ecdc9474f4930a1 +Subproject commit 11259a92a6a802b090679bfb951d09acaa01910e diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp index 7a34290..fa7e4c2 100644 --- a/NetworkPerception.cpp +++ b/NetworkPerception.cpp @@ -2,7 +2,7 @@ #include "NetworkSync.h" -#define RC_DEBUG true +// #define RC_DEBUG true #if RC_DEBUG #include #endif @@ -82,24 +82,25 @@ void NetworkPerception::ReceivePlane(unsigned char* data, Roboid* roboid) { // both position and orientation are required return; - Vector3 worldPosition = ReceiveVector3(data, 4); - Vector3 roboidPosition = roboid->GetPosition(); - Vector3 deltaPosition = worldPosition - roboidPosition; + Spherical16 worldPosition = Spherical16::FromVector3(ReceiveVector3(data, 4)); + Spherical16 roboidPosition = roboid->GetPosition(); + Spherical16 deltaPosition = worldPosition - roboidPosition; - float distance = deltaPosition.magnitude(); + float distance = deltaPosition.distance; // magnitude(); if (roboid->perception->IsInteresting(distance) == false) { // printf(" plane not interesting\n"); return; } - Quaternion roboidOrientation = roboid->GetOrientation(); - Vector3 localPosition = - Quaternion::Inverse(roboidOrientation) * deltaPosition; + SwingTwist16 roboidOrientation = roboid->GetOrientation(); + Spherical16 localPosition = + SwingTwist16::Inverse(roboidOrientation) * deltaPosition; // float angle = // Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up); // Polar position = Polar(angle, distance); - Spherical16 position = Spherical16::FromVector3(localPosition); + Spherical16 position = + localPosition; // Spherical16::FromVector3(localPosition); // printf("Received plane (%f %f %f) (%f %f %f) %f %f %f\n", worldPosition.x, // worldPosition.y, worldPosition.z, roboidPosition.x, @@ -107,7 +108,7 @@ void NetworkPerception::ReceivePlane(unsigned char* data, Roboid* roboid) { // (float)position.horizontalAngle, (float)position.verticalAngle); // printf("Received plane %f (%f %f)\n", position.distance, // (float)position.horizontalAngle, (float)position.verticalAngle); - roboid->perception->AddTrackedObject(this, position, Quaternion::identity, + roboid->perception->AddTrackedObject(this, position, SwingTwist16::identity, 0x80, 0x80, networkId); } @@ -116,17 +117,18 @@ void NetworkPerception::ReceiveSphere(unsigned char* data, Roboid* roboid) { float radius = ReceiveFloat100(data, 3); - Vector3 worldPosition = ReceiveVector3(data, 7); - Vector3 roboidPosition = roboid->GetPosition(); - Vector3 deltaPosition = worldPosition - roboidPosition; + Spherical16 worldPosition = Spherical16::FromVector3(ReceiveVector3(data, 7)); + Spherical16 roboidPosition = roboid->GetPosition(); + Spherical16 deltaPosition = worldPosition - roboidPosition; - Quaternion roboidOrientation = roboid->GetOrientation(); - Vector3 localPosition = - Quaternion::Inverse(roboidOrientation) * deltaPosition; + SwingTwist16 roboidOrientation = roboid->GetOrientation(); + Spherical16 localPosition = + SwingTwist16::Inverse(roboidOrientation) * deltaPosition; - Spherical16 position = Spherical16::FromVector3(localPosition); + Spherical16 position = + localPosition; // Spherical16::FromVector3(localPosition); - roboid->perception->AddTrackedObject(this, position, Quaternion::identity, + roboid->perception->AddTrackedObject(this, position, SwingTwist16::identity, 0x81, 0x81, networkId); } @@ -147,19 +149,20 @@ void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) { return ReceiveSphere(data, roboid); #if RC_DEBUG - // printf("Received PoseMsg [%d/%d]\n", networkId, objectId); + printf("Received PoseMsg [%d/%d]\n", networkId, objectId); #endif - Quaternion roboidOrientation = roboid->GetOrientation(); + SwingTwist16 roboidOrientation = roboid->GetOrientation(); Spherical16 position = Spherical16::zero; - Quaternion orientation = Quaternion::identity; + SwingTwist16 orientation = SwingTwist16::identity; if ((poseType & NetworkSync::Pose_Position) != 0) { - Vector3 worldPosition = ReceiveVector3(data, 4); + Spherical16 worldPosition = + Spherical16::FromVector3(ReceiveVector3(data, 4)); if (objectId == 0) { roboid->SetPosition(worldPosition); } else { - Vector3 roboidPosition = roboid->GetPosition(); + Spherical16 roboidPosition = roboid->GetPosition(); // float distance = Vector3::Distance(roboidPosition, worldPosition); // if (roboid->perception->IsInteresting(distance) == false) { @@ -167,19 +170,29 @@ void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) { // return; // } - Vector3 localPosition = Quaternion::Inverse(roboidOrientation) * - (worldPosition - roboidPosition); - position = Spherical16::FromVector3(localPosition); + Spherical16 localPosition = SwingTwist16::Inverse(roboidOrientation) * + (worldPosition - roboidPosition); + position = localPosition; // Spherical16::FromVector3(localPosition); + // printf(" worldPosition (%f %f %f) localPosition (%f %f %f)\n", + // worldPosition.Right(), worldPosition.Up(), + // worldPosition.Forward(), localPosition.Right(), + // localPosition.Up(), localPosition.Forward()); + // printf(" position: %f (%f %f)\n", position.distance, + // position.horizontal.InDegrees(), position.vertical.InDegrees()); } } if ((poseType & NetworkSync::Pose_Orientation) != 0) { Vector3 worldAngles = ReceiveVector3(data, 16); - Quaternion worldOrientation = Quaternion::Euler(worldAngles); + SwingTwist16 worldOrientation = SwingTwist16( + Angle16::Degrees(worldAngles.Up()), + Angle16::Degrees(worldAngles.Right()), + Angle16::Degrees( + worldAngles.Forward())); // Quaternion::Euler(worldAngles); if (objectId == 0) { roboid->SetOrientation(worldOrientation); } else { - orientation = Quaternion::Inverse(roboidOrientation) * worldOrientation; + orientation = SwingTwist16::Inverse(roboidOrientation) * worldOrientation; } } diff --git a/NetworkSync.cpp b/NetworkSync.cpp index dec63ae..2efbc06 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -46,9 +46,6 @@ void NetworkSync::ReceiveNetworkId() { } void NetworkSync::PublishState(Roboid* roboid) { - // if (roboid->updated == false) - // return; - SendPose(roboid); PublishPerception(roboid); } @@ -128,9 +125,9 @@ void NetworkSync::SendModel(Roboid* roboid) { unsigned char ix = 0; buffer[ix++] = ModelMsg; - buffer[ix++] = 0x00; // objectId - Spherical s = Spherical::zero; //(roboid->modelPosition); - SendSpherical(buffer, &ix, s); + buffer[ix++] = 0x00; // objectId + Spherical16 s = Spherical16::zero; //(roboid->modelPosition); + SendSpherical16(buffer, &ix, s); SendFloat16(buffer, &ix, 1); // roboid->modelScale); buffer[ix++] = len; @@ -150,9 +147,9 @@ void NetworkSync::SendModel(Thing* thing) { unsigned char ix = 0; buffer[ix++] = ModelMsg; - buffer[ix++] = thing->id; // objectId - Spherical s = Spherical::zero; // Spherical(thing->modelPosition); - SendSpherical(buffer, &ix, s); + buffer[ix++] = thing->id; // objectId + Spherical16 s = Spherical16::zero; // Spherical(thing->modelPosition); + SendSpherical16(buffer, &ix, s); SendFloat16(buffer, &ix, 1); // thing->modelScale); buffer[ix++] = len; @@ -215,13 +212,14 @@ void NetworkSync::SendPose(Thing* thing, bool recurse) { buffer[ix++] = thing->id; buffer[ix++] = Pose_Position | Pose_Orientation; SendSpherical16(buffer, &ix, thing->position); - SendQuat32(buffer, &ix, thing->orientation); + SendQuat32(buffer, &ix, thing->orientation.ToQuaternion()); SendBuffer(ix); - } #if RC_DEBUG - printf("Sent PoseMsg Thing [%d/%d]\n", networkId, buffer[1]); + if (thing->id == 0) + printf("Sent PoseMsg Thing [%d/%d]\n", networkId, buffer[1]); #endif + } if (recurse) { for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) { @@ -298,21 +296,23 @@ void NetworkSync::PublishTrackedObject(Roboid* roboid, // if (object->parentId != 0) // return PublishRelativeObject(object); - Vector3 roboidPosition = roboid->GetPosition(); - Quaternion roboidOrientation = roboid->GetOrientation(); + Spherical16 roboidPosition = roboid->GetPosition(); + SwingTwist16 roboidOrientation = roboid->GetOrientation(); // Vector3 localPosition = object->position.ToVector3(); Vector3 localPosition = object->position.ToVector3(); - Vector3 worldPosition = roboidPosition + roboidOrientation * localPosition; - Quaternion worldOrientation = - roboidOrientation * object->orientation.ToQuaternion(); + Spherical16 worldPosition = + roboidPosition + roboidOrientation * object->position; // localPosition; + SwingTwist16 worldOrientation = + roboidOrientation * object->orientation; //.ToQuaternion(); unsigned char ix = 0; buffer[ix++] = PoseMsg; // Position2DMsg; buffer[ix++] = object->id; // objectId; buffer[ix++] = Pose_Position | Pose_Orientation; - SendSpherical(buffer, &ix, Spherical::FromVector3(worldPosition)); - SendQuat32(buffer, &ix, worldOrientation); + SendSpherical16(buffer, &ix, + worldPosition); // Spherical::FromVector3(worldPosition)); + SendQuat32(buffer, &ix, worldOrientation.ToQuaternion()); // SendPolar(buffer, &ix, polar); // 3 bytes // SendVector3(buffer, &ix, worldPosition); // SendQuat32(buffer, &ix, worldOrientation); @@ -450,22 +450,22 @@ void NetworkSync::SendPolar(unsigned char* data, SendSingle100(data, (*startIndex) + 1, p.distance); } -void NetworkSync::SendSpherical(unsigned char* data, - unsigned char* startIndex, - Spherical s) { - SendAngle8(data, (*startIndex)++, s.horizontal.ToFloat()); - SendAngle8(data, (*startIndex)++, s.vertical.ToFloat()); - 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()); + SendAngle8(data, (*startIndex)++, s.horizontal.InDegrees()); + SendAngle8(data, (*startIndex)++, s.vertical.InDegrees()); 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); +// } + void NetworkSync::SendQuat32(unsigned char* data, unsigned char* startIndex, const Quaternion q) { diff --git a/NetworkSync.h b/NetworkSync.h index 7614a77..44a88f0 100644 --- a/NetworkSync.h +++ b/NetworkSync.h @@ -75,8 +75,9 @@ class NetworkSync { void PublishPerception(Roboid* roboid); void PublishTrackedObjects(Roboid* roboid, InterestingThing** objects); - virtual void SendPosition(Vector3 worldPosition) {}; - virtual void SendPose(Vector3 worldPosition, Quaternion worldOrientation) {}; + virtual void SendPosition(Spherical16 worldPosition) {}; + virtual void SendPose(Spherical16 worldPosition, + SwingTwist16 worldOrientation) {}; // void SendPose(Roboid* roboid, bool recurse = true); void SendPose(Thing* thing, bool recurse = true); @@ -113,9 +114,9 @@ class NetworkSync { const Quaternion q); void SendPolar(unsigned char* data, unsigned char* startIndex, Polar p); - void SendSpherical(unsigned char* data, - unsigned char* startIndex, - Spherical s); + // void SendSpherical(unsigned char* data, + // unsigned char* startIndex, + // Spherical s); void SendSpherical16(unsigned char* data, unsigned char* startIndex, Spherical16 s); diff --git a/Perception.cpp b/Perception.cpp index e7ae8a1..df0ad50 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -199,7 +199,7 @@ bool Perception::ObjectNearby(float direction, float range) { InterestingThing* Perception::AddTrackedObject(Sensor* sensor, Spherical16 position, - Quaternion orientation, + SwingTwist16 orientation, unsigned char thingType, unsigned char thingId, unsigned char networkId) { @@ -218,7 +218,7 @@ InterestingThing* Perception::AddTrackedObject(Sensor* sensor, else { if (thing->IsTheSameAs(this->trackedObjects[thingIx])) { this->trackedObjects[thingIx]->Refresh( - thing->position, thing->orientation.ToQuaternion()); + thing->position, thing->orientation); //.ToQuaternion()); delete thing; return this->trackedObjects[thingIx]; @@ -262,7 +262,7 @@ InterestingThing* Perception::AddTrackedObject(Sensor* sensor, unsigned char networkId, unsigned char thingId, Spherical16 position, - Quaternion orientation) { + SwingTwist16 orientation) { InterestingThing* thing = FindTrackedObject(networkId, thingId); if (thing == nullptr) { thing = AddTrackedObject(sensor, position, orientation, 0xFF, thingId, @@ -472,14 +472,15 @@ void Perception::UpdatePose(Polar translation) { } } -void Perception::UpdatePose(Quaternion rotation) { +void Perception::UpdatePose(SwingTwist16 rotation) { // only rotation around vertical axis is supported for now - float rotationAngle; - Vector3 rotationAxis; - rotation.ToAngleAxis(&rotationAngle, &rotationAxis); + // float rotationAngle; + // Vector3 rotationAxis; + // rotation.ToAngleAxis(&rotationAngle, &rotationAxis); // Make sure rotation axis is positive - if (rotationAxis.Up() < 0) - rotationAngle = -rotationAngle; + // if (rotationAxis.Up() < 0) + // rotationAngle = -rotationAngle; + Angle16 rotationAngle = rotation.swing.horizontal; for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) { InterestingThing* thing = trackedObjects[thingIx]; @@ -491,8 +492,7 @@ void Perception::UpdatePose(Quaternion rotation) { // (float)thing->position.verticalAngle); // printf("| rotate %f | ", rotationAngle); - thing->position.horizontal = Angle16(Angle16::Normalize( - thing->position.horizontal.ToFloat() - rotationAngle)); + thing->position.horizontal = thing->position.horizontal - rotationAngle; // printf("-> %f (%f %f) \n", thing->position.distance, // (float)thing->position.horizontalAngle, diff --git a/Perception.h b/Perception.h index a46144b..5b8d961 100644 --- a/Perception.h +++ b/Perception.h @@ -96,7 +96,7 @@ class Perception { InterestingThing* AddTrackedObject( Sensor* sensor, Spherical16 position, - Quaternion orientation = Quaternion::identity, + SwingTwist16 orientation = SwingTwist16::identity, unsigned char objectType = 0xFF, unsigned char objectId = 0x00, unsigned char networkId = 0x00); @@ -106,7 +106,7 @@ class Perception { unsigned char networkId, unsigned char objectId, Spherical16 position, - Quaternion orientation = Quaternion::identity); + SwingTwist16 orientation = SwingTwist16::identity); bool IsInteresting(float distance); @@ -153,7 +153,7 @@ class Perception { /// @brief Update the orientation of the perceived objecst from the given /// roboid rotation /// @param rotation The rotation of the roboid in world space - void UpdatePose(Quaternion rotation); + void UpdatePose(SwingTwist16 rotation); /// @brief Objects with a distance closed that this value will be considered /// nearby. diff --git a/Roboid.cpp b/Roboid.cpp index cab60a0..8f872b9 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -21,9 +21,10 @@ Roboid::Roboid() : Thing(0) { this->propulsion = nullptr; this->networkSync = nullptr; // this->actuation = nullptr; - this->worldPosition = Vector3::zero; + this->worldPosition = Spherical16::zero; + this->worldOrientation = SwingTwist16::identity; // this->worldOrientation = Quaternion::identity; - this->worldAngleAxis = AngleAxisOf(); + // this->worldAngleAxis = AngleAxisOf(); } Roboid::Roboid(Propulsion* propulsion) : Roboid() { @@ -44,13 +45,14 @@ void Roboid::Update(unsigned long currentTimeMs) { propulsion->Update(currentTimeMs); float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000; - Quaternion roboidOrientation = this->GetOrientation(); + SwingTwist16 roboidOrientation = this->GetOrientation(); SetPosition(this->worldPosition + - roboidOrientation * Vector3::forward * + roboidOrientation * Spherical16::forward * this->propulsion->GetVelocity().distance * deltaTime); - SetOrientation(roboidOrientation * - Quaternion::AngleAxis(this->propulsion->GetAngularVelocity(), - Vector3::up)); + SetOrientation( + roboidOrientation * + SwingTwist16::AngleAxis(this->propulsion->GetAngularVelocity(), + Spherical16::up)); } if (childCount > 0 && children != nullptr) { @@ -65,38 +67,39 @@ void Roboid::Update(unsigned long currentTimeMs) { lastUpdateTimeMs = currentTimeMs; } -Vector3 Roboid::GetPosition() { +Spherical16 Roboid::GetPosition() { return this->worldPosition; } -Vector2 Roboid::GetPosition2D() { - return Vector2(this->worldPosition.Right(), this->worldPosition.Forward()); +// Vector2 Roboid::GetPosition2D() { +// return Vector2(this->worldPosition.Right(), this->worldPosition.Forward()); +// } + +SwingTwist16 Roboid::GetOrientation() { + // Vector3 axis = this->worldAngleAxis.axis.ToVector3(); + // SwingTwist16 q = SwingTwist16::AngleAxis(this->worldAngleAxis.angle, axis); + return this->worldOrientation; } -Quaternion Roboid::GetOrientation() { - Vector3 axis = this->worldAngleAxis.axis.ToVector3(); - Quaternion q = Quaternion::AngleAxis(this->worldAngleAxis.angle, axis); - return q; -} +// float Roboid::GetOrientation2D() { +// float maxAngle = 90 - Float::epsilon; // note: range vertical angle = +// -90..90 -float Roboid::GetOrientation2D() { - float maxAngle = 90 - Float::epsilon; // note: range vertical angle = -90..90 +// // rotation axis is vertical, so we have a simple 2D orientation +// if (this->worldAngleAxis.axis.vertical.InDegrees() > maxAngle) +// return this->worldAngleAxis.angle; +// if (this->worldAngleAxis.axis.vertical.InDegrees() < -maxAngle) +// return -this->worldAngleAxis.angle; - // rotation axis is vertical, so we have a simple 2D orientation - if (this->worldAngleAxis.axis.vertical.InDegrees() > maxAngle) - return this->worldAngleAxis.angle; - if (this->worldAngleAxis.axis.vertical.InDegrees() < -maxAngle) - return -this->worldAngleAxis.angle; +// SwingTwist16 q = GetOrientation(); +// return Quaternion::GetAngleAround(Vector3::up, q); +// } - Quaternion q = GetOrientation(); - return Quaternion::GetAngleAround(Vector3::up, q); -} - -void Roboid::SetPosition(Vector3 newWorldPosition) { - Quaternion roboidOrientation = this->GetOrientation(); - Vector3 translation = newWorldPosition - this->worldPosition; - float distance = translation.magnitude(); - AngleOf angle = Vector3::SignedAngle( - roboidOrientation * Vector3::forward, translation, Vector3::up); +void Roboid::SetPosition(Spherical16 newWorldPosition) { + SwingTwist16 roboidOrientation = this->GetOrientation(); + Spherical16 translation = newWorldPosition - this->worldPosition; + float distance = translation.distance; + Angle16 angle = Spherical16::SignedAngleBetween( + roboidOrientation * Spherical16::forward, translation, Spherical16::up); Polar polarTranslation = Polar(angle.ToFloat(), distance); if (perception != nullptr) perception->UpdatePose(polarTranslation); @@ -108,23 +111,24 @@ void Roboid::SetPosition(Vector3 newWorldPosition) { } #include -void Roboid::SetOrientation(Quaternion worldOrientation) { - float angle; - Vector3 axis; - worldOrientation.ToAngleAxis(&angle, &axis); +void Roboid::SetOrientation(SwingTwist16 worldOrientation) { + // float angle; + // Vector3 axis; + // worldOrientation.ToAngleAxis(&angle, &axis); - Quaternion delta = Quaternion::Inverse(GetOrientation()) * worldOrientation; + SwingTwist16 delta = + SwingTwist16::Inverse(GetOrientation()) * worldOrientation; if (perception != nullptr) perception->UpdatePose(delta); - AngleAxisOf angleAxis = - AngleAxisOf(angle, DirectionOf(axis)); - this->worldAngleAxis = angleAxis; + // AngleAxisOf angleAxis = + // AngleAxisOf(angle, DirectionOf(axis)); + // this->worldAngleAxis = angleAxis; } -void Roboid::SetOrientation2D(float angle) { - this->worldAngleAxis = AngleAxisOf(angle, DirectionOf::up); -} +// void Roboid::SetOrientation2D(float angle) { +// this->worldAngleAxis = AngleAxisOf(angle, DirectionOf::up); +// } Vector3 Passer::RoboidControl::Roboid::GetVelocity() { return Vector3(); diff --git a/Roboid.h b/Roboid.h index a308101..72ec04a 100644 --- a/Roboid.h +++ b/Roboid.h @@ -42,15 +42,15 @@ class Roboid : public Thing { /// @details The origin and units of the position depends on the position /// tracking system used. This value will be Vector3::zero unless a position /// is received through network synchronisation - virtual Vector3 GetPosition(); - Vector2 GetPosition2D(); + virtual Spherical16 GetPosition(); + // Vector2 GetPosition2D(); /// @brief Retrieve the current orientation of the roboid /// @return The orientation quaternion in world space /// @details The origin orientation depends on the position tracking system /// used. This value will be Quaternion::identity unless an orientation is /// received though network synchronization - virtual Quaternion GetOrientation(); - float GetOrientation2D(); + virtual SwingTwist16 GetOrientation(); + // float GetOrientation2D(); /// @brief Update the current position of the roboid /// @param worldPosition The position of the roboid in carthesian coordinates @@ -59,14 +59,14 @@ class Roboid : public Thing { /// orientations of the perceived objects by the roboid /// (roboid->perception->perceivedObjects), as these are local to the /// roboid's position. - virtual void SetPosition(Vector3 worldPosition); + virtual void SetPosition(Spherical16 worldPosition); /// @brief Update the current orientation of the roboid /// @param worldOrientation The orientation of the roboid in world space /// @details The use of this function will also update the orientations of the /// perceived objects by the roboid (roboid->perception->perceivedObjets), /// as these are local to the roboid' orientation. - virtual void SetOrientation(Quaternion worldOrientation); - void SetOrientation2D(float angle); + virtual void SetOrientation(SwingTwist16 worldOrientation); + // void SetOrientation2D(float angle); virtual Vector3 GetVelocity(); @@ -77,14 +77,14 @@ class Roboid : public Thing { /// @details This position may be set when NetworkSync is used to receive /// positions from an external tracking system. These values should not be set /// directly, but SetPosition should be used instead. - Vector3 worldPosition = Vector3::zero; + Spherical16 worldPosition = Spherical16::zero; /// @brief The orientation of the roboid in world space /// @details The position may be set when NetworkSync is used to receive /// orientations from an external tracking system. This value should not be /// set directly, but SetOrientation should be used instead. // Quaternion worldOrientation = Quaternion::identity; - AngleAxis worldAngleAxis = AngleAxis(); + // AngleAxis worldAngleAxis = AngleAxis(); unsigned long lastUpdateTimeMs = 0; }; diff --git a/Thing.h b/Thing.h index 6257d55..f57f099 100644 --- a/Thing.h +++ b/Thing.h @@ -3,6 +3,7 @@ #include "LinearAlgebra/AngleAxis.h" #include "LinearAlgebra/Quaternion.h" #include "LinearAlgebra/Spherical.h" +#include "LinearAlgebra/SwingTwist.h" namespace Passer { namespace RoboidControl { @@ -49,8 +50,8 @@ class Thing { /// @brief The orientation of this Thing /// @remark When this Thing has a parent, the orientation is relative to the /// parent's orientation - Quaternion orientation; - Quaternion worldOrientation; + SwingTwist16 orientation; + SwingTwist16 worldOrientation; virtual Spherical16 GetLinearVelocity(); virtual AngleAxis16 GetAngularVelocity(); diff --git a/TrackedObject.cpp b/TrackedObject.cpp index 5e883eb..49892e0 100644 --- a/TrackedObject.cpp +++ b/TrackedObject.cpp @@ -13,15 +13,15 @@ InterestingThing::InterestingThing(Sensor* sensor, Polar position) { InterestingThing::InterestingThing(Sensor* sensor, Spherical16 position, - Quaternion orientation) { + SwingTwist16 orientation) { this->id = 0; this->confidence = maxConfidence; this->sensor = sensor; this->position = position; - float angle; - Vector3 axis; - orientation.ToAngleAxis(&angle, &axis); - this->orientation = AngleAxisOf(angle, axis); + // float angle; + // Vector3 axis; + // orientation.ToAngleAxis(&angle, &axis); + this->orientation = orientation; // AngleAxisOf(angle, axis); this->updated = true; } @@ -72,20 +72,20 @@ bool InterestingThing::DegradeConfidence(float deltaTime) { } } -void InterestingThing::Refresh(Polar position) { - this->position = - Spherical16(position.distance, Angle16(position.angle.ToFloat()), 0); - this->confidence = maxConfidence; - this->updated = true; -} +// void InterestingThing::Refresh(Polar position) { +// this->position = +// Spherical16(position.distance, Angle16(position.angle.ToFloat()), 0); +// this->confidence = maxConfidence; +// this->updated = true; +// } -void InterestingThing::Refresh(Spherical16 position, Quaternion orientation) { +void InterestingThing::Refresh(Spherical16 position, SwingTwist16 orientation) { this->position = position; - - float angle; - Vector3 axis; - orientation.ToAngleAxis(&angle, &axis); - this->orientation = AngleAxisOf(angle, axis); + this->orientation = orientation; + // float angle; + // Vector3 axis; + // orientation.ToAngleAxis(&angle, &axis); + // this->orientation = AngleAxisOf(angle, axis); this->confidence = maxConfidence; this->updated = true; } diff --git a/TrackedObject.h b/TrackedObject.h index 54c743e..06da9c7 100644 --- a/TrackedObject.h +++ b/TrackedObject.h @@ -5,6 +5,7 @@ #include "LinearAlgebra/Polar.h" #include "LinearAlgebra/Quaternion.h" #include "LinearAlgebra/Spherical.h" +#include "LinearAlgebra/SwingTwist.h" #include "Sensor.h" namespace Passer { @@ -19,15 +20,15 @@ class InterestingThing { InterestingThing(Sensor* sensor, Polar position); InterestingThing(Sensor* sensor, Spherical16 position, - Quaternion orientation = Quaternion::identity); + SwingTwist16 orientation = SwingTwist16::identity); /// @brief Update the position of the object /// @param position The latest known position of the object /// @details This will also update the confidence of the object to the /// maxConfidence value - void Refresh(Polar position); + // void Refresh(Polar position); void Refresh(Spherical16 position, - Quaternion orientation = Quaternion::identity); + SwingTwist16 orientation = SwingTwist16::identity); /// @brief Decrease the confidence based on the elapsed time /// @param deltaTime The time since the last DegradeConfidence call @@ -69,7 +70,7 @@ class InterestingThing { /// @brief The current position of the object Spherical16 position = Spherical16::zero; /// @brief The current orientation of the object - AngleAxisOf orientation = AngleAxisOf(); + SwingTwist16 orientation = SwingTwist16(); // Quaternion orientation = Quaternion::identity; /// @brief The sensor which provided that lastet pose this object Sensor* sensor = nullptr;