decision_control/rrtplanner.cc

   1 /*
   2 This file is part of I am car.
   3
   4 I am car is free software: you can redistribute it and/or modify
   5 it under the terms of the GNU General Public License as published by
   6 the Free Software Foundation, either version 3 of the License, or
   7 (at your option) any later version.
   8
   9 I am car is distributed in the hope that it will be useful,
  10 but WITHOUT ANY WARRANTY; without even the implied warranty of
  11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12 GNU General Public License for more details.
  13
  14 You should have received a copy of the GNU General Public License
  15 along with I am car. If not, see <http://www.gnu.org/licenses/>.
  16 */
  17
  18 #include <iostream>
  19 #include <cstdlib>
  20 #include <ctime>
  21 #include "compile.h"
  22 #include "nn.h"
  23 #include "nv.h"
  24 #include "sample.h"
  25 #include "steer.h"
  26 #include "rrtplanner.h"
  27 #include "cost.h"
  28
  29 #define CATI(a, b) a ## b
  30 #define CAT(a, b) CATI(a, b)
  31 #define KUWATA2008_CCOST CAT(c, CO)
  32 #define KUWATA2008_DCOST CO
  33
  34 LaValle1998::LaValle1998(RRTNode *init, RRTNode *goal):
  35         RRTBase(init, goal),
  36         steer(ST)
  37 {
  38         srand(static_cast<unsigned>(time(0)));
  39 }
  40
  41 bool LaValle1998::next()
  42 {
  43         RRTNode *rs;
  44 #if GOALFIRST > 0
  45         if (this->samples().size() == 0)
  46                 rs = this->goal();
  47         else
  48                 rs = this->sample();
  49 #else
  50         rs = this->sample();
  51 #endif
  52         this->samples().push_back(rs);
  53         RRTNode *nn = this->nn(rs);
  54         RRTNode *pn = nn;
  55         bool en_add = true;
  56         for (auto ns: this->steer(nn, rs)) {
  57                 if (!en_add) {
  58                         delete ns;
  59                 } else {
  60                         this->nodes().push_back(ns);
  61                         this->add_iy(ns);
  62                         pn->add_child(ns, this->cost(pn, ns));
  63                         if (this->collide(pn, ns)) {
  64                                 pn->children().pop_back();
  65                                 ns->remove_parent();
  66                                 this->iy_[IYI(ns->y())].pop_back();
  67                                 en_add = false;
  68                         } else {
  69                                 this->ocost(ns);
  70                                 pn = ns;
  71                                 if (this->goal_found(pn, CO)) {
  72                                         this->tlog(this->findt());
  73                                         en_add = false;
  74                                 }
  75                         }
  76                 }
  77         }
  78         return this->goal_found();
  79 }
  80
  81 Kuwata2008::Kuwata2008(RRTNode *init, RRTNode *goal):
  82         RRTBase(init, goal),
  83         steer(ST)
  84 {
  85         srand(static_cast<unsigned>(time(0)));
  86 }
  87
  88 bool Kuwata2008::next()
  89 {
  90         RRTNode *rs;
  91         if (this->samples().size() == 0) {
  92                 rs = this->goal();
  93         } else {
  94                 rs = this->sample();
  95         }
  96         this->samples().push_back(rs);
  97         float heur = static_cast<float>(rand()) / static_cast<float>(RAND_MAX);
  98         if (this->goal_found()) {
  99                 if (heur < 0.7)
 100                         {}//this->cost = &KUWATA2008_CCOST;
 101                 else
 102                         {}//this->cost = &KUWATA2008_DCOST;
 103         } else {
 104                 if (heur < 0.3)
 105                         {}//this->cost = &KUWATA2008_CCOST;
 106                 else
 107                         {}//this->cost = &KUWATA2008_DCOST;
 108         }
 109         RRTNode *nn = this->nn(rs);
 110         RRTNode *pn = nn;
 111         std::vector<RRTNode *> newly_added;
 112         bool en_add = true;
 113         for (auto ns: this->steer(nn, rs)) {
 114                 if (!en_add) {
 115                         delete ns;
 116                 } else {
 117                         this->nodes().push_back(ns);
 118                         this->add_iy(ns);
 119                         pn->add_child(ns, KUWATA2008_DCOST(pn, ns));
 120                         if (this->collide(pn, ns)) {
 121                                 pn->children().pop_back();
 122                                 ns->remove_parent();
 123                                 this->iy_[IYI(ns->y())].pop_back();
 124                                 en_add = false;
 125                         } else {
 126                                 this->ocost(ns);
 127                                 pn = ns;
 128                                 newly_added.push_back(pn);
 129                                 if (this->goal_found(pn, &KUWATA2008_DCOST)) {
 130                                         this->tlog(this->findt());
 131                                         en_add = false;
 132                                 }
 133                         }
 134                 }
 135         }
 136         if (this->samples().size() <= 1)
 137                 return this->goal_found();
 138         for (auto na: newly_added) {
 139                 pn = na;
 140                 en_add = true;
 141                 for (auto ns: this->steer(na, this->goal())) {
 142                         if (!en_add) {
 143                                 delete ns;
 144                         } else {
 145                                 this->nodes().push_back(ns);
 146                                 this->add_iy(ns);
 147                                 pn->add_child(ns, KUWATA2008_DCOST(pn, ns));
 148                                 if (this->collide(pn, ns)) {
 149                                         pn->children().pop_back();
 150                                         ns->remove_parent();
 151                                         this->iy_[IYI(ns->y())].pop_back();
 152                                         en_add = false;
 153                                 } else {
 154                                         this->ocost(ns);
 155                                         pn = ns;
 156                                         if (this->goal_found(pn,
 157                                                         &KUWATA2008_DCOST)) {
 158                                                 this->tlog(this->findt());
 159                                                 en_add = false;
 160                                         }
 161                                 }
 162                         }
 163                 }
 164         }
 165         return this->goal_found();
 166 }
 167
 168 Karaman2011::Karaman2011(RRTNode *init, RRTNode *goal):
 169         RRTBase(init, goal),
 170         steer(ST)
 171 {
 172         srand(static_cast<unsigned>(time(0)));
 173 }
 174
 175 bool Karaman2011::next()
 176 {
 177         RRTNode *rs;
 178 #if GOALFIRST > 0
 179         if (this->samples().size() == 0)
 180                 rs = this->goal();
 181         else
 182                 rs = this->sample();
 183 #else
 184         rs = this->sample();
 185 #endif
 186         this->samples().push_back(rs);
 187         RRTNode *nn = this->nn(rs);
 188         RRTNode *pn = nn;
 189         std::vector<RRTNode *> nvs;
 190         bool en_add = true;
 191         for (auto ns: this->steer(nn, rs)) {
 192                 if (!en_add) {
 193                         delete ns;
 194                 } else {
 195                         nvs = this->nv(ns,
 196                                         MIN(GAMMA_RRTSTAR(
 197                                                         this->nodes().size()),
 198                                                         0.2));
 199                         this->nodes().push_back(ns);
 200                         this->add_iy(ns);
 201                         // connect
 202                         if (!this->connect(pn, ns, nvs)) {
 203                                 this->iy_[IYI(ns->y())].pop_back();
 204                                 en_add = false;
 205                         } else {
 206                                 // rewire
 207                                 this->rewire(nvs, ns);
 208                                 pn = ns;
 209                                 if (this->goal_found(pn, CO)) {
 210                                         this->tlog(this->findt());
 211                                         en_add = false;
 212                                 }
 213                         }
 214                 }
 215         }
 216         return this->goal_found();
 217 }
 218
 219 bool Karaman2011::connect(
 220                 RRTNode *pn,
 221                 RRTNode *ns,
 222                 std::vector<RRTNode *> nvs)
 223 {
 224         RRTNode *op; // old parent
 225         float od; // old direct cost
 226         float oc; // old cumulative cost
 227         bool connected = false;
 228         pn->add_child(ns, this->cost(pn, ns));
 229         if (this->collide(pn, ns)) {
 230                 pn->children().pop_back();
 231                 ns->remove_parent();
 232         } else {
 233                 this->ocost(ns);
 234                 connected = true;
 235         }
 236         for (auto nv: nvs) {
 237                 if (!connected || (nv->ccost() + this->cost(nv, ns) <
 238                                 ns->ccost())) {
 239                         op = ns->parent();
 240                         od = ns->dcost();
 241                         oc = ns->ccost();
 242                         nv->add_child(ns, this->cost(nv, ns));
 243                         if (this->collide(nv, ns)) {
 244                                 nv->children().pop_back();
 245                                 if (op)
 246                                         ns->parent(op);
 247                                 else
 248                                         ns->remove_parent();
 249                                 ns->dcost(od);
 250                                 ns->ccost(oc);
 251                         } else if (connected) {
 252                                 op->children().pop_back();
 253                         } else {
 254                                 this->ocost(ns);
 255                                 connected = true;
 256                         }
 257                 }
 258         }
 259         return connected;
 260 }
 261
 262 bool Karaman2011::rewire(std::vector<RRTNode *> nvs, RRTNode *ns)
 263 {
 264         RRTNode *op; // old parent
 265         float od; // old direct cost
 266         float oc; // old cumulative cost
 267         for (auto nv: nvs) {
 268                 if (ns->ccost() + this->cost(ns, nv) < nv->ccost()) {
 269                         op = nv->parent();
 270                         od = nv->dcost();
 271                         oc = nv->ccost();
 272                         ns->add_child(nv, this->cost(ns, nv));
 273                         if (this->collide(ns, nv)) {
 274                                 ns->children().pop_back();
 275                                 nv->parent(op);
 276                                 nv->dcost(od);
 277                                 nv->ccost(oc);
 278                         } else {
 279                                 op->rem_child(nv);
 280                         }
 281                 }
 282         }
 283         return true;
 284 }
 285
 286 T1::T1(RRTNode *init, RRTNode *goal):
 287         RRTBase(init, goal),
 288         steer(ST)
 289 {
 290         srand(static_cast<unsigned>(time(0)));
 291 }
 292
 293 bool T1::next()
 294 {
 295         RRTNode *rs;
 296         if (this->samples().size() == 0)
 297                 rs = this->goal();
 298         else
 299                 rs = this->sample();
 300         this->samples().push_back(rs);
 301         RRTNode *nn = this->nn(rs);
 302         RRTNode *pn = nn;
 303         std::vector<RRTNode *> nvs;
 304         bool connected;
 305         RRTNode *op; // old parent
 306         float od; // old direct cost
 307         float oc; // old cumulative cost
 308         std::vector<RRTNode *> steered = this->steer(nn, rs);
 309         // RRT* for first node
 310         RRTNode *ns = steered[0];
 311         {
 312                 nvs = this->nv(ns, MIN(
 313                                 GAMMA_RRTSTAR(this->nodes().size()),
 314                                 0.2)); // TODO const
 315                 this->nodes().push_back(ns);
 316                 this->add_iy(ns);
 317                 connected = false;
 318                 pn->add_child(ns, this->cost(pn, ns));
 319                 if (this->collide(pn, ns)) {
 320                         pn->children().pop_back();
 321                 } else {
 322                         connected = true;
 323                 }
 324                 // connect
 325                 for (auto nv: nvs) {
 326                         if (!connected || (nv->ccost() + this->cost(nv, ns) <
 327                                         ns->ccost())) {
 328                                 op = ns->parent();
 329                                 od = ns->dcost();
 330                                 oc = ns->ccost();
 331                                 nv->add_child(ns, this->cost(nv, ns));
 332                                 if (this->collide(nv, ns)) {
 333                                         nv->children().pop_back();
 334                                         ns->parent(op);
 335                                         ns->dcost(od);
 336                                         ns->ccost(oc);
 337                                 } else if (connected) {
 338                                         op->children().pop_back();
 339                                 } else {
 340                                         connected = true;
 341                                 }
 342                         }
 343                 }
 344                 if (!connected)
 345                         return false;
 346                 // rewire
 347                 for (auto nv: nvs) {
 348                         if (ns->ccost() + this->cost(ns, nv) < nv->ccost()) {
 349                                 op = nv->parent();
 350                                 od = nv->dcost();
 351                                 oc = nv->ccost();
 352                                 ns->add_child(nv, this->cost(ns, nv));
 353                                 if (this->collide(ns, nv)) {
 354                                         ns->children().pop_back();
 355                                         nv->parent(op);
 356                                         nv->dcost(od);
 357                                         nv->ccost(oc);
 358                                 } else {
 359                                         op->rem_child(nv);
 360                                 }
 361                         }
 362                 }
 363                 pn = ns;
 364                 if (this->goal_found(pn, CO)) {
 365                         this->tlog(this->findt());
 366                 }
 367         }
 368         unsigned int i = 0;
 369         for (i = 1; i < steered.size(); i++) {
 370                 ns = steered[i];
 371                 this->nodes().push_back(ns);
 372                 this->add_iy(ns);
 373                 pn->add_child(ns, this->cost(pn, ns));
 374                 if (this->collide(pn, ns)) {
 375                         pn->children().pop_back();
 376                         break;
 377                 }
 378                 pn = ns;
 379                 if (this->goal_found(pn, CO)) {
 380                         this->tlog(this->findt());
 381                         break;
 382                 }
 383         }
 384         return this->goal_found();
 385 }
 386
 387 bool T2::next()
 388 {
 389         RRTNode *rs;
 390 #if GOALFIRST > 0
 391         if (this->samples().size() == 0)
 392                 rs = this->goal();
 393         else
 394                 rs = this->sample();
 395 #else
 396         rs = this->sample();
 397 #endif
 398         this->samples().push_back(rs);
 399         RRTNode *nn = this->nn(rs);
 400         if (!nn)
 401                 return false;
 402         RRTNode *pn = nn;
 403         std::vector<RRTNode *> nvs;
 404         std::vector<RRTNode *> newly_added;
 405         bool en_add = true;
 406         int cusps = 0;
 407         for (auto ns: this->steer(nn, rs)) {
 408                 if (!en_add) {
 409                         delete ns;
 410                 } else if (IS_NEAR(pn, ns)) {
 411                         delete ns;
 412                 } else {
 413                         if (sgn(ns->s()) == 0 || sgn(pn->s()) != sgn(ns->s()))
 414                                 cusps++;
 415                         if (cusps > 4)
 416                                 en_add = false;
 417                         nvs = this->nv(
 418                                         ns,
 419                                         MIN(
 420                                                 GAMMA_RRTSTAR(
 421                                                         this->nodes().size()),
 422                                                 0.2)); // TODO const
 423                         this->nodes().push_back(ns);
 424                         this->add_iy(ns);
 425                         // connect
 426                         if (!this->connect(pn, ns, nvs)) {
 427                                 this->iy_[IYI(ns->y())].pop_back();
 428                                 en_add = false;
 429                         } else {
 430                                 // rewire
 431                                 this->rewire(nvs, ns);
 432                                 pn = ns;
 433                                 newly_added.push_back(pn);
 434                                 if (this->goal_found(pn, CO)) {
 435                                         this->goal_cost();
 436                                         this->tlog(this->findt());
 437                                         en_add = false;
 438                                 }
 439                         }
 440                 }
 441         }
 442         if (this->samples().size() <= 1)
 443                 return this->goal_found();
 444         for (auto na: newly_added) {
 445                 pn = na;
 446                 en_add = true;
 447                 cusps = 0;
 448                 for (auto ns: this->steer(na, this->goal())) {
 449                         if (!en_add) {
 450                                 delete ns;
 451                         } else if (IS_NEAR(pn, ns)) {
 452                                 delete ns;
 453                         } else {
 454                                 if (sgn(pn->s()) != sgn(ns->s()))
 455                                         cusps++;
 456                                 if (cusps > 4)
 457                                         en_add = false;
 458                                 this->nodes().push_back(ns);
 459                                 this->add_iy(ns);
 460                                 pn->add_child(ns, this->cost(pn, ns));
 461                                 if (this->collide(pn, ns)) {
 462                                         pn->children().pop_back();
 463                                         ns->remove_parent();
 464                                         this->iy_[IYI(ns->y())].pop_back();
 465                                         en_add = false;
 466                                 } else {
 467                                         this->ocost(ns);
 468                                         pn = ns;
 469                                         if (this->goal_found(pn, CO)) {
 470                                                 this->goal_cost();
 471                                                 this->tlog(this->findt());
 472                                                 en_add = false;
 473                                         }
 474                                 }
 475                         }
 476                 }
 477         }
 478         return this->goal_found();
 479 }
 480
 481 float T2::goal_cost()
 482 {
 483         std::vector<RRTNode *> nvs;
 484         nvs = this->nv(this->goal(), 0.2);
 485         for (auto nv: nvs) {
 486                 if (std::abs(this->goal()->h() - nv->h()) >=
 487                                 this->GOAL_FOUND_ANGLE)
 488                         continue;
 489                 if (nv->ccost() + this->cost(nv, this->goal()) >=
 490                                 this->goal()->ccost())
 491                         continue;
 492                 RRTNode *op; // old parent
 493                 float oc; // old cumulative cost
 494                 float od; // old direct cost
 495                 op = this->goal()->parent();
 496                 oc = this->goal()->ccost();
 497                 od = this->goal()->dcost();
 498                 nv->add_child(this->goal(),
 499                                 this->cost(nv, this->goal()));
 500                 if (this->collide(nv, this->goal())) {
 501                         nv->children().pop_back();
 502                         this->goal()->parent(op);
 503                         this->goal()->ccost(oc);
 504                         this->goal()->dcost(od);
 505                 } else {
 506                         op->rem_child(this->goal());
 507                 }
 508         }
 509         return this->goal()->ccost();
 510 }
 511
 512 T3::T3(RRTNode *init, RRTNode *goal):
 513         RRTBase(init, goal),
 514         p_root_(init, goal),
 515         p_goal_(goal, init)
 516 {
 517         srand(static_cast<unsigned>(time(0)));
 518 }
 519
 520 bool T3::next()
 521 {
 522         RRTNode *ron = nullptr;
 523         RRTNode *gon = nullptr;
 524         bool ret = false;
 525         ret = this->p_root_.next();
 526         ret &= this->p_goal_.next();
 527         if (this->overlaptrees(&ron, &gon)) {
 528                 if (this->connecttrees(ron, gon))
 529                         this->goal_found(true);
 530                 this->tlog(this->findt());
 531                 ret &= true;
 532         }
 533         return ret;
 534 }
 535
 536 bool T3::link_obstacles(
 537         std::vector<CircleObstacle> *cobstacles,
 538         std::vector<SegmentObstacle> *sobstacles)
 539 {
 540         bool ret = false;
 541         ret = RRTBase::link_obstacles(cobstacles, sobstacles);
 542         ret &= this->p_root_.link_obstacles(cobstacles, sobstacles);
 543         ret &= this->p_goal_.link_obstacles(cobstacles, sobstacles);
 544         return ret;
 545 }
 546
 547 bool T3::connecttrees(RRTNode *rn, RRTNode *gn)
 548 {
 549         while (gn != this->goal()) {
 550                 this->p_root_.nodes().push_back(new RRTNode(
 551                                 gn->x(),
 552                                 gn->y(),
 553                                 gn->h()));
 554                 rn->add_child(
 555                                 this->p_root_.nodes().back(),
 556                                 this->p_root_.cost(
 557                                                 rn,
 558                                                 this->p_root_.nodes().back()));
 559                 rn = this->p_root_.nodes().back();
 560                 gn = gn->parent();
 561         }
 562         rn->add_child(this->goal(), this->p_root_.cost(rn, this->goal()));
 563         return true;
 564 }
 565
 566 bool T3::overlaptrees(RRTNode **ron, RRTNode **gon)
 567 {
 568         for (auto rn: this->p_root_.nodes()) {
 569                 if (rn->parent() == nullptr)
 570                         continue;
 571                 for (auto gn: this->p_goal_.nodes()) {
 572                         if (gn->parent() == nullptr)
 573                                 continue;
 574                         if (IS_NEAR(rn, gn)) {
 575                                 *ron = rn;
 576                                 *gon = gn;
 577                                 return true;
 578                         }
 579                 }
 580         }
 581         return false;
 582 }
 583
 584 Klemm2015::Klemm2015(RRTNode *init, RRTNode *goal):
 585         Karaman2011(init, goal),
 586         steer(ST),
 587         orig_root_(init),
 588         orig_goal_(goal)
 589 {
 590         srand(static_cast<unsigned>(time(0)));
 591         this->root()->tree('R');
 592         this->goal()->tree('G');
 593         this->add_iy(this->goal());
 594 }
 595
 596 bool Klemm2015::next()
 597 {
 598         RRTNode *xn = nullptr;
 599         RRTNode *rs;
 600         int ret = 0;
 601 #if GOALFIRST > 0
 602         if (this->samples().size() == 0)
 603                 rs = this->goal();
 604         else
 605                 rs = this->sample();
 606 #else
 607         rs = this->sample();
 608 #endif
 609         this->samples().push_back(rs);
 610         //std::cerr << "next" << std::endl;
 611         if (this->extendstar1(rs, &xn) != 2) {
 612         //        if (xn) {
 613         //                std::cerr << "- xn: " << xn->x() << ", " << xn->y();
 614         //                std::cerr << std::endl;
 615         //        } else {
 616         //                std::cerr << "- xn: nullptr" << std::endl;
 617         //        }
 618                 this->swap();
 619                 ret = this->connectstar(xn);
 620         } else {
 621                 this->swap();
 622         }
 623         if (ret == 1) {
 624                 this->tlog(this->findt());
 625                 return true;
 626         }
 627         return this->goal_found();
 628 }
 629
 630 int Klemm2015::extendstar1(RRTNode *rs, RRTNode **xn)
 631 {
 632         int ret = 0; // 0 - advanced, 1 - reached, 2 - trapped
 633         char tree = this->root()->tree();
 634         //std::cerr << "extend*1" << std::endl;
 635         //std::cerr << "- tree is " << tree << std::endl;
 636         //std::cerr << "  - rs: " << rs->x() << ", " << rs->y() << std::endl;
 637         //if (xn && *xn) {
 638         //        std::cerr << "  - xn: " << (*xn)->x() << ", " << (*xn)->y();
 639         //        std::cerr << std::endl;
 640         //}
 641         //for (int i = 0; i < IYSIZE; i++) {
 642         //        if (this->iy_[i].size() > 0) {
 643         //                RRTNode *tmpn = this->iy_[i].back();
 644         //                float tmpd = EDIST(tmpn, this->goal());
 645
 646         //                std::cerr << i << ": " << tmpn->x();
 647         //                std::cerr << ", " << tmpn->y();
 648         //                std::cerr << ", " << tmpn->tree();
 649         //                std::cerr << " (" << tmpd << ")";
 650
 651         //                if (tmpn == this->root())
 652         //                        std::cerr << " root";
 653         //                if (tmpn == this->goal())
 654         //                        std::cerr << " goal";
 655         //                std::cerr << std::endl;
 656         //        }
 657         //}
 658         RRTNode *nn = this->nn(rs);
 659         //std::cerr << "  - nn: " << nn->x() << ", " << nn->y() << std::endl;
 660         std::vector<RRTNode *> nvs;
 661         std::vector<RRTNode *> steered = this->steer(nn, rs);
 662         RRTNode *ns = steered[1];
 663         ns->tree(tree);
 664         nvs = this->nv(
 665                         ns,
 666                         MIN(
 667                                 GAMMA_RRTSTAR(
 668                                         this->nodes().size()),
 669                                 0.2)); // TODO const
 670         this->nodes().push_back(ns);
 671         this->add_iy(ns);
 672         // connect
 673         if (!this->connect(nn, ns, nvs)) {
 674                 this->iy_[IYI(ns->y())].pop_back();
 675                 ret = 2;
 676         } else {
 677                 // rewire
 678                 this->rewire(nvs, ns);
 679         }
 680         for (auto n: steered) {
 681                 if (n != steered[1])
 682                         delete n;
 683         }
 684         *xn = ns;
 685         //std::cerr << "  - xn: " << (*xn)->x() << ", " << (*xn)->y();
 686         //std::cerr << std::endl;
 687         return ret;
 688 }
 689
 690 int Klemm2015::extendstarC(RRTNode *rs)
 691 {
 692         int ret = 0; // 0 - advanced, 1 - reached, 2 - trapped
 693         char tree = this->root()->tree();
 694         //std::cerr << "extend*C" << std::endl;
 695         //std::cerr << "- tree is " << tree << std::endl;
 696         //std::cerr << "  - rs: " << rs->x() << ", " << rs->y() << std::endl;
 697         RRTNode *nn = this->nn(rs);
 698         RRTNode *pn = nn;
 699         std::vector<RRTNode *> nvs;
 700         bool en_add = true;
 701         for (auto ns: this->steer(nn, rs)) {
 702                 if (!en_add) {
 703                         delete ns;
 704                 } else {
 705                         nvs = this->nv(
 706                                         ns,
 707                                         MIN(
 708                                                 GAMMA_RRTSTAR(
 709                                                         this->nodes().size()),
 710                                                 0.2)); // TODO const
 711                         this->nodes().push_back(ns);
 712                         this->add_iy(ns);
 713                         // connect
 714                         if (!this->connect(pn, ns, nvs)) {
 715                                 this->iy_[IYI(ns->y())].pop_back();
 716                                 en_add = false;
 717                                 ret = 2;
 718                         } else {
 719                                 // rewire
 720                                 this->rewire(nvs, ns);
 721                                 pn = ns;
 722 if (IS_NEAR(pn, rs)) { // GOAL FOUND !
 723         RRTNode *tmp;
 724         if (this->orig_root_ == this->root()) { // rs is in G tree
 725                 // add all rs parents to pn
 726                 tmp = rs->parent();
 727         } else { // rs is in R tree
 728                 tmp = pn->parent();
 729                 pn = rs;
 730                 this->swap();
 731         }
 732         while (tmp != this->goal()) {
 733                 this->nodes().push_back(new RRTNode(
 734                                 tmp->x(),
 735                                 tmp->y(),
 736                                 tmp->h()));
 737                 this->nodes().back()->s(tmp->s());
 738                 this->nodes().back()->tree('R');
 739                 pn->add_child(
 740                                 this->nodes().back(),
 741                                 this->cost(pn, this->nodes().back()));
 742                 pn = this->nodes().back();
 743                 tmp = tmp->parent();
 744         }
 745         pn->add_child(tmp, this->cost(pn, tmp)); // add goal()
 746         en_add = false;
 747         ret = 1;
 748 }
 749                         }
 750                 }
 751         }
 752         return ret;
 753 }
 754
 755 int Klemm2015::connectstar(RRTNode *x)
 756 {
 757         int ret = 0; // 0 - advanced, 1 - reached, 2 - trapped
 758         //std::cerr << "connect* (start)" << std::endl;
 759         ret = this->extendstarC(x);
 760         //std::cerr << "connect* (end)" << std::endl;
 761         return ret;
 762 }
 763
 764 void Klemm2015::swap()
 765 {
 766         RRTNode *tmp;
 767         tmp = this->root();
 768         this->root(this->goal());
 769         this->goal(tmp);
 770 }