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()
 167 {
 168         srand(static_cast<unsigned>(time(0)));
 169 }
 170
 171 Karaman2011::Karaman2011(RRTNode *init, RRTNode *goal):
 172         RRTBase(init, goal)
 173 {
 174         srand(static_cast<unsigned>(time(0)));
 175 }
 176
 177 bool Karaman2011::next()
 178 {
 179         RRTNode *rs;
 180 #if GOALFIRST > 0
 181         if (this->samples().size() == 0)
 182                 rs = this->goal();
 183         else
 184                 rs = this->sample();
 185 #else
 186         rs = this->sample();
 187 #endif
 188         this->samples().push_back(rs);
 189         RRTNode *nn = this->nn(rs);
 190         RRTNode *pn = nn;
 191         std::vector<RRTNode *> nvs;
 192         bool en_add = true;
 193         bool skip = false;
 194         for (auto ns: this->steer(nn, rs)) {
 195                 if (!en_add) {
 196                         delete ns;
 197                 } else if (IS_NEAR(nn, ns)) {
 198                         delete ns;
 199                 } else {
 200                         if (IS_NEAR(ns, rs))
 201                                 en_add = false;
 202                         nvs = this->nv(ns,
 203                                         MIN(GAMMA_RRTSTAR(
 204                                                         this->nodes().size()),
 205                                                         0.2));
 206                         skip = false;
 207                         for (auto nv: nvs) {
 208                                 if (IS_NEAR(nv, ns))
 209                                         skip = true;
 210                         }
 211                         if (skip)
 212                                 continue;
 213                         this->nodes().push_back(ns);
 214                         this->add_iy(ns);
 215                         // connect
 216                         if (!this->connect(pn, ns, nvs)) {
 217                                 this->iy_[IYI(ns->y())].pop_back();
 218                                 en_add = false;
 219                         } else {
 220                                 // rewire
 221                                 this->rewire(nvs, ns);
 222                                 pn = ns;
 223                                 if (this->goal_found(pn, CO)) {
 224                                         this->tlog(this->findt());
 225                                         en_add = false;
 226                                 }
 227                         }
 228                 }
 229         }
 230         return this->goal_found();
 231 }
 232
 233 bool Karaman2011::connect(
 234                 RRTNode *pn,
 235                 RRTNode *ns,
 236                 std::vector<RRTNode *> nvs)
 237 {
 238         RRTNode *op; // old parent
 239         float od; // old direct cost
 240         float oc; // old cumulative cost
 241         bool connected = false;
 242         pn->add_child(ns, this->cost(pn, ns));
 243         if (this->collide(pn, ns)) {
 244                 pn->children().pop_back();
 245                 ns->remove_parent();
 246         } else {
 247                 this->ocost(ns);
 248                 connected = true;
 249         }
 250         for (auto nv: nvs) {
 251                 if (!connected || (nv->ccost() + this->cost(nv, ns) <
 252                                 ns->ccost())) {
 253                         op = ns->parent();
 254                         od = ns->dcost();
 255                         oc = ns->ccost();
 256                         nv->add_child(ns, this->cost(nv, ns));
 257                         if (this->collide(nv, ns)) {
 258                                 nv->children().pop_back();
 259                                 if (op)
 260                                         ns->parent(op);
 261                                 else
 262                                         ns->remove_parent();
 263                                 ns->dcost(od);
 264                                 ns->ccost(oc);
 265                         } else if (connected) {
 266                                 op->children().pop_back();
 267                         } else {
 268                                 this->ocost(ns);
 269                                 connected = true;
 270                         }
 271                 }
 272         }
 273         return connected;
 274 }
 275
 276 bool Karaman2011::rewire(std::vector<RRTNode *> nvs, RRTNode *ns)
 277 {
 278         RRTNode *op; // old parent
 279         float od; // old direct cost
 280         float oc; // old cumulative cost
 281         for (auto nv: nvs) {
 282                 if (ns->ccost() + this->cost(ns, nv) < nv->ccost()) {
 283                         op = nv->parent();
 284                         od = nv->dcost();
 285                         oc = nv->ccost();
 286                         ns->add_child(nv, this->cost(ns, nv));
 287                         if (this->collide(ns, nv)) {
 288                                 ns->children().pop_back();
 289                                 nv->parent(op);
 290                                 nv->dcost(od);
 291                                 nv->ccost(oc);
 292                         } else {
 293                                 op->rem_child(nv);
 294                         }
 295                 }
 296         }
 297         return true;
 298 }
 299
 300 T1::T1(RRTNode *init, RRTNode *goal):
 301         RRTBase(init, goal)
 302 {
 303         srand(static_cast<unsigned>(time(0)));
 304 }
 305
 306 bool T1::next()
 307 {
 308         RRTNode *rs;
 309         if (this->samples().size() == 0)
 310                 rs = this->goal();
 311         else
 312                 rs = this->sample();
 313         this->samples().push_back(rs);
 314         RRTNode *nn = this->nn(rs);
 315         RRTNode *pn = nn;
 316         std::vector<RRTNode *> nvs;
 317         bool connected;
 318         RRTNode *op; // old parent
 319         float od; // old direct cost
 320         float oc; // old cumulative cost
 321         std::vector<RRTNode *> steered = this->steer(nn, rs);
 322         // RRT* for first node
 323         RRTNode *ns = steered[0];
 324         {
 325                 nvs = this->nv(ns, MIN(
 326                                 GAMMA_RRTSTAR(this->nodes().size()),
 327                                 0.2)); // TODO const
 328                 this->nodes().push_back(ns);
 329                 this->add_iy(ns);
 330                 connected = false;
 331                 pn->add_child(ns, this->cost(pn, ns));
 332                 if (this->collide(pn, ns)) {
 333                         pn->children().pop_back();
 334                 } else {
 335                         connected = true;
 336                 }
 337                 // connect
 338                 for (auto nv: nvs) {
 339                         if (!connected || (nv->ccost() + this->cost(nv, ns) <
 340                                         ns->ccost())) {
 341                                 op = ns->parent();
 342                                 od = ns->dcost();
 343                                 oc = ns->ccost();
 344                                 nv->add_child(ns, this->cost(nv, ns));
 345                                 if (this->collide(nv, ns)) {
 346                                         nv->children().pop_back();
 347                                         ns->parent(op);
 348                                         ns->dcost(od);
 349                                         ns->ccost(oc);
 350                                 } else if (connected) {
 351                                         op->children().pop_back();
 352                                 } else {
 353                                         connected = true;
 354                                 }
 355                         }
 356                 }
 357                 if (!connected)
 358                         return false;
 359                 // rewire
 360                 for (auto nv: nvs) {
 361                         if (ns->ccost() + this->cost(ns, nv) < nv->ccost()) {
 362                                 op = nv->parent();
 363                                 od = nv->dcost();
 364                                 oc = nv->ccost();
 365                                 ns->add_child(nv, this->cost(ns, nv));
 366                                 if (this->collide(ns, nv)) {
 367                                         ns->children().pop_back();
 368                                         nv->parent(op);
 369                                         nv->dcost(od);
 370                                         nv->ccost(oc);
 371                                 } else {
 372                                         op->rem_child(nv);
 373                                 }
 374                         }
 375                 }
 376                 pn = ns;
 377                 if (this->goal_found(pn, CO)) {
 378                         this->tlog(this->findt());
 379                 }
 380         }
 381         unsigned int i = 0;
 382         for (i = 1; i < steered.size(); i++) {
 383                 ns = steered[i];
 384                 this->nodes().push_back(ns);
 385                 this->add_iy(ns);
 386                 pn->add_child(ns, this->cost(pn, ns));
 387                 if (this->collide(pn, ns)) {
 388                         pn->children().pop_back();
 389                         break;
 390                 }
 391                 pn = ns;
 392                 if (this->goal_found(pn, CO)) {
 393                         this->tlog(this->findt());
 394                         break;
 395                 }
 396         }
 397         return this->goal_found();
 398 }
 399
 400 bool T2::next()
 401 {
 402         RRTNode *rs;
 403 #if GOALFIRST > 0
 404         if (this->samples().size() == 0)
 405                 rs = this->goal();
 406         else
 407                 rs = this->sample();
 408 #else
 409         rs = this->sample();
 410 #endif
 411         this->samples().push_back(rs);
 412         RRTNode *nn = this->nn(rs);
 413         if (!nn)
 414                 return false;
 415         RRTNode *pn = nn;
 416         std::vector<RRTNode *> nvs;
 417         std::vector<RRTNode *> newly_added;
 418         bool en_add = true;
 419         int cusps = 0;
 420         for (auto ns: this->steer(nn, rs)) {
 421                 ns->rs(rs);
 422                 if (!en_add) {
 423                         delete ns;
 424                 } else if (IS_NEAR(pn, ns)) {
 425                         delete ns;
 426                 } else {
 427                         if (sgn(ns->s()) == 0 || sgn(pn->s()) != sgn(ns->s()))
 428                                 cusps++;
 429                         if (cusps > 4)
 430                                 en_add = false;
 431                         nvs = this->nv(
 432                                         ns,
 433                                         MIN(
 434                                                 GAMMA_RRTSTAR(
 435                                                         this->nodes().size()),
 436                                                 0.2)); // TODO const
 437                         this->nodes().push_back(ns);
 438                         this->add_iy(ns);
 439                         // connect
 440                         if (!this->connect(pn, ns, nvs)) {
 441                                 this->iy_[IYI(ns->y())].pop_back();
 442                                 this->nodes().pop_back();
 443                                 delete ns;
 444                                 en_add = false;
 445                         } else {
 446                                 this->add_ixy(ns);
 447                                 // rewire
 448                                 this->rewire(nvs, ns);
 449                                 pn = ns;
 450                                 newly_added.push_back(pn);
 451                                 if (this->goal_found(pn, CO)) {
 452                                         this->goal_cost();
 453                                         this->tlog(this->findt());
 454                                         en_add = false;
 455                                 }
 456                         }
 457                 }
 458         }
 459         en_add = true;
 460         cusps = 0;
 461         for (auto ns: this->steer(pn, rs, 0.01)) {
 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(ns->s()) == 0 || sgn(pn->s()) != sgn(ns->s()))
 469                                 cusps++;
 470                         if (cusps > 4)
 471                                 en_add = false;
 472                         nvs = this->nv(
 473                                         ns,
 474                                         MIN(
 475                                                 GAMMA_RRTSTAR(
 476                                                         this->nodes().size()),
 477                                                 0.2)); // TODO const
 478                         this->nodes().push_back(ns);
 479                         this->add_iy(ns);
 480                         // connect
 481                         if (!this->connect(pn, ns, nvs)) {
 482                                 this->iy_[IYI(ns->y())].pop_back();
 483                                 this->nodes().pop_back();
 484                                 delete ns;
 485                                 en_add = false;
 486                         } else {
 487                                 this->add_ixy(ns);
 488                                 // rewire
 489                                 this->rewire(nvs, ns);
 490                                 pn = ns;
 491                                 newly_added.push_back(pn);
 492                                 if (this->goal_found(pn, CO)) {
 493                                         this->goal_cost();
 494                                         this->tlog(this->findt());
 495                                         en_add = false;
 496                                 }
 497                         }
 498                 }
 499         }
 500         if (this->samples().size() <= 1)
 501                 return this->goal_found();
 502         // steer to goal
 503         for (auto na: newly_added) {
 504                 pn = na;
 505                 en_add = true;
 506                 cusps = 0;
 507                 for (auto ns: this->steer(na, this->goal())) {
 508                         ns->rs(rs);
 509                         if (!en_add) {
 510                                 delete ns;
 511                         } else if (IS_NEAR(pn, ns)) {
 512                                 delete ns;
 513                         } else {
 514                                 if (sgn(pn->s()) != sgn(ns->s()))
 515                                         cusps++;
 516                                 if (cusps > 4)
 517                                         en_add = false;
 518                                 this->nodes().push_back(ns);
 519                                 this->add_iy(ns);
 520                                 pn->add_child(ns, this->cost(pn, ns));
 521                                 if (this->collide(pn, ns)) {
 522                                         pn->children().pop_back();
 523                                         ns->remove_parent();
 524                                         this->iy_[IYI(ns->y())].pop_back();
 525                                         this->nodes().pop_back();
 526                                         delete ns;
 527                                         en_add = false;
 528                                 } else {
 529                                         this->add_ixy(ns);
 530                                         this->ocost(ns);
 531                                         pn = ns;
 532                                         if (this->goal_found(pn, CO)) {
 533                                                 this->goal_cost();
 534                                                 this->tlog(this->findt());
 535                                                 en_add = false;
 536                                         }
 537                                 }
 538                         }
 539                 }
 540                 en_add = true;
 541                 cusps = 0;
 542                 for (auto ns: this->steer(pn, this->goal(), 0.01)) {
 543                         ns->rs(rs);
 544                         if (!en_add) {
 545                                 delete ns;
 546                         } else if (IS_NEAR(pn, ns)) {
 547                                 delete ns;
 548                         } else {
 549                                 if (sgn(pn->s()) != sgn(ns->s()))
 550                                         cusps++;
 551                                 if (cusps > 4)
 552                                         en_add = false;
 553                                 this->nodes().push_back(ns);
 554                                 this->add_iy(ns);
 555                                 pn->add_child(ns, this->cost(pn, ns));
 556                                 if (this->collide(pn, ns)) {
 557                                         pn->children().pop_back();
 558                                         ns->remove_parent();
 559                                         this->iy_[IYI(ns->y())].pop_back();
 560                                         this->nodes().pop_back();
 561                                         delete ns;
 562                                         en_add = false;
 563                                 } else {
 564                                         this->add_ixy(ns);
 565                                         this->ocost(ns);
 566                                         pn = ns;
 567                                         if (this->goal_found(pn, CO)) {
 568                                                 this->goal_cost();
 569                                                 this->tlog(this->findt());
 570                                                 en_add = false;
 571                                         }
 572                                 }
 573                         }
 574                 }
 575         }
 576         // steer to goals
 577         for (auto na: newly_added) {
 578         for (auto go: this->goals()) {
 579                 pn = na;
 580                 en_add = true;
 581                 cusps = 0;
 582                 for (auto ns: this->steer(na, go)) {
 583                         ns->rs(rs);
 584                         if (!en_add) {
 585                                 delete ns;
 586                         } else if (IS_NEAR(pn, ns)) {
 587                                 delete ns;
 588                         } else {
 589                                 if (sgn(pn->s()) != sgn(ns->s()))
 590                                         cusps++;
 591                                 if (cusps > 4)
 592                                         en_add = false;
 593                                 this->nodes().push_back(ns);
 594                                 this->add_iy(ns);
 595                                 pn->add_child(ns, this->cost(pn, ns));
 596                                 if (this->collide(pn, ns)) {
 597                                         pn->children().pop_back();
 598                                         ns->remove_parent();
 599                                         this->iy_[IYI(ns->y())].pop_back();
 600                                         this->nodes().pop_back();
 601                                         delete ns;
 602                                         en_add = false;
 603                                 } else {
 604                                         this->add_ixy(ns);
 605                                         this->ocost(ns);
 606                                         pn = ns;
 607                                         RRTNode *oldgoal = this->goal();
 608                                         this->goal(go);
 609                                         if (this->goal_found(pn, CO)) {
 610                                                 this->goal_cost();
 611                                                 this->tlog(this->findt());
 612                                                 en_add = false;
 613                                         } else {
 614                                                 this->goal(oldgoal);
 615                                         }
 616                                 }
 617                         }
 618                 }
 619         }}
 620         return this->goal_found();
 621 }
 622
 623 float T2::goal_cost()
 624 {
 625         std::vector<RRTNode *> nvs;
 626         nvs = this->nv(this->goal(), 0.2);
 627         for (auto nv: nvs) {
 628                 if (std::abs(this->goal()->h() - nv->h()) >=
 629                                 this->GOAL_FOUND_ANGLE)
 630                         continue;
 631                 if (nv->ccost() + this->cost(nv, this->goal()) >=
 632                                 this->goal()->ccost())
 633                         continue;
 634                 RRTNode *op; // old parent
 635                 float oc; // old cumulative cost
 636                 float od; // old direct cost
 637                 op = this->goal()->parent();
 638                 oc = this->goal()->ccost();
 639                 od = this->goal()->dcost();
 640                 nv->add_child(this->goal(),
 641                                 this->cost(nv, this->goal()));
 642                 if (this->collide(nv, this->goal())) {
 643                         nv->children().pop_back();
 644                         this->goal()->parent(op);
 645                         this->goal()->ccost(oc);
 646                         this->goal()->dcost(od);
 647                 } else {
 648                         op->rem_child(this->goal());
 649                 }
 650         }
 651         return this->goal()->ccost();
 652 }
 653
 654 T3::~T3()
 655 {
 656         for (auto n: this->p_root_.nodes())
 657                 if (n != this->p_root_.root() && n != this->p_root_.goal())
 658                         delete n;
 659         for (auto n: this->p_root_.dnodes())
 660                 if (n != this->p_root_.root() && n != this->p_root_.goal())
 661                         delete n;
 662         for (auto s: this->p_root_.samples())
 663                 if (s != this->p_root_.goal())
 664                         delete s;
 665         for (auto edges: this->p_root_.rlog())
 666                 for (auto e: edges)
 667                         delete e;
 668
 669         for (auto n: this->p_goal_.nodes())
 670                 if (n != this->p_goal_.root() && n != this->p_goal_.goal())
 671                         delete n;
 672         for (auto n: this->p_goal_.dnodes())
 673                 if (n != this->p_goal_.root() && n != this->p_goal_.goal())
 674                         delete n;
 675         for (auto s: this->p_goal_.samples())
 676                 if (s != this->p_goal_.goal())
 677                         delete s;
 678         for (auto edges: this->p_goal_.rlog())
 679                 for (auto e: edges)
 680                         delete e;
 681
 682         for (auto n: this->nodes())
 683                 if (n != this->root())
 684                         delete n;
 685         for (auto n: this->dnodes())
 686                 if (n != this->root() && n != this->goal())
 687                         delete n;
 688         for (auto s: this->samples())
 689                 if (s != this->goal())
 690                         delete s;
 691         for (auto edges: this->rlog())
 692                 for (auto e: edges)
 693                         delete e;
 694
 695         delete this->root();
 696         delete this->goal();
 697 }
 698
 699 T3::T3()
 700 {
 701         srand(static_cast<unsigned>(time(0)));
 702 }
 703
 704 T3::T3(RRTNode *init, RRTNode *goal):
 705         RRTBase(init, goal),
 706         p_root_(init, goal),
 707         p_goal_(goal, init)
 708 {
 709         srand(static_cast<unsigned>(time(0)));
 710 }
 711
 712 bool T3::next()
 713 {
 714         RRTNode *ron = nullptr;
 715         RRTNode *gon = nullptr;
 716         bool ret = false;
 717         ret = this->p_root_.next();
 718         ret |= this->p_goal_.next();
 719         if (this->overlaptrees(&ron, &gon)) {
 720                 if (this->connecttrees(ron, gon))
 721                         this->goal_found(true);
 722                 this->tlog(this->findt());
 723                 ret |= true;
 724         }
 725         return ret;
 726 }
 727
 728 bool T3::link_obstacles(
 729         std::vector<CircleObstacle> *cobstacles,
 730         std::vector<SegmentObstacle> *sobstacles)
 731 {
 732         bool ret = false;
 733         ret = RRTBase::link_obstacles(cobstacles, sobstacles);
 734         ret &= this->p_root_.link_obstacles(cobstacles, sobstacles);
 735         ret &= this->p_goal_.link_obstacles(cobstacles, sobstacles);
 736         return ret;
 737 }
 738
 739 bool T3::connecttrees(RRTNode *rn, RRTNode *gn)
 740 {
 741         while (gn != this->goal()) {
 742                 this->p_root_.nodes().push_back(new RRTNode(
 743                                 gn->x(),
 744                                 gn->y(),
 745                                 gn->h()));
 746                 rn->add_child(
 747                                 this->p_root_.nodes().back(),
 748                                 this->p_root_.cost(
 749                                                 rn,
 750                                                 this->p_root_.nodes().back()));
 751                 rn = this->p_root_.nodes().back();
 752                 gn = gn->parent();
 753         }
 754         rn->add_child(this->goal(), this->p_root_.cost(rn, this->goal()));
 755         return true;
 756 }
 757
 758 bool T3::overlaptrees(RRTNode **ron, RRTNode **gon)
 759 {
 760         for (auto rn: this->p_root_.nodes()) {
 761                 if (rn->parent() == nullptr)
 762                         continue;
 763                 for (auto gn: this->p_goal_.nodes()) {
 764                         if (gn->parent() == nullptr)
 765                                 continue;
 766                         if (IS_NEAR(rn, gn)) {
 767                                 *ron = rn;
 768                                 *gon = gn;
 769                                 return true;
 770                         }
 771                 }
 772         }
 773         return false;
 774 }
 775
 776 Klemm2015::Klemm2015(RRTNode *init, RRTNode *goal):
 777         Karaman2011(init, goal),
 778         orig_root_(init),
 779         orig_goal_(goal)
 780 {
 781         srand(static_cast<unsigned>(time(0)));
 782         this->root()->tree('R');
 783         this->goal()->tree('G');
 784         this->add_iy(this->goal());
 785 }
 786
 787 bool Klemm2015::next()
 788 {
 789         RRTNode *xn = nullptr;
 790         RRTNode *rs;
 791         int ret = 0;
 792 #if GOALFIRST > 0
 793         if (this->samples().size() == 0)
 794                 rs = this->goal();
 795         else
 796                 rs = this->sample();
 797 #else
 798         rs = this->sample();
 799 #endif
 800         this->samples().push_back(rs);
 801         //std::cerr << "next" << std::endl;
 802         if (this->extendstar1(rs, &xn) != 2) {
 803         //        if (xn) {
 804         //                std::cerr << "- xn: " << xn->x() << ", " << xn->y();
 805         //                std::cerr << std::endl;
 806         //        } else {
 807         //                std::cerr << "- xn: nullptr" << std::endl;
 808         //        }
 809                 this->swap();
 810                 ret = this->connectstar(xn);
 811         } else {
 812                 this->swap();
 813         }
 814         if (ret == 1) {
 815                 this->tlog(this->findt());
 816                 return true;
 817         }
 818         return this->goal_found();
 819 }
 820
 821 int Klemm2015::extendstar1(RRTNode *rs, RRTNode **xn)
 822 {
 823         int ret = 0; // 0 - advanced, 1 - reached, 2 - trapped
 824         char tree = this->root()->tree();
 825         //std::cerr << "extend*1" << std::endl;
 826         //std::cerr << "- tree is " << tree << std::endl;
 827         //std::cerr << "  - rs: " << rs->x() << ", " << rs->y() << std::endl;
 828         //if (xn && *xn) {
 829         //        std::cerr << "  - xn: " << (*xn)->x() << ", " << (*xn)->y();
 830         //        std::cerr << std::endl;
 831         //}
 832         //for (int i = 0; i < IYSIZE; i++) {
 833         //        if (this->iy_[i].size() > 0) {
 834         //                RRTNode *tmpn = this->iy_[i].back();
 835         //                float tmpd = EDIST(tmpn, this->goal());
 836
 837         //                std::cerr << i << ": " << tmpn->x();
 838         //                std::cerr << ", " << tmpn->y();
 839         //                std::cerr << ", " << tmpn->tree();
 840         //                std::cerr << " (" << tmpd << ")";
 841
 842         //                if (tmpn == this->root())
 843         //                        std::cerr << " root";
 844         //                if (tmpn == this->goal())
 845         //                        std::cerr << " goal";
 846         //                std::cerr << std::endl;
 847         //        }
 848         //}
 849         RRTNode *nn = this->nn(rs);
 850         //std::cerr << "  - nn: " << nn->x() << ", " << nn->y() << std::endl;
 851         std::vector<RRTNode *> nvs;
 852         std::vector<RRTNode *> steered = this->steer(nn, rs);
 853         RRTNode *ns = steered[1];
 854         ns->tree(tree);
 855         nvs = this->nv(
 856                         ns,
 857                         MIN(
 858                                 GAMMA_RRTSTAR(
 859                                         this->nodes().size()),
 860                                 0.2)); // TODO const
 861         this->nodes().push_back(ns);
 862         this->add_iy(ns);
 863         // connect
 864         if (!this->connect(nn, ns, nvs)) {
 865                 this->iy_[IYI(ns->y())].pop_back();
 866                 ret = 2;
 867         } else {
 868                 // rewire
 869                 this->rewire(nvs, ns);
 870         }
 871         for (auto n: steered) {
 872                 if (n != steered[1])
 873                         delete n;
 874         }
 875         *xn = ns;
 876         //std::cerr << "  - xn: " << (*xn)->x() << ", " << (*xn)->y();
 877         //std::cerr << std::endl;
 878         return ret;
 879 }
 880
 881 int Klemm2015::extendstarC(RRTNode *rs)
 882 {
 883         int ret = 0; // 0 - advanced, 1 - reached, 2 - trapped
 884         char tree = this->root()->tree();
 885         //std::cerr << "extend*C" << std::endl;
 886         //std::cerr << "- tree is " << tree << std::endl;
 887         //std::cerr << "  - rs: " << rs->x() << ", " << rs->y() << std::endl;
 888         RRTNode *nn = this->nn(rs);
 889         RRTNode *pn = nn;
 890         std::vector<RRTNode *> nvs;
 891         bool en_add = true;
 892         for (auto ns: this->steer(nn, rs)) {
 893                 if (!en_add) {
 894                         delete ns;
 895                 } else {
 896                         nvs = this->nv(
 897                                         ns,
 898                                         MIN(
 899                                                 GAMMA_RRTSTAR(
 900                                                         this->nodes().size()),
 901                                                 0.2)); // TODO const
 902                         this->nodes().push_back(ns);
 903                         this->add_iy(ns);
 904                         // connect
 905                         if (!this->connect(pn, ns, nvs)) {
 906                                 this->iy_[IYI(ns->y())].pop_back();
 907                                 en_add = false;
 908                                 ret = 2;
 909                         } else {
 910                                 // rewire
 911                                 this->rewire(nvs, ns);
 912                                 pn = ns;
 913 if (IS_NEAR(pn, rs)) { // GOAL FOUND !
 914         RRTNode *tmp;
 915         if (this->orig_root_ == this->root()) { // rs is in G tree
 916                 // add all rs parents to pn
 917                 tmp = rs->parent();
 918         } else { // rs is in R tree
 919                 tmp = pn->parent();
 920                 pn = rs;
 921                 this->swap();
 922         }
 923         while (tmp != this->goal()) {
 924                 this->nodes().push_back(new RRTNode(
 925                                 tmp->x(),
 926                                 tmp->y(),
 927                                 tmp->h()));
 928                 this->nodes().back()->s(tmp->s());
 929                 this->nodes().back()->tree('R');
 930                 pn->add_child(
 931                                 this->nodes().back(),
 932                                 this->cost(pn, this->nodes().back()));
 933                 pn = this->nodes().back();
 934                 tmp = tmp->parent();
 935         }
 936         pn->add_child(tmp, this->cost(pn, tmp)); // add goal()
 937         en_add = false;
 938         ret = 1;
 939 }
 940                         }
 941                 }
 942         }
 943         return ret;
 944 }
 945
 946 int Klemm2015::connectstar(RRTNode *x)
 947 {
 948         int ret = 0; // 0 - advanced, 1 - reached, 2 - trapped
 949         //std::cerr << "connect* (start)" << std::endl;
 950         ret = this->extendstarC(x);
 951         //std::cerr << "connect* (end)" << std::endl;
 952         return ret;
 953 }
 954
 955 void Klemm2015::swap()
 956 {
 957         RRTNode *tmp;
 958         tmp = this->root();
 959         this->root(this->goal());
 960         this->goal(tmp);
 961 }