+#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;
+}