float x = this->slot().bnodes().front()->x();
float y = this->slot().bnodes().front()->y();
float h = this->slotHeading();
float x = this->slot().bnodes().front()->x();
float y = this->slot().bnodes().front()->y();
float h = this->slotHeading();
- h += M_PI / 2;
- nx = x + (BCAR_LENGTH - BCAR_WHEEL_BASE) / 2
- * cos(h);
- ny = y + (BCAR_LENGTH - BCAR_WHEEL_BASE) / 2
- * sin(h);
- x = nx + (BCAR_DIAG_RRADI) * cos(h - M_PI / 2);
- y = ny + (BCAR_DIAG_RRADI) * sin(h - M_PI / 2);
+ nx = x + (BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 * cos(ph);
+ ny = y + (BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 * sin(ph);
+ x = nx + (BCAR_DIAG_RRADI) * cos(h);
+ y = ny + (BCAR_DIAG_RRADI) * sin(h);
- h -= M_PI / 2;
- nx = x + (BCAR_LENGTH - BCAR_WHEEL_BASE) / 2
- * cos(h);
- ny = y + (BCAR_LENGTH - BCAR_WHEEL_BASE) / 2
- * sin(h);
- x = nx + (BCAR_DIAG_RRADI) * cos(h + M_PI / 2);
- y = ny + (BCAR_DIAG_RRADI) * sin(h + M_PI / 2);
+ nx = x + (BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 * cos(ph);
+ ny = y + (BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 * sin(ph);
+ x = nx + (BCAR_DIAG_RRADI) * cos(h);
+ y = ny + (BCAR_DIAG_RRADI) * sin(h);