void updateDrive(int controlState) {
//find desired speed
float desiredSpeed = 0;
switch (controlState & (TDC_UP | TDC_DOWN)) {
case TDC_UP: desiredSpeed = maxForwardSpeed; break;
case TDC_DOWN: desiredSpeed = maxBackwardSpeed; break;
default: return;//do nothing
}
//find current speed in forward direction
b2Vec2 currentForwardNormal = w_body->GetWorldVector(b2Vec2(0, 1));
float currentSpeed = b2Dot(getForwardVelocity(), currentForwardNormal);
//apply necessary force
float force = 0;
if (desiredSpeed > currentSpeed)
force = maxDriveForce;
else if (desiredSpeed < currentSpeed)
force = -maxDriveForce;
else
return;
//w_body->ApplyForce(currentTraction * force * currentForwardNormal, w_body->GetWorldCenter());
}
b2Vec2 getForwardVelocity() {
b2Vec2 currentForwardNormal = w_body->GetWorldVector(b2Vec2(0, 1));
return b2Dot(currentForwardNormal, w_body->GetLinearVelocity()) * currentForwardNormal;
}
b2Vec2 getLateralVelocity() {
b2Vec2 currentRightNormal = w_body->GetWorldVector(b2Vec2(1, 0));
return b2Dot(currentRightNormal, w_body->GetLinearVelocity()) * currentRightNormal;
}
