SteerOptimizer.hpp

[詳解]

//
//    独立ステアリング機構最適化クラス
//
//    Copyright (c) 2022 udonrobo
//
 
#pragma once
 
#include <Udon/Stl/EnableSTL.hpp>
#include <Udon/Types/Polar.hpp>
#include <array>
 
#include "Math.hpp"
 
namespace Udon
{

    class SteerModuleOptimizer
    {

        double offset;

        Udon::Polar optimized;
 
    public:
        SteerModuleOptimizer()
            : offset()
            , optimized()
        {
        }

        void clear()
        {
            offset    = 0;
            optimized = {};
        }

        Udon::Polar operator()(const Udon::Polar& current, const Udon::Polar& target)
        {
            // タイヤのどれも動いていない -> 前回値
            if (target.r == 0)
            {
                optimized.r = 0;
                return optimized;
            }
 
            // atan2 等から求めた角度 [-π ~ π] から無限回転に変換
            const auto infinitemized = WrapAngle(current, /*ref*/ offset, target);
 
            // 旋回方向、ホイール回転方向最適化後
            const auto reversibled = OptimizeSteeringAndWheelRotation(current, infinitemized);
 
            // 最適化値更新
            optimized = reversibled;
 
            return optimized;
        }

        Udon::Polar operator()(const Udon::Polar& target)
        {
            return (*this)(optimized, target);
        }
 
    private:
        static Udon::Polar WrapAngle(const Udon::Polar& current, double& offsetRef, const Udon::Polar& target)
        {
            // 変化角
            const auto dTheta = target.theta + offsetRef - current.theta;
 
            // 変化量がいきなり半周を超えた -> 計算値が-π~π間を通過 -> 一周分オフセットを加減算
            if (dTheta > Udon::Pi)
            {
                offsetRef -= Udon::Pi * 2;
            }
            else if (dTheta < -Udon::Pi)
            {
                offsetRef += Udon::Pi * 2;
            }
            return { target.r, target.theta + offsetRef };
        }

        static Udon::Polar OptimizeSteeringAndWheelRotation(const Polar& current, const Polar& target)
        {
            // 変化角
            const auto dTheta = target.theta - current.theta;
 
            // 90度以上回転するときはホイールを逆回転させ、半周旋回
            if (dTheta > Udon::Pi / 2)
            {
                return { target.r * -1, target.theta - Udon::Pi };
            }
            else if (dTheta < -Udon::Pi / 2)
            {
                return { target.r * -1, target.theta + Udon::Pi };
            }
            else
            {
                return target;
            }
        }
    };

    template <size_t WheelCount = 4>
    class SteerOptimizer
    {

        std::array<SteerModuleOptimizer, WheelCount> modules;
 
    public:
        SteerOptimizer() = default;

        void clear()
        {
            for (auto&& module : modules)
            {
                module.clear();
            }
        }

        inline auto operator()(
            const std::array<Udon::Polar, WheelCount>& current,
            const std::array<Udon::Polar, WheelCount>& target) -> std::array<Udon::Polar, WheelCount>
        {
            std::array<Udon::Polar, WheelCount> optimized;
            for (size_t i = 0; i < WheelCount; ++i)
            {
                optimized.at(i) = modules.at(i)(current.at(i), target.at(i));
            }
            return optimized;
        }

        auto operator()(const std::array<Udon::Polar, WheelCount>& target) -> std::array<Udon::Polar, WheelCount>
        {
            std::array<Udon::Polar, WheelCount> optimized;
            for (size_t i = 0; i < WheelCount; ++i)
            {
                optimized.at(i) = modules.at(i)(target.at(i));
            }
            return optimized;
        }
    };
 
}    // namespace Udon