bool T2::next()
{
RRTNode *rs;
-#if GOALFIRST > 0
- if (this->samples().size() == 0)
- rs = this->goal();
- else
+ if (this->firsts().size() > 0) {
+ rs = this->firsts().front();
+ this->firsts().pop();
+ } else {
rs = this->sample();
-#else
- rs = this->sample();
-#endif
+ }
this->samples().push_back(rs);
RRTNode *nn = this->nn(rs);
if (!nn)