RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Move to Spherical/SwingTwist

Browse Source
This commit is contained in:
Pascal Serrarens 2024-09-20 12:30:21 +02:00
parent 5b4dfbb0ae
commit 58046f96ab
11 changed files with 172 additions and 152 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
LinearAlgebra

@ -1 +1 @@
Subproject commit 3363388a95d8fe9708b615fc2ecdc9474f4930a1
Subproject commit 11259a92a6a802b090679bfb951d09acaa01910e

69
NetworkPerception.cpp
View File

@ -2,7 +2,7 @@
#include "NetworkSync.h"
#define RC_DEBUG true
// #define RC_DEBUG true
#if RC_DEBUG
#include <Arduino.h>
#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;
}
}

58
NetworkSync.cpp
View File

@ -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) {

11
NetworkSync.h
View File

@ -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);

22
Perception.cpp
View File

@ -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,

6
Perception.h
View File

@ -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.

90
Roboid.cpp
View File

@ -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<float>();
// this->worldAngleAxis = AngleAxisOf<float>();
}
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<float> 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 <math.h>
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<float> angleAxis =
AngleAxisOf<float>(angle, DirectionOf<float>(axis));
this->worldAngleAxis = angleAxis;
// AngleAxisOf<float> angleAxis =
// AngleAxisOf<float>(angle, DirectionOf<float>(axis));
// this->worldAngleAxis = angleAxis;
}
void Roboid::SetOrientation2D(float angle) {
this->worldAngleAxis = AngleAxisOf<float>(angle, DirectionOf<float>::up);
}
// void Roboid::SetOrientation2D(float angle) {
// this->worldAngleAxis = AngleAxisOf<float>(angle, DirectionOf<float>::up);
// }
Vector3 Passer::RoboidControl::Roboid::GetVelocity() {
return Vector3();

18
Roboid.h
View File

@ -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;
};

5
Thing.h
View File

@ -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();

34
TrackedObject.cpp
View File

@ -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<float>(angle, axis);
// float angle;
// Vector3 axis;
// orientation.ToAngleAxis(&angle, &axis);
this->orientation = orientation; // AngleAxisOf<float>(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<float>(angle, axis);
this->orientation = orientation;
// float angle;
// Vector3 axis;
// orientation.ToAngleAxis(&angle, &axis);
// this->orientation = AngleAxisOf<float>(angle, axis);
this->confidence = maxConfidence;
this->updated = true;
}

9
TrackedObject.h
View File

@ -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<float> orientation = AngleAxisOf<float>();
SwingTwist16 orientation = SwingTwist16();
// Quaternion orientation = Quaternion::identity;
/// @brief The sensor which provided that lastet pose this object
Sensor* sensor = nullptr;