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

67
NetworkPerception.cpp
View File

@ -2,7 +2,7 @@
#include "NetworkSync.h" #include "NetworkSync.h"
#define RC_DEBUG true // #define RC_DEBUG true
#if RC_DEBUG #if RC_DEBUG
#include <Arduino.h> #include <Arduino.h>
#endif #endif
@ -82,24 +82,25 @@ void NetworkPerception::ReceivePlane(unsigned char* data, Roboid* roboid) {
// both position and orientation are required // both position and orientation are required
return; return;
Vector3 worldPosition = ReceiveVector3(data, 4); Spherical16 worldPosition = Spherical16::FromVector3(ReceiveVector3(data, 4));
Vector3 roboidPosition = roboid->GetPosition(); Spherical16 roboidPosition = roboid->GetPosition();
Vector3 deltaPosition = worldPosition - roboidPosition; Spherical16 deltaPosition = worldPosition - roboidPosition;
float distance = deltaPosition.magnitude(); float distance = deltaPosition.distance; // magnitude();
if (roboid->perception->IsInteresting(distance) == false) { if (roboid->perception->IsInteresting(distance) == false) {
// printf(" plane not interesting\n"); // printf(" plane not interesting\n");
return; return;
} }
Quaternion roboidOrientation = roboid->GetOrientation(); SwingTwist16 roboidOrientation = roboid->GetOrientation();
Vector3 localPosition = Spherical16 localPosition =
Quaternion::Inverse(roboidOrientation) * deltaPosition; SwingTwist16::Inverse(roboidOrientation) * deltaPosition;
// float angle = // float angle =
// Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up); // Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up);
// Polar position = Polar(angle, distance); // 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, // printf("Received plane (%f %f %f) (%f %f %f) %f %f %f\n", worldPosition.x,
// worldPosition.y, worldPosition.z, roboidPosition.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); // (float)position.horizontalAngle, (float)position.verticalAngle);
// printf("Received plane %f (%f %f)\n", position.distance, // printf("Received plane %f (%f %f)\n", position.distance,
// (float)position.horizontalAngle, (float)position.verticalAngle); // (float)position.horizontalAngle, (float)position.verticalAngle);
roboid->perception->AddTrackedObject(this, position, Quaternion::identity, roboid->perception->AddTrackedObject(this, position, SwingTwist16::identity,
0x80, 0x80, networkId); 0x80, 0x80, networkId);
} }
@ -116,17 +117,18 @@ void NetworkPerception::ReceiveSphere(unsigned char* data, Roboid* roboid) {
float radius = ReceiveFloat100(data, 3); float radius = ReceiveFloat100(data, 3);
Vector3 worldPosition = ReceiveVector3(data, 7); Spherical16 worldPosition = Spherical16::FromVector3(ReceiveVector3(data, 7));
Vector3 roboidPosition = roboid->GetPosition(); Spherical16 roboidPosition = roboid->GetPosition();
Vector3 deltaPosition = worldPosition - roboidPosition; Spherical16 deltaPosition = worldPosition - roboidPosition;
Quaternion roboidOrientation = roboid->GetOrientation(); SwingTwist16 roboidOrientation = roboid->GetOrientation();
Vector3 localPosition = Spherical16 localPosition =
Quaternion::Inverse(roboidOrientation) * deltaPosition; 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); 0x81, 0x81, networkId);
} }
@ -147,19 +149,20 @@ void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) {
return ReceiveSphere(data, roboid); return ReceiveSphere(data, roboid);
#if RC_DEBUG #if RC_DEBUG
// printf("Received PoseMsg [%d/%d]\n", networkId, objectId); printf("Received PoseMsg [%d/%d]\n", networkId, objectId);
#endif #endif
Quaternion roboidOrientation = roboid->GetOrientation(); SwingTwist16 roboidOrientation = roboid->GetOrientation();
Spherical16 position = Spherical16::zero; Spherical16 position = Spherical16::zero;
Quaternion orientation = Quaternion::identity; SwingTwist16 orientation = SwingTwist16::identity;
if ((poseType & NetworkSync::Pose_Position) != 0) { if ((poseType & NetworkSync::Pose_Position) != 0) {
Vector3 worldPosition = ReceiveVector3(data, 4); Spherical16 worldPosition =
Spherical16::FromVector3(ReceiveVector3(data, 4));
if (objectId == 0) { if (objectId == 0) {
roboid->SetPosition(worldPosition); roboid->SetPosition(worldPosition);
} else { } else {
Vector3 roboidPosition = roboid->GetPosition(); Spherical16 roboidPosition = roboid->GetPosition();
// float distance = Vector3::Distance(roboidPosition, worldPosition); // float distance = Vector3::Distance(roboidPosition, worldPosition);
// if (roboid->perception->IsInteresting(distance) == false) { // if (roboid->perception->IsInteresting(distance) == false) {
@ -167,19 +170,29 @@ void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) {
// return; // return;
// } // }
Vector3 localPosition = Quaternion::Inverse(roboidOrientation) * Spherical16 localPosition = SwingTwist16::Inverse(roboidOrientation) *
(worldPosition - roboidPosition); (worldPosition - roboidPosition);
position = Spherical16::FromVector3(localPosition); 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) { if ((poseType & NetworkSync::Pose_Orientation) != 0) {
Vector3 worldAngles = ReceiveVector3(data, 16); 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) { if (objectId == 0) {
roboid->SetOrientation(worldOrientation); roboid->SetOrientation(worldOrientation);
} else { } else {
orientation = Quaternion::Inverse(roboidOrientation) * worldOrientation; orientation = SwingTwist16::Inverse(roboidOrientation) * worldOrientation;
} }
} }

52
NetworkSync.cpp
View File

@ -46,9 +46,6 @@ void NetworkSync::ReceiveNetworkId() {
} }
void NetworkSync::PublishState(Roboid* roboid) { void NetworkSync::PublishState(Roboid* roboid) {
// if (roboid->updated == false)
// return;
SendPose(roboid); SendPose(roboid);
PublishPerception(roboid); PublishPerception(roboid);
} }
@ -129,8 +126,8 @@ void NetworkSync::SendModel(Roboid* roboid) {
unsigned char ix = 0; unsigned char ix = 0;
buffer[ix++] = ModelMsg; buffer[ix++] = ModelMsg;
buffer[ix++] = 0x00; // objectId buffer[ix++] = 0x00; // objectId
Spherical s = Spherical::zero; //(roboid->modelPosition); Spherical16 s = Spherical16::zero; //(roboid->modelPosition);
SendSpherical(buffer, &ix, s); SendSpherical16(buffer, &ix, s);
SendFloat16(buffer, &ix, 1); // roboid->modelScale); SendFloat16(buffer, &ix, 1); // roboid->modelScale);
buffer[ix++] = len; buffer[ix++] = len;
@ -151,8 +148,8 @@ void NetworkSync::SendModel(Thing* thing) {
unsigned char ix = 0; unsigned char ix = 0;
buffer[ix++] = ModelMsg; buffer[ix++] = ModelMsg;
buffer[ix++] = thing->id; // objectId buffer[ix++] = thing->id; // objectId
Spherical s = Spherical::zero; // Spherical(thing->modelPosition); Spherical16 s = Spherical16::zero; // Spherical(thing->modelPosition);
SendSpherical(buffer, &ix, s); SendSpherical16(buffer, &ix, s);
SendFloat16(buffer, &ix, 1); // thing->modelScale); SendFloat16(buffer, &ix, 1); // thing->modelScale);
buffer[ix++] = len; buffer[ix++] = len;
@ -215,13 +212,14 @@ void NetworkSync::SendPose(Thing* thing, bool recurse) {
buffer[ix++] = thing->id; buffer[ix++] = thing->id;
buffer[ix++] = Pose_Position | Pose_Orientation; buffer[ix++] = Pose_Position | Pose_Orientation;
SendSpherical16(buffer, &ix, thing->position); SendSpherical16(buffer, &ix, thing->position);
SendQuat32(buffer, &ix, thing->orientation); SendQuat32(buffer, &ix, thing->orientation.ToQuaternion());
SendBuffer(ix); SendBuffer(ix);
}
#if RC_DEBUG #if RC_DEBUG
if (thing->id == 0)
printf("Sent PoseMsg Thing [%d/%d]\n", networkId, buffer[1]); printf("Sent PoseMsg Thing [%d/%d]\n", networkId, buffer[1]);
#endif #endif
}
if (recurse) { if (recurse) {
for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) { for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) {
@ -298,21 +296,23 @@ void NetworkSync::PublishTrackedObject(Roboid* roboid,
// if (object->parentId != 0) // if (object->parentId != 0)
// return PublishRelativeObject(object); // return PublishRelativeObject(object);
Vector3 roboidPosition = roboid->GetPosition(); Spherical16 roboidPosition = roboid->GetPosition();
Quaternion roboidOrientation = roboid->GetOrientation(); SwingTwist16 roboidOrientation = roboid->GetOrientation();
// Vector3 localPosition = object->position.ToVector3(); // Vector3 localPosition = object->position.ToVector3();
Vector3 localPosition = object->position.ToVector3(); Vector3 localPosition = object->position.ToVector3();
Vector3 worldPosition = roboidPosition + roboidOrientation * localPosition; Spherical16 worldPosition =
Quaternion worldOrientation = roboidPosition + roboidOrientation * object->position; // localPosition;
roboidOrientation * object->orientation.ToQuaternion(); SwingTwist16 worldOrientation =
roboidOrientation * object->orientation; //.ToQuaternion();
unsigned char ix = 0; unsigned char ix = 0;
buffer[ix++] = PoseMsg; // Position2DMsg; buffer[ix++] = PoseMsg; // Position2DMsg;
buffer[ix++] = object->id; // objectId; buffer[ix++] = object->id; // objectId;
buffer[ix++] = Pose_Position | Pose_Orientation; buffer[ix++] = Pose_Position | Pose_Orientation;
SendSpherical(buffer, &ix, Spherical::FromVector3(worldPosition)); SendSpherical16(buffer, &ix,
SendQuat32(buffer, &ix, worldOrientation); worldPosition); // Spherical::FromVector3(worldPosition));
SendQuat32(buffer, &ix, worldOrientation.ToQuaternion());
// SendPolar(buffer, &ix, polar); // 3 bytes // SendPolar(buffer, &ix, polar); // 3 bytes
// SendVector3(buffer, &ix, worldPosition); // SendVector3(buffer, &ix, worldPosition);
// SendQuat32(buffer, &ix, worldOrientation); // SendQuat32(buffer, &ix, worldOrientation);
@ -450,22 +450,22 @@ void NetworkSync::SendPolar(unsigned char* data,
SendSingle100(data, (*startIndex) + 1, p.distance); 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, void NetworkSync::SendSpherical16(unsigned char* data,
unsigned char* startIndex, unsigned char* startIndex,
Spherical16 s) { Spherical16 s) {
SendAngle8(data, (*startIndex)++, s.horizontal.ToFloat()); SendAngle8(data, (*startIndex)++, s.horizontal.InDegrees());
SendAngle8(data, (*startIndex)++, s.vertical.ToFloat()); SendAngle8(data, (*startIndex)++, s.vertical.InDegrees());
SendFloat16(data, startIndex, s.distance); 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, void NetworkSync::SendQuat32(unsigned char* data,
unsigned char* startIndex, unsigned char* startIndex,
const Quaternion q) { const Quaternion q) {

11
NetworkSync.h
View File

@ -75,8 +75,9 @@ class NetworkSync {
void PublishPerception(Roboid* roboid); void PublishPerception(Roboid* roboid);
void PublishTrackedObjects(Roboid* roboid, InterestingThing** objects); void PublishTrackedObjects(Roboid* roboid, InterestingThing** objects);
virtual void SendPosition(Vector3 worldPosition) {}; virtual void SendPosition(Spherical16 worldPosition) {};
virtual void SendPose(Vector3 worldPosition, Quaternion worldOrientation) {}; virtual void SendPose(Spherical16 worldPosition,
SwingTwist16 worldOrientation) {};
// void SendPose(Roboid* roboid, bool recurse = true); // void SendPose(Roboid* roboid, bool recurse = true);
void SendPose(Thing* thing, bool recurse = true); void SendPose(Thing* thing, bool recurse = true);
@ -113,9 +114,9 @@ class NetworkSync {
const Quaternion q); const Quaternion q);
void SendPolar(unsigned char* data, unsigned char* startIndex, Polar p); void SendPolar(unsigned char* data, unsigned char* startIndex, Polar p);
void SendSpherical(unsigned char* data, // void SendSpherical(unsigned char* data,
unsigned char* startIndex, // unsigned char* startIndex,
Spherical s); // Spherical s);
void SendSpherical16(unsigned char* data, void SendSpherical16(unsigned char* data,
unsigned char* startIndex, unsigned char* startIndex,
Spherical16 s); Spherical16 s);

22
Perception.cpp
View File

@ -199,7 +199,7 @@ bool Perception::ObjectNearby(float direction, float range) {
InterestingThing* Perception::AddTrackedObject(Sensor* sensor, InterestingThing* Perception::AddTrackedObject(Sensor* sensor,
Spherical16 position, Spherical16 position,
Quaternion orientation, SwingTwist16 orientation,
unsigned char thingType, unsigned char thingType,
unsigned char thingId, unsigned char thingId,
unsigned char networkId) { unsigned char networkId) {
@ -218,7 +218,7 @@ InterestingThing* Perception::AddTrackedObject(Sensor* sensor,
else { else {
if (thing->IsTheSameAs(this->trackedObjects[thingIx])) { if (thing->IsTheSameAs(this->trackedObjects[thingIx])) {
this->trackedObjects[thingIx]->Refresh( this->trackedObjects[thingIx]->Refresh(
thing->position, thing->orientation.ToQuaternion()); thing->position, thing->orientation); //.ToQuaternion());
delete thing; delete thing;
return this->trackedObjects[thingIx]; return this->trackedObjects[thingIx];
@ -262,7 +262,7 @@ InterestingThing* Perception::AddTrackedObject(Sensor* sensor,
unsigned char networkId, unsigned char networkId,
unsigned char thingId, unsigned char thingId,
Spherical16 position, Spherical16 position,
Quaternion orientation) { SwingTwist16 orientation) {
InterestingThing* thing = FindTrackedObject(networkId, thingId); InterestingThing* thing = FindTrackedObject(networkId, thingId);
if (thing == nullptr) { if (thing == nullptr) {
thing = AddTrackedObject(sensor, position, orientation, 0xFF, thingId, 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 // only rotation around vertical axis is supported for now
float rotationAngle; // float rotationAngle;
Vector3 rotationAxis; // Vector3 rotationAxis;
rotation.ToAngleAxis(&rotationAngle, &rotationAxis); // rotation.ToAngleAxis(&rotationAngle, &rotationAxis);
// Make sure rotation axis is positive // Make sure rotation axis is positive
if (rotationAxis.Up() < 0) // if (rotationAxis.Up() < 0)
rotationAngle = -rotationAngle; // rotationAngle = -rotationAngle;
Angle16 rotationAngle = rotation.swing.horizontal;
for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) { for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
InterestingThing* thing = trackedObjects[thingIx]; InterestingThing* thing = trackedObjects[thingIx];
@ -491,8 +492,7 @@ void Perception::UpdatePose(Quaternion rotation) {
// (float)thing->position.verticalAngle); // (float)thing->position.verticalAngle);
// printf("| rotate %f | ", rotationAngle); // printf("| rotate %f | ", rotationAngle);
thing->position.horizontal = Angle16(Angle16::Normalize( thing->position.horizontal = thing->position.horizontal - rotationAngle;
thing->position.horizontal.ToFloat() - rotationAngle));
// printf("-> %f (%f %f) \n", thing->position.distance, // printf("-> %f (%f %f) \n", thing->position.distance,
// (float)thing->position.horizontalAngle, // (float)thing->position.horizontalAngle,

6
Perception.h
View File

@ -96,7 +96,7 @@ class Perception {
InterestingThing* AddTrackedObject( InterestingThing* AddTrackedObject(
Sensor* sensor, Sensor* sensor,
Spherical16 position, Spherical16 position,
Quaternion orientation = Quaternion::identity, SwingTwist16 orientation = SwingTwist16::identity,
unsigned char objectType = 0xFF, unsigned char objectType = 0xFF,
unsigned char objectId = 0x00, unsigned char objectId = 0x00,
unsigned char networkId = 0x00); unsigned char networkId = 0x00);
@ -106,7 +106,7 @@ class Perception {
unsigned char networkId, unsigned char networkId,
unsigned char objectId, unsigned char objectId,
Spherical16 position, Spherical16 position,
Quaternion orientation = Quaternion::identity); SwingTwist16 orientation = SwingTwist16::identity);
bool IsInteresting(float distance); bool IsInteresting(float distance);
@ -153,7 +153,7 @@ class Perception {
/// @brief Update the orientation of the perceived objecst from the given /// @brief Update the orientation of the perceived objecst from the given
/// roboid rotation /// roboid rotation
/// @param rotation The rotation of the roboid in world space /// @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 /// @brief Objects with a distance closed that this value will be considered
/// nearby. /// nearby.

90
Roboid.cpp
View File

@ -21,9 +21,10 @@ Roboid::Roboid() : Thing(0) {
this->propulsion = nullptr; this->propulsion = nullptr;
this->networkSync = nullptr; this->networkSync = nullptr;
// this->actuation = nullptr; // this->actuation = nullptr;
this->worldPosition = Vector3::zero; this->worldPosition = Spherical16::zero;
this->worldOrientation = SwingTwist16::identity;
// this->worldOrientation = Quaternion::identity; // this->worldOrientation = Quaternion::identity;
this->worldAngleAxis = AngleAxisOf<float>(); // this->worldAngleAxis = AngleAxisOf<float>();
} }
Roboid::Roboid(Propulsion* propulsion) : Roboid() { Roboid::Roboid(Propulsion* propulsion) : Roboid() {
@ -44,13 +45,14 @@ void Roboid::Update(unsigned long currentTimeMs) {
propulsion->Update(currentTimeMs); propulsion->Update(currentTimeMs);
float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000; float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000;
Quaternion roboidOrientation = this->GetOrientation(); SwingTwist16 roboidOrientation = this->GetOrientation();
SetPosition(this->worldPosition + SetPosition(this->worldPosition +
roboidOrientation * Vector3::forward * roboidOrientation * Spherical16::forward *
this->propulsion->GetVelocity().distance * deltaTime); this->propulsion->GetVelocity().distance * deltaTime);
SetOrientation(roboidOrientation * SetOrientation(
Quaternion::AngleAxis(this->propulsion->GetAngularVelocity(), roboidOrientation *
Vector3::up)); SwingTwist16::AngleAxis(this->propulsion->GetAngularVelocity(),
Spherical16::up));
} }
if (childCount > 0 && children != nullptr) { if (childCount > 0 && children != nullptr) {
@ -65,38 +67,39 @@ void Roboid::Update(unsigned long currentTimeMs) {
lastUpdateTimeMs = currentTimeMs; lastUpdateTimeMs = currentTimeMs;
} }
Vector3 Roboid::GetPosition() { Spherical16 Roboid::GetPosition() {
return this->worldPosition; return this->worldPosition;
} }
Vector2 Roboid::GetPosition2D() { // Vector2 Roboid::GetPosition2D() {
return Vector2(this->worldPosition.Right(), this->worldPosition.Forward()); // 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() { // float Roboid::GetOrientation2D() {
Vector3 axis = this->worldAngleAxis.axis.ToVector3(); // float maxAngle = 90 - Float::epsilon; // note: range vertical angle =
Quaternion q = Quaternion::AngleAxis(this->worldAngleAxis.angle, axis); // -90..90
return q;
}
float Roboid::GetOrientation2D() { // // rotation axis is vertical, so we have a simple 2D orientation
float maxAngle = 90 - Float::epsilon; // note: range vertical angle = -90..90 // 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 // SwingTwist16 q = GetOrientation();
if (this->worldAngleAxis.axis.vertical.InDegrees() > maxAngle) // return Quaternion::GetAngleAround(Vector3::up, q);
return this->worldAngleAxis.angle; // }
if (this->worldAngleAxis.axis.vertical.InDegrees() < -maxAngle)
return -this->worldAngleAxis.angle;
Quaternion q = GetOrientation(); void Roboid::SetPosition(Spherical16 newWorldPosition) {
return Quaternion::GetAngleAround(Vector3::up, q); SwingTwist16 roboidOrientation = this->GetOrientation();
} Spherical16 translation = newWorldPosition - this->worldPosition;
float distance = translation.distance;
void Roboid::SetPosition(Vector3 newWorldPosition) { Angle16 angle = Spherical16::SignedAngleBetween(
Quaternion roboidOrientation = this->GetOrientation(); roboidOrientation * Spherical16::forward, translation, Spherical16::up);
Vector3 translation = newWorldPosition - this->worldPosition;
float distance = translation.magnitude();
AngleOf<float> angle = Vector3::SignedAngle(
roboidOrientation * Vector3::forward, translation, Vector3::up);
Polar polarTranslation = Polar(angle.ToFloat(), distance); Polar polarTranslation = Polar(angle.ToFloat(), distance);
if (perception != nullptr) if (perception != nullptr)
perception->UpdatePose(polarTranslation); perception->UpdatePose(polarTranslation);
@ -108,23 +111,24 @@ void Roboid::SetPosition(Vector3 newWorldPosition) {
} }
#include <math.h> #include <math.h>
void Roboid::SetOrientation(Quaternion worldOrientation) { void Roboid::SetOrientation(SwingTwist16 worldOrientation) {
float angle; // float angle;
Vector3 axis; // Vector3 axis;
worldOrientation.ToAngleAxis(&angle, &axis); // worldOrientation.ToAngleAxis(&angle, &axis);
Quaternion delta = Quaternion::Inverse(GetOrientation()) * worldOrientation; SwingTwist16 delta =
SwingTwist16::Inverse(GetOrientation()) * worldOrientation;
if (perception != nullptr) if (perception != nullptr)
perception->UpdatePose(delta); perception->UpdatePose(delta);
AngleAxisOf<float> angleAxis = // AngleAxisOf<float> angleAxis =
AngleAxisOf<float>(angle, DirectionOf<float>(axis)); // AngleAxisOf<float>(angle, DirectionOf<float>(axis));
this->worldAngleAxis = angleAxis; // this->worldAngleAxis = angleAxis;
} }
void Roboid::SetOrientation2D(float angle) { // void Roboid::SetOrientation2D(float angle) {
this->worldAngleAxis = AngleAxisOf<float>(angle, DirectionOf<float>::up); // this->worldAngleAxis = AngleAxisOf<float>(angle, DirectionOf<float>::up);
} // }
Vector3 Passer::RoboidControl::Roboid::GetVelocity() { Vector3 Passer::RoboidControl::Roboid::GetVelocity() {
return Vector3(); 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 /// @details The origin and units of the position depends on the position
/// tracking system used. This value will be Vector3::zero unless a position /// tracking system used. This value will be Vector3::zero unless a position
/// is received through network synchronisation /// is received through network synchronisation
virtual Vector3 GetPosition(); virtual Spherical16 GetPosition();
Vector2 GetPosition2D(); // Vector2 GetPosition2D();
/// @brief Retrieve the current orientation of the roboid /// @brief Retrieve the current orientation of the roboid
/// @return The orientation quaternion in world space /// @return The orientation quaternion in world space
/// @details The origin orientation depends on the position tracking system /// @details The origin orientation depends on the position tracking system
/// used. This value will be Quaternion::identity unless an orientation is /// used. This value will be Quaternion::identity unless an orientation is
/// received though network synchronization /// received though network synchronization
virtual Quaternion GetOrientation(); virtual SwingTwist16 GetOrientation();
float GetOrientation2D(); // float GetOrientation2D();
/// @brief Update the current position of the roboid /// @brief Update the current position of the roboid
/// @param worldPosition The position of the roboid in carthesian coordinates /// @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 /// orientations of the perceived objects by the roboid
/// (roboid->perception->perceivedObjects), as these are local to the /// (roboid->perception->perceivedObjects), as these are local to the
/// roboid's position. /// roboid's position.
virtual void SetPosition(Vector3 worldPosition); virtual void SetPosition(Spherical16 worldPosition);
/// @brief Update the current orientation of the roboid /// @brief Update the current orientation of the roboid
/// @param worldOrientation The orientation of the roboid in world space /// @param worldOrientation The orientation of the roboid in world space
/// @details The use of this function will also update the orientations of the /// @details The use of this function will also update the orientations of the
/// perceived objects by the roboid (roboid->perception->perceivedObjets), /// perceived objects by the roboid (roboid->perception->perceivedObjets),
/// as these are local to the roboid' orientation. /// as these are local to the roboid' orientation.
virtual void SetOrientation(Quaternion worldOrientation); virtual void SetOrientation(SwingTwist16 worldOrientation);
void SetOrientation2D(float angle); // void SetOrientation2D(float angle);
virtual Vector3 GetVelocity(); virtual Vector3 GetVelocity();
@ -77,14 +77,14 @@ class Roboid : public Thing {
/// @details This position may be set when NetworkSync is used to receive /// @details This position may be set when NetworkSync is used to receive
/// positions from an external tracking system. These values should not be set /// positions from an external tracking system. These values should not be set
/// directly, but SetPosition should be used instead. /// directly, but SetPosition should be used instead.
Vector3 worldPosition = Vector3::zero; Spherical16 worldPosition = Spherical16::zero;
/// @brief The orientation of the roboid in world space /// @brief The orientation of the roboid in world space
/// @details The position may be set when NetworkSync is used to receive /// @details The position may be set when NetworkSync is used to receive
/// orientations from an external tracking system. This value should not be /// orientations from an external tracking system. This value should not be
/// set directly, but SetOrientation should be used instead. /// set directly, but SetOrientation should be used instead.
// Quaternion worldOrientation = Quaternion::identity; // Quaternion worldOrientation = Quaternion::identity;
AngleAxis worldAngleAxis = AngleAxis(); // AngleAxis worldAngleAxis = AngleAxis();
unsigned long lastUpdateTimeMs = 0; unsigned long lastUpdateTimeMs = 0;
}; };

5
Thing.h
View File

@ -3,6 +3,7 @@
#include "LinearAlgebra/AngleAxis.h" #include "LinearAlgebra/AngleAxis.h"
#include "LinearAlgebra/Quaternion.h" #include "LinearAlgebra/Quaternion.h"
#include "LinearAlgebra/Spherical.h" #include "LinearAlgebra/Spherical.h"
#include "LinearAlgebra/SwingTwist.h"
namespace Passer { namespace Passer {
namespace RoboidControl { namespace RoboidControl {
@ -49,8 +50,8 @@ class Thing {
/// @brief The orientation of this Thing /// @brief The orientation of this Thing
/// @remark When this Thing has a parent, the orientation is relative to the /// @remark When this Thing has a parent, the orientation is relative to the
/// parent's orientation /// parent's orientation
Quaternion orientation; SwingTwist16 orientation;
Quaternion worldOrientation; SwingTwist16 worldOrientation;
virtual Spherical16 GetLinearVelocity(); virtual Spherical16 GetLinearVelocity();
virtual AngleAxis16 GetAngularVelocity(); virtual AngleAxis16 GetAngularVelocity();

34
TrackedObject.cpp
View File

@ -13,15 +13,15 @@ InterestingThing::InterestingThing(Sensor* sensor, Polar position) {
InterestingThing::InterestingThing(Sensor* sensor, InterestingThing::InterestingThing(Sensor* sensor,
Spherical16 position, Spherical16 position,
Quaternion orientation) { SwingTwist16 orientation) {
this->id = 0; this->id = 0;
this->confidence = maxConfidence; this->confidence = maxConfidence;
this->sensor = sensor; this->sensor = sensor;
this->position = position; this->position = position;
float angle; // float angle;
Vector3 axis; // Vector3 axis;
orientation.ToAngleAxis(&angle, &axis); // orientation.ToAngleAxis(&angle, &axis);
this->orientation = AngleAxisOf<float>(angle, axis); this->orientation = orientation; // AngleAxisOf<float>(angle, axis);
this->updated = true; this->updated = true;
} }
@ -72,20 +72,20 @@ bool InterestingThing::DegradeConfidence(float deltaTime) {
} }
} }
void InterestingThing::Refresh(Polar position) { // void InterestingThing::Refresh(Polar position) {
this->position = // this->position =
Spherical16(position.distance, Angle16(position.angle.ToFloat()), 0); // Spherical16(position.distance, Angle16(position.angle.ToFloat()), 0);
this->confidence = maxConfidence; // this->confidence = maxConfidence;
this->updated = true; // this->updated = true;
} // }
void InterestingThing::Refresh(Spherical16 position, Quaternion orientation) { void InterestingThing::Refresh(Spherical16 position, SwingTwist16 orientation) {
this->position = position; this->position = position;
this->orientation = orientation;
float angle; // float angle;
Vector3 axis; // Vector3 axis;
orientation.ToAngleAxis(&angle, &axis); // orientation.ToAngleAxis(&angle, &axis);
this->orientation = AngleAxisOf<float>(angle, axis); // this->orientation = AngleAxisOf<float>(angle, axis);
this->confidence = maxConfidence; this->confidence = maxConfidence;
this->updated = true; this->updated = true;
} }

9
TrackedObject.h
View File

@ -5,6 +5,7 @@
#include "LinearAlgebra/Polar.h" #include "LinearAlgebra/Polar.h"
#include "LinearAlgebra/Quaternion.h" #include "LinearAlgebra/Quaternion.h"
#include "LinearAlgebra/Spherical.h" #include "LinearAlgebra/Spherical.h"
#include "LinearAlgebra/SwingTwist.h"
#include "Sensor.h" #include "Sensor.h"
namespace Passer { namespace Passer {
@ -19,15 +20,15 @@ class InterestingThing {
InterestingThing(Sensor* sensor, Polar position); InterestingThing(Sensor* sensor, Polar position);
InterestingThing(Sensor* sensor, InterestingThing(Sensor* sensor,
Spherical16 position, Spherical16 position,
Quaternion orientation = Quaternion::identity); SwingTwist16 orientation = SwingTwist16::identity);
/// @brief Update the position of the object /// @brief Update the position of the object
/// @param position The latest known 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 /// @details This will also update the confidence of the object to the
/// maxConfidence value /// maxConfidence value
void Refresh(Polar position); // void Refresh(Polar position);
void Refresh(Spherical16 position, void Refresh(Spherical16 position,
Quaternion orientation = Quaternion::identity); SwingTwist16 orientation = SwingTwist16::identity);
/// @brief Decrease the confidence based on the elapsed time /// @brief Decrease the confidence based on the elapsed time
/// @param deltaTime The time since the last DegradeConfidence call /// @param deltaTime The time since the last DegradeConfidence call
@ -69,7 +70,7 @@ class InterestingThing {
/// @brief The current position of the object /// @brief The current position of the object
Spherical16 position = Spherical16::zero; Spherical16 position = Spherical16::zero;
/// @brief The current orientation of the object /// @brief The current orientation of the object
AngleAxisOf<float> orientation = AngleAxisOf<float>(); SwingTwist16 orientation = SwingTwist16();
// Quaternion orientation = Quaternion::identity; // Quaternion orientation = Quaternion::identity;
/// @brief The sensor which provided that lastet pose this object /// @brief The sensor which provided that lastet pose this object
Sensor* sensor = nullptr; Sensor* sensor = nullptr;