Pose2D.hpp

Go to the documentation of this file.

#pragma once
 
#include <cmath>
#include <numbers>
 
namespace libstp::sim
{
    struct Vec2
    {
        float x{0.0f};
        float y{0.0f};
    };

    struct Pose2D
    {
        float x{0.0f};
        float y{0.0f};
        float theta{0.0f};
    };
 
    inline float normalizeAngle(float angle) noexcept
    {
        constexpr float kPi = std::numbers::pi_v<float>;
        constexpr float kTwoPi = 2.0f * kPi;
        while (angle > kPi) angle -= kTwoPi;
        while (angle < -kPi) angle += kTwoPi;
        return angle;
    }

    inline Pose2D applyLocalDelta(const Pose2D& pose, float dxLocal, float dyLocal, float dTheta) noexcept
    {
        const float c = std::cos(pose.theta);
        const float s = std::sin(pose.theta);
        return {
            pose.x + dxLocal * c - dyLocal * s,
            pose.y + dxLocal * s + dyLocal * c,
            normalizeAngle(pose.theta + dTheta),
        };
    }
 
    inline Pose2D forwardMove(const Pose2D& pose, float distanceCm) noexcept
    {
        return {
            pose.x + distanceCm * std::cos(pose.theta),
            pose.y + distanceCm * std::sin(pose.theta),
            pose.theta,
        };
    }
 
    inline Pose2D strafeMove(const Pose2D& pose, float distanceCm) noexcept
    {
        constexpr float kHalfPi = std::numbers::pi_v<float> / 2.0f;
        const float perp = pose.theta + kHalfPi;
        return {
            pose.x + distanceCm * std::cos(perp),
            pose.y + distanceCm * std::sin(perp),
            pose.theta,
        };
    }
 
    inline Pose2D rotate(const Pose2D& pose, float angleRad) noexcept
    {
        return {pose.x, pose.y, normalizeAngle(pose.theta + angleRad)};
    }
}