// BFS - find entry current car `cc` and corresponding goal car `gc`
unsigned int iter_cntr = 0;
while (!q.empty() && iter_cntr < 30) {
- std::cerr << "backw " << iter_cntr << std::endl;
this->cc() = BicycleCar(q.front());
q.pop();
if (this->ps().right() && this->cc().sp() < 0) {
}
// TODO if cc.h < ps.h --> continue
} else if (this->ps().right() && this->cc().sp() > 0) {
- std::cerr << "forw " << iter_cntr << std::endl;
double ccrx = this->cc().ccr().x();
double ccry = this->cc().ccr().y();