Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #ifndef PLANNER_CSPACE_PLANNER_3D_MOTION_CACHE_H
00031 #define PLANNER_CSPACE_PLANNER_3D_MOTION_CACHE_H
00032
00033 #include <unordered_map>
00034 #include <vector>
00035
00036 #include <planner_cspace/cyclic_vec.h>
00037
00038 class MotionCache
00039 {
00040 public:
00041 class Page
00042 {
00043 protected:
00044 friend class MotionCache;
00045
00046 std::vector<CyclicVecInt<3, 2>> motion_;
00047 float distance_;
00048
00049 public:
00050 inline float getDistance() const
00051 {
00052 return distance_;
00053 }
00054 const std::vector<CyclicVecInt<3, 2>>& getMotion() const
00055 {
00056 return motion_;
00057 }
00058 };
00059
00060 using Cache =
00061 std::unordered_map<CyclicVecInt<3, 2>, Page, CyclicVecInt<3, 2>>;
00062
00063 using Ptr = std::shared_ptr<MotionCache>;
00064
00065 inline const typename Cache::const_iterator find(
00066 const int start_yaw,
00067 const CyclicVecInt<3, 2>& goal) const
00068 {
00069 int i = start_yaw % page_size_;
00070 if (i < 0)
00071 i += page_size_;
00072 return cache_[i].find(goal);
00073 }
00074 inline const typename Cache::const_iterator end(
00075 const int start_yaw) const
00076 {
00077 int i = start_yaw % page_size_;
00078 if (i < 0)
00079 i += page_size_;
00080 return cache_[i].cend();
00081 }
00082
00083 void reset(
00084 const float linear_resolution,
00085 const float angular_resolution,
00086 const int range,
00087 const std::function<void(CyclicVecInt<3, 2>, size_t&, size_t&)> gm_addr);
00088
00089 protected:
00090 std::vector<Cache> cache_;
00091 int page_size_;
00092 };
00093
00094 #endif // PLANNER_CSPACE_PLANNER_3D_MOTION_CACHE_H