rs = this->sample();
#endif
this->samples().push_back(rs);
-#if NNVERSION>1
+#if NNVERSION>2
+ RRTNode *nn = this->nn(this->iy_, rs, this->cost, '0');
+#elif NNVERSION>1
RRTNode *nn = this->nn(this->iy_, rs, this->cost);
#else
RRTNode *nn = this->nn(this->nodes(), rs, this->cost);
else
this->cost = &KUWATA2008_DCOST;
}
-#if NNVERSION>1
+#if NNVERSION>2
+ RRTNode *nn = this->nn(this->iy_, rs, this->cost, '0');
+#elif NNVERSION>1
RRTNode *nn = this->nn(this->iy_, rs, this->cost);
#else
RRTNode *nn = this->nn(this->nodes(), rs, this->cost);
rs = this->sample();
#endif
this->samples().push_back(rs);
-#if NNVERSION>1
+#if NNVERSION>2
+ RRTNode *nn = this->nn(this->iy_, rs, this->cost, '0');
+#elif NNVERSION>1
RRTNode *nn = this->nn(this->iy_, rs, this->cost);
#else
RRTNode *nn = this->nn(this->nodes(), rs, this->cost);
if (!en_add) {
delete ns;
} else {
-#if NVVERSION>1
+#if NVVERSION>2
+ nvs = this->nv(
+ this->iy_,
+ ns,
+ this->cost,
+ MIN(
+ GAMMA_RRTSTAR(
+ this->nodes().size()),
+ 0.2),
+ '0'); // TODO const
+#elif NVVERSION>1
nvs = this->nv(
this->iy_,
ns,
else
rs = this->sample();
this->samples().push_back(rs);
-#if NNVERSION>1
+#if NNVERSION>2
+ RRTNode *nn = this->nn(this->iy_, rs, this->cost, '0');
+#elif NNVERSION>1
RRTNode *nn = this->nn(this->iy_, rs, this->cost);
#else
RRTNode *nn = this->nn(this->nodes(), rs, this->cost);
// RRT* for first node
RRTNode *ns = steered[0];
{
-#if NVVERSION>1
+#if NVVERSION>2
+ nvs = this->nv(
+ this->iy_,
+ ns,
+ this->cost,
+ MIN(
+ GAMMA_RRTSTAR(
+ this->nodes().size()),
+ 0.2),
+ '0'); // TODO const
+#elif NVVERSION>1
nvs = this->nv(this->iy_, ns, this->cost, MIN(
GAMMA_RRTSTAR(this->nodes().size()),
0.2)); // TODO const
rs = this->sample();
#endif
this->samples().push_back(rs);
-#if NNVERSION>1
+#if NNVERSION>2
+ RRTNode *nn = this->nn(this->iy_, rs, this->cost, '0');
+#elif NNVERSION>1
RRTNode *nn = this->nn(this->iy_, rs, this->cost);
#else
RRTNode *nn = this->nn(this->nodes(), rs, this->cost);
cusps++;
if (cusps > 4)
en_add = false;
-#if NVVERSION>1
+#if NVVERSION>2
+ nvs = this->nv(
+ this->iy_,
+ ns,
+ this->cost,
+ MIN(
+ GAMMA_RRTSTAR(
+ this->nodes().size()),
+ 0.2),
+ '0'); // TODO const
+#elif NVVERSION>1
nvs = this->nv(
this->iy_,
ns,
float T2::goal_cost()
{
std::vector<RRTNode *> nvs;
-#if NVVERSION>1
+#if NVVERSION>2
+ nvs = this->nv(
+ this->iy_,
+ this->goal(),
+ this->cost,
+ MIN(
+ GAMMA_RRTSTAR(
+ this->nodes().size()),
+ 0.2),
+ '0'); // TODO const
+#elif NVVERSION>1
nvs = this->nv(
this->iy_,
this->goal(),
std::vector<RRTNode *> &nodes,
#endif
RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *));
+ float (*cost)(RRTNode *, RRTNode *)
+#if NNVERSION>2
+ , char tree
+#endif
+ );
RRTNode *(*sample)();
std::vector<RRTNode *> (*steer)(
RRTNode *init,
std::vector<RRTNode *> &nodes,
#endif
RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *));
+ float (*cost)(RRTNode *, RRTNode *)
+#if NNVERSION>2
+ , char tree
+#endif
+ );
RRTNode *(*sample)();
std::vector<RRTNode *> (*steer)(
RRTNode *init,
std::vector<RRTNode *> &nodes,
#endif
RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *));
+ float (*cost)(RRTNode *, RRTNode *)
+#if NNVERSION>2
+ , char tree
+#endif
+ );
std::vector<RRTNode *> (*nv)(
#if NVVERSION>1
std::vector<RRTNode *> (&nodes)[IYSIZE],
#endif
RRTNode *node,
float (*cost)(RRTNode *, RRTNode *),
- float dist);
+ float dist
+#if NVVERSION>2
+ , char tree
+#endif
+ );
RRTNode *(*sample)();
std::vector<RRTNode *> (*steer)(
RRTNode *init,
std::vector<RRTNode *> &nodes,
#endif
RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *));
+ float (*cost)(RRTNode *, RRTNode *)
+#if NNVERSION>2
+ , char tree
+#endif
+ );
std::vector<RRTNode *> (*nv)(
#if NVVERSION>1
std::vector<RRTNode *> (&nodes)[IYSIZE],
#endif
RRTNode *node,
float (*cost)(RRTNode *, RRTNode *),
- float dist);
+ float dist
+#if NVVERSION>2
+ , char tree
+#endif
+ );
RRTNode *(*sample)();
std::vector<RRTNode *> (*steer)(
RRTNode *init,
std::vector<RRTNode *> &nodes,
#endif
RRTNode *node,
- float (*cost)(RRTNode *, RRTNode *));
+ float (*cost)(RRTNode *, RRTNode *)
+#if NNVERSION>2
+ , char tree
+#endif
+ );
std::vector<RRTNode *> (*nv)(
#if NVVERSION>1
std::vector<RRTNode *> (&nodes)[IYSIZE],
#endif
RRTNode *node,
float (*cost)(RRTNode *, RRTNode *),
- float dist);
+ float dist
+#if NVVERSION>2
+ , char tree
+#endif
+ );
RRTNode *(*sample)();
std::vector<RRTNode *> (*steer)(
RRTNode *init,