{
// TODO for right parallel parking also
// it's only for lpar scenario now
-
std::vector<RRTNode *> cusp;
cusp.push_back(new RRTNode(B->x(), B->y(), B->h()));
// just copied from fip()
- this->DH(-0.01 / B->out_radi());
+ int di = 1;
+ if (this->slotSide() == LEFT)
+ di = -1;
+ this->DH(di * 0.01 / B->out_radi());
BicycleCar *c;
c = this->flncr(B);
- while (c->lfx() < 0) {
+ while (this->slot().collide(new RRTNode(c->lfx(), c->lfy(), 0))) {
cusp.push_back(new RRTNode(c->x(), c->y(), c->h()));
BicycleCar *cc = this->flncr(c);
cc->s(c->s() + 1);
BicycleCar *p;
int i = 1;
p = B->move(cc, i * this->DH());
- while (!this->slot().collide(p->frame())
- && (
- (this->DH() > 0 && p->x() >= 0)
- || (this->DH() < 0 && p->lfx() <= 0)
- )) {
+ while (
+ !this->slot().collide(p->frame())
+ && this->slot().collide(new RRTNode(p->lfx(), p->lfy(), 0))
+ ) {
delete p;
i += 10;
p = B->move(cc, i * this->DH());
i -= 10;
p = B->move(cc, i * this->DH());
while (!this->slot().collide(p->frame())) {
- if (this->DH() > 0 && p->x() <= 0) {
- i += 1;
- break;
- }
- if (this->DH() < 0 && p->lfx() >= 0) {
+ if(!this->slot().collide(new RRTNode(p->lfx(), p->lfy(), 0))) {
i += 1;
break;
}
+ //if (this->DH() < 0 && p->lfx() >= 0) {
+ // i += 1;
+ // break;
+ //}
delete p;
i += 1;
p = B->move(cc, i * this->DH());