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                         if (IS_NEAR(nv, ns))
 240                                 ns = nv;
 241                         else
 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 {
 289         srand(static_cast<unsigned>(time(0)));
 290 }
 291
 292 bool T1::next()
 293 {
 294         RRTNode *rs;
 295         if (this->samples().size() == 0)
 296                 rs = this->goal();
 297         else
 298                 rs = this->sample();
 299         this->samples().push_back(rs);
 300         RRTNode *nn = this->nn(rs);
 301         RRTNode *pn = nn;
 302         std::vector<RRTNode *> nvs;
 303         bool connected;
 304         RRTNode *op; // old parent
 305         float od; // old direct cost
 306         float oc; // old cumulative cost
 307         std::vector<RRTNode *> steered = this->steer(nn, rs);
 308         // RRT* for first node
 309         RRTNode *ns = steered[0];
 310         {
 311                 nvs = this->nv(ns, MIN(
 312                                 GAMMA_RRTSTAR(this->nodes().size()),
 313                                 0.2)); // TODO const
 314                 this->nodes().push_back(ns);
 315                 this->add_iy(ns);
 316                 connected = false;
 317                 pn->add_child(ns, this->cost(pn, ns));
 318                 if (this->collide(pn, ns)) {
 319                         pn->children().pop_back();
 320                 } else {
 321                         connected = true;
 322                 }
 323                 // connect
 324                 for (auto nv: nvs) {
 325                         if (!connected || (nv->ccost() + this->cost(nv, ns) <
 326                                         ns->ccost())) {
 327                                 op = ns->parent();
 328                                 od = ns->dcost();
 329                                 oc = ns->ccost();
 330                                 nv->add_child(ns, this->cost(nv, ns));
 331                                 if (this->collide(nv, ns)) {
 332                                         nv->children().pop_back();
 333                                         ns->parent(op);
 334                                         ns->dcost(od);
 335                                         ns->ccost(oc);
 336                                 } else if (connected) {
 337                                         op->children().pop_back();
 338                                 } else {
 339                                         connected = true;
 340                                 }
 341                         }
 342                 }
 343                 if (!connected)
 344                         return false;
 345                 // rewire
 346                 for (auto nv: nvs) {
 347                         if (ns->ccost() + this->cost(ns, nv) < nv->ccost()) {
 348                                 op = nv->parent();
 349                                 od = nv->dcost();
 350                                 oc = nv->ccost();
 351                                 ns->add_child(nv, this->cost(ns, nv));
 352                                 if (this->collide(ns, nv)) {
 353                                         ns->children().pop_back();
 354                                         nv->parent(op);
 355                                         nv->dcost(od);
 356                                         nv->ccost(oc);
 357                                 } else {
 358                                         op->rem_child(nv);
 359                                 }
 360                         }
 361                 }
 362                 pn = ns;
 363                 if (this->goal_found(pn, CO)) {
 364                         this->tlog(this->findt());
 365                 }
 366         }
 367         unsigned int i = 0;
 368         for (i = 1; i < steered.size(); i++) {
 369                 ns = steered[i];
 370                 this->nodes().push_back(ns);
 371                 this->add_iy(ns);
 372                 pn->add_child(ns, this->cost(pn, ns));
 373                 if (this->collide(pn, ns)) {
 374                         pn->children().pop_back();
 375                         break;
 376                 }
 377                 pn = ns;
 378                 if (this->goal_found(pn, CO)) {
 379                         this->tlog(this->findt());
 380                         break;
 381                 }
 382         }
 383         return this->goal_found();
 384 }
 385
 386 bool T2::next()
 387 {
 388         RRTNode *rs;
 389 #if GOALFIRST > 0
 390         if (this->samples().size() == 0)
 391                 rs = this->goal();
 392         else
 393                 rs = this->sample();
 394 #else
 395         rs = this->sample();
 396 #endif
 397         this->samples().push_back(rs);
 398         RRTNode *nn = this->nn(rs);
 399         if (!nn)
 400                 return false;
 401         RRTNode *pn = nn;
 402         std::vector<RRTNode *> nvs;
 403         std::vector<RRTNode *> newly_added;
 404         bool en_add = true;
 405         int cusps = 0;
 406         for (auto ns: this->steer(nn, rs)) {
 407                 if (!en_add) {
 408                         delete ns;
 409                 } else if (IS_NEAR(pn, ns)) {
 410                         delete ns;
 411                 } else {
 412                         if (sgn(ns->s()) == 0 || sgn(pn->s()) != sgn(ns->s()))
 413                                 cusps++;
 414                         if (cusps > 4)
 415                                 en_add = false;
 416                         nvs = this->nv(
 417                                         ns,
 418                                         MIN(
 419                                                 GAMMA_RRTSTAR(
 420                                                         this->nodes().size()),
 421                                                 0.2)); // TODO const
 422                         this->nodes().push_back(ns);
 423                         this->add_iy(ns);
 424                         // connect
 425                         if (!this->connect(pn, ns, nvs)) {
 426                                 this->iy_[IYI(ns->y())].pop_back();
 427                                 en_add = false;
 428                         } else {
 429                                 // rewire
 430                                 this->rewire(nvs, ns);
 431                                 pn = ns;
 432                                 newly_added.push_back(pn);
 433                                 if (this->goal_found(pn, CO)) {
 434                                         this->goal_cost();
 435                                         this->tlog(this->findt());
 436                                         en_add = false;
 437                                 }
 438                         }
 439                 }
 440         }
 441         if (this->samples().size() <= 1)
 442                 return this->goal_found();
 443         for (auto na: newly_added) {
 444                 pn = na;
 445                 en_add = true;
 446                 cusps = 0;
 447                 for (auto ns: this->steer(na, this->goal())) {
 448                         if (!en_add) {
 449                                 delete ns;
 450                         } else if (IS_NEAR(pn, ns)) {
 451                                 delete ns;
 452                         } else {
 453                                 if (sgn(pn->s()) != sgn(ns->s()))
 454                                         cusps++;
 455                                 if (cusps > 4)
 456                                         en_add = false;
 457                                 this->nodes().push_back(ns);
 458                                 this->add_iy(ns);
 459                                 pn->add_child(ns, this->cost(pn, ns));
 460                                 if (this->collide(pn, ns)) {
 461                                         pn->children().pop_back();
 462                                         ns->remove_parent();
 463                                         this->iy_[IYI(ns->y())].pop_back();
 464                                         en_add = false;
 465                                 } else {
 466                                         this->ocost(ns);
 467                                         pn = ns;
 468                                         if (this->goal_found(pn, CO)) {
 469                                                 this->goal_cost();
 470                                                 this->tlog(this->findt());
 471                                                 en_add = false;
 472                                         }
 473                                 }
 474                         }
 475                 }
 476         }
 477         return this->goal_found();
 478 }
 479
 480 float T2::goal_cost()
 481 {
 482         std::vector<RRTNode *> nvs;
 483         nvs = this->nv(this->goal(), 0.2);
 484         for (auto nv: nvs) {
 485                 if (std::abs(this->goal()->h() - nv->h()) >=
 486                                 this->GOAL_FOUND_ANGLE)
 487                         continue;
 488                 if (nv->ccost() + this->cost(nv, this->goal()) >=
 489                                 this->goal()->ccost())
 490                         continue;
 491                 RRTNode *op; // old parent
 492                 float oc; // old cumulative cost
 493                 float od; // old direct cost
 494                 op = this->goal()->parent();
 495                 oc = this->goal()->ccost();
 496                 od = this->goal()->dcost();
 497                 nv->add_child(this->goal(),
 498                                 this->cost(nv, this->goal()));
 499                 if (this->collide(nv, this->goal())) {
 500                         nv->children().pop_back();
 501                         this->goal()->parent(op);
 502                         this->goal()->ccost(oc);
 503                         this->goal()->dcost(od);
 504                 } else {
 505                         op->rem_child(this->goal());
 506                 }
 507         }
 508         return this->goal()->ccost();
 509 }
 510
 511 T3::T3(RRTNode *init, RRTNode *goal):
 512         RRTBase(init, goal),
 513         p_root_(init, goal),
 514         p_goal_(goal, init)
 515 {
 516         srand(static_cast<unsigned>(time(0)));
 517 }
 518
 519 bool T3::next()
 520 {
 521         RRTNode *ron = nullptr;
 522         RRTNode *gon = nullptr;
 523         bool ret = false;
 524         ret = this->p_root_.next();
 525         ret &= this->p_goal_.next();
 526         if (this->overlaptrees(&ron, &gon)) {
 527                 if (this->connecttrees(ron, gon))
 528                         this->goal_found(true);
 529                 this->tlog(this->findt());
 530                 ret &= true;
 531         }
 532         return ret;
 533 }
 534
 535 bool T3::link_obstacles(
 536         std::vector<CircleObstacle> *cobstacles,
 537         std::vector<SegmentObstacle> *sobstacles)
 538 {
 539         bool ret = false;
 540         ret = RRTBase::link_obstacles(cobstacles, sobstacles);
 541         ret &= this->p_root_.link_obstacles(cobstacles, sobstacles);
 542         ret &= this->p_goal_.link_obstacles(cobstacles, sobstacles);
 543         return ret;
 544 }
 545
 546 bool T3::connecttrees(RRTNode *rn, RRTNode *gn)
 547 {
 548         while (gn != this->goal()) {
 549                 this->p_root_.nodes().push_back(new RRTNode(
 550                                 gn->x(),
 551                                 gn->y(),
 552                                 gn->h()));
 553                 rn->add_child(
 554                                 this->p_root_.nodes().back(),
 555                                 this->p_root_.cost(
 556                                                 rn,
 557                                                 this->p_root_.nodes().back()));
 558                 rn = this->p_root_.nodes().back();
 559                 gn = gn->parent();
 560         }
 561         rn->add_child(this->goal(), this->p_root_.cost(rn, this->goal()));
 562         return true;
 563 }
 564
 565 bool T3::overlaptrees(RRTNode **ron, RRTNode **gon)
 566 {
 567         for (auto rn: this->p_root_.nodes()) {
 568                 if (rn->parent() == nullptr)
 569                         continue;
 570                 for (auto gn: this->p_goal_.nodes()) {
 571                         if (gn->parent() == nullptr)
 572                                 continue;
 573                         if (IS_NEAR(rn, gn)) {
 574                                 *ron = rn;
 575                                 *gon = gn;
 576                                 return true;
 577                         }
 578                 }
 579         }
 580         return false;
 581 }
 582
 583 Klemm2015::Klemm2015(RRTNode *init, RRTNode *goal):
 584         Karaman2011(init, goal),
 585         orig_root_(init),
 586         orig_goal_(goal)
 587 {
 588         srand(static_cast<unsigned>(time(0)));
 589         this->root()->tree('R');
 590         this->goal()->tree('G');
 591         this->add_iy(this->goal());
 592 }
 593
 594 bool Klemm2015::next()
 595 {
 596         RRTNode *xn = nullptr;
 597         RRTNode *rs;
 598         int ret = 0;
 599 #if GOALFIRST > 0
 600         if (this->samples().size() == 0)
 601                 rs = this->goal();
 602         else
 603                 rs = this->sample();
 604 #else
 605         rs = this->sample();
 606 #endif
 607         this->samples().push_back(rs);
 608         //std::cerr << "next" << std::endl;
 609         if (this->extendstar1(rs, &xn) != 2) {
 610         //        if (xn) {
 611         //                std::cerr << "- xn: " << xn->x() << ", " << xn->y();
 612         //                std::cerr << std::endl;
 613         //        } else {
 614         //                std::cerr << "- xn: nullptr" << std::endl;
 615         //        }
 616                 this->swap();
 617                 ret = this->connectstar(xn);
 618         } else {
 619                 this->swap();
 620         }
 621         if (ret == 1) {
 622                 this->tlog(this->findt());
 623                 return true;
 624         }
 625         return this->goal_found();
 626 }
 627
 628 int Klemm2015::extendstar1(RRTNode *rs, RRTNode **xn)
 629 {
 630         int ret = 0; // 0 - advanced, 1 - reached, 2 - trapped
 631         char tree = this->root()->tree();
 632         //std::cerr << "extend*1" << std::endl;
 633         //std::cerr << "- tree is " << tree << std::endl;
 634         //std::cerr << "  - rs: " << rs->x() << ", " << rs->y() << std::endl;
 635         //if (xn && *xn) {
 636         //        std::cerr << "  - xn: " << (*xn)->x() << ", " << (*xn)->y();
 637         //        std::cerr << std::endl;
 638         //}
 639         //for (int i = 0; i < IYSIZE; i++) {
 640         //        if (this->iy_[i].size() > 0) {
 641         //                RRTNode *tmpn = this->iy_[i].back();
 642         //                float tmpd = EDIST(tmpn, this->goal());
 643
 644         //                std::cerr << i << ": " << tmpn->x();
 645         //                std::cerr << ", " << tmpn->y();
 646         //                std::cerr << ", " << tmpn->tree();
 647         //                std::cerr << " (" << tmpd << ")";
 648
 649         //                if (tmpn == this->root())
 650         //                        std::cerr << " root";
 651         //                if (tmpn == this->goal())
 652         //                        std::cerr << " goal";
 653         //                std::cerr << std::endl;
 654         //        }
 655         //}
 656         RRTNode *nn = this->nn(rs);
 657         //std::cerr << "  - nn: " << nn->x() << ", " << nn->y() << std::endl;
 658         std::vector<RRTNode *> nvs;
 659         std::vector<RRTNode *> steered = this->steer(nn, rs);
 660         RRTNode *ns = steered[1];
 661         ns->tree(tree);
 662         nvs = this->nv(
 663                         ns,
 664                         MIN(
 665                                 GAMMA_RRTSTAR(
 666                                         this->nodes().size()),
 667                                 0.2)); // TODO const
 668         this->nodes().push_back(ns);
 669         this->add_iy(ns);
 670         // connect
 671         if (!this->connect(nn, ns, nvs)) {
 672                 this->iy_[IYI(ns->y())].pop_back();
 673                 ret = 2;
 674         } else {
 675                 // rewire
 676                 this->rewire(nvs, ns);
 677         }
 678         for (auto n: steered) {
 679                 if (n != steered[1])
 680                         delete n;
 681         }
 682         *xn = ns;
 683         //std::cerr << "  - xn: " << (*xn)->x() << ", " << (*xn)->y();
 684         //std::cerr << std::endl;
 685         return ret;
 686 }
 687
 688 int Klemm2015::extendstarC(RRTNode *rs)
 689 {
 690         int ret = 0; // 0 - advanced, 1 - reached, 2 - trapped
 691         char tree = this->root()->tree();
 692         //std::cerr << "extend*C" << std::endl;
 693         //std::cerr << "- tree is " << tree << std::endl;
 694         //std::cerr << "  - rs: " << rs->x() << ", " << rs->y() << std::endl;
 695         RRTNode *nn = this->nn(rs);
 696         RRTNode *pn = nn;
 697         std::vector<RRTNode *> nvs;
 698         bool en_add = true;
 699         for (auto ns: this->steer(nn, rs)) {
 700                 if (!en_add) {
 701                         delete ns;
 702                 } else {
 703                         nvs = this->nv(
 704                                         ns,
 705                                         MIN(
 706                                                 GAMMA_RRTSTAR(
 707                                                         this->nodes().size()),
 708                                                 0.2)); // TODO const
 709                         this->nodes().push_back(ns);
 710                         this->add_iy(ns);
 711                         // connect
 712                         if (!this->connect(pn, ns, nvs)) {
 713                                 this->iy_[IYI(ns->y())].pop_back();
 714                                 en_add = false;
 715                                 ret = 2;
 716                         } else {
 717                                 // rewire
 718                                 this->rewire(nvs, ns);
 719                                 pn = ns;
 720 if (IS_NEAR(pn, rs)) { // GOAL FOUND !
 721         RRTNode *tmp;
 722         if (this->orig_root_ == this->root()) { // rs is in G tree
 723                 // add all rs parents to pn
 724                 tmp = rs->parent();
 725         } else { // rs is in R tree
 726                 tmp = pn->parent();
 727                 pn = rs;
 728                 this->swap();
 729         }
 730         while (tmp != this->goal()) {
 731                 this->nodes().push_back(new RRTNode(
 732                                 tmp->x(),
 733                                 tmp->y(),
 734                                 tmp->h()));
 735                 this->nodes().back()->s(tmp->s());
 736                 this->nodes().back()->tree('R');
 737                 pn->add_child(
 738                                 this->nodes().back(),
 739                                 this->cost(pn, this->nodes().back()));
 740                 pn = this->nodes().back();
 741                 tmp = tmp->parent();
 742         }
 743         pn->add_child(tmp, this->cost(pn, tmp)); // add goal()
 744         en_add = false;
 745         ret = 1;
 746 }
 747                         }
 748                 }
 749         }
 750         return ret;
 751 }
 752
 753 int Klemm2015::connectstar(RRTNode *x)
 754 {
 755         int ret = 0; // 0 - advanced, 1 - reached, 2 - trapped
 756         //std::cerr << "connect* (start)" << std::endl;
 757         ret = this->extendstarC(x);
 758         //std::cerr << "connect* (end)" << std::endl;
 759         return ret;
 760 }
 761
 762 void Klemm2015::swap()
 763 {
 764         RRTNode *tmp;
 765         tmp = this->root();
 766         this->root(this->goal());
 767         this->goal(tmp);
 768 }