// other
void ParallelSlot::fip()
{
- std::queue<RRTNode *, std::list<RRTNode *>> q;
+ // see https://courses.cs.washington.edu/courses/cse326/03su/homework/hw3/bfs.html
+ // RRTNode.s() works as iteration level
+ std::queue<BicycleCar *, std::list<BicycleCar *>> q;
+
+ // TODO add init nodes
+ // for now just copy fpose()
+ bool left = false; // right parking slot
+ float di = -1;
+ BicycleCar *CC = new BicycleCar(
+ this->fposecenter()->x(),
+ this->fposecenter()->y() - 0.01,
+ M_PI / 2
+ );
+ BicycleCar *B = new BicycleCar(
+ CC->x() - CC->width() / 2,
+ CC->y() - (CC->length() + CC->wheelbase()) / 2,
+ M_PI / 2
+ );
+ if (this->slot().bnodes()[0]->x() > this->slot().bnodes()[1]->x()) {
+ left = true;
+ di = 1;
+ delete B;
+ B = new BicycleCar(
+ CC->x() + CC->width() / 2,
+ CC->y() - (CC->length() + CC->wheelbase()) / 2,
+ M_PI / 2
+ );
+ }
+ this->DH(di * 0.01 / CC->out_radi());
+ BicycleCar *c;
+ int i = 0;
+ c = B->move(CC, -i * di * 0.01 / CC->diag_radi());
+ while (!this->slot().collide(c->frame())) {
+ q.push(c);
+ c = B->move(CC, -i * di * 0.01 / CC->diag_radi());
+ i += 1;
+ }
+ delete c; // not in q and collide
+ // BFS
+ while (!q.empty()) {
+ c = q.front();
+ q.pop();
+ if (this->DH() > 0 && c->rfx() <= 0 && c->rrx() <= 0) {
+ // TODO goal found
+ } else if (this->DH() < 0 && c->lfx() >= 0 && c->lrx() >= 0) {
+ goto createcuspandfinish;
+ } else if (c->s() < 9) {
+ BicycleCar *cc = this->flnc(c);
+ cc->s(c->s() + 1);
+ cc->bcparent(c);
+ q.push(cc);
+ } else {
+ delete c; // not in q and collide
+ }
+ }
+ // delete all from q
+ // return c
}
RRTNode *ParallelSlot::fposecenter()