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_ROTATION_CACHE_H
00031 #define PLANNER_CSPACE_PLANNER_3D_ROTATION_CACHE_H
00032
00033 #include <cmath>
00034 #include <list>
00035 #include <memory>
00036 #include <utility>
00037 #include <vector>
00038
00039 #include <planner_cspace/cyclic_vec.h>
00040
00041 class RotationCache
00042 {
00043 private:
00044 class Page
00045 {
00046 private:
00047 std::unique_ptr<CyclicVecFloat<3, 2>[]> c_;
00048 std::unique_ptr<std::pair<float, float>[]> r_;
00049 CyclicVecInt<3, 2> size_;
00050 int ser_size_;
00051
00052 inline size_t addr(const CyclicVecInt<3, 2>& pos) const
00053 {
00054 size_t addr = pos[2];
00055 for (int i = 1; i >= 0; i--)
00056 addr = addr * size_[i] + pos[i];
00057 return addr;
00058 }
00059
00060 public:
00061 void reset(const CyclicVecInt<3, 2>& size);
00062 inline CyclicVecFloat<3, 2>& motion(const CyclicVecInt<3, 2>& pos)
00063 {
00064 return c_[addr(pos)];
00065 }
00066 inline const CyclicVecFloat<3, 2>& motion(const CyclicVecInt<3, 2>& pos) const
00067 {
00068 return c_[addr(pos)];
00069 }
00070 inline std::pair<float, float>& radiuses(const CyclicVecInt<3, 2>& pos)
00071 {
00072 return r_[addr(pos)];
00073 }
00074 inline const std::pair<float, float>& radiuses(const CyclicVecInt<3, 2>& pos) const
00075 {
00076 return r_[addr(pos)];
00077 }
00078 };
00079
00080 std::vector<Page> pages_;
00081
00082 public:
00083 void reset(const float linear_resolution, const float angular_resolution, const int range);
00084 inline const CyclicVecFloat<3, 2>& getMotion(
00085 const int start_angle,
00086 const CyclicVecInt<3, 2>& end) const
00087 {
00088 return pages_[start_angle].motion(end);
00089 }
00090 inline const std::pair<float, float>& getRadiuses(
00091 const int start_angle,
00092 const CyclicVecInt<3, 2>& end) const
00093 {
00094 return pages_[start_angle].radiuses(end);
00095 }
00096
00097 std::list<CyclicVecFloat<3, 2>> interpolate(
00098 const std::list<CyclicVecInt<3, 2>>& path_grid,
00099 const float interval,
00100 const int local_range) const;
00101 };
00102
00103 #endif // PLANNER_CSPACE_PLANNER_3D_ROTATION_CACHE_H