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; }