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