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