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_CYCLIC_VEC_H
00031 #define PLANNER_CSPACE_CYCLIC_VEC_H
00032
00033 #include <memory>
00034 #define _USE_MATH_DEFINES
00035 #include <cmath>
00036 #include <cfloat>
00037 #include <initializer_list>
00038 #include <list>
00039 #include <map>
00040 #include <unordered_map>
00041 #include <vector>
00042
00043 #include <boost/chrono.hpp>
00044
00045 #include <planner_cspace/reservable_priority_queue.h>
00046
00047 namespace cyclic_vec_type_conversion_rule
00048 {
00049 template <typename T>
00050 void convert(const T val, float& ret)
00051 {
00052 ret = val;
00053 }
00054 inline void convert(const int val, float& ret)
00055 {
00056 ret = val;
00057 }
00058 inline void convert(const float val, int& ret)
00059 {
00060 ret = lroundf(val);
00061 }
00062
00063 inline void normalizeFloatAngle(float& val)
00064 {
00065 if (val > M_PI)
00066 val -= 2 * M_PI;
00067 else if (val < -M_PI)
00068 val += 2 * M_PI;
00069 }
00070 inline void normalizeFloatAngle(int&)
00071 {
00072 }
00073 }
00074
00075 template <int DIM, int NONCYCLIC, typename T>
00076 class CyclicVecBase
00077 {
00078 protected:
00079 static_assert(
00080 std::is_same<float, T>() || std::is_same<int, T>(), "T must be float or int");
00081 T e_[DIM];
00082
00083 template <typename T2, typename... ArgList>
00084 void setElements(const int i, const T2& first, const ArgList&... rest) noexcept
00085 {
00086 assert(i < DIM);
00087
00088 cyclic_vec_type_conversion_rule::convert(first, e_[i]);
00089 setElements(i + 1, rest...);
00090 }
00091 void setElements(const int i) noexcept
00092 {
00093 assert(i == DIM);
00094 }
00095
00096 template <typename... ArgList>
00097 void cycleElements(
00098 const int i,
00099 const int res, const ArgList&... rest)
00100 {
00101 assert(i < DIM);
00102
00103 e_[i] = e_[i] % res;
00104 if (e_[i] < res / 2 - res)
00105 e_[i] += res;
00106 else if (e_[i] >= res / 2)
00107 e_[i] -= res;
00108
00109 cycleElements(i + 1, rest...);
00110 }
00111 void cycleElements(const int i)
00112 {
00113 assert(i == DIM);
00114 }
00115 template <typename... ArgList>
00116 void cycleUnsignedElements(
00117 const int i,
00118 const int res, const ArgList&... rest)
00119 {
00120 assert(i < DIM);
00121
00122 e_[i] = e_[i] % res;
00123 if (e_[i] < 0)
00124 e_[i] += res;
00125
00126 cycleUnsignedElements(i + 1, rest...);
00127 }
00128 void cycleUnsignedElements(const int i)
00129 {
00130 assert(i == DIM);
00131 }
00132
00133 public:
00134 template <typename T2>
00135 explicit CyclicVecBase(const CyclicVecBase<DIM, NONCYCLIC, T2>& c) noexcept
00136 {
00137 for (int i = 0; i < DIM; i++)
00138 cyclic_vec_type_conversion_rule::convert(c[i], e_[i]);
00139 }
00140 template <typename... ArgList>
00141 explicit CyclicVecBase(const float& v, const ArgList&... args) noexcept
00142 {
00143 setElements(0, v, args...);
00144 }
00145 template <typename... ArgList>
00146 explicit CyclicVecBase(const int& v, const ArgList&... args) noexcept
00147 {
00148 setElements(0, v, args...);
00149 }
00150 CyclicVecBase() noexcept
00151 {
00152 }
00153 template <typename T2>
00154 bool operator==(const CyclicVecBase<DIM, NONCYCLIC, T2>& v) const
00155 {
00156 for (int i = 0; i < DIM; i++)
00157 if (v.e_[i] != e_[i])
00158 return false;
00159 return true;
00160 }
00161 template <typename T2>
00162 bool operator!=(const CyclicVecBase<DIM, NONCYCLIC, T2>& v) const
00163 {
00164 return !(*this == v);
00165 }
00166 template <typename T2>
00167 CyclicVecBase<DIM, NONCYCLIC, T2> operator+(const CyclicVecBase<DIM, NONCYCLIC, T2>& v) const
00168 {
00169 CyclicVecBase<DIM, NONCYCLIC, T2> out(*this);
00170 for (int i = 0; i < DIM; i++)
00171 {
00172 out[i] += v[i];
00173 }
00174 return out;
00175 }
00176 template <typename T2>
00177 CyclicVecBase<DIM, NONCYCLIC, T2> operator-(const CyclicVecBase<DIM, NONCYCLIC, T2>& v) const
00178 {
00179 CyclicVecBase<DIM, NONCYCLIC, T2> out(*this);
00180 for (int i = 0; i < DIM; i++)
00181 {
00182 out[i] -= v[i];
00183 }
00184 return out;
00185 }
00186 template <typename T2>
00187 CyclicVecBase<DIM, NONCYCLIC, T2> operator*(const CyclicVecBase<DIM, NONCYCLIC, T2>& v) const
00188 {
00189 CyclicVecBase<DIM, NONCYCLIC, T2> out;
00190 for (int i = 0; i < DIM; i++)
00191 {
00192 out[i] = e_[i] * v[i];
00193 }
00194 return out;
00195 }
00196 float cross2d(const CyclicVecBase<DIM, NONCYCLIC, T>& a) const
00197 {
00198 return (*this)[0] * a[1] - (*this)[1] * a[0];
00199 }
00200 float dot2d(const CyclicVecBase<DIM, NONCYCLIC, T>& a) const
00201 {
00202 return (*this)[0] * a[0] + (*this)[1] * a[1];
00203 }
00204 float distLine2d(
00205 const CyclicVecBase<DIM, NONCYCLIC, T>& a,
00206 const CyclicVecBase<DIM, NONCYCLIC, T>& b) const
00207 {
00208 return (b - a).cross2d((*this) - a) / (b - a).len();
00209 }
00210 float distLinestrip2d(
00211 const CyclicVecBase<DIM, NONCYCLIC, T>& a,
00212 const CyclicVecBase<DIM, NONCYCLIC, T>& b) const
00213 {
00214 const auto to_a = (*this) - a;
00215 if ((b - a).dot2d(to_a) <= 0)
00216 return to_a.len();
00217 const auto to_b = (*this) - b;
00218 if ((a - b).dot2d(to_b) <= 0)
00219 return to_b.len();
00220 return fabs(distLine2d(a, b));
00221 }
00222 T& operator[](const int& x)
00223 {
00224 return e_[x];
00225 }
00226 const T& operator[](const int& x) const
00227 {
00228 return e_[x];
00229 }
00230 T sqlen() const
00231 {
00232 T out = 0;
00233 for (int i = 0; i < NONCYCLIC; i++)
00234 {
00235 out += e_[i] * e_[i];
00236 }
00237 return out;
00238 }
00239 float len() const
00240 {
00241 return sqrtf(sqlen());
00242 }
00243 float gridToLenFactor() const
00244 {
00245 const auto l = len();
00246 return l / static_cast<int>(l);
00247 }
00248 float norm() const
00249 {
00250 float out = 0;
00251 for (int i = 0; i < DIM; i++)
00252 {
00253 out += powf(e_[i], 2.0);
00254 }
00255 return sqrtf(out);
00256 }
00257
00258 void rotate(const float ang)
00259 {
00260 const auto tmp = *this;
00261 const float cos_v = cosf(ang);
00262 const float sin_v = sinf(ang);
00263
00264 cyclic_vec_type_conversion_rule::convert(cos_v * tmp[0] - sin_v * tmp[1], e_[0]);
00265 cyclic_vec_type_conversion_rule::convert(sin_v * tmp[0] + cos_v * tmp[1], e_[1]);
00266 this->e_[2] = tmp[2] + ang;
00267 cyclic_vec_type_conversion_rule::normalizeFloatAngle(this->e_[2]);
00268 }
00269
00270
00271 template <typename... ArgList>
00272 void cycle(const int res, const ArgList&... rest)
00273 {
00274 static_assert(
00275 std::is_same<int, T>(), "cycle is provided only for int");
00276 cycleElements(NONCYCLIC, res, rest...);
00277 }
00278 template <typename... ArgList>
00279 void cycleUnsigned(const int res, const ArgList&... rest)
00280 {
00281 static_assert(
00282 std::is_same<int, T>(), "cycle is provided only for int");
00283 cycleUnsignedElements(NONCYCLIC, res, rest...);
00284 }
00285 void cycle(const CyclicVecBase<DIM, NONCYCLIC, T>& res)
00286 {
00287 static_assert(
00288 std::is_same<int, T>(), "cycle is provided only for int");
00289 for (int i = NONCYCLIC; i < DIM; ++i)
00290 cycleElements(i, res[i]);
00291 }
00292 void cycleUnsigned(const CyclicVecBase<DIM, NONCYCLIC, T>& res)
00293 {
00294 static_assert(
00295 std::is_same<int, T>(), "cycle is provided only for int");
00296 for (int i = NONCYCLIC; i < DIM; ++i)
00297 cycleUnsignedElements(i, res[i]);
00298 }
00299
00300
00301 size_t operator()(const CyclicVecBase& key) const
00302 {
00303 size_t hash = static_cast<size_t>(key.e_[0]);
00304 for (int i = 1; i < DIM; i++)
00305 {
00306 const size_t n = 8 * sizeof(std::size_t) * i / DIM;
00307 const size_t x = static_cast<size_t>(key.e_[i]);
00308 hash ^= (x << n) | (x >> ((8 * sizeof(size_t)) - n));
00309 }
00310 return hash;
00311 }
00312 };
00313
00314 template <int DIM, int NONCYCLIC>
00315 using CyclicVecInt = CyclicVecBase<DIM, NONCYCLIC, int>;
00316 template <int DIM, int NONCYCLIC>
00317 using CyclicVecFloat = CyclicVecBase<DIM, NONCYCLIC, float>;
00318
00319 #endif // PLANNER_CSPACE_CYCLIC_VEC_H