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 <algorithm>
00031 #include <unordered_map>
00032 #include <vector>
00033
00034 #include <planner_cspace/cyclic_vec.h>
00035 #include <planner_cspace/planner_3d/motion_cache.h>
00036
00037 void MotionCache::reset(
00038 const float linear_resolution,
00039 const float angular_resolution,
00040 const int range,
00041 const std::function<void(CyclicVecInt<3, 2>, size_t&, size_t&)> gm_addr)
00042 {
00043 const int angle = std::lround(M_PI * 2 / angular_resolution);
00044
00045 page_size_ = angle;
00046 cache_.resize(angle);
00047 for (int syaw = 0; syaw < angle; syaw++)
00048 {
00049 const float yaw = syaw * angular_resolution;
00050 CyclicVecInt<3, 2> d;
00051 for (d[0] = -range; d[0] <= range; d[0]++)
00052 {
00053 for (d[1] = -range; d[1] <= range; d[1]++)
00054 {
00055 if (d[0] == 0 && d[1] == 0)
00056 continue;
00057 if (d.sqlen() > range * range)
00058 continue;
00059 for (d[2] = 0; d[2] < angle; d[2]++)
00060 {
00061 Page page;
00062 const float yaw_e = d[2] * angular_resolution;
00063 const float diff_val[3] =
00064 {
00065 d[0] * linear_resolution,
00066 d[1] * linear_resolution,
00067 d[2] * angular_resolution
00068 };
00069 std::unordered_map<CyclicVecInt<3, 2>, bool, CyclicVecInt<3, 2>> registered;
00070 registered[d] = true;
00071
00072 CyclicVecFloat<3, 2> motion(diff_val[0], diff_val[1], diff_val[2]);
00073 motion.rotate(-syaw * angular_resolution);
00074 const float cos_v = cosf(motion[2]);
00075 const float sin_v = sinf(motion[2]);
00076
00077 const float inter = 1.0 / d.len();
00078
00079 if (fabs(sin_v) < 0.1)
00080 {
00081 for (float i = 0; i < 1.0; i += inter)
00082 {
00083 const float x = diff_val[0] * i;
00084 const float y = diff_val[1] * i;
00085
00086 CyclicVecInt<3, 2> pos(
00087 x / linear_resolution, y / linear_resolution, yaw / angular_resolution);
00088 pos.cycleUnsigned(angle);
00089 if (registered.find(pos) == registered.end())
00090 {
00091 page.motion_.push_back(pos);
00092 registered[pos] = true;
00093 }
00094 }
00095 page.distance_ = d.len();
00096 cache_[syaw][d] = page;
00097 continue;
00098 }
00099
00100 float distf = 0.0;
00101 const float r1 = motion[1] + motion[0] * cos_v / sin_v;
00102 const float r2 = std::copysign(
00103 sqrtf(powf(motion[0], 2.0) + powf(motion[0] * cos_v / sin_v, 2.0)),
00104 motion[0] * sin_v);
00105
00106 float dyaw = yaw_e - yaw;
00107 if (dyaw < -M_PI)
00108 dyaw += 2 * M_PI;
00109 else if (dyaw > M_PI)
00110 dyaw -= 2 * M_PI;
00111
00112 const float cx = d[0] * linear_resolution + r2 * cosf(yaw_e + M_PI / 2);
00113 const float cy = d[1] * linear_resolution + r2 * sinf(yaw_e + M_PI / 2);
00114 const float cx_s = r1 * cosf(yaw + M_PI / 2);
00115 const float cy_s = r1 * sinf(yaw + M_PI / 2);
00116
00117 CyclicVecFloat<3, 2> posf_prev(0, 0, 0);
00118
00119 for (float i = 0; i < 1.0; i += inter)
00120 {
00121 const float r = r1 * (1.0 - i) + r2 * i;
00122 const float cx2 = cx_s * (1.0 - i) + cx * i;
00123 const float cy2 = cy_s * (1.0 - i) + cy * i;
00124 const float cyaw = yaw + i * dyaw;
00125
00126 const float posf_raw[3] =
00127 {
00128 (cx2 - r * cosf(cyaw + M_PI / 2)) / linear_resolution,
00129 (cy2 - r * sinf(cyaw + M_PI / 2)) / linear_resolution,
00130 cyaw / angular_resolution
00131 };
00132 const CyclicVecFloat<3, 2> posf(posf_raw[0], posf_raw[1], posf_raw[2]);
00133 CyclicVecInt<3, 2> pos(posf_raw[0], posf_raw[1], posf_raw[2]);
00134 pos.cycleUnsigned(angle);
00135 if (registered.find(pos) == registered.end())
00136 {
00137 page.motion_.push_back(pos);
00138 registered[pos] = true;
00139 }
00140 distf += (posf - posf_prev).len();
00141 posf_prev = posf;
00142 }
00143 distf += (CyclicVecFloat<3, 2>(d) - posf_prev).len();
00144 page.distance_ = distf;
00145 cache_[syaw][d] = page;
00146 }
00147 }
00148 }
00149
00150 for (auto& cache : cache_[syaw])
00151 {
00152 auto comp = [this, &gm_addr](const CyclicVecInt<3, 2> a, const CyclicVecInt<3, 2> b)
00153 {
00154 size_t a_baddr, a_addr;
00155 size_t b_baddr, b_addr;
00156 gm_addr(a, a_baddr, a_addr);
00157 gm_addr(b, b_baddr, b_addr);
00158 if (a_baddr == b_baddr)
00159 {
00160 return (a_addr < b_addr);
00161 }
00162 return (a_baddr < b_baddr);
00163 };
00164 std::sort(cache.second.motion_.begin(), cache.second.motion_.end(), comp);
00165 }
00166 }
00167 }