bool PSPlanner::forward()
{
- double heading = this->ps().heading();
+ if (this->ps().parallel())
+ return false;
+ double heading = atan2(
+ this->ps().y2() - this->ps().y1(),
+ this->ps().x2() - this->ps().x1()
+ );
while (heading < 0) heading += 2 * M_PI;
- if (!this->ps().parallel())
- heading -= M_PI / 2;
double h = this->gc().h();
while (h < 0) h += 2 * M_PI;
- if (-0.00001 < heading - h && heading - h < 0.00001)
+ if (std::abs(heading - h) < M_PI / 4)
return true;
- else
- return false;
+ return false;
}
void PSPlanner::guess_gc()