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 #include <cmath>
00031 #include <memory>
00032 #include <utility>
00033 #include <vector>
00034
00035 #include <planner_cspace/cyclic_vec.h>
00036
00037 #include <planner_cspace/planner_3d/rotation_cache.h>
00038
00039 void RotationCache::Page::reset(const CyclicVecInt<3, 2>& size)
00040 {
00041 size_t ser_size = 1;
00042 for (int i = 0; i < 3; i++)
00043 ser_size *= size[i];
00044
00045 size_ = size;
00046 ser_size_ = ser_size;
00047
00048 c_.reset(new CyclicVecFloat<3, 2>[ser_size]);
00049 r_.reset(new std::pair<float, float>[ser_size]);
00050 }
00051
00052 void RotationCache::reset(
00053 const float linear_resolution,
00054 const float angular_resolution,
00055 const int range)
00056 {
00057 const int angle = std::lround(M_PI * 2 / angular_resolution);
00058
00059 pages_.resize(angle);
00060 for (int i = 0; i < angle; i++)
00061 {
00062 Page& r = pages_[i];
00063 r.reset(CyclicVecInt<3, 2>(range * 2 + 1, range * 2 + 1, angle));
00064
00065 CyclicVecInt<3, 2> d;
00066
00067 for (d[0] = 0; d[0] <= range * 2; d[0]++)
00068 {
00069 for (d[1] = 0; d[1] <= range * 2; d[1]++)
00070 {
00071 for (d[2] = 0; d[2] < angle; d[2]++)
00072 {
00073 auto v = CyclicVecFloat<3, 2>(
00074 (d[0] - range) * linear_resolution,
00075 (d[1] - range) * linear_resolution,
00076 d[2] * angular_resolution);
00077 v.rotate(-i * angular_resolution);
00078 r.motion(d) = v;
00079
00080 const float sin_v = std::sin(v[2]);
00081 const float cos_v = std::cos(v[2]);
00082 const float r1 = v[1] + v[0] * cos_v / sin_v;
00083 const float r2 = std::copysign(
00084 sqrtf(powf(v[0], 2.0) + powf(v[0] * cos_v / sin_v, 2.0)),
00085 v[0] * sin_v);
00086 r.radiuses(d) = std::pair<float, float>(r1, r2);
00087 }
00088 }
00089 }
00090 }
00091 }