39 lines
No EOL
1.4 KiB
C++
39 lines
No EOL
1.4 KiB
C++
#include "PosTracker.hpp"
|
|
#include <SFML/System/Vector2.hpp>
|
|
|
|
using sf::Vector2f;
|
|
using sf::Transform;
|
|
|
|
void PosTracker::reset(Vector2f initialPos, float initialRot) {
|
|
transform = transformFromPosRot(initialPos, initialRot);
|
|
}
|
|
|
|
void PosTracker::update(std::size_t value_left, std::size_t value_right) {
|
|
auto leftDistance = (value_left - _lastLeftValue) * wheelIncrementDistance() * _wheel_dir_left;
|
|
auto rightDistance = (value_right - _lastRightValue) * wheelIncrementDistance() * _wheel_dir_right;
|
|
updateTransform(leftDistance, rightDistance);
|
|
_lastLeftValue = value_left;
|
|
_lastRightValue = value_right;
|
|
}
|
|
|
|
void PosTracker::updateTransform(float leftDistance, float rightDistance) {
|
|
float linearStep = (leftDistance + rightDistance) / 2.0;
|
|
float angularStep = (leftDistance - rightDistance) / _wheelDistance * 180.0 / std::numbers::pi;
|
|
|
|
auto orientation = Transform(sf::Transform().rotate(transform.getRotation()));
|
|
auto new_ = transformFromPosRot(
|
|
transform.getPosition() + orientation.transformPoint(sf::Vector2f(linearStep, 0.0)),
|
|
transform.getRotation() + angularStep
|
|
);
|
|
transform = new_;
|
|
}
|
|
|
|
Vector2f PosTracker::get_pos() { return this->transform.getPosition(); }
|
|
float PosTracker::get_rot() { return this->transform.getRotation(); }
|
|
|
|
void PosTracker::hint_right(bool forward) {
|
|
_wheel_dir_right = forward ? 1 : -1;
|
|
}
|
|
void PosTracker::hint_left(bool forward) {
|
|
_wheel_dir_left = forward ? 1 : -1;
|
|
} |