divided_map_state.h

[詳解]

 
 
// Copyright(c) 2023-2025 Design Engineering Laboratory, Saitama University
// Released under the MIT license
// https://opensource.org/licenses/mit-license.php
 
#ifndef DESIGNLAB_DIVIDED_MAP_STATE_H_
#define DESIGNLAB_DIVIDED_MAP_STATE_H_
 
#include <vector>
 
#include "map_state.h"
#include "math_vector3.h"
 
 
namespace designlab
{
 
class DividedMapState final
{
public:
    static constexpr int kDividedMapPointNum{ 4 };
 
    static constexpr float kDividedAreaLength{ MapState::kMapPointDistance * kDividedMapPointNum };
 
    static constexpr int kDividedNum{ 15 };  
 
    static constexpr float kDividedMapMaxX{ kDividedAreaLength * kDividedNum / 2.0f };
 
    static constexpr float kDividedMapMinX{ -kDividedMapMaxX };
 
    static constexpr float kDividedMapMaxY{ kDividedAreaLength * kDividedNum / 2.0f };
 
    static constexpr float kDividedMapMinY{ -kDividedMapMaxY };
 
 
    explicit DividedMapState(float min_z = MapState::kMapMinZ);
 
    void Init(const MapState& map_state, const Vector3 global_robot_com);
 
    void Clear();
 
    constexpr bool IsInMap(const float x, const float y) const
    {
        if (x < global_robot_com_.x + kDividedMapMinX ||
            global_robot_com_.x + kDividedMapMaxX < x)
        {
            return false;
        }
 
        if (y < global_robot_com_.y + kDividedMapMinY ||
            global_robot_com_.y + kDividedMapMaxY < y)
        {
            return false;
        }
 
        return true;
    }
 
    constexpr bool IsInMap(const Vector3& pos) const noexcept
    {
        return IsInMap(pos.x, pos.y);
    }
 
    constexpr int GetDividedMapIndexX(const float pos_x) const noexcept
    {
        return static_cast<int>(
            (pos_x - global_robot_com_.x - kDividedMapMinX) *
            static_cast<float>(kDividedNum) / (kDividedMapMaxX - kDividedMapMinX));
    }
 
    constexpr int GetDividedMapIndexY(const float pos_y) const noexcept
    {
        return static_cast<int>(
            (pos_y - global_robot_com_.y - kDividedMapMinY) *
            static_cast<float>(kDividedNum) / (kDividedMapMaxY - kDividedMapMinY));
    }
 
    static constexpr int ClampDividedMapIndex(const int index) noexcept
    {
        if (index < 0) { return 0; }
        if (kDividedNum <= index) { return kDividedNum - 1; }
 
        return index;
    }
 
    int GetPointNum(int x_index, int y_index) const;
 
    Vector3 GetPointPos(int x_index, int y_index, int divided_map_index) const;
 
    void GetPointVector(int x_index, int y_index, std::vector<Vector3>* point_vec) const;
 
    float GetTopZ(int x_index, int y_index) const;
 
    float GetMapMinZ() const noexcept { return kMapMinZ; }
 
private:
    constexpr int GetDividedMapIndex(const int x_index, const int y_index) const noexcept
    {
        return x_index * kDividedNum + y_index;
    }
 
    constexpr bool IsValidIndex(const int index) const noexcept
    {
        if (index < 0 || kDividedNum <= index)
        {
            return false;
        }
 
        return true;
    }
 
    const float kMapMinZ;  
 
    Vector3 global_robot_com_;  
 
    std::vector<std::vector<Vector3> > divided_map_point_;
 
    std::vector<float> divided_map_top_z_;
 
 
    static_assert(kDividedMapPointNum > 0,
                  "kDividedMapPointNum は正の整数である必要があります．");
 
    static_assert(kDividedAreaLength > 0.0f,
                  "kDividedAreaLengthは正の実数である必要があります．");
 
    static_assert(kDividedNum > 0,
                  "kDividedNum は正の整数である必要があります．");
 
    static_assert(kDividedMapMaxX > kDividedMapMinX,
                  " kDividedMapMaxX は kDividedMapMinX より大きい必要があります．");
 
    static_assert(kDividedMapMaxY > kDividedMapMinY,
                  "kDividedMapMaxY は kDividedMapMinY より大きい必要があります．");
};
 
}  // namespace designlab
 
 
#endif  // DESIGNLAB_DIVIDED_MAP_STATE_H_