{
if (this->ps().parallel())
return false;
+ else
+ return true;
double heading = atan2(
this->ps().y2() - this->ps().y1(),
this->ps().x2() - this->ps().x1()
y += (this->gc().w() / 2 + 0.01) * sin(h + dts);
y += (this->gc().dr() + 0.01) * sin(h);
} else {
- // This is for backward parking only.
+ // Forward parking
double entry_width = edist(
this->ps().x1(), this->ps().y1(),
this->ps().x4(), this->ps().y4()
);
- double dist_l =
- this->gc().orradi()
- - (this->gc().mtr() + this->gc().w() / 2)
- ;
- double move1 = dist_l + this->gc().w() / 2;
- double dist_r = entry_width - this->gc().w() - dist_l;
- double move2 = sqrt(
- pow(this->gc().iradi(), 2)
- - pow(this->gc().iradi() - dist_r, 2)
+ x += entry_width / 2 * cos(h);
+ y += entry_width / 2 * sin(h);
+ h = atan2(
+ this->ps().y2() - this->ps().y1(),
+ this->ps().x2() - this->ps().x1()
);
- move2 -= this->gc().dr() / 2; // workaround
- x += move1 * cos(h);
- y += move1 * sin(h);
- x += move2 * cos(h + dts);
- y += move2 * sin(h + dts);
- h += dts + M_PI;
+ while (h < 0) h += 2 * M_PI;
+ x += 2 * cos(h);
+ y += 2 * sin(h);
+
+ //// This is for backward parking only.
+ //double entry_width = edist(
+ // this->ps().x1(), this->ps().y1(),
+ // this->ps().x4(), this->ps().y4()
+ //);
+ //double dist_l =
+ // this->gc().orradi()
+ // - (this->gc().mtr() + this->gc().w() / 2)
+ //;
+ //double move1 = dist_l + this->gc().w() / 2;
+ //double dist_r = entry_width - this->gc().w() - dist_l;
+ //double move2 = sqrt(
+ // pow(this->gc().iradi(), 2)
+ // - pow(this->gc().iradi() - dist_r, 2)
+ //);
+ //move2 -= this->gc().dr() / 2; // workaround
+ //x += move1 * cos(h);
+ //y += move1 * sin(h);
+ //dts = atan2(
+ // this->ps().y2() - this->ps().y1(),
+ // this->ps().x2() - this->ps().x1()
+ //);
+ //while (dts < 0) dts += 2 * M_PI;
+ //x += move2 * cos(h + dts);
+ //y += move2 * sin(h + dts);
+ //h += dts - M_PI / 2;
}
while (h > M_PI)
h -= 2 * M_PI;
} else {
this->guess_gc();
this->cc() = BicycleCar(this->gc());
- this->cc().set_max_steer();
- if (this->ps().right())
- this->cc().st(this->cc().st() * -1);
- this->cc().sp(1);
+ //this->cc().set_max_steer();
+ //if (this->ps().right())
+ // this->cc().st(this->cc().st() * -1);
+ this->cc().sp(-0.2);
}
}