void motor_preSolve(cpConstraint motor, cpSpace space)
{
float dt = space.GetCurrentTimeStep();
float target_x = CCMouse.Instance.Position.x;
paint = new shape
{
point1 = new cpVect(target_x, -1000.0f),
point2 = new cpVect(target_x, 1000.0f),
};
float max_v = 500.0f;
float target_v = cp.cpfclamp(cp.bias_coef(0.5f, dt / 1.2f) * (target_x - balance_body.GetPosition().x) / dt, -max_v, max_v);
float error_v = (target_v - balance_body.GetVelocity().x);
float target_sin = 3.0e-3f * cp.bias_coef(0.1f, dt) * error_v / dt;
float max_sin = cp.cpfsin(0.6f);
balance_sin = cp.cpfclamp(balance_sin - 6.0e-5f * cp.bias_coef(0.2f, dt) * error_v / dt, -max_sin, max_sin);
float target_a = (float)Math.Asin(cp.cpfclamp(-target_sin + balance_sin, -max_sin, max_sin));
float angular_diff = (float)Math.Asin(cpVect.cpvcross(balance_body.GetRotation(), cpVect.cpvforangle(target_a)));
float target_w = cp.bias_coef(0.1f, dt / 0.4f) * (angular_diff) / dt;
float max_rate = 50.0f;
float rate = cp.cpfclamp(wheel_body.GetAngularVelocity() + balance_body.GetAngularVelocity() - target_w, -max_rate, max_rate);
motor.SetRate(cp.cpfclamp(rate, -max_rate, max_rate));
motor.SetMaxForce(8.0e4f);
}