init

src/PosTracker.cpp

@@ -0,0 +1,28 @@
+#include "PosTracker.hpp"
+
+using sf::Vector2f;
+using sf::Transform;
+
+void PosTracker::reset(Vector2f initialPos, float initialRot) {
+    transform = transformFromPosRot(initialPos, initialRot);
+}
+
+void PosTracker::update(float newLeftValue, float newRightValue) {
+    auto leftDistance = (newLeftValue - _lastLeftValue) * wheelIncrementDistance();
+    auto rightDistance = (newRightValue - _lastRightValue) * wheelIncrementDistance();
+    updateTransform(leftDistance, rightDistance);
+    _lastLeftValue = newLeftValue;
+    _lastRightValue = newRightValue;
+}
+
+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_;
+}