132 lines
3.9 KiB
C++
132 lines
3.9 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 = Vector3::zero;
|
|
// this->worldOrientation = Quaternion::identity;
|
|
this->worldAngleAxis = AngleAxis<float>();
|
|
}
|
|
|
|
Roboid::Roboid(Propulsion* propulsion) : Roboid() {
|
|
this->propulsion = propulsion;
|
|
if (propulsion != nullptr)
|
|
propulsion->roboid = this;
|
|
}
|
|
|
|
void Roboid::Update(unsigned long currentTimeMs) {
|
|
if (perception != nullptr)
|
|
perception->Update(currentTimeMs);
|
|
|
|
if (propulsion != nullptr) {
|
|
propulsion->Update(currentTimeMs);
|
|
|
|
float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000;
|
|
Quaternion roboidOrientation = this->GetOrientation();
|
|
SetPosition(this->worldPosition +
|
|
roboidOrientation * Vector3::forward *
|
|
this->propulsion->GetVelocity().distance * deltaTime);
|
|
SetOrientation(roboidOrientation *
|
|
Quaternion::AngleAxis(this->propulsion->GetAngularVelocity(),
|
|
Vector3::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;
|
|
}
|
|
|
|
Vector3 Roboid::GetPosition() {
|
|
return this->worldPosition;
|
|
}
|
|
Vector2 Roboid::GetPosition2D() {
|
|
return Vector2(this->worldPosition.Right(), this->worldPosition.Forward());
|
|
}
|
|
|
|
Quaternion Roboid::GetOrientation() {
|
|
Vector3 axis = this->worldAngleAxis.axis.ToVector3();
|
|
Quaternion q =
|
|
Quaternion::AngleAxis(this->worldAngleAxis.angle.ToFloat(), axis);
|
|
return q;
|
|
}
|
|
|
|
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.verticalAngle.ToFloat() > maxAngle)
|
|
return this->worldAngleAxis.angle.ToFloat();
|
|
if (this->worldAngleAxis.axis.verticalAngle.ToFloat() < -maxAngle)
|
|
return -this->worldAngleAxis.angle.ToFloat();
|
|
|
|
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);
|
|
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(Quaternion worldOrientation) {
|
|
float angle;
|
|
Vector3 axis;
|
|
worldOrientation.ToAngleAxis(&angle, &axis);
|
|
|
|
Quaternion delta = Quaternion::Inverse(GetOrientation()) * worldOrientation;
|
|
if (perception != nullptr)
|
|
perception->UpdatePose(delta);
|
|
|
|
AngleAxis<float> angleAxis = AngleAxis<float>(angle, Direction<float>(axis));
|
|
this->worldAngleAxis = angleAxis;
|
|
}
|
|
|
|
void Roboid::SetOrientation2D(float angle) {
|
|
this->worldAngleAxis = AngleAxis<float>(angle, Direction<float>::up);
|
|
}
|
|
|
|
void Roboid::AddChild(Thing* child) {
|
|
Thing::AddChild(child);
|
|
if (child->IsSensor()) {
|
|
Sensor* childSensor = (Sensor*)child;
|
|
this->perception->AddSensor(childSensor);
|
|
}
|
|
}
|