143 lines
4.2 KiB
C++
143 lines
4.2 KiB
C++
#include "Roboid.h"
|
|
|
|
#include "LinearAlgebra/FloatSingle.h"
|
|
#include "NetworkSync.h"
|
|
|
|
#include <string.h>
|
|
|
|
// #define RC_DEBUG
|
|
|
|
#ifdef RC_DEBUG
|
|
#include <Arduino.h>
|
|
#endif
|
|
|
|
Roboid::Roboid() : Thing(0) {
|
|
#ifdef RC_DEBUG
|
|
Serial.begin(115200);
|
|
#endif
|
|
this->type = RoboidType;
|
|
this->perception = new Perception();
|
|
this->perception->roboid = this;
|
|
this->propulsion = nullptr;
|
|
this->networkSync = nullptr;
|
|
// this->actuation = nullptr;
|
|
this->worldPosition = Spherical16::zero;
|
|
this->worldOrientation = SwingTwist16::identity;
|
|
// this->worldOrientation = Quaternion::identity;
|
|
// this->worldAngleAxis = AngleAxisOf<float>();
|
|
}
|
|
|
|
Roboid::Roboid(Propulsion* propulsion) : Roboid() {
|
|
this->propulsion = propulsion;
|
|
if (propulsion != nullptr)
|
|
propulsion->roboid = this;
|
|
}
|
|
|
|
void Passer::RoboidControl::Roboid::SetName(char* name) {
|
|
this->name = name;
|
|
}
|
|
|
|
void Roboid::Update(unsigned long currentTimeMs) {
|
|
if (perception != nullptr)
|
|
perception->Update(currentTimeMs);
|
|
|
|
if (propulsion != nullptr) {
|
|
propulsion->Update(currentTimeMs);
|
|
|
|
float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000;
|
|
|
|
SetPosition(this->worldPosition +
|
|
this->worldOrientation * Spherical16::forward *
|
|
this->propulsion->GetVelocity().distance * deltaTime);
|
|
SetOrientation(this->worldOrientation *
|
|
SwingTwist16::AngleAxis(
|
|
this->propulsion->GetAngularVelocity() * deltaTime,
|
|
Spherical16::up));
|
|
}
|
|
|
|
if (childCount > 0 && children != nullptr) {
|
|
for (unsigned char childIx = 0; childIx < this->childCount; childIx++)
|
|
children[childIx]->Update(currentTimeMs);
|
|
}
|
|
|
|
if (networkSync != nullptr)
|
|
networkSync->NetworkUpdate(this);
|
|
|
|
lastUpdateTimeMs = currentTimeMs;
|
|
}
|
|
|
|
Spherical16 Roboid::GetPosition() {
|
|
return this->worldPosition;
|
|
}
|
|
// 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;
|
|
}
|
|
|
|
// 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;
|
|
|
|
// SwingTwist16 q = GetOrientation();
|
|
// return Quaternion::GetAngleAround(Vector3::up, q);
|
|
// }
|
|
|
|
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);
|
|
this->worldPosition = newWorldPosition;
|
|
|
|
if (networkSync != nullptr)
|
|
// networkSync->SendPosition(this->worldPosition);
|
|
networkSync->SendPose(this->worldPosition, roboidOrientation);
|
|
}
|
|
|
|
#include <math.h>
|
|
void Roboid::SetOrientation(SwingTwist16 worldOrientation) {
|
|
// float angle;
|
|
// Vector3 axis;
|
|
// worldOrientation.ToAngleAxis(&angle, &axis);
|
|
|
|
SwingTwist16 delta =
|
|
SwingTwist16::Inverse(GetOrientation()) * worldOrientation;
|
|
if (perception != nullptr)
|
|
perception->UpdatePose(delta);
|
|
|
|
// 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);
|
|
// }
|
|
|
|
Vector3 Passer::RoboidControl::Roboid::GetVelocity() {
|
|
return Vector3();
|
|
}
|
|
|
|
void Roboid::AddChild(Thing* child) {
|
|
Thing::AddChild(child);
|
|
if (child->IsSensor()) {
|
|
Sensor* childSensor = (Sensor*)child;
|
|
this->perception->AddSensor(childSensor);
|
|
}
|
|
}
|