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 <string>
#include <vector>
 
#include "map_state.h"
#include "math_vector3.h"
#include "my_expected.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 noexcept {
    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;
  }
 
  nostd::expected<int, std::string> GetPointNum(int x_index, int y_index) const;
 
  nostd::expected<Vector3, std::string> 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_