add_library(rrts SHARED
src/rrts.cc
+ src/rrtext7.cc
src/rrtext6.cc
src/rrtext5.cc
src/rrtext4.cc
## Implemented extensions
There is a list of implemented extensions and what they include:
+- `rrtext7.cc`: [K-d tree][] for node storage.
- `rrtext6.cc`: Reeds and Shepp for both -- building and search costs,
- `rrtext5.cc`: different cost for building (Reeds and Shepp) and searching
(Euclidean distance),
(Matej's heuristics).
[cute c2]: https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
+[K-d tree]: https://en.wikipedia.org/wiki/K-d_tree
## Compound extensions
There is a list of classes with reference to extensions used:
#define GRID_MAX_XI ((unsigned int) floor(GRID_WIDTH / GRID)) // min is 0
#define GRID_MAX_YI ((unsigned int) floor(GRID_HEIGHT / GRID)) // min is 0
+// ext7
+#define MAX_DIM 2
+
+/*! \brief Use k-d tree data structure to store RRT nodes.
+
+This approach speeds up the search process for the nearest neighbor and
+the near vertices procedures. This extension implements 2D K-d tree.
+
+\see https://en.wikipedia.org/wiki/K-d_tree
+*/
+class RRTExt7 : public virtual RRTS {
+ private:
+ class KdNode {
+ private:
+ RRTNode *node_ = nullptr;
+ KdNode *left_ = nullptr;
+ KdNode *right_ = nullptr;
+ public:
+ // getter, setter
+ RRTNode *node() const { return this->node_; }
+ KdNode *&left() { return this->left_; }
+ KdNode *&right() { return this->right_; }
+ KdNode(RRTNode *n);
+ };
+ KdNode *kdroot_ = nullptr;
+ void store_node(RRTNode *n, KdNode *&r, int l);
+ public:
+ void init();
+ void deinit();
+ void store_node(RRTNode n);
+ RRTNode *nn(RRTNode &t);
+ std::vector<RRTNode *> nv(RRTNode &t);
+};
+
/*! \brief Reeds and Shepp cost for building and search.
*/
class RRTExt6 : public virtual RRTS {
--- /dev/null
+#include "rrtext.h"
+
+RRTExt7::KdNode::KdNode(RRTNode *n)
+ : node_(n)
+ , left_(nullptr)
+ , right_(nullptr)
+{
+}
+
+void RRTExt7::store_node(RRTNode *n, KdNode *&r, int l)
+{
+ if (r == nullptr)
+ r = new KdNode(n);
+ else if (l % 2 == 0 && n->x() < r->node()->x())
+ store_node(n, r->left(), l + 1);
+ else if (l % 2 == 0)
+ store_node(n, r->right(), l + 1);
+ else if (l % 2 == 1 && n->y() < r->node()->y())
+ store_node(n, r->left(), l + 1);
+ else
+ store_node(n, r->right(), l + 1);
+}
+
+// API
+void RRTExt7::init()
+{
+}
+
+void RRTExt7::deinit()
+{
+}
+
+void RRTExt7::store_node(RRTNode n)
+{
+ RRTS::store_node(n);
+ RRTNode *sn = &this->nodes().back();
+ this->store_node(sn, this->kdroot_, 0);
+}
+
+RRTNode *RRTExt7::nn(RRTNode &t)
+{
+ RRTNode *nn = this->kdroot_->node();
+ return nn;
+}
+
+std::vector<RRTNode *> RRTExt7::nv(RRTNode &t)
+{
+ std::vector<RRTNode *> nv;
+ return nv;
+}